add InverseDynamics example for example browser

add CMake support for BulletInverseDynamics and BulletInverseDynamicsUtils

CMakeLists.txt

@@ -278,6 +278,7 @@ ENDIF()
 
 set (BULLET_CONFIG_CMAKE_PATH lib${LIB_SUFFIX}/cmake/bullet )
 list (APPEND BULLET_LIBRARIES LinearMath)
+list (APPEND BULLET_LIBRARIES BulletInverseDynamics)
 list (APPEND BULLET_LIBRARIES BulletCollision)
 list (APPEND BULLET_LIBRARIES BulletDynamics)
 list (APPEND BULLET_LIBRARIES BulletSoftBody)

Extras/CMakeLists.txt

@@ -1,4 +1,4 @@
-SUBDIRS( Serialize ConvexDecomposition HACD GIMPACTUtils )
+SUBDIRS( Serialize InverseDynamics ConvexDecomposition HACD GIMPACTUtils )
 
 #Maya Dynamica plugin is moved to http://dynamica.googlecode.com
 

Extras/InverseDynamics/BulletInverseDynamicsUtilsCommon.h

@@ -0,0 +1,17 @@
+#ifndef BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H
+#define BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H
+#include "CoilCreator.hpp"
+#include "MultiBodyTreeCreator.hpp"
+#include "btMultiBodyFromURDF.hpp"
+#include "DillCreator.hpp"
+#include "MultiBodyTreeDebugGraph.hpp"
+#include "btMultiBodyTreeCreator.hpp"
+#include "IDRandomUtil.hpp"
+#include "SimpleTreeCreator.hpp"
+#include "invdyn_bullet_comparison.hpp"
+#include "MultiBodyNameMap.hpp"
+#include "User2InternalIndex.hpp"
+
+#endif//BULLET_INVERSE_DYNAMICS_UTILS_COMMON_H
+
+

Extras/InverseDynamics/CMakeLists.txt

@@ -0,0 +1,45 @@
+INCLUDE_DIRECTORIES( 
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+ADD_LIBRARY(
+BulletInverseDynamicsUtils
+CoilCreator.cpp
+MultiBodyTreeCreator.cpp
+btMultiBodyTreeCreator.cpp
+DillCreator.cpp
+MultiBodyTreeDebugGraph.cpp
+invdyn_bullet_comparison.cpp
+IDRandomUtil.cpp
+SimpleTreeCreator.cpp
+MultiBodyNameMap.cpp
+User2InternalIndex.cpp
+)
+
+SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (BUILD_SHARED_LIBS)
+        TARGET_LINK_LIBRARIES(BulletInverseDynamicsUtils LinearMath)
+ENDIF (BUILD_SHARED_LIBS)
+
+IF (INSTALL_EXTRA_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletInverseDynamicsUtils  DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletInverseDynamicsUtils DESTINATION lib${LIB_SUFFIX})
+				INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h"  PATTERN
+".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
+			ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+		ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+
+		IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+			SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils  PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(BulletInverseDynamicsUtils PROPERTIES PUBLIC_HEADER "BulletInverseDynamicsUtilsCommon.h" )
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_EXTRA_LIBS)

examples/CommonInterfaces/CommonParameterInterface.h

@@ -25,7 +25,7 @@ struct SliderParams
 	m_callback(0),
 	m_paramValuePointer(targetValuePointer),
 	m_userPointer(0),
-	m_clampToNotches(false),
+	m_clampToNotches(true),
     m_showValues(true)
 	{
 	}

examples/ExampleBrowser/CMakeLists.txt

@@ -36,6 +36,8 @@ SET(App_ExampleBrowser_SRCS
 	../SharedMemory/PhysicsServerCommandProcessor.h
 	../BasicDemo/BasicExample.cpp
 	../BasicDemo/BasicExample.h
+	../InverseDynamics/InverseDynamicsExample.cpp
+	../InverseDynamics/InverseDynamicsExample.h
 	../ForkLift/ForkLiftDemo.cpp
 	../ForkLift/ForkLiftDemo.h
 	../Tutorial/Tutorial.cpp
@@ -191,7 +193,7 @@ SET(App_ExampleBrowser_SRCS
 )
 
 LINK_LIBRARIES(
-        Bullet3Common BulletSoftBody BulletDynamics BulletCollision LinearMath OpenGLWindow gwen ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+        Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
 )
 
 IF (WIN32)

examples/ExampleBrowser/ExampleEntries.cpp

@@ -40,7 +40,7 @@
 #include "../Tutorial/Tutorial.h"
 #include "../Tutorial/Dof6ConstraintTutorial.h"
 #include "../MultiThreading/MultiThreadingExample.h"
-//#include "../InverseDynamics/InverseDynamicsExample.h"
+#include "../InverseDynamics/InverseDynamicsExample.h"
 
 #ifdef ENABLE_LUA
 #include "../LuaDemo/LuaPhysicsSetup.h"
@@ -110,9 +110,11 @@ static ExampleEntry gDefaultExamples[]=
  ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 	ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
 
-//	ExampleEntry(0,"Inverse Dynamics"),
+	 ExampleEntry(0,"Inverse Dynamics"),
-//	ExampleEntry(1,"Inverse Dynamics URDF", "write some explanation", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
+      ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
-//	ExampleEntry(1,"Inverse Dynamics Prog", "write some explanation2", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
+      ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
+
+
 	
 	ExampleEntry(0,"Tutorial"),
 	ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY),

examples/ExampleBrowser/premake4.lua

@@ -29,7 +29,7 @@
 	end
 
 			
-		links{"gwen", "OpenGL_Window","BulletSoftBody", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
+		links{"gwen", "OpenGL_Window","BulletSoftBody", "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision","LinearMath","Bullet3Common"}
 		initOpenGL()
 		initGlew()
 

examples/InverseDynamics/InverseDynamicsExample.cpp

@@ -32,16 +32,35 @@ subject to the following restrictions:
 
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
 
-//those are static global to make it easy for the GUI to tweak them
+#include "BulletInverseDynamics/IDConfig.hpp"
-static btScalar radius(0.2);
+#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
-static btScalar kp = 100;
+
-static btScalar kd = 20;
+#include "../RenderingExamples/TimeSeriesCanvas.h"
-static btScalar maxForce = 100;
+
+#include <unistd.h>
+#include <vector>
+
+// the UI interface makes it easier to use static variables & free functions
+// as parameters and callbacks
+static btScalar kp =10*10;
+static btScalar kd = 2*10;
+static bool useInverseModel = true;
+static std::vector<btScalar> qd;
+static std::vector<std::string> qd_name;
+static std::vector<std::string> q_name;
+
+void toggleUseInverseModel(int buttonId, bool buttonState, void* userPointer){
+    useInverseModel=!useInverseModel;
+    // todo(thomas) is there a way to get a toggle button with changing text?
+    b3Printf("switched inverse model %s", useInverseModel?"on":"off");
+}
 
 class InverseDynamicsExample : public CommonMultiBodyBase
 {
     btInverseDynamicsExampleOptions m_option;
     btMultiBody* m_multiBody;
+    btInverseDynamics::MultiBodyTree *m_inverseModel;
+    TimeSeriesCanvas* m_timeSeriesCanvas;
 public:
     InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option);
     virtual ~InverseDynamicsExample();
@@ -64,13 +83,17 @@ public:
 InverseDynamicsExample::InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option)
     :CommonMultiBodyBase(helper),
     m_option(option),
-	m_multiBody(0)
+    m_multiBody(0),
+    m_inverseModel(0),
+    m_timeSeriesCanvas(0)
 {
 }
 
 InverseDynamicsExample::~InverseDynamicsExample()
 {
-	
+    delete m_multiBody;
+    delete m_inverseModel;
+    delete m_timeSeriesCanvas;
 }
 
 //todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this.
@@ -91,23 +114,25 @@ void InverseDynamicsExample::initPhysics()
 
     {
         SliderParams slider("Kp",&kp);
-		slider.m_minVal=-200;
+        slider.m_minVal=0;
-		slider.m_maxVal=200;
+        slider.m_maxVal=2000;
         m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
     }
     {
         SliderParams slider("Kd",&kd);
-		slider.m_minVal=-50;
+        slider.m_minVal=0;
         slider.m_maxVal=50;
         m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
     }
+
+    if (m_option == BT_ID_PROGRAMMATICALLY)
     {
-		SliderParams slider("max force",&maxForce);
+        ButtonParams button("toggle inverse model",0,true);
-		slider.m_minVal=0;
+        button.m_callback = toggleUseInverseModel;
-		slider.m_maxVal=100;
+        m_guiHelper->getParameterInterface()->registerButtonParameter(button);
-		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
     }
 
+   
     switch (m_option)
     {
     case BT_ID_LOAD_URDF:
@@ -127,8 +152,8 @@ void InverseDynamicsExample::initPhysics()
                     {
                         //kuka without joint control/constraints will gain energy explode soon due to timestep/integrator
                         //temporarily set some extreme damping factors until we have some joint control or constraints
-						m_multiBody->setAngularDamping(0.99);
+                        m_multiBody->setAngularDamping(0*0.99);
-						m_multiBody->setLinearDamping(0.99);
+                        m_multiBody->setLinearDamping(0*0.99);
                         b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
                     }
             }
@@ -147,14 +172,104 @@ void InverseDynamicsExample::initPhysics()
             b3Assert(0);
         }
     };
+
+
+    if(m_multiBody) {
+        {
+            m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "q time series");
+            m_timeSeriesCanvas ->setupTimeSeries(3,100, 0);
+           
+        }
+        
+        // construct inverse model
+        btInverseDynamics::btMultiBodyTreeCreator id_creator;
+        if (-1 == id_creator.createFromBtMultiBody(m_multiBody, false)) {
+            b3Error("error creating tree\n");
+        } else {
+            m_inverseModel = btInverseDynamics::CreateMultiBodyTree(id_creator);
+        }
+        // add joint target controls
+        qd.resize(m_multiBody->getNumDofs());
+        qd_name.resize(m_multiBody->getNumDofs());
+       	q_name.resize(m_multiBody->getNumDofs()); 
+	for(std::size_t dof=0;dof<qd.size();dof++) {
+            qd[dof] = 0;
+            char tmp[25];
+            snprintf(tmp,25,"q_desired[%zu]",dof);
+            qd_name[dof] = tmp;
+            SliderParams slider(qd_name[dof].c_str(),&qd[dof]);
+            slider.m_minVal=-3.14;
+            slider.m_maxVal=3.14;
+         snprintf(tmp,25,"q[%zu]",dof); 
+		q_name[dof] = tmp;   
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+		m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), 255,0,0);
+         }
+        
+        
+    }
+    
+    
+
 }
 
 
 void InverseDynamicsExample::stepSimulation(float deltaTime)
 {
+    static uint64_t call=0;
+    call++;
+    if(m_multiBody) {
+        const int num_dofs=m_multiBody->getNumDofs();
+        btInverseDynamics::vecx nu(num_dofs), qdot(num_dofs), q(num_dofs),joint_force(num_dofs);
+        btInverseDynamics::vecx pd_control(num_dofs);
+
+        // compute joint forces from one of two control laws:
+        // 1) "computed torque" control, which gives perfect, decoupled,
+        //    linear second order error dynamics per dof in case of a
+        //    perfect model and (and negligible time discretization effects)
+        // 2) decoupled PD control per joint, without a model
+        for(int dof=0;dof<num_dofs;dof++) {
+            q(dof) = m_multiBody->getJointPos(dof);
+            qdot(dof)= m_multiBody->getJointVel(dof);
+
+            const btScalar qd_dot=0;
+            const btScalar qd_ddot=0;
+            m_timeSeriesCanvas->insertDataAtCurrentTime(q[dof],dof,true);
+            
+            // pd_control is either desired joint torque for pd control,
+            // or the feedback contribution to nu
+            pd_control(dof) = kd*(qd_dot-qdot(dof)) + kp*(qd[dof]-q(dof));
+            // nu is the desired joint acceleration for computed torque control
+            nu(dof) = qd_ddot + pd_control(dof);
+
+        }
+        // calculate joint forces corresponding to desired accelerations nu
+        if(-1 != m_inverseModel->calculateInverseDynamics(q,qdot,nu,&joint_force)) {
+            for(int dof=0;dof<num_dofs;dof++) {
+                if(useInverseModel) {
+                    //joint_force(dof) += damping*dot_q(dof);
+                    // use inverse model: apply joint force corresponding to
+                    // desired acceleration nu
+                    m_multiBody->addJointTorque(dof,joint_force(dof));
+                } else {
+                    // no model: just apply PD control law
+                    m_multiBody->addJointTorque(dof,pd_control(dof));
+                }
+            }
+        }
+    }
+
+    if (m_timeSeriesCanvas)
+        m_timeSeriesCanvas->nextTick();
+
+    //todo: joint damping for btMultiBody, tune parameters
+    
+    // step the simulation
     if (m_dynamicsWorld)
     {
-		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
+        // todo(thomas) check that this is correct:
+        // want to advance by 10ms, with 1ms timesteps
+        m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3);
     }
 }
 

src/BulletInverseDynamics/CMakeLists.txt

@@ -0,0 +1,66 @@
+INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/src  )
+
+SET(BulletInverseDynamics_SRCS
+	IDMath.cpp
+	MultiBodyTree.cpp
+	details/MultiBodyTreeInitCache.cpp
+	details/MultiBodyTreeImpl.cpp
+)
+
+SET(BulletInverseDynamicsRoot_HDRS
+	IDConfig.hpp
+	IDConfigEigen.hpp
+	IDMath.hpp
+	IDConfigBuiltin.hpp
+	IDErrorMessages.hpp
+	MultiBodyTree.hpp
+)
+SET(BulletInverseDynamicsDetails_HDRS
+	details/IDEigenInterface.hpp
+	details/IDMatVec.hpp
+	details/IDLinearMathInterface.hpp
+	details/MultiBodyTreeImpl.hpp
+	details/MultiBodyTreeInitCache.hpp
+)
+
+SET(BulletInverseDynamics_HDRS
+	${BulletInverseDynamicsRoot_HDRS}
+	${BulletInverseDynamicsDetails_HDRS}
+)
+
+
+ADD_LIBRARY(BulletInverseDynamics ${BulletInverseDynamics_SRCS} ${BulletInverseDynamics_HDRS})
+SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES SOVERSION ${BULLET_VERSION})
+IF (BUILD_SHARED_LIBS)
+  TARGET_LINK_LIBRARIES(BulletInverseDynamics LinearMath)
+ENDIF (BUILD_SHARED_LIBS)
+
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#INSTALL of other files requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletInverseDynamics DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS BulletInverseDynamics RUNTIME DESTINATION bin
+								LIBRARY DESTINATION lib${LIB_SUFFIX}
+								ARCHIVE DESTINATION lib${LIB_SUFFIX})
+				INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
+				INSTALL(FILES ../btBulletCollisionCommon.h
+DESTINATION ${INCLUDE_INSTALL_DIR}/BulletInverseDynamics)
+			ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+		ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+		
+		IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+			SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES FRAMEWORK true)
+		
+			SET_TARGET_PROPERTIES(BulletInverseDynamics PROPERTIES PUBLIC_HEADER "${BulletInverseDynamicsRoot_HDRS}")
+			# Have to list out sub-directories manually:
+			SET_PROPERTY(SOURCE ${BulletInverseDynamicsDetails_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/details)
+		
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

src/CMakeLists.txt

@@ -3,7 +3,7 @@ IF(BUILD_BULLET3)
 	SUBDIRS(  Bullet3OpenCL Bullet3Serialize/Bullet2FileLoader Bullet3Dynamics Bullet3Collision Bullet3Geometry Bullet3Common )
 ENDIF(BUILD_BULLET3)
 
-SUBDIRS(  BulletSoftBody BulletCollision BulletDynamics LinearMath )
+SUBDIRS(  BulletSoftBody BulletInverseDynamics BulletCollision BulletDynamics LinearMath )
 
 
 IF(INSTALL_LIBS)