Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add InverseDynamics example for example browser

Browse Source 
add CMake support for BulletInverseDynamics and BulletInverseDynamicsUtils
This commit is contained in:
Erwin Coumans
2015-11-24 11:12:02 -08:00
parent b4701613c8
commit 03cc4f0554
12 changed files with 347 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -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)

2
Extras/CMakeLists.txt
View File

@@ -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

17
Extras/InverseDynamics/BulletInverseDynamicsUtilsCommon.h Normal file
View File

@@ -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

45
Extras/InverseDynamics/CMakeLists.txt Normal file
View File

@@ -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)

BIN
data/multibody.bullet
View File

Binary file not shown.

2
examples/CommonInterfaces/CommonParameterInterface.h
View File

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

4
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -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)

10
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -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),

2
examples/ExampleBrowser/premake4.lua
View File

@@ -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()

149
examples/InverseDynamics/InverseDynamicsExample.cpp
View File

@@ -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
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 <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_maxVal=200;
slider.m_minVal=0;
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);
slider.m_minVal=0;
slider.m_maxVal=100;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
ButtonParams button("toggle inverse model",0,true);
button.m_callback = toggleUseInverseModel;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
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->setLinearDamping(0.99);
m_multiBody->setAngularDamping(0*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);
}
}

66
src/BulletInverseDynamics/CMakeLists.txt Normal file
View File

@@ -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)

2
src/CMakeLists.txt
View File

@@ -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)