From 03cc4f055486b8f4aa5a4e8d79580a3d7a247fb1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 24 Nov 2015 11:12:02 -0800 Subject: [PATCH] add InverseDynamics example for example browser add CMake support for BulletInverseDynamics and BulletInverseDynamicsUtils --- CMakeLists.txt | 1 + Extras/CMakeLists.txt | 2 +- .../BulletInverseDynamicsUtilsCommon.h | 17 + Extras/InverseDynamics/CMakeLists.txt | 45 +++ data/multibody.bullet | Bin 14520 -> 14520 bytes .../CommonParameterInterface.h | 2 +- examples/ExampleBrowser/CMakeLists.txt | 4 +- examples/ExampleBrowser/ExampleEntries.cpp | 10 +- examples/ExampleBrowser/premake4.lua | 2 +- .../InverseDynamicsExample.cpp | 295 ++++++++++++------ src/BulletInverseDynamics/CMakeLists.txt | 66 ++++ src/CMakeLists.txt | 2 +- 12 files changed, 347 insertions(+), 99 deletions(-) create mode 100644 Extras/InverseDynamics/BulletInverseDynamicsUtilsCommon.h create mode 100644 Extras/InverseDynamics/CMakeLists.txt create mode 100644 src/BulletInverseDynamics/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 72374090a..631c0fae6 100644 --- a/CMakeLists.txt +++ b/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) diff --git a/Extras/CMakeLists.txt b/Extras/CMakeLists.txt index 24fc0f159..d7e1d3e04 100644 --- a/Extras/CMakeLists.txt +++ b/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 diff --git a/Extras/InverseDynamics/BulletInverseDynamicsUtilsCommon.h b/Extras/InverseDynamics/BulletInverseDynamicsUtilsCommon.h new file mode 100644 index 000000000..25e77cda6 --- /dev/null +++ b/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 + + diff --git a/Extras/InverseDynamics/CMakeLists.txt b/Extras/InverseDynamics/CMakeLists.txt new file mode 100644 index 000000000..893bc1a60 --- /dev/null +++ b/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) diff --git a/data/multibody.bullet b/data/multibody.bullet index 0cc941bd5ca3f341052c09977678d08712f6128e..664da01928e31c0ca7cfaf3af502469a329a5221 100644 GIT binary patch delta 869 zcmbu-OG_J36bJBolgT6z5#uW+PIauKiS;cSsSq_TTuH6OECs~}1g$1UL2+U56F3}K zu3Y2`6uVGp+Pa^B3s+E&*?1!&I2=R=ko;1Q!` zSI|DJrG8icY_-S_sSa69R4h5hEvI+s(@Bn%>mk7p6@o5YlYTCxVS@thAbr&}&pKu) zo}COKE6)&jfcth;KBwkf=bnVBbG`@%!2SF_TaX04cMtZ#1b*)k;BPh#6JSC&;PvcW zfC}ztoJ*U)x30o}eL6eacx7molu7#TNzsnKj?O&|_CqcG@qD3reMaNMq$S;wZaUH< zmFYjd+s$Q1x9C!jYC=j$hS!qAl<)=v61(6{^C&s#<tH~~%TVr_0khf<(1omN;8mvT!TX>cv}tXpEg4A% zCDamX={H3WMIOWBczb?Qp!v`7Ya;K=k;moz;mHmigt|JW0=D@MkXO#AMT9HeyNzl?ef$8YJXXl=R?i|PPtJ>Wm#foknp maWnq%tR>pBR)Shk2zUm2R=#8GYbfoGs!Zbg=JeftYW)Rs6pB#* diff --git a/examples/CommonInterfaces/CommonParameterInterface.h b/examples/CommonInterfaces/CommonParameterInterface.h index 1d79b8329..dd8554dac 100644 --- a/examples/CommonInterfaces/CommonParameterInterface.h +++ b/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) { } diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 706199489..d775d9d51 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/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) diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 45065f447..692b74936 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/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(1,"Inverse Dynamics URDF", "write some explanation", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF), -// ExampleEntry(1,"Inverse Dynamics Prog", "write some explanation2", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY), + ExampleEntry(0,"Inverse Dynamics"), + 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", "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), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index a30f0b950..624f70e8a 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/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() diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index 9548d8591..8954f61a7 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -4,8 +4,8 @@ Copyright (c) 2015 Google Inc. http://bulletphysics.org This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. @@ -32,45 +32,68 @@ subject to the following restrictions: #include "../CommonInterfaces/CommonRigidBodyBase.h" -//those are static global to make it easy for the GUI to tweak them -static btScalar radius(0.2); -static btScalar kp = 100; -static btScalar kd = 20; -static btScalar maxForce = 100; +#include "BulletInverseDynamics/IDConfig.hpp" +#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" + +#include "../RenderingExamples/TimeSeriesCanvas.h" + +#include +#include + +// 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 qd; +static std::vector qd_name; +static std::vector 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; + btInverseDynamicsExampleOptions m_option; + btMultiBody* m_multiBody; + btInverseDynamics::MultiBodyTree *m_inverseModel; + TimeSeriesCanvas* m_timeSeriesCanvas; public: InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option); virtual ~InverseDynamicsExample(); - virtual void initPhysics(); - virtual void stepSimulation(float deltaTime); + virtual void initPhysics(); + virtual void stepSimulation(float deltaTime); void setFileName(const char* urdfFileName); - virtual void resetCamera() - { - float dist = 3.5; - float pitch = -136; - float yaw = 28; - float targetPos[3]={0.47,0,-0.64}; - m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); - } + virtual void resetCamera() + { + float dist = 3.5; + float pitch = -136; + float yaw = 28; + float targetPos[3]={0.47,0,-0.64}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } }; InverseDynamicsExample::InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option) - :CommonMultiBodyBase(helper), - m_option(option), - m_multiBody(0) + :CommonMultiBodyBase(helper), + m_option(option), + 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. @@ -78,89 +101,181 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU void InverseDynamicsExample::initPhysics() { - //roboticists like Z up - int upAxis = 2; - m_guiHelper->setUpAxis(upAxis); + //roboticists like Z up + int upAxis = 2; + m_guiHelper->setUpAxis(upAxis); - createEmptyDynamicsWorld(); - btVector3 gravity(0,0,0); - gravity[upAxis]=-9.8; - m_dynamicsWorld->setGravity(gravity); + createEmptyDynamicsWorld(); + btVector3 gravity(0,0,0); + gravity[upAxis]=-9.8; + m_dynamicsWorld->setGravity(gravity); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - { - SliderParams slider("Kp",&kp); - slider.m_minVal=-200; - slider.m_maxVal=200; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + { + SliderParams slider("Kp",&kp); + slider.m_minVal=0; + slider.m_maxVal=2000; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - { - SliderParams slider("Kd",&kd); - slider.m_minVal=-50; - slider.m_maxVal=50; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - } - { - SliderParams slider("max force",&maxForce); - slider.m_minVal=0; - slider.m_maxVal=100; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + { + SliderParams slider("Kd",&kd); + slider.m_minVal=0; + slider.m_maxVal=50; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - switch (m_option) - { - case BT_ID_LOAD_URDF: - { - BulletURDFImporter u2b(m_guiHelper); - bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf"); - if (loadOk) - { - int rootLinkIndex = u2b.getRootLinkIndex(); - b3Printf("urdf root link index = %d\n",rootLinkIndex); - MyMultiBodyCreator creation(m_guiHelper); - btTransform identityTrans; - identityTrans.setIdentity(); - ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); - m_multiBody = creation.getBulletMultiBody(); - if (m_multiBody) - { - //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->setLinearDamping(0.99); - b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); - } - } - break; - } - case BT_ID_PROGRAMMATICALLY: - { - btTransform baseWorldTrans; - baseWorldTrans.setIdentity(); - m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); - break; - } - default: - { - b3Error("Unknown option in InverseDynamicsExample::initPhysics"); - b3Assert(0); - } - }; + if (m_option == BT_ID_PROGRAMMATICALLY) + { + ButtonParams button("toggle inverse model",0,true); + button.m_callback = toggleUseInverseModel; + m_guiHelper->getParameterInterface()->registerButtonParameter(button); + } + + + switch (m_option) + { + case BT_ID_LOAD_URDF: + { + BulletURDFImporter u2b(m_guiHelper); + bool loadOk = u2b.loadURDF("kuka_lwr/kuka.urdf"); + if (loadOk) + { + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_guiHelper); + btTransform identityTrans; + identityTrans.setIdentity(); + ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); + m_multiBody = creation.getBulletMultiBody(); + if (m_multiBody) + { + //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*0.99); + m_multiBody->setLinearDamping(0*0.99); + b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); + } + } + break; + } + case BT_ID_PROGRAMMATICALLY: + { + btTransform baseWorldTrans; + baseWorldTrans.setIdentity(); + m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); + break; + } + default: + { + b3Error("Unknown option in 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;dofgetParameterInterface()->registerSliderFloatParameter(slider); + m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), 255,0,0); + } + + + } + + + } void InverseDynamicsExample::stepSimulation(float deltaTime) { - if (m_dynamicsWorld) - { - m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); - } + 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;dofgetJointPos(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;dofaddJointTorque(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) + { + // todo(thomas) check that this is correct: + // want to advance by 10ms, with 1ms timesteps + m_dynamicsWorld->stepSimulation(1e-3,0);//,1e-3); + } } CommonExampleInterface* InverseDynamicsExampleCreateFunc(CommonExampleOptions& options) { - return new InverseDynamicsExample(options.m_guiHelper, btInverseDynamicsExampleOptions(options.m_option)); + return new InverseDynamicsExample(options.m_guiHelper, btInverseDynamicsExampleOptions(options.m_option)); } diff --git a/src/BulletInverseDynamics/CMakeLists.txt b/src/BulletInverseDynamics/CMakeLists.txt new file mode 100644 index 000000000..d2617ea89 --- /dev/null +++ b/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) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f6755e520..712139ff0 100644 --- a/src/CMakeLists.txt +++ b/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)