From ad29d277000e8bf98e84a5222a26a35556b241ef Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 13 Nov 2015 10:37:43 -0800 Subject: [PATCH 1/7] add initial inverse dynamics example skeleton, with urdf and programmatically created btMultiBody. disabled in Bullet/examples/ExampleBrowser/ExampleEntries.cpp --- .../CommonInterfaces/CommonMultiBodyBase.h | 5 + examples/ExampleBrowser/ExampleEntries.cpp | 5 + examples/ExampleBrowser/premake4.lua | 2 + .../ImportURDFDemo/MyMultiBodyCreator.h | 1 + .../InverseDynamicsExample.cpp | 167 ++++++ .../InverseDynamics/InverseDynamicsExample.h | 28 + .../MultiBody/InvertedPendulumPDControl.cpp | 533 +++++++++--------- 7 files changed, 475 insertions(+), 266 deletions(-) create mode 100644 examples/InverseDynamics/InverseDynamicsExample.cpp create mode 100644 examples/InverseDynamics/InverseDynamicsExample.h diff --git a/examples/CommonInterfaces/CommonMultiBodyBase.h b/examples/CommonInterfaces/CommonMultiBodyBase.h index a2ed9d562..3219f8294 100644 --- a/examples/CommonInterfaces/CommonMultiBodyBase.h +++ b/examples/CommonInterfaces/CommonMultiBodyBase.h @@ -118,14 +118,19 @@ struct CommonMultiBodyBase : public CommonExampleInterface m_collisionShapes.clear(); delete m_dynamicsWorld; + m_dynamicsWorld = 0; delete m_solver; + m_solver=0; delete m_broadphase; + m_broadphase=0; delete m_dispatcher; + m_dispatcher=0; delete m_collisionConfiguration; + m_collisionConfiguration=0; } virtual void syncPhysicsToGraphics() diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e6f84bab3..45065f447 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -40,6 +40,8 @@ #include "../Tutorial/Tutorial.h" #include "../Tutorial/Dof6ConstraintTutorial.h" #include "../MultiThreading/MultiThreadingExample.h" +//#include "../InverseDynamics/InverseDynamicsExample.h" + #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" #endif @@ -108,6 +110,9 @@ 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,"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 d1503e05f..249b0e636 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -65,6 +65,8 @@ "../MultiThreading/b3PosixThreadSupport.cpp", "../MultiThreading/b3Win32ThreadSupport.cpp", "../MultiThreading/b3ThreadSupportInterface.cpp", + "../InverseDynamics/InverseDynamicsExample.cpp", + "../InverseDynamics/InverseDynamicsExample.h", "../BasicDemo/BasicExample.*", "../Tutorial/*", "../Collision/*", diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h index b19146022..7334fbd90 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h @@ -21,6 +21,7 @@ struct GenericConstraintUserInfo class MyMultiBodyCreator : public MultiBodyCreationInterface { +protected: btMultiBody* m_bulletMultiBody; diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp new file mode 100644 index 000000000..8b67ccfba --- /dev/null +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -0,0 +1,167 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#include "InverseDynamicsExample.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "Bullet3Common/b3FileUtils.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "../../Utils/b3ResourcePath.h" +#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../CommonInterfaces/CommonMultiBodyBase.h" + +#include "btBulletDynamicsCommon.h" + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +#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; + +class InverseDynamicsExample : public CommonMultiBodyBase +{ + btInverseDynamicsExampleOptions m_option; + btMultiBody* m_multiBody; +public: + InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option); + virtual ~InverseDynamicsExample(); + + 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]); + } +}; + +InverseDynamicsExample::InverseDynamicsExample(struct GUIHelperInterface* helper, btInverseDynamicsExampleOptions option) + :CommonMultiBodyBase(helper), + m_option(option), + m_multiBody(0) +{ +} + +InverseDynamicsExample::~InverseDynamicsExample() +{ + +} + +//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this. +btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans); + +void InverseDynamicsExample::initPhysics() +{ + //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); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + SliderParams slider("Kp",&kp); + slider.m_minVal=-200; + slider.m_maxVal=200; + 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); + } + + 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())); + } + } + 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); + } + }; +} + + +void InverseDynamicsExample::stepSimulation(float deltaTime) +{ + if (m_dynamicsWorld) + { + m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); + } +} + +CommonExampleInterface* InverseDynamicsExampleCreateFunc(CommonExampleOptions& options) +{ + return new InverseDynamicsExample(options.m_guiHelper, btInverseDynamicsExampleOptions(options.m_option)); +} + + + diff --git a/examples/InverseDynamics/InverseDynamicsExample.h b/examples/InverseDynamics/InverseDynamicsExample.h new file mode 100644 index 000000000..13c37a490 --- /dev/null +++ b/examples/InverseDynamics/InverseDynamicsExample.h @@ -0,0 +1,28 @@ +/* +Bullet Continuous Collision Detection and Physics Library +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, +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef INVERSE_DYNAMICS_EXAMPLE_H +#define INVERSE_DYNAMICS_EXAMPLE_H + +enum btInverseDynamicsExampleOptions +{ + BT_ID_LOAD_URDF=1, + BT_ID_PROGRAMMATICALLY=2 +}; + +class CommonExampleInterface* InverseDynamicsExampleCreateFunc(struct CommonExampleOptions& options); + + +#endif //INVERSE_DYNAMICS_EXAMPLE_H diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index 371065199..9c58e115a 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -55,7 +55,262 @@ InvertedPendulumPDControl::~InvertedPendulumPDControl() extern bool gJointFeedbackInWorldSpace; extern bool gJointFeedbackInJointFrame; +btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GUIHelperInterface* guiHelper, const btTransform& baseWorldTrans) +{ + btVector4 colors[4] = + { + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), + }; + int curColor = 0; + bool fixedBase = true; + bool damping = false; + bool gyro = false; + int numLinks = 2; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = false; + btVector3 linkHalfExtents(0.05, 0.37, 0.1); + btVector3 baseHalfExtents(0.04, 0.35, 0.08); + + + //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 0.f; + + if(baseMass) + { + //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + shape->calculateLocalInertia(baseMass, baseInertiaDiag); + delete shape; + } + + + btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); + + + + pMultiBody->setBaseWorldTransform(baseWorldTrans); + btVector3 vel(0, 0, 0); +// pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 1.f * SIMD_PI/ 180.f; + btQuaternion quat0(btVector3(1, 0, 0).normalized(), q0); + quat0.normalize(); + ///// + + for(int i = 0; i < numLinks; ++i) + { + float linkMass = 1.f; + //if (i==3 || i==2) + // linkMass= 1000; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* shape = 0; + if (i==0) + { + shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// + } else + { + shape = new btSphereShape(radius); + } + shape->calculateLocalInertia(linkMass, linkInertiaDiag); + delete shape; + + + if(!spherical) + { + //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + + if (i==0) + { + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, + btQuaternion(0.f, 0.f, 0.f, 1.f), + hingeJointAxis, + parentComToCurrentPivot, + currentPivotToCurrentCom, false); + } else + { + btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + + pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, + btQuaternion(0.f, 0.f, 0.f, 1.f), + parentComToCurrentPivot, + currentPivotToCurrentCom); + } + + //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); + + } + else + { + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + } + } + + pMultiBody->finalizeMultiDof(); + + + + /// + world->addMultiBody(pMultiBody); + btMultiBody* mbC = pMultiBody; + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if(!damping) + { + mbC->setLinearDamping(0.f); + mbC->setAngularDamping(0.f); + }else + { mbC->setLinearDamping(0.1f); + mbC->setAngularDamping(0.9f); + } + // + + + ////////////////////////////////////////////// + if(numLinks > 0) + { + btScalar q0 = 180.f * SIMD_PI/ 180.f; + if(!spherical) + { + mbC->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + double friction = 1; + { + + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; + + + if (1) + { + btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]); + guiHelper->createCollisionShapeGraphicsObject(shape); + + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(shape); + + btTransform tr; + tr.setIdentity(); +//if we don't set the initial pose of the btCollisionObject, the simulator will do this + //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider + + tr.setOrigin(local_origin[0]); + btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538); + + tr.setRotation(orn); + col->setWorldTransform(tr); + + bool isDynamic = (baseMass > 0 && !fixedBase); + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + + world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2); + + btVector3 color(0.0,0.0,0.5); + guiHelper->createCollisionObjectGraphicsObject(col,color); + +// col->setFriction(friction); + pMultiBody->setBaseCollider(col); + + } + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; + local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); + } + + + for (int i=0; i < pMultiBody->getNumLinks(); ++i) + { + + btVector3 posr = local_origin[i+1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; + btCollisionShape* shape =0; + + if (i==0) + { + shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); + } else + { + + shape = new btSphereShape(radius); + } + + guiHelper->createCollisionShapeGraphicsObject(shape); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(shape); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); + col->setWorldTransform(tr); + // col->setFriction(friction); + bool isDynamic = 1;//(linkMass > 0); + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + //if (i==0||i>numLinks-2) + { + world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + guiHelper->createCollisionObjectGraphicsObject(col,color); + + + pMultiBody->getLink(i).m_collider=col; + } + + } + + return pMultiBody; +} void InvertedPendulumPDControl::initPhysics() { @@ -87,19 +342,7 @@ void InvertedPendulumPDControl::initPhysics() m_guiHelper->setUpAxis(upAxis); - btVector4 colors[4] = - { - btVector4(1,0,0,1), - btVector4(0,1,0,1), - btVector4(0,1,1,1), - btVector4(1,1,0,1), - }; - int curColor = 0; - - - - this->createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( @@ -110,262 +353,20 @@ void InvertedPendulumPDControl::initPhysics() );//+btIDebugDraw::DBG_DrawConstraintLimits); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + btTransform baseWorldTrans; + baseWorldTrans.setIdentity(); + baseWorldTrans.setOrigin(btVector3(1,2,3)); + m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans); - { - bool fixedBase = true; - bool damping = false; - bool gyro = false; - int numLinks = 2; - bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals - bool canSleep = false; - bool selfCollide = false; - btVector3 linkHalfExtents(0.05, 0.37, 0.1); - btVector3 baseHalfExtents(0.04, 0.35, 0.08); - - btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); - //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm - //init the base - btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 0.f; - - if(baseMass) - { - //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); - btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); - shape->calculateLocalInertia(baseMass, baseInertiaDiag); - delete shape; - } - + //for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)// + for (int i=0;igetNumLinks();i++) + { + btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); + m_multiBody->getLink(i).m_jointFeedback = fb; + m_jointFeedbacks.push_back(fb); + //break; + } - btMultiBody *pMultiBody = new btMultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); - - m_multiBody = pMultiBody; - btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); - // baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI); - pMultiBody->setBasePos(basePosition); - pMultiBody->setWorldToBaseRot(baseOriQuat); - btVector3 vel(0, 0, 0); - // pMultiBody->setBaseVel(vel); - - //init the links - btVector3 hingeJointAxis(1, 0, 0); - - //y-axis assumed up - btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset - btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset - btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset - - ////// - btScalar q0 = 1.f * SIMD_PI/ 180.f; - btQuaternion quat0(btVector3(1, 0, 0).normalized(), q0); - quat0.normalize(); - ///// - - for(int i = 0; i < numLinks; ++i) - { - float linkMass = 1.f; - //if (i==3 || i==2) - // linkMass= 1000; - btVector3 linkInertiaDiag(0.f, 0.f, 0.f); - - btCollisionShape* shape = 0; - if (i==0) - { - shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// - } else - { - shape = new btSphereShape(radius); - } - shape->calculateLocalInertia(linkMass, linkInertiaDiag); - delete shape; - - - if(!spherical) - { - //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); - - if (i==0) - { - pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, - btQuaternion(0.f, 0.f, 0.f, 1.f), - hingeJointAxis, - parentComToCurrentPivot, - currentPivotToCurrentCom, false); - } else - { - btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset - btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset - btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset - - - pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, - btQuaternion(0.f, 0.f, 0.f, 1.f), - parentComToCurrentPivot, - currentPivotToCurrentCom); - } - - //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); - - } - else - { - //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); - pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); - } - } - - pMultiBody->finalizeMultiDof(); - - //for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)// - for (int i=0;igetNumLinks();i++) - { - btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); - pMultiBody->getLink(i).m_jointFeedback = fb; - m_jointFeedbacks.push_back(fb); - //break; - } - btMultiBodyDynamicsWorld* world = m_dynamicsWorld; - - /// - world->addMultiBody(pMultiBody); - btMultiBody* mbC = pMultiBody; - mbC->setCanSleep(canSleep); - mbC->setHasSelfCollision(selfCollide); - mbC->setUseGyroTerm(gyro); - // - if(!damping) - { - mbC->setLinearDamping(0.f); - mbC->setAngularDamping(0.f); - }else - { mbC->setLinearDamping(0.1f); - mbC->setAngularDamping(0.9f); - } - // - - - ////////////////////////////////////////////// - if(numLinks > 0) - { - btScalar q0 = 180.f * SIMD_PI/ 180.f; - if(!spherical) - { - mbC->setJointPosMultiDof(0, &q0); - } - else - { - btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); - quat0.normalize(); - mbC->setJointPosMultiDof(0, quat0); - } - } - /// - - btAlignedObjectArray world_to_local; - world_to_local.resize(pMultiBody->getNumLinks() + 1); - - btAlignedObjectArray local_origin; - local_origin.resize(pMultiBody->getNumLinks() + 1); - world_to_local[0] = pMultiBody->getWorldToBaseRot(); - local_origin[0] = pMultiBody->getBasePos(); - double friction = 1; - { - - // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; - float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; - - - if (1) - { - btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]); - m_guiHelper->createCollisionShapeGraphicsObject(shape); - - btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); - col->setCollisionShape(shape); - - btTransform tr; - tr.setIdentity(); -//if we don't set the initial pose of the btCollisionObject, the simulator will do this - //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider - - tr.setOrigin(local_origin[0]); - btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538); - - tr.setRotation(orn); - col->setWorldTransform(tr); - - bool isDynamic = (baseMass > 0 && !fixedBase); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - - world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2); - - btVector3 color(0.0,0.0,0.5); - m_guiHelper->createCollisionObjectGraphicsObject(col,color); - -// col->setFriction(friction); - pMultiBody->setBaseCollider(col); - - } - } - - - for (int i=0; i < pMultiBody->getNumLinks(); ++i) - { - const int parent = pMultiBody->getParent(i); - world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; - local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); - } - - - for (int i=0; i < pMultiBody->getNumLinks(); ++i) - { - - btVector3 posr = local_origin[i+1]; - // float pos[4]={posr.x(),posr.y(),posr.z(),1}; - - float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; - btCollisionShape* shape =0; - - if (i==0) - { - shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); - } else - { - - shape = new btSphereShape(radius); - } - - m_guiHelper->createCollisionShapeGraphicsObject(shape); - btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); - - col->setCollisionShape(shape); - btTransform tr; - tr.setIdentity(); - tr.setOrigin(posr); - tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); - col->setWorldTransform(tr); - // col->setFriction(friction); - bool isDynamic = 1;//(linkMass > 0); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - //if (i==0||i>numLinks-2) - { - world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2); - btVector4 color = colors[curColor]; - curColor++; - curColor&=3; - m_guiHelper->createCollisionObjectGraphicsObject(col,color); - - - pMultiBody->getLink(i).m_collider=col; - } - - } - } - } char fileName[1024]; From 7d9365319c6659068798cd8c290543729a61686a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 17 Nov 2015 08:26:27 -0800 Subject: [PATCH 2/7] add InverseDynamics test/lib to premake --- Extras/premake4.lua | 2 +- build3/premake4.lua | 4 +++- examples/InverseDynamics/InverseDynamicsExample.cpp | 4 ++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Extras/premake4.lua b/Extras/premake4.lua index a044468da..dd4853f6e 100644 --- a/Extras/premake4.lua +++ b/Extras/premake4.lua @@ -1,7 +1,7 @@ include "HACD" include "ConvexDecomposition" - +include "InverseDynamics" include "Serialize/BulletFileLoader" include "Serialize/BulletWorldImporter" include "Serialize/BulletXmlWorldImporter" diff --git a/build3/premake4.lua b/build3/premake4.lua index a3f020cba..569aeda48 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -189,12 +189,14 @@ if not _OPTIONS["ios"] then include "../test/gtest-1.7.0" -- include "../test/hello_gtest" include "../test/collision" + include "../test/InverseDynamics" include "../test/TestBullet3OpenCL" include "../test/GwenOpenGLTest" end end - include "../test/Bullet2" + include "../test/Bullet2" + include "../src/BulletInverseDynamics" include "../src/BulletSoftBody" include "../src/BulletDynamics" include "../src/BulletCollision" diff --git a/examples/InverseDynamics/InverseDynamicsExample.cpp b/examples/InverseDynamics/InverseDynamicsExample.cpp index 8b67ccfba..9548d8591 100644 --- a/examples/InverseDynamics/InverseDynamicsExample.cpp +++ b/examples/InverseDynamics/InverseDynamicsExample.cpp @@ -19,7 +19,7 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "../CommonInterfaces/CommonParameterInterface.h" -#include "../../Utils/b3ResourcePath.h" +#include "../Utils/b3ResourcePath.h" #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" @@ -129,7 +129,7 @@ void InverseDynamicsExample::initPhysics() //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())); + b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); } } break; From aa4d119f98171d1ec7882aa1a98a6b2026e0f811 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 17 Nov 2015 08:27:38 -0800 Subject: [PATCH 3/7] add real-time safe Inverse Dynamics library+test+utils an example for the example browser will follow. thanks to Thomas Buschmann --- Extras/InverseDynamics/CoilCreator.cpp | 67 ++ Extras/InverseDynamics/CoilCreator.hpp | 40 + Extras/InverseDynamics/DillCreator.cpp | 122 +++ Extras/InverseDynamics/DillCreator.hpp | 47 + Extras/InverseDynamics/IDRandomUtil.cpp | 70 ++ Extras/InverseDynamics/IDRandomUtil.hpp | 34 + Extras/InverseDynamics/MultiBodyNameMap.cpp | 78 ++ Extras/InverseDynamics/MultiBodyNameMap.hpp | 54 ++ .../InverseDynamics/MultiBodyTreeCreator.cpp | 64 ++ .../InverseDynamics/MultiBodyTreeCreator.hpp | 45 + .../MultiBodyTreeDebugGraph.cpp | 64 ++ .../MultiBodyTreeDebugGraph.hpp | 17 + Extras/InverseDynamics/SimpleTreeCreator.cpp | 69 ++ Extras/InverseDynamics/SimpleTreeCreator.hpp | 34 + Extras/InverseDynamics/User2InternalIndex.cpp | 99 ++ Extras/InverseDynamics/User2InternalIndex.hpp | 46 + .../InverseDynamics/btMultiBodyFromURDF.hpp | 91 ++ .../btMultiBodyTreeCreator.cpp | 270 ++++++ .../btMultiBodyTreeCreator.hpp | 50 ++ .../invdyn_bullet_comparison.cpp | 307 +++++++ .../invdyn_bullet_comparison.hpp | 34 + Extras/InverseDynamics/premake4.lua | 12 + src/BulletInverseDynamics/IDConfig.hpp | 40 + src/BulletInverseDynamics/IDConfigBuiltin.hpp | 36 + src/BulletInverseDynamics/IDConfigEigen.hpp | 43 + src/BulletInverseDynamics/IDErrorMessages.hpp | 34 + src/BulletInverseDynamics/IDMath.cpp | 371 ++++++++ src/BulletInverseDynamics/IDMath.hpp | 88 ++ src/BulletInverseDynamics/MultiBodyTree.cpp | 331 +++++++ src/BulletInverseDynamics/MultiBodyTree.hpp | 308 +++++++ .../details/IDEigenInterface.hpp | 17 + .../details/IDLinearMathInterface.hpp | 112 +++ .../details/IDMatVec.hpp | 326 +++++++ .../details/MultiBodyTreeImpl.cpp | 844 ++++++++++++++++++ .../details/MultiBodyTreeImpl.hpp | 236 +++++ .../details/MultiBodyTreeInitCache.cpp | 113 +++ .../details/MultiBodyTreeInitCache.hpp | 109 +++ src/BulletInverseDynamics/premake4.lua | 12 + test/InverseDynamics/premake4.lua | 34 + test/InverseDynamics/test_invdyn_bullet.cpp | 115 +++ .../test_invdyn_kinematics.cpp | 350 ++++++++ 41 files changed, 5233 insertions(+) create mode 100644 Extras/InverseDynamics/CoilCreator.cpp create mode 100644 Extras/InverseDynamics/CoilCreator.hpp create mode 100644 Extras/InverseDynamics/DillCreator.cpp create mode 100644 Extras/InverseDynamics/DillCreator.hpp create mode 100644 Extras/InverseDynamics/IDRandomUtil.cpp create mode 100644 Extras/InverseDynamics/IDRandomUtil.hpp create mode 100644 Extras/InverseDynamics/MultiBodyNameMap.cpp create mode 100644 Extras/InverseDynamics/MultiBodyNameMap.hpp create mode 100644 Extras/InverseDynamics/MultiBodyTreeCreator.cpp create mode 100644 Extras/InverseDynamics/MultiBodyTreeCreator.hpp create mode 100644 Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp create mode 100644 Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp create mode 100644 Extras/InverseDynamics/SimpleTreeCreator.cpp create mode 100644 Extras/InverseDynamics/SimpleTreeCreator.hpp create mode 100644 Extras/InverseDynamics/User2InternalIndex.cpp create mode 100644 Extras/InverseDynamics/User2InternalIndex.hpp create mode 100644 Extras/InverseDynamics/btMultiBodyFromURDF.hpp create mode 100644 Extras/InverseDynamics/btMultiBodyTreeCreator.cpp create mode 100644 Extras/InverseDynamics/btMultiBodyTreeCreator.hpp create mode 100644 Extras/InverseDynamics/invdyn_bullet_comparison.cpp create mode 100644 Extras/InverseDynamics/invdyn_bullet_comparison.hpp create mode 100644 Extras/InverseDynamics/premake4.lua create mode 100644 src/BulletInverseDynamics/IDConfig.hpp create mode 100644 src/BulletInverseDynamics/IDConfigBuiltin.hpp create mode 100644 src/BulletInverseDynamics/IDConfigEigen.hpp create mode 100644 src/BulletInverseDynamics/IDErrorMessages.hpp create mode 100644 src/BulletInverseDynamics/IDMath.cpp create mode 100644 src/BulletInverseDynamics/IDMath.hpp create mode 100644 src/BulletInverseDynamics/MultiBodyTree.cpp create mode 100644 src/BulletInverseDynamics/MultiBodyTree.hpp create mode 100644 src/BulletInverseDynamics/details/IDEigenInterface.hpp create mode 100644 src/BulletInverseDynamics/details/IDLinearMathInterface.hpp create mode 100644 src/BulletInverseDynamics/details/IDMatVec.hpp create mode 100644 src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp create mode 100644 src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp create mode 100644 src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp create mode 100644 src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp create mode 100644 src/BulletInverseDynamics/premake4.lua create mode 100644 test/InverseDynamics/premake4.lua create mode 100644 test/InverseDynamics/test_invdyn_bullet.cpp create mode 100644 test/InverseDynamics/test_invdyn_kinematics.cpp diff --git a/Extras/InverseDynamics/CoilCreator.cpp b/Extras/InverseDynamics/CoilCreator.cpp new file mode 100644 index 000000000..905ba7a57 --- /dev/null +++ b/Extras/InverseDynamics/CoilCreator.cpp @@ -0,0 +1,67 @@ +#include + +#include "CoilCreator.hpp" + +namespace btInverseDynamics { +CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) { + for (int i = 0; i < m_num_bodies; i++) { + m_parent[i] = i - 1; + } + + // DH parameters (that's what's in the paper ...) + const idScalar theta_DH = 0; + const idScalar d_DH = 0.0; + const idScalar a_DH = 1.0 / m_num_bodies; + const idScalar alpha_DH = 5.0 * M_PI / m_num_bodies; + getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref, + &m_body_T_parent_ref); + // always z-axis + m_body_axis_of_motion(0) = 0.0; + m_body_axis_of_motion(1) = 0.0; + m_body_axis_of_motion(2) = 1.0; + + m_mass = 1.0 / m_num_bodies; + m_body_r_body_com(0) = 1.0 / (2.0 * m_num_bodies); + m_body_r_body_com(1) = 0.0; + m_body_r_body_com(2) = 0.0; + + m_body_I_body(0, 0) = 1e-4 / (2.0 * m_num_bodies); + m_body_I_body(0, 1) = 0.0; + m_body_I_body(0, 2) = 0.0; + m_body_I_body(1, 0) = 0.0; + m_body_I_body(1, 1) = (3e-4 + 4.0 / pow(m_num_bodies, 2)) / (12.0 * m_num_bodies); + m_body_I_body(1, 2) = 0.0; + m_body_I_body(2, 0) = 0.0; + m_body_I_body(2, 1) = 0.0; + m_body_I_body(2, 2) = m_body_I_body(1, 1); +} + +CoilCreator::~CoilCreator() {} + +int CoilCreator::getNumBodies(int* num_bodies) const { + *num_bodies = m_num_bodies; + return 0; +} + +int CoilCreator::getBody(int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, + vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, + mat33* body_I_body, int* user_int, void** user_ptr) const { + if (body_index < 0 || body_index >= m_num_bodies) { + error_message("invalid body index %d\n", body_index); + return -1; + } + *parent_index = m_parent[body_index]; + *joint_type = REVOLUTE; + *parent_r_parent_body_ref = m_parent_r_parent_body_ref; + *body_T_parent_ref = m_body_T_parent_ref; + *body_axis_of_motion = m_body_axis_of_motion; + *mass = m_mass; + *body_r_body_com = m_body_r_body_com; + *body_I_body = m_body_I_body; + + *user_int = 0; + *user_ptr = 0; + return 0; +} +} diff --git a/Extras/InverseDynamics/CoilCreator.hpp b/Extras/InverseDynamics/CoilCreator.hpp new file mode 100644 index 000000000..a57ee3595 --- /dev/null +++ b/Extras/InverseDynamics/CoilCreator.hpp @@ -0,0 +1,40 @@ +#ifndef COILCREATOR_HPP_ +#define COILCREATOR_HPP_ + +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +/// Creator class for building a "coil" system as intruduced as benchmark example in +/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n)) +/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International +/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628. +/// +/// This is a serial chain, with an initial configuration resembling a coil. +class CoilCreator : public MultiBodyTreeCreator { +public: + /// ctor. + /// @param n the number of bodies in the system + CoilCreator(int n); + /// dtor + ~CoilCreator(); + // \copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int* num_bodies) const; + // \copydoc MultiBodyTreeCreator::getBody + int getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion, + idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int, + void** user_ptr) const; + +private: + int m_num_bodies; + std::vector m_parent; + vec3 m_parent_r_parent_body_ref; + mat33 m_body_T_parent_ref; + vec3 m_body_axis_of_motion; + idScalar m_mass; + vec3 m_body_r_body_com; + mat33 m_body_I_body; +}; +} +#endif diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp new file mode 100644 index 000000000..cdb3e06a6 --- /dev/null +++ b/Extras/InverseDynamics/DillCreator.cpp @@ -0,0 +1,122 @@ +#include "DillCreator.hpp" +#include +namespace btInverseDynamics { + +DillCreator::DillCreator(int level) + : m_level(level), + m_num_bodies(std::pow(2, level)), + m_parent(m_num_bodies), + m_parent_r_parent_body_ref(m_num_bodies), + m_body_T_parent_ref(m_num_bodies), + m_body_axis_of_motion(m_num_bodies), + m_mass(m_num_bodies), + m_body_r_body_com(m_num_bodies), + m_body_I_body(m_num_bodies) { + // generate names (for debugging) + for (int i = 0; i < m_num_bodies; i++) { + m_parent[i] = i - 1; + + // all z-axis (DH convention) + m_body_axis_of_motion[i](0) = 0.0; + m_body_axis_of_motion[i](1) = 0.0; + m_body_axis_of_motion[i](2) = 1.0; + } + + // recursively build data structures + m_current_body = 0; + const int parent = -1; + const idScalar d_DH = 0.0; + const idScalar a_DH = 0.0; + const idScalar alpha_DH = 0.0; + + if (-1 == recurseDill(m_level, parent, d_DH, a_DH, alpha_DH)) { + error_message("recurseDill failed\n"); + abort(); + } +} + +DillCreator::~DillCreator() {} + +int DillCreator::getNumBodies(int* num_bodies) const { + *num_bodies = m_num_bodies; + return 0; +} + +int DillCreator::getBody(int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, + vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, + mat33* body_I_body, int* user_int, void** user_ptr) const { + if (body_index < 0 || body_index >= m_num_bodies) { + error_message("invalid body index %d\n", body_index); + return -1; + } + *parent_index = m_parent[body_index]; + *joint_type = REVOLUTE; + *parent_r_parent_body_ref = m_parent_r_parent_body_ref[body_index]; + *body_T_parent_ref = m_body_T_parent_ref[body_index]; + *body_axis_of_motion = m_body_axis_of_motion[body_index]; + *mass = m_mass[body_index]; + *body_r_body_com = m_body_r_body_com[body_index]; + *body_I_body = m_body_I_body[body_index]; + + *user_int = 0; + *user_ptr = 0; + return 0; +} + +int DillCreator::recurseDill(const int level, const int parent, const idScalar d_DH_in, + const idScalar a_DH_in, const idScalar alpha_DH_in) { + if (level < 0) { + error_message("invalid level parameter (%d)\n", level); + return -1; + } + + if (m_current_body >= m_num_bodies || m_current_body < 0) { + error_message("invalid body parameter (%d, num_bodies: %d)\n", m_current_body, + m_num_bodies); + return -1; + } + + idScalar size = std::max(level, 1); + const int body = m_current_body; + // length = 0.1 * size; + // with = 2 * 0.01 * size; + + /// these parameters are from the paper ... + /// TODO: add proper citation + m_parent[body] = parent; + m_mass[body] = 0.1 * std::pow(size, 3); + m_body_r_body_com[body](0) = 0.05 * size; + m_body_r_body_com[body](1) = 0; + m_body_r_body_com[body](2) = 0; + // initialization + for (int i = 0; i < 3; i++) { + m_parent_r_parent_body_ref[body](i) = 0; + for (int j = 0; j < 3; j++) { + m_body_I_body[body](i, j) = 0.0; + m_body_T_parent_ref[body](i, j) = 0.0; + } + } + const idScalar size_5 = pow(size, 5); + m_body_I_body[body](0, 0) = size_5 / 0.2e6; + m_body_I_body[body](1, 1) = size_5 * 403 / 1.2e6; + m_body_I_body[body](2, 2) = m_body_I_body[body](1, 1); + + getVecMatFromDH(0, 0, a_DH_in, alpha_DH_in, &m_parent_r_parent_body_ref[body], + &m_body_T_parent_ref[body]); + + // attach "level" Dill systems of levels 1...level + for (int i = 1; i <= level; i++) { + idScalar d_DH = 0.01 * size; + if (i == level) { + d_DH = 0.0; + } + const idScalar a_DH = i * 0.1; + const idScalar alpha_DH = i * M_PI / 3.0; + m_current_body++; + recurseDill(i - 1, body, d_DH, a_DH, alpha_DH); + } + + return 0; // ok! +} +} diff --git a/Extras/InverseDynamics/DillCreator.hpp b/Extras/InverseDynamics/DillCreator.hpp new file mode 100644 index 000000000..42f1b1ba0 --- /dev/null +++ b/Extras/InverseDynamics/DillCreator.hpp @@ -0,0 +1,47 @@ +#ifndef DILLCREATOR_HPP_ +#define DILLCREATOR_HPP_ + +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + + +/// Creator class for building a "Dill" system as intruduced as benchmark example in +/// Featherstone (1999), "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n)) +/// Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops, and Accuracy.", The International +/// Journal of Robotics Research 18 (9): 876–892. doi : 10.1177 / 02783649922066628. +/// +/// This is a self-similar branched tree, somewhat resembling a dill plant +class DillCreator : public MultiBodyTreeCreator { +public: + /// ctor + /// @param levels the number of dill levels + DillCreator(int levels); + /// dtor + ~DillCreator(); + ///\copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int* num_bodies) const; + ///\copydoc MultiBodyTreeCreator::getBody + int getBody(int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion, + idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int, + void** user_ptr) const; + +private: + /// recursively generate dill bodies. + /// TODO better documentation + int recurseDill(const int levels, const int parent, const idScalar d_DH_in, + const idScalar a_DH_in, const idScalar alpha_DH_in); + int m_level; + int m_num_bodies; + std::vector m_parent; + std::vector m_parent_r_parent_body_ref; + std::vector m_body_T_parent_ref; + std::vector m_body_axis_of_motion; + std::vector m_mass; + std::vector m_body_r_body_com; + std::vector m_body_I_body; + int m_current_body; +}; +} +#endif diff --git a/Extras/InverseDynamics/IDRandomUtil.cpp b/Extras/InverseDynamics/IDRandomUtil.cpp new file mode 100644 index 000000000..8896eaf55 --- /dev/null +++ b/Extras/InverseDynamics/IDRandomUtil.cpp @@ -0,0 +1,70 @@ +#include +#include +#include + +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/IDMath.hpp" +#include "IDRandomUtil.hpp" + + +namespace btInverseDynamics { + +// constants for random mass and inertia generation +// these are arbitrary positive values. +static const float mass_min = 0.001; +static const float mass_max = 1.0; + +void randomInit() { srand(time(NULL)); } + +int randomInt(int low, int high) { return rand() % (high + 1 - low) + low; } + +float randomFloat(float low, float high) { + return low + static_cast(rand()) / RAND_MAX * (high - low); +} + +float randomMass() { return randomFloat(mass_min, mass_max); } + +vec3 randomInertiaPrincipal() { + vec3 inertia; + do { + inertia(0) = randomFloat(mass_min, mass_max); + inertia(1) = randomFloat(mass_min, mass_max); + inertia(2) = randomFloat(mass_min, mass_max); + } while (inertia(0) + inertia(1) < inertia(2) || inertia(0) + inertia(2) < inertia(1) || + inertia(1) + inertia(2) < inertia(0)); + return inertia; +} + +mat33 randomInertiaMatrix() { + // generate random valid inertia matrix by first getting valid components + // along major axes and then rotating by random amount + vec3 principal = randomInertiaPrincipal(); + mat33 rot(transformX(randomFloat(-M_PI, M_PI)) * transformY(randomFloat(-M_PI, M_PI)) * + transformZ(randomFloat(-M_PI, M_PI))); + mat33 inertia; + inertia(0, 0) = principal(0); + inertia(0, 1) = 0; + inertia(0, 2) = 0; + inertia(1, 0) = 0; + inertia(1, 1) = principal(1); + inertia(1, 2) = 0; + inertia(2, 0) = 0; + inertia(2, 1) = 0; + inertia(2, 2) = principal(2); + return rot * inertia * rot.transpose(); +} + +vec3 randomAxis() { + vec3 axis; + idScalar length; + do { + axis(0) = randomFloat(-1.0, 1.0); + axis(1) = randomFloat(-1.0, 1.0); + axis(2) = randomFloat(-1.0, 1.0); + + length = std::sqrt(std::pow(axis(0), 2) + std::pow(axis(1), 2) + std::pow(axis(2), 2)); + } while (length < 0.01); + + return axis / length; +} +} diff --git a/Extras/InverseDynamics/IDRandomUtil.hpp b/Extras/InverseDynamics/IDRandomUtil.hpp new file mode 100644 index 000000000..a9d363a5b --- /dev/null +++ b/Extras/InverseDynamics/IDRandomUtil.hpp @@ -0,0 +1,34 @@ +#ifndef ID_RANDOM_UTIL_HPP_ +#define ID_RANDOM_UTIL_HPP_ +#include "BulletInverseDynamics/IDConfig.hpp" +namespace btInverseDynamics { +/// seed random number generator +void randomInit(); +/// Generate (not quite) uniformly distributed random integers in [low, high] +/// Note: this is a low-quality implementation using only rand(), as +/// C++11 is not supported in bullet. +/// The results will *not* be perfectly uniform. +/// \param low is the lower bound (inclusive) +/// \param high is the lower bound (inclusive) +/// \return a random number within [\param low, \param high] +int randomInt(int low, int high); +/// Generate a (not quite) uniformly distributed random floats in [low, high] +/// Note: this is a low-quality implementation using only rand(), as +/// C++11 is not supported in bullet. +/// The results will *not* be perfectly uniform. +/// \param low is the lower bound (inclusive) +/// \param high is the lower bound (inclusive) +/// \return a random number within [\param low, \param high] +float randomFloat(float low, float high); + +/// generate a random valid mass value +/// \returns random mass +float randomMass(); +/// generate a random valid vector of principal moments of inertia +vec3 randomInertiaPrincipal(); +/// generate a random valid moment of inertia matrix +mat33 randomInertiaMatrix(); +/// generate a random unit vector +vec3 randomAxis(); +} +#endif diff --git a/Extras/InverseDynamics/MultiBodyNameMap.cpp b/Extras/InverseDynamics/MultiBodyNameMap.cpp new file mode 100644 index 000000000..4e82641c1 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyNameMap.cpp @@ -0,0 +1,78 @@ +#include "MultiBodyNameMap.hpp" + +namespace btInverseDynamics { + +MultiBodyNameMap::MultiBodyNameMap() {} + +int MultiBodyNameMap::addBody(const int index, const std::string& name) { + if (m_index_to_body_name.count(index) > 0) { + error_message("trying to add index %d again\n", index); + return -1; + } + if (m_body_name_to_index.count(name) > 0) { + error_message("trying to add name %s again\n", name.c_str()); + return -1; + } + + m_index_to_body_name[index] = name; + m_body_name_to_index[name] = index; + + return 0; +} + +int MultiBodyNameMap::addJoint(const int index, const std::string& name) { + if (m_index_to_joint_name.count(index) > 0) { + error_message("trying to add index %d again\n", index); + return -1; + } + if (m_joint_name_to_index.count(name) > 0) { + error_message("trying to add name %s again\n", name.c_str()); + return -1; + } + + m_index_to_joint_name[index] = name; + m_joint_name_to_index[name] = index; + + return 0; +} + +int MultiBodyNameMap::getBodyName(const int index, std::string* name) const { + std::map::const_iterator it = m_index_to_body_name.find(index); + if (it == m_index_to_body_name.end()) { + error_message("index %d not known\n", index); + return -1; + } + *name = it->second; + return 0; +} + +int MultiBodyNameMap::getJointName(const int index, std::string* name) const { + std::map::const_iterator it = m_index_to_joint_name.find(index); + if (it == m_index_to_joint_name.end()) { + error_message("index %d not known\n", index); + return -1; + } + *name = it->second; + return 0; +} + +int MultiBodyNameMap::getBodyIndex(const std::string& name, int* index) const { + std::map::const_iterator it = m_body_name_to_index.find(name); + if (it == m_body_name_to_index.end()) { + error_message("name %s not known\n", name.c_str()); + return -1; + } + *index = it->second; + return 0; +} + +int MultiBodyNameMap::getJointIndex(const std::string& name, int* index) const { + std::map::const_iterator it = m_joint_name_to_index.find(name); + if (it == m_joint_name_to_index.end()) { + error_message("name %s not known\n", name.c_str()); + return -1; + } + *index = it->second; + return 0; +} +} diff --git a/Extras/InverseDynamics/MultiBodyNameMap.hpp b/Extras/InverseDynamics/MultiBodyNameMap.hpp new file mode 100644 index 000000000..a5e024ce0 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyNameMap.hpp @@ -0,0 +1,54 @@ +#ifndef MULTIBODYNAMEMAP_HPP_ +#define MULTIBODYNAMEMAP_HPP_ + +#include "BulletInverseDynamics/IDConfig.hpp" +#include +#include + +namespace btInverseDynamics { + +/// \brief The MultiBodyNameMap class +/// Utility class that stores a maps from body/joint indices to/from body and joint names +class MultiBodyNameMap { +public: + MultiBodyNameMap(); + /// add a body to the map + /// @param index of the body + /// @param name name of the body + /// @return 0 on success, -1 on failure + int addBody(const int index, const std::string& name); + /// add a joint to the map + /// @param index of the joint + /// @param name name of the joint + /// @return 0 on success, -1 on failure + int addJoint(const int index, const std::string& name); + /// get body name from index + /// @param index of the body + /// @param body_name name of the body + /// @return 0 on success, -1 on failure + int getBodyName(const int index, std::string* name) const; + /// get joint name from index + /// @param index of the joint + /// @param joint_name name of the joint + /// @return 0 on success, -1 on failure + int getJointName(const int index, std::string* name) const; + /// get body index from name + /// @param index of the body + /// @param name name of the body + /// @return 0 on success, -1 on failure + int getBodyIndex(const std::string& name, int* index) const; + /// get joint index from name + /// @param index of the joint + /// @param name name of the joint + /// @return 0 on success, -1 on failure + int getJointIndex(const std::string& name, int* index) const; + +private: + std::map m_index_to_joint_name; + std::map m_index_to_body_name; + + std::map m_joint_name_to_index; + std::map m_body_name_to_index; +}; +} +#endif // MULTIBODYNAMEMAP_HPP_ diff --git a/Extras/InverseDynamics/MultiBodyTreeCreator.cpp b/Extras/InverseDynamics/MultiBodyTreeCreator.cpp new file mode 100644 index 000000000..300910d7a --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeCreator.cpp @@ -0,0 +1,64 @@ +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator) { + int num_bodies; + int parent_index; + JointType joint_type; + vec3 body_r_parent_body_ref; + mat33 body_R_parent_ref; + vec3 body_axis_of_motion; + idScalar mass; + vec3 body_r_body_com; + mat33 body_I_body; + int user_int; + void* user_ptr; + + MultiBodyTree* tree = new MultiBodyTree(); + if (0x0 == tree) { + error_message("cannot allocate tree\n"); + return 0x0; + } + + // TODO: move to some policy argument + tree->setAcceptInvalidMassParameters(false); + + // get number of bodies in the system + if (-1 == creator.getNumBodies(&num_bodies)) { + error_message("getting body indices\n"); + delete tree; + return 0x0; + } + + // get data for all bodies + for (int index = 0; index < num_bodies; index++) { + // get body parameters from user callbacks + if (-1 == + creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref, + &body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com, + &body_I_body, &user_int, &user_ptr)) { + error_message("getting data for body %d\n", index); + delete tree; + return 0x0; + } + // add body to system + if (-1 == + tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref, + body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com, + body_I_body, user_int, user_ptr)) { + error_message("adding body %d\n", index); + delete tree; + return 0x0; + } + } + // finalize initialization + if (-1 == tree->finalize()) { + error_message("building system\n"); + delete tree; + return 0x0; + } + + return tree; +} +} diff --git a/Extras/InverseDynamics/MultiBodyTreeCreator.hpp b/Extras/InverseDynamics/MultiBodyTreeCreator.hpp new file mode 100644 index 000000000..bbf552eb6 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeCreator.hpp @@ -0,0 +1,45 @@ +#ifndef MULTI_BODY_TREE_CREATOR_HPP_ +#define MULTI_BODY_TREE_CREATOR_HPP_ + +#include +#include +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/IDMath.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" +#include "MultiBodyNameMap.hpp" + +namespace btInverseDynamics { +/// Interface class for initializing a MultiBodyTree instance. +/// Data to be provided is modeled on the URDF specification. +/// The user can derive from this class in order to programmatically +/// initialize a system. +class MultiBodyTreeCreator { +public: + /// the dtor + virtual ~MultiBodyTreeCreator() {} + /// Get the number of bodies in the system + /// @param num_bodies write number of bodies here + /// @return 0 on success, -1 on error + virtual int getNumBodies(int* num_bodies) const = 0; + /// Interface for accessing link mass properties. + /// For detailed description of data, @sa MultiBodyTree::addBody + /// \copydoc MultiBodyTree::addBody + virtual int getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, + vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, + mat33* body_I_body, int* user_int, void** user_ptr) const = 0; + /// @return a pointer to a name mapping utility class, or 0x0 if not available + virtual const MultiBodyNameMap* getNameMap() const {return 0x0;} +}; + +/// Create a multibody object. +/// @param creator an object implementing the MultiBodyTreeCreator interface +/// that returns data defining the system +/// @return A pointer to an allocated multibodytree instance, or +/// 0x0 if an error occured. +MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator); +} + +// does urdf have gravity direction ?? + +#endif // MULTI_BODY_TREE_CREATOR_HPP_ diff --git a/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp new file mode 100644 index 000000000..587a02be1 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.cpp @@ -0,0 +1,64 @@ +#include "MultiBodyTreeDebugGraph.hpp" + +#include + +namespace btInverseDynamics { + +int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map, + const char* filename) { + if (0x0 == tree) { + error_message("tree pointer is null\n"); + return -1; + } + if (0x0 == filename) { + error_message("filename is null\n"); + return -1; + } + + FILE* fp = fopen(filename, "w"); + if (NULL == fp) { + error_message("cannot open file %s for writing\n", filename); + return -1; + } + fprintf(fp, "// to generate postscript file, run dot -Tps %s -o %s.ps\n" + "// details see graphviz documentation at http://graphviz.org\n" + "digraph tree {\n", + filename, filename); + + for (int body = 0; body < tree->numBodies(); body++) { + std::string name; + if (0x0 != map) { + if (-1 == map->getBodyName(body, &name)) { + error_message("can't get name of body %d\n", body); + return -1; + } + fprintf(fp, " %d [label=\"%d/%s\"];\n", body, body, name.c_str()); + } + } + for (int body = 0; body < tree->numBodies(); body++) { + int parent; + const char* joint_type; + int qi; + if (-1 == tree->getParentIndex(body, &parent)) { + error_message("indexing error\n"); + return -1; + } + if (-1 == tree->getJointTypeStr(body, &joint_type)) { + error_message("indexing error\n"); + return -1; + } + if (-1 == tree->getDoFOffset(body, &qi)) { + error_message("indexing error\n"); + return -1; + } + if (-1 != parent) { + fprintf(fp, " %d -> %d [label= \"type:%s, q=%d\"];\n", parent, body, + joint_type, qi); + } + } + + fprintf(fp, "}\n"); + fclose(fp); + return 0; +} +} diff --git a/Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp new file mode 100644 index 000000000..f249d0952 --- /dev/null +++ b/Extras/InverseDynamics/MultiBodyTreeDebugGraph.hpp @@ -0,0 +1,17 @@ +#ifndef MULTIBODYTREEDEBUGGRAPH_HPP_ +#define MULTIBODYTREEDEBUGGRAPH_HPP_ +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" +#include "MultiBodyNameMap.hpp" + +namespace btInverseDynamics { +/// generate a dot-file of the multibody tree for generating a graph using graphviz' dot tool +/// @param tree the multibody tree +/// @param map to add names of links (if 0x0, no names will be added) +/// @param filename name for the output file +/// @return 0 on success, -1 on error +int writeGraphvizDotFile(const MultiBodyTree* tree, const MultiBodyNameMap* map, + const char* filename); +} + +#endif // MULTIBODYTREEDEBUGGRAPH_HPP diff --git a/Extras/InverseDynamics/SimpleTreeCreator.cpp b/Extras/InverseDynamics/SimpleTreeCreator.cpp new file mode 100644 index 000000000..e2b308ebe --- /dev/null +++ b/Extras/InverseDynamics/SimpleTreeCreator.cpp @@ -0,0 +1,69 @@ +#include "SimpleTreeCreator.hpp" + +#include + +namespace btInverseDynamics { +/// minimal "tree" (chain) +SimpleTreeCreator::SimpleTreeCreator(int dim) : m_num_bodies(dim) { + m_mass = 1.0; + m_body_T_parent_ref(0, 0) = 1; + m_body_T_parent_ref(0, 1) = 0; + m_body_T_parent_ref(0, 2) = 0; + m_body_T_parent_ref(1, 0) = 0; + m_body_T_parent_ref(1, 1) = 1; + m_body_T_parent_ref(1, 2) = 0; + m_body_T_parent_ref(2, 0) = 0; + m_body_T_parent_ref(2, 1) = 0; + m_body_T_parent_ref(2, 2) = 1; + + m_parent_r_parent_body_ref(0) = 1.0; + m_parent_r_parent_body_ref(1) = 0.0; + m_parent_r_parent_body_ref(2) = 0.0; + + m_body_r_body_com(0) = 0.5; + m_body_r_body_com(1) = 0.0; + m_body_r_body_com(2) = 0.0; + + m_body_I_body(0, 0) = 1; + m_body_I_body(0, 1) = 0; + m_body_I_body(0, 2) = 0; + m_body_I_body(1, 0) = 0; + m_body_I_body(1, 1) = 1; + m_body_I_body(1, 2) = 0; + m_body_I_body(2, 0) = 0; + m_body_I_body(2, 1) = 0; + m_body_I_body(2, 2) = 1; + + m_axis(0) = 0; + m_axis(1) = 0; + m_axis(2) = 1; +} +int SimpleTreeCreator::getNumBodies(int* num_bodies) const { + *num_bodies = m_num_bodies; + return 0; +} + +int SimpleTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, + vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com, + mat33* body_I_body, int* user_int, void** user_ptr) const { + *parent_index = body_index - 1; + if (body_index % 2) { + *joint_type = PRISMATIC; + } else { + *joint_type = REVOLUTE; + } + *parent_r_parent_body_ref = m_parent_r_parent_body_ref; + if (0 == body_index) { + (*parent_r_parent_body_ref)(2) = 1.0; + } + *body_T_parent_ref = m_body_T_parent_ref; + *body_axis_of_motion = m_axis; + *mass = m_mass; + *body_r_body_com = m_body_r_body_com; + *body_I_body = m_body_I_body; + *user_int = 0; + *user_ptr = 0; + return 0; +} +} diff --git a/Extras/InverseDynamics/SimpleTreeCreator.hpp b/Extras/InverseDynamics/SimpleTreeCreator.hpp new file mode 100644 index 000000000..f336eae72 --- /dev/null +++ b/Extras/InverseDynamics/SimpleTreeCreator.hpp @@ -0,0 +1,34 @@ +#ifndef SIMPLETREECREATOR_HPP_ +#define SIMPLETREECREATOR_HPP_ + +#include "MultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +/// minimal "tree" (chain) +class SimpleTreeCreator : public MultiBodyTreeCreator { +public: + /// ctor + /// @param dim number of bodies + SimpleTreeCreator(int dim); + // dtor + ~SimpleTreeCreator() {} + ///\copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int* num_bodies) const; + ///\copydoc MultiBodyTreeCreator::getBody + int getBody(const int body_index, int* parent_index, JointType* joint_type, + vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref, vec3* body_axis_of_motion, + idScalar* mass, vec3* body_r_body_com, mat33* body_I_body, int* user_int, + void** user_ptr) const; + +private: + int m_num_bodies; + idScalar m_mass; + mat33 m_body_T_parent_ref; + vec3 m_parent_r_parent_body_ref; + vec3 m_body_r_body_com; + mat33 m_body_I_body; + vec3 m_axis; +}; +} +#endif // SIMPLETREECREATOR_HPP_ diff --git a/Extras/InverseDynamics/User2InternalIndex.cpp b/Extras/InverseDynamics/User2InternalIndex.cpp new file mode 100644 index 000000000..3e39cb227 --- /dev/null +++ b/Extras/InverseDynamics/User2InternalIndex.cpp @@ -0,0 +1,99 @@ +#include "User2InternalIndex.hpp" + +namespace btInverseDynamics { +User2InternalIndex::User2InternalIndex() : m_map_built(false) {} + +void User2InternalIndex::addBody(const int body, const int parent) { + m_user_parent_index_map[body] = parent; +} + +int User2InternalIndex::findRoot(int index) { + if (0 == m_user_parent_index_map.count(index)) { + return index; + } + return findRoot(m_user_parent_index_map[index]); +} + +// modelled after URDF2Bullet.cpp:void ComputeParentIndices(const +// URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, +// int urdfParentIndex) +void User2InternalIndex::recurseIndexSets(const int user_body_index) { + m_user_to_internal[user_body_index] = m_current_index; + m_current_index++; + for (size_t i = 0; i < m_user_child_indices[user_body_index].size(); i++) { + recurseIndexSets(m_user_child_indices[user_body_index][i]); + } +} + +int User2InternalIndex::buildMapping() { + // find root index + int user_root_index = -1; + for (std::map::iterator it = m_user_parent_index_map.begin(); + it != m_user_parent_index_map.end(); it++) { + int current_root_index = findRoot(it->second); + if (it == m_user_parent_index_map.begin()) { + user_root_index = current_root_index; + } else { + if (user_root_index != current_root_index) { + error_message("multiple roots (at least) %d and %d\n", user_root_index, + current_root_index); + return -1; + } + } + } + + // build child index map + for (std::map::iterator it = m_user_parent_index_map.begin(); + it != m_user_parent_index_map.end(); it++) { + m_user_child_indices[it->second].push_back(it->first); + } + + m_current_index = -1; + // build internal index set + m_user_to_internal[user_root_index] = -1; // add map for root link + recurseIndexSets(user_root_index); + + // reverse mapping + for (std::map::iterator it = m_user_to_internal.begin(); + it != m_user_to_internal.end(); it++) { + m_internal_to_user[it->second] = it->first; + } + + m_map_built = true; + return 0; +} + +int User2InternalIndex::user2internal(const int user, int *internal) const { + + if (!m_map_built) { + return -1; + } + + std::map::const_iterator it; + it = m_user_to_internal.find(user); + if (it != m_user_to_internal.end()) { + *internal = it->second; + return 0; + } else { + error_message("no user index %d\n", user); + return -1; + } +} + +int User2InternalIndex::internal2user(const int internal, int *user) const { + + if (!m_map_built) { + return -1; + } + + std::map::const_iterator it; + it = m_internal_to_user.find(internal); + if (it != m_internal_to_user.end()) { + *user = it->second; + return 0; + } else { + error_message("no internal index %d\n", internal); + return -1; + } +} +} diff --git a/Extras/InverseDynamics/User2InternalIndex.hpp b/Extras/InverseDynamics/User2InternalIndex.hpp new file mode 100644 index 000000000..06c7ef2aa --- /dev/null +++ b/Extras/InverseDynamics/User2InternalIndex.hpp @@ -0,0 +1,46 @@ +#ifndef USER2INTERNALINDEX_HPP +#define USER2INTERNALINDEX_HPP +#include +#include + +#include "BulletInverseDynamics/IDConfig.hpp" + +namespace btInverseDynamics { + +/// Convert arbitrary indexing scheme to internal indexing +/// used for MultiBodyTree +class User2InternalIndex { +public: + /// Ctor + User2InternalIndex(); + /// add body to index maps + /// @param body index of body to add (external) + /// @param parent index of parent body (external) + void addBody(const int body, const int parent); + /// build mapping from external to internal indexing + /// @return 0 on success, -1 on failure + int buildMapping(); + /// get internal index from external index + /// @param user external (user) index + /// @param internal pointer for storage of corresponding internal index + /// @return 0 on success, -1 on failure + int user2internal(const int user, int *internal) const; + /// get internal index from external index + /// @param user external (user) index + /// @param internal pointer for storage of corresponding internal index + /// @return 0 on success, -1 on failure + int internal2user(const int internal, int *user) const; + +private: + int findRoot(int index); + void recurseIndexSets(const int user_body_index); + bool m_map_built; + std::map m_user_parent_index_map; + std::map m_user_to_internal; + std::map m_internal_to_user; + std::map > m_user_child_indices; + int m_current_index; +}; +} + +#endif // USER2INTERNALINDEX_HPP diff --git a/Extras/InverseDynamics/btMultiBodyFromURDF.hpp b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp new file mode 100644 index 000000000..978bad743 --- /dev/null +++ b/Extras/InverseDynamics/btMultiBodyFromURDF.hpp @@ -0,0 +1,91 @@ +#ifndef BTMULTIBODYFROMURDF_HPP +#define BTMULTIBODYFROMURDF_HPP + +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" +#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" +#include "../Importers/ImportURDFDemo/URDF2Bullet.h" + +/// Create a btMultiBody model from URDF. +/// This is adapted from Bullet URDF loader example +class MyBtMultiBodyFromURDF { +public: + /// ctor + /// @param gravity gravitational acceleration (in world frame) + /// @param base_fixed if true, the root body is treated as fixed, + /// if false, it is treated as floating + MyBtMultiBodyFromURDF(const btVector3 &gravity, const bool base_fixed) + : m_gravity(gravity), m_base_fixed(base_fixed) { + m_broadphase = 0x0; + m_dispatcher = 0x0; + m_solver = 0x0; + m_collisionConfiguration = 0x0; + m_dynamicsWorld = 0x0; + m_multibody = 0x0; + } + /// dtor + ~MyBtMultiBodyFromURDF() { + delete m_dynamicsWorld; + delete m_solver; + delete m_broadphase; + delete m_dispatcher; + delete m_collisionConfiguration; + delete m_multibody; + } + /// @param name path to urdf file + void setFileName(const std::string name) { m_filename = name; } + /// load urdf file and build btMultiBody model + void init() { + this->createEmptyDynamicsWorld(); + m_dynamicsWorld->setGravity(m_gravity); + BulletURDFImporter urdf_importer(&m_nogfx); + URDFImporterInterface &u2b(urdf_importer); + bool loadOk = u2b.loadURDF(m_filename.c_str(), m_base_fixed); + + if (loadOk) { + btTransform identityTrans; + identityTrans.setIdentity(); + MyMultiBodyCreator creation(&m_nogfx); + const bool use_multibody = true; + ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, use_multibody, + u2b.getPathPrefix()); + m_multibody = creation.getBulletMultiBody(); + m_dynamicsWorld->stepSimulation(1. / 240., 0); + } + } + /// @return pointer to the btMultiBody model + btMultiBody *getBtMultiBody() { return m_multibody; } + +private: + // internal utility function + void createEmptyDynamicsWorld() { + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + /// use the default collision dispatcher. For parallel processing you can use a diffent + /// dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + m_broadphase = new btDbvtBroadphase(); + m_solver = new btMultiBodyConstraintSolver; + m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, + m_collisionConfiguration); + m_dynamicsWorld->setGravity(m_gravity); + } + + btBroadphaseInterface *m_broadphase; + btCollisionDispatcher *m_dispatcher; + btMultiBodyConstraintSolver *m_solver; + btDefaultCollisionConfiguration *m_collisionConfiguration; + btMultiBodyDynamicsWorld *m_dynamicsWorld; + std::string m_filename; + DummyGUIHelper m_nogfx; + btMultiBody *m_multibody; + const btVector3 m_gravity; + const bool m_base_fixed; +}; +#endif // BTMULTIBODYFROMURDF_HPP diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp new file mode 100644 index 000000000..b91a10cce --- /dev/null +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -0,0 +1,270 @@ +#include "btMultiBodyTreeCreator.hpp" + +namespace btInverseDynamics { + +btMultiBodyTreeCreator::btMultiBodyTreeCreator() : m_initialized(false) {} + +int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const bool verbose) { + if (0x0 == btmb) { + error_message("cannot create MultiBodyTree from null pointer\n"); + return -1; + } + + // in case this is a second call, discard old data + m_data.clear(); + m_initialized = false; + + // btMultiBody treats base link separately + m_data.resize(1 + btmb->getNumLinks()); + + // add base link data + { + LinkData &link = m_data[0]; + + link.parent_index = -1; + if (btmb->hasFixedBase()) { + link.joint_type = FIXED; + } else { + link.joint_type = FLOATING; + } + btTransform transform(btmb->getBaseWorldTransform()); + + link.parent_r_parent_body_ref(0) = transform.getOrigin()[0]; + link.parent_r_parent_body_ref(1) = transform.getOrigin()[1]; + link.parent_r_parent_body_ref(2) = transform.getOrigin()[2]; + + link.body_T_parent_ref(0, 0) = transform.getBasis()[0][0]; + link.body_T_parent_ref(0, 1) = transform.getBasis()[0][1]; + link.body_T_parent_ref(0, 2) = transform.getBasis()[0][2]; + + link.body_T_parent_ref(1, 0) = transform.getBasis()[1][0]; + link.body_T_parent_ref(1, 1) = transform.getBasis()[1][1]; + link.body_T_parent_ref(1, 2) = transform.getBasis()[1][2]; + + link.body_T_parent_ref(2, 0) = transform.getBasis()[2][0]; + link.body_T_parent_ref(2, 1) = transform.getBasis()[2][1]; + link.body_T_parent_ref(2, 2) = transform.getBasis()[2][2]; + + // random unit vector. value not used for fixed or floating joints. + link.body_axis_of_motion(0) = 0; + link.body_axis_of_motion(1) = 0; + link.body_axis_of_motion(2) = 1; + + link.mass = btmb->getBaseMass(); + // link frame in the center of mass + link.body_r_body_com(0) = 0; + link.body_r_body_com(1) = 0; + link.body_r_body_com(2) = 0; + // BulletDynamics uses body-fixed frame in the cog, aligned with principal axes + link.body_I_body(0, 0) = btmb->getBaseInertia()[0]; + link.body_I_body(0, 1) = 0.0; + link.body_I_body(0, 2) = 0.0; + link.body_I_body(1, 0) = 0.0; + link.body_I_body(1, 1) = btmb->getBaseInertia()[1]; + link.body_I_body(1, 2) = 0.0; + link.body_I_body(2, 0) = 0.0; + link.body_I_body(2, 1) = 0.0; + link.body_I_body(2, 2) = btmb->getBaseInertia()[2]; + // shift reference point to link origin (in joint axis) + mat33 tilde_r_com = tildeOperator(link.body_r_body_com); + link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com; + if (verbose) { + id_printf("base: mass= %f, bt_inertia= [%f %f %f]\n" + "Io= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + link.mass, btmb->getBaseInertia()[0], btmb->getBaseInertia()[1], + btmb->getBaseInertia()[2], link.body_I_body(0, 0), link.body_I_body(0, 1), + link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1), + link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1), + link.body_I_body(2, 2)); + } + } + + for (int bt_index = 0; bt_index < btmb->getNumLinks(); bt_index++) { + if (verbose) { + id_printf("bt->id: converting link %d\n", bt_index); + } + const btMultibodyLink &bt_link = btmb->getLink(bt_index); + LinkData &link = m_data[bt_index + 1]; + + link.parent_index = bt_link.m_parent + 1; + + link.mass = bt_link.m_mass; + if (verbose) { + id_printf("mass= %f\n", link.mass); + } + // from this body's pivot to this body's com in this body's frame + link.body_r_body_com[0] = bt_link.m_dVector[0]; + link.body_r_body_com[1] = bt_link.m_dVector[1]; + link.body_r_body_com[2] = bt_link.m_dVector[2]; + if (verbose) { + id_printf("com= %f %f %f\n", link.body_r_body_com[0], link.body_r_body_com[1], + link.body_r_body_com[2]); + } + // BulletDynamics uses a body-fixed frame in the CoM, aligned with principal axes + link.body_I_body(0, 0) = bt_link.m_inertiaLocal[0]; + link.body_I_body(0, 1) = 0.0; + link.body_I_body(0, 2) = 0.0; + link.body_I_body(1, 0) = 0.0; + link.body_I_body(1, 1) = bt_link.m_inertiaLocal[1]; + link.body_I_body(1, 2) = 0.0; + link.body_I_body(2, 0) = 0.0; + link.body_I_body(2, 1) = 0.0; + link.body_I_body(2, 2) = bt_link.m_inertiaLocal[2]; + // shift reference point to link origin (in joint axis) + mat33 tilde_r_com = tildeOperator(link.body_r_body_com); + link.body_I_body = link.body_I_body - link.mass * tilde_r_com * tilde_r_com; + + if (verbose) { + id_printf("link %d: mass= %f, bt_inertia= [%f %f %f]\n" + "Io= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + bt_index, link.mass, bt_link.m_inertiaLocal[0], bt_link.m_inertiaLocal[1], + bt_link.m_inertiaLocal[2], link.body_I_body(0, 0), link.body_I_body(0, 1), + link.body_I_body(0, 2), link.body_I_body(1, 0), link.body_I_body(1, 1), + link.body_I_body(1, 2), link.body_I_body(2, 0), link.body_I_body(2, 1), + link.body_I_body(2, 2)); + } + // transform for vectors written in parent frame to this link's body-fixed frame + btMatrix3x3 basis = btTransform(bt_link.m_zeroRotParentToThis).getBasis(); + link.body_T_parent_ref(0, 0) = basis[0][0]; + link.body_T_parent_ref(0, 1) = basis[0][1]; + link.body_T_parent_ref(0, 2) = basis[0][2]; + link.body_T_parent_ref(1, 0) = basis[1][0]; + link.body_T_parent_ref(1, 1) = basis[1][1]; + link.body_T_parent_ref(1, 2) = basis[1][2]; + link.body_T_parent_ref(2, 0) = basis[2][0]; + link.body_T_parent_ref(2, 1) = basis[2][1]; + link.body_T_parent_ref(2, 2) = basis[2][2]; + if (verbose) { + id_printf("body_T_parent_ref= %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2], + basis[2][0], basis[2][1], basis[2][2]); + } + switch (bt_link.m_jointType) { + case btMultibodyLink::eRevolute: + link.joint_type = REVOLUTE; + if (verbose) { + id_printf("type= revolute\n"); + } + link.body_axis_of_motion(0) = bt_link.m_axes[0].m_topVec[0]; + link.body_axis_of_motion(1) = bt_link.m_axes[0].m_topVec[1]; + link.body_axis_of_motion(2) = bt_link.m_axes[0].m_topVec[2]; + + // for revolute joints, m_eVector = parentComToThisPivotOffset + // m_dVector = thisPivotToThisComOffset + // from parent com to pivot, in parent frame + link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0]; + link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1]; + link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; + break; + case btMultibodyLink::ePrismatic: + link.joint_type = PRISMATIC; + if (verbose) { + id_printf("type= prismatic\n"); + } + link.body_axis_of_motion(0) = bt_link.m_axes[0].m_bottomVec[0]; + link.body_axis_of_motion(1) = bt_link.m_axes[0].m_bottomVec[1]; + link.body_axis_of_motion(2) = bt_link.m_axes[0].m_bottomVec[2]; + + // for prismatic joints, eVector + // according to documentation : + // parentComToThisComOffset + // but seems to be: from parent's com to parent's + // pivot ?? + // m_dVector = thisPivotToThisComOffset + link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0]; + link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1]; + link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; + break; + case btMultibodyLink::eSpherical: + error_message("spherical joints not implemented\n"); + return -1; + case btMultibodyLink::ePlanar: + error_message("planar joints not implemented\n"); + return -1; + case btMultibodyLink::eFixed: + link.joint_type = FIXED; + // random unit vector + link.body_axis_of_motion(0) = 0; + link.body_axis_of_motion(1) = 0; + link.body_axis_of_motion(2) = 1; + + // for fixed joints, m_dVector = thisPivotToThisComOffset; + // m_eVector = parentComToThisPivotOffset; + link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0]; + link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1]; + link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2]; + break; + default: + error_message("unknown btMultiBody::eFeatherstoneJointType %d\n", + bt_link.m_jointType); + return -1; + } + if (link.parent_index > 0) { // parent body isn't the root + const btMultibodyLink &bt_parent_link = btmb->getLink(link.parent_index - 1); + // from parent pivot to parent com, in parent frame + link.parent_r_parent_body_ref(0) += bt_parent_link.m_dVector[0]; + link.parent_r_parent_body_ref(1) += bt_parent_link.m_dVector[1]; + link.parent_r_parent_body_ref(2) += bt_parent_link.m_dVector[2]; + } else { + // parent is root body. btMultiBody only knows 6-DoF or 0-DoF root bodies, + // whose link frame is in the CoM (ie, no notion of a pivot point) + } + + if (verbose) { + id_printf("parent_r_parent_body_ref= %f %f %f\n", link.parent_r_parent_body_ref[0], + link.parent_r_parent_body_ref[1], link.parent_r_parent_body_ref[2]); + } + } + + m_initialized = true; + + return 0; +} + +int btMultiBodyTreeCreator::getNumBodies(int *num_bodies) const { + if (false == m_initialized) { + error_message("btMultiBody not converted yet\n"); + return -1; + } + + *num_bodies = static_cast(m_data.size()); + return 0; +} + +int btMultiBodyTreeCreator::getBody(int body_index, int *parent_index, JointType *joint_type, + vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, + vec3 *body_axis_of_motion, idScalar *mass, + vec3 *body_r_body_com, mat33 *body_I_body, int *user_int, + void **user_ptr) const { + if (false == m_initialized) { + error_message("MultiBodyTree not created yet\n"); + return -1; + } + + if (body_index < 0 || body_index >= static_cast(m_data.size())) { + error_message("index out of range (got %d but only %zu bodies)\n", body_index, + m_data.size()); + return -1; + } + + *parent_index = m_data[body_index].parent_index; + *joint_type = m_data[body_index].joint_type; + *parent_r_parent_body_ref = m_data[body_index].parent_r_parent_body_ref; + *body_T_parent_ref = m_data[body_index].body_T_parent_ref; + *body_axis_of_motion = m_data[body_index].body_axis_of_motion; + *mass = m_data[body_index].mass; + *body_r_body_com = m_data[body_index].body_r_body_com; + *body_I_body = m_data[body_index].body_I_body; + + *user_int = -1; + *user_ptr = 0x0; + + return 0; +} +} diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp new file mode 100644 index 000000000..1ae00ff79 --- /dev/null +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp @@ -0,0 +1,50 @@ +#ifndef BTMULTIBODYTREECREATOR_HPP_ +#define BTMULTIBODYTREECREATOR_HPP_ + +#include + +#include "BulletInverseDynamics/IDConfig.hpp" +#include "MultiBodyTreeCreator.hpp" +#include "BulletDynamics/Featherstone/btMultiBody.h" + +namespace btInverseDynamics { + +/// MultiBodyTreeCreator implementation for converting +/// a btMultiBody forward dynamics model into a MultiBodyTree inverse dynamics model +class btMultiBodyTreeCreator : public MultiBodyTreeCreator { +public: + /// ctor + btMultiBodyTreeCreator(); + /// dtor + ~btMultiBodyTreeCreator() {} + /// extract model data from a btMultiBody + /// @param btmb pointer to btMultiBody to convert + /// @param verbose if true, some information is printed + /// @return -1 on error, 0 on success + int createFromBtMultiBody(const btMultiBody *btmb, const bool verbose = false); + /// \copydoc MultiBodyTreeCreator::getNumBodies + int getNumBodies(int *num_bodies) const; + ///\copydoc MultiBodyTreeCreator::getBody + int getBody(int body_index, int *parent_index, JointType *joint_type, + vec3 *parent_r_parent_body_ref, mat33 *body_T_parent_ref, vec3 *body_axis_of_motion, + idScalar *mass, vec3 *body_r_body_com, mat33 *body_I_body, int *user_int, + void **user_ptr) const; + +private: + // internal struct holding data extracted from btMultiBody + struct LinkData { + int parent_index; + JointType joint_type; + vec3 parent_r_parent_body_ref; + mat33 body_T_parent_ref; + vec3 body_axis_of_motion; + idScalar mass; + vec3 body_r_body_com; + mat33 body_I_body; + }; + std::vector m_data; + bool m_initialized; +}; +} + +#endif // BTMULTIBODYTREECREATOR_HPP_ diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.cpp b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp new file mode 100644 index 000000000..21e693f62 --- /dev/null +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.cpp @@ -0,0 +1,307 @@ +#include "invdyn_bullet_comparison.hpp" +#include +#include "BulletInverseDynamics/IDConfig.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" +#include "btBulletDynamicsCommon.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" + +namespace btInverseDynamics { +int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose, + btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error, + double *acc_error) { +// call function and return -1 if it does, printing an error_message +#define RETURN_ON_FAILURE(x) \ + do { \ + if (-1 == x) { \ + error_message("calling " #x "\n"); \ + return -1; \ + } \ + } while (0) + + if (verbose) { + printf("\n ===================================== \n"); + } + vecx joint_forces(q.size()); + + // set positions and velocities for btMultiBody + // base link + mat33 world_T_base; + vec3 world_pos_base; + btTransform base_transform; + vec3 base_velocity; + vec3 base_angular_velocity; + + RETURN_ON_FAILURE(id_tree->setGravityInWorldFrame(gravity)); + RETURN_ON_FAILURE(id_tree->getBodyOrigin(0, &world_pos_base)); + RETURN_ON_FAILURE(id_tree->getBodyTransform(0, &world_T_base)); + RETURN_ON_FAILURE(id_tree->getBodyAngularVelocity(0, &base_angular_velocity)); + RETURN_ON_FAILURE(id_tree->getBodyLinearVelocityCoM(0, &base_velocity)); + + base_transform.setBasis(world_T_base); + base_transform.setOrigin(world_pos_base); + btmb->setBaseWorldTransform(base_transform); + btmb->setBaseOmega(base_angular_velocity); + btmb->setBaseVel(base_velocity); + btmb->setLinearDamping(0); + btmb->setAngularDamping(0); + + // remaining links + int q_index; + if (btmb->hasFixedBase()) { + q_index = 0; + } else { + q_index = 6; + } + if (verbose) { + printf("bt:num_links= %d, num_dofs= %d\n", btmb->getNumLinks(), btmb->getNumDofs()); + } + for (int l = 0; l < btmb->getNumLinks(); l++) { + const btMultibodyLink &link = btmb->getLink(l); + if (verbose) { + printf("link %d, pos_var_count= %d, dof_count= %d\n", l, link.m_posVarCount, + link.m_dofCount); + } + if (link.m_posVarCount == 1) { + btmb->setJointPosMultiDof(l, &q(q_index)); + btmb->setJointVelMultiDof(l, &u(q_index)); + if (verbose) { + printf("set q[%d]= %f, u[%d]= %f\n", q_index, q(q_index), q_index, u(q_index)); + } + q_index++; + } + } + // sanity check + if (q_index != q.size()) { + error_message("error in number of dofs for btMultibody and MultiBodyTree\n"); + return -1; + } + + // run inverse dynamics to determine joint_forces for given q, u, dot_u + if (-1 == id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces)) { + error_message("calculating inverse dynamics\n"); + return -1; + } + + // set up bullet forward dynamics model + btScalar dt = 0; + btAlignedObjectArray scratch_r; + btAlignedObjectArray scratch_v; + btAlignedObjectArray scratch_m; + // this triggers switch between using either appliedConstraintForce or appliedForce + bool isConstraintPass = false; + // apply gravity forces for btMultiBody model. Must be done manually. + btmb->addBaseForce(btmb->getBaseMass() * gravity); + + for (int link = 0; link < btmb->getNumLinks(); link++) { + btmb->addLinkForce(link, gravity * btmb->getLinkMass(link)); + if (verbose) { + printf("link %d, applying gravity %f %f %f\n", link, + gravity[0] * btmb->getLinkMass(link), gravity[1] * btmb->getLinkMass(link), + gravity[2] * btmb->getLinkMass(link)); + } + } + + // apply generalized forces + if (btmb->hasFixedBase()) { + q_index = 0; + } else { + vec3 base_force; + base_force(0) = joint_forces(3); + base_force(1) = joint_forces(4); + base_force(2) = joint_forces(5); + + vec3 base_moment; + base_moment(0) = joint_forces(0); + base_moment(1) = joint_forces(1); + base_moment(2) = joint_forces(2); + + btmb->addBaseForce(world_T_base * base_force); + btmb->addBaseTorque(world_T_base * base_moment); + if (verbose) { + printf("base force from id: %f %f %f\n", joint_forces(3), joint_forces(4), + joint_forces(5)); + printf("base moment from id: %f %f %f\n", joint_forces(0), joint_forces(1), + joint_forces(2)); + } + q_index = 6; + } + + for (int l = 0; l < btmb->getNumLinks(); l++) { + const btMultibodyLink &link = btmb->getLink(l); + if (link.m_posVarCount == 1) { + if (verbose) { + printf("id:joint_force[%d]= %f, applied to link %d\n", q_index, + joint_forces(q_index), l); + } + btmb->addJointTorque(l, joint_forces(q_index)); + q_index++; + } + } + + // sanity check + if (q_index != q.size()) { + error_message("error in number of dofs for btMultibody and MultiBodyTree\n"); + return -1; + } + + // run forward kinematics & forward dynamics + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + btmb->forwardKinematics(world_to_local, local_origin); + btmb->computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass); + + // read generalized accelerations back from btMultiBody + // the mapping from scratch variables to accelerations is taken from the implementation + // of stepVelocitiesMultiDof + btScalar *base_accel = &scratch_r[btmb->getNumDofs()]; + btScalar *joint_accel = base_accel + 6; + *acc_error = 0; + int dot_u_offset = 0; + if (btmb->hasFixedBase()) { + dot_u_offset = 0; + } else { + dot_u_offset = 6; + } + + if (true == btmb->hasFixedBase()) { + for (int i = 0; i < btmb->getNumDofs(); i++) { + if (verbose) { + printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i], + dot_u(i + dot_u_offset), joint_accel[i] - dot_u(i)); + } + *acc_error += std::pow(joint_accel[i] - dot_u(i + dot_u_offset), 2); + } + } else { + vec3 base_dot_omega; + vec3 world_dot_omega; + world_dot_omega(0) = base_accel[0]; + world_dot_omega(1) = base_accel[1]; + world_dot_omega(2) = base_accel[2]; + base_dot_omega = world_T_base.transpose() * world_dot_omega; + + // com happens to coincide with link origin here. If that changes, we need to calculate + // ddot_com + vec3 base_ddot_com; + vec3 world_ddot_com; + world_ddot_com(0) = base_accel[3]; + world_ddot_com(1) = base_accel[4]; + world_ddot_com(2) = base_accel[5]; + base_ddot_com = world_T_base.transpose()*world_ddot_com; + + for (int i = 0; i < 3; i++) { + if (verbose) { + printf("bt::base_dot_omega(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_dot_omega(i), + i, dot_u[i], base_dot_omega(i) - dot_u[i]); + } + *acc_error += std::pow(base_dot_omega(i) - dot_u(i), 2); + } + for (int i = 0; i < 3; i++) { + if (verbose) { + printf("bt::base_ddot_com(%d)= %e dot_u[%d]= %e, diff= %e\n", i, base_ddot_com(i), + i, dot_u[i + 3], base_ddot_com(i) - dot_u[i + 3]); + } + *acc_error += std::pow(base_ddot_com(i) - dot_u(i + 3), 2); + } + + for (int i = 0; i < btmb->getNumDofs(); i++) { + if (verbose) { + printf("bt:ddot_q[%d]= %f, id:ddot_q= %e, diff= %e\n", i, joint_accel[i], + dot_u(i + 6), joint_accel[i] - dot_u(i + 6)); + } + *acc_error += std::pow(joint_accel[i] - dot_u(i + 6), 2); + } + } + *acc_error = std::sqrt(*acc_error); + if (verbose) { + printf("======dynamics-err: %e\n", *acc_error); + } + *pos_error = 0.0; + + { + mat33 world_T_body; + if (-1 == id_tree->getBodyTransform(0, &world_T_body)) { + error_message("getting transform for body %d\n", 0); + return -1; + } + vec3 world_com; + if (-1 == id_tree->getBodyCoM(0, &world_com)) { + error_message("getting com for body %d\n", 0); + return -1; + } + if (verbose) { + printf("id:com: %f %f %f\n", world_com(0), world_com(1), world_com(2)); + + printf("id:transform: %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + world_T_body(0, 0), world_T_body(0, 1), world_T_body(0, 2), world_T_body(1, 0), + world_T_body(1, 1), world_T_body(1, 2), world_T_body(2, 0), world_T_body(2, 1), + world_T_body(2, 2)); + } + } + + for (int l = 0; l < btmb->getNumLinks(); l++) { + const btMultibodyLink &bt_link = btmb->getLink(l); + + vec3 bt_origin = bt_link.m_cachedWorldTransform.getOrigin(); + mat33 bt_basis = bt_link.m_cachedWorldTransform.getBasis(); + if (verbose) { + printf("------------- link %d\n", l + 1); + printf("bt:com: %f %f %f\n", bt_origin(0), bt_origin(1), bt_origin(2)); + printf("bt:transform: %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + bt_basis(0, 0), bt_basis(0, 1), bt_basis(0, 2), bt_basis(1, 0), bt_basis(1, 1), + bt_basis(1, 2), bt_basis(2, 0), bt_basis(2, 1), bt_basis(2, 2)); + } + mat33 id_world_T_body; + vec3 id_world_com; + + if (-1 == id_tree->getBodyTransform(l + 1, &id_world_T_body)) { + error_message("getting transform for body %d\n", l); + return -1; + } + if (-1 == id_tree->getBodyCoM(l + 1, &id_world_com)) { + error_message("getting com for body %d\n", l); + return -1; + } + if (verbose) { + printf("id:com: %f %f %f\n", id_world_com(0), id_world_com(1), id_world_com(2)); + + printf("id:transform: %f %f %f\n" + " %f %f %f\n" + " %f %f %f\n", + id_world_T_body(0, 0), id_world_T_body(0, 1), id_world_T_body(0, 2), + id_world_T_body(1, 0), id_world_T_body(1, 1), id_world_T_body(1, 2), + id_world_T_body(2, 0), id_world_T_body(2, 1), id_world_T_body(2, 2)); + } + vec3 diff_com = bt_origin - id_world_com; + mat33 diff_basis = bt_basis - id_world_T_body; + if (verbose) { + printf("diff-com: %e %e %e\n", diff_com(0), diff_com(1), diff_com(2)); + + printf("diff-transform: %e %e %e %e %e %e %e %e %e\n", diff_basis(0, 0), + diff_basis(0, 1), diff_basis(0, 2), diff_basis(1, 0), diff_basis(1, 1), + diff_basis(1, 2), diff_basis(2, 0), diff_basis(2, 1), diff_basis(2, 2)); + } + double total_pos_err = + std::sqrt(std::pow(diff_com(0), 2) + std::pow(diff_com(1), 2) + + std::pow(diff_com(2), 2) + std::pow(diff_basis(0, 0), 2) + + std::pow(diff_basis(0, 1), 2) + std::pow(diff_basis(0, 2), 2) + + std::pow(diff_basis(1, 0), 2) + std::pow(diff_basis(1, 1), 2) + + std::pow(diff_basis(1, 2), 2) + std::pow(diff_basis(2, 0), 2) + + std::pow(diff_basis(2, 1), 2) + std::pow(diff_basis(2, 2), 2)); + if (verbose) { + printf("======kin-pos-err: %e\n", total_pos_err); + } + if (total_pos_err > *pos_error) { + *pos_error = total_pos_err; + } + } + + return 0; +} +} diff --git a/Extras/InverseDynamics/invdyn_bullet_comparison.hpp b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp new file mode 100644 index 000000000..2a74af59e --- /dev/null +++ b/Extras/InverseDynamics/invdyn_bullet_comparison.hpp @@ -0,0 +1,34 @@ +#ifndef INVDYN_BULLET_COMPARISON_HPP +#define INVDYN_BULLET_COMPARISON_HPP + +class btMultiBody; +class btVector3; + +namespace btInverseDynamics { +class MultiBodyTree; +class vecx; + +/// this function compares the forward dynamics computations implemented in btMultiBody to +/// the inverse dynamics implementation in MultiBodyTree. This is done in three steps +/// 1. run inverse dynamics for (q, u, dot_u) to obtain joint forces f +/// 2. run forward dynamics (btMultiBody) for (q,u,f) to obtain dot_u_bullet +/// 3. compare dot_u with dot_u_bullet for cross check of forward and inverse dynamics computations +/// @param btmb the bullet forward dynamics model +/// @param id_tree the inverse dynamics model +/// @param q vector of generalized coordinates (matches id_tree) +/// @param u vector of generalized speeds (matches id_tree) +/// @param gravity gravitational acceleration in world frame +/// @param dot_u vector of generalized accelerations (matches id_tree) +/// @param gravity gravitational acceleration in world frame +/// @param base_fixed set base joint to fixed or +/// @param pos_error is set to the maximum of the euclidean norm of position+rotation errors of all +/// center of gravity positions and link frames +/// @param acc_error is set to the square root of the sum of squared differences of generalized +/// accelerations +/// computed in step 3 relative to dot_u +/// @return -1 on error, 0 on success +int compareInverseAndForwardDynamics(vecx &q, vecx &u, vecx &dot_u, btVector3 &gravity, bool verbose, + btMultiBody *btmb, MultiBodyTree *id_tree, double *pos_error, + double *acc_error); +} +#endif // INVDYN_BULLET_COMPARISON_HPP diff --git a/Extras/InverseDynamics/premake4.lua b/Extras/InverseDynamics/premake4.lua new file mode 100644 index 000000000..0a524d46a --- /dev/null +++ b/Extras/InverseDynamics/premake4.lua @@ -0,0 +1,12 @@ + project "BulletInverseDynamicsUtils" + + kind "StaticLib" + + includedirs { + "../../src" + } + + files { + "*.cpp", + "*.hpp" + } diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp new file mode 100644 index 000000000..6df61071f --- /dev/null +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -0,0 +1,40 @@ +///@file Configuration for Inverse Dynamics Library, +/// such as choice of linear algebra library and underlying scalar type +#ifndef IDCONFIG_HPP_ +#define IDCONFIG_HPP_ +// If we have a custom configuration, compile without using other parts of bullet. +#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +#define BT_ID_WO_BULLET +#endif +// error messages +#include "IDErrorMessages.hpp" + +#ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H +#define INVDYN_INCLUDE_HELPER_2(x) #x +#define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) +#include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) +#else +// Use default configuration with bullet's types +// Use the same scalar type as rest of bullet library +#include "LinearMath/btScalar.h" +typedef btScalar idScalar; +// use bullet types for arrays and array indices +#include "Bullet3Common/b3AlignedObjectArray.h" +// this is to make it work with C++2003, otherwise we could do this: +// template +// using idArray = b3AlignedObjectArray; +template +struct idArray { + typedef b3AlignedObjectArray type; +}; +typedef int idArrayIdx; +#define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR + +// use bullet's allocator functions +#define idMalloc btAllocFunc +#define idFree btFreeFunc + +#define ID_LINEAR_MATH_USE_BULLET +#include "details/IDLinearMathInterface.hpp" +#endif +#endif diff --git a/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/src/BulletInverseDynamics/IDConfigBuiltin.hpp new file mode 100644 index 000000000..2427c16f0 --- /dev/null +++ b/src/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -0,0 +1,36 @@ +///@file Configuration for Inverse Dynamics Library without external dependencies +#ifndef INVDYNCONFIG_BUILTIN_HPP_ +#define INVDYNCONFIG_BUILTIN_HPP_ +#ifdef BT_USE_DOUBLE_PRECISION +// choose double/single precision version +typedef double idScalar; +#else +typedef float idScalar; +#endif +// use std::vector for arrays +#include +// this is to make it work with C++2003, otherwise we could do this +// template +// using idArray = std::vector; +template +struct idArray { + typedef std::vector type; +}; +typedef std::vector::size_type idArrayIdx; +// default to standard malloc/free +#include +#define idMalloc ::malloc +#define idFree ::free +// currently not aligned at all... +#define ID_DECLARE_ALIGNED_ALLOCATOR() \ + inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete(void* ptr) { idFree(ptr); } \ + inline void* operator new(std::size_t, void* ptr) { return ptr; } \ + inline void operator delete(void*, void*) {} \ + inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete[](void* ptr) { idFree(ptr); } \ + inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ + inline void operator delete[](void*, void*) {} + +#include "details/IDMatVec.hpp" +#endif diff --git a/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp new file mode 100644 index 000000000..e04b149a6 --- /dev/null +++ b/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -0,0 +1,43 @@ +///@file Configuration for Inverse Dynamics Library with Eigen +#ifndef INVDYNCONFIG_EIGEN_HPP_ +#define INVDYNCONFIG_EIGEN_HPP_ +#ifdef BT_USE_DOUBLE_PRECISION +// choose double/single precision version +typedef double idScalar; +#else +typedef float idScalar; +#endif + +// use std::vector for arrays +#include +// this is to make it work with C++2003, otherwise we could do this +// template +// using idArray = std::vector; +template +struct idArray { + typedef std::vector type; +}; +typedef std::vector::size_type idArrayIdx; +// default to standard malloc/free +#include +#define idMalloc ::malloc + +#define idFree ::free +// currently not aligned at all... +#define ID_DECLARE_ALIGNED_ALLOCATOR() \ + inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete(void* ptr) { idFree(ptr); } \ + inline void* operator new(std::size_t, void* ptr) { return ptr; } \ + inline void operator delete(void*, void*) {} \ + inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete[](void* ptr) { idFree(ptr); } \ + inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ + inline void operator delete[](void*, void*) {} + +// Note on interfaces: +// Eigen::Matrix has data(), to get c-array storage +// HOWEVER: default storage is column-major! +#define ID_LINEAR_MATH_USE_EIGEN +#include "Eigen/Eigen" +#include "details/IDEigenInterface.hpp" +#endif diff --git a/src/BulletInverseDynamics/IDErrorMessages.hpp b/src/BulletInverseDynamics/IDErrorMessages.hpp new file mode 100644 index 000000000..a66ea8323 --- /dev/null +++ b/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -0,0 +1,34 @@ +///@file error message utility functions +#ifndef IDUTILS_HPP_ +#define IDUTILS_HPP_ +#include +/// name of file being compiled, without leading path components +#define __INVDYN_FILE_WO_DIR__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__) + +#ifndef BT_ID_WO_BULLET +#include "Bullet3Common/b3Logging.h" +#define error_message(...) b3Error(__VA_ARGS__) +#define warning_message(...) b3Warning(__VA_ARGS__) +#define id_printf(...) b3Printf(__VA_ARGS__) +#else // BT_ID_WO_BULLET +#include +/// print error message with file/line information +#define error_message(...) \ + do { \ + fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +/// print warning message with file/line information +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define id_printf(...) printf(__VA_ARGS__) +#endif // BT_ID_WO_BULLET +#endif diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp new file mode 100644 index 000000000..a3daaf1f5 --- /dev/null +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -0,0 +1,371 @@ +#include "IDMath.hpp" + +#include +#include + +namespace btInverseDynamics { +static const idScalar kIsZero = 5 * std::numeric_limits::epsilon(); +// requirements for axis length deviation from 1.0 +// experimentally set from random euler angle rotation matrices +static const idScalar kAxisLengthEpsilon = 10 * kIsZero; + +void setZero(vec3 &v) { + v(0) = 0; + v(1) = 0; + v(2) = 0; +} + +void setZero(vecx &v) { + for (int i = 0; i < v.size(); i++) { + v(i) = 0; + } +} + +void setZero(mat33 &m) { + m(0, 0) = 0; + m(0, 1) = 0; + m(0, 2) = 0; + m(1, 0) = 0; + m(1, 1) = 0; + m(1, 2) = 0; + m(2, 0) = 0; + m(2, 1) = 0; + m(2, 2) = 0; +} + +idScalar maxAbs(const vecx &v) { + idScalar result = 0.0; + for (int i = 0; i < v.size(); i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; +} + +idScalar maxAbs(const vec3 &v) { + idScalar result = 0.0; + for (int i = 0; i < 3; i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; +} + +mat33 transformX(const idScalar &alpha) { + mat33 T; + const idScalar cos_alpha = std::cos(alpha); + const idScalar sin_alpha = std::sin(alpha); + // [1 0 0] + // [0 c s] + // [0 -s c] + T(0, 0) = 1.0; + T(0, 1) = 0.0; + T(0, 2) = 0.0; + + T(1, 0) = 0.0; + T(1, 1) = cos_alpha; + T(1, 2) = sin_alpha; + + T(2, 0) = 0.0; + T(2, 1) = -sin_alpha; + T(2, 2) = cos_alpha; + + return T; +} + +mat33 transformY(const idScalar &beta) { + mat33 T; + const idScalar cos_beta = std::cos(beta); + const idScalar sin_beta = std::sin(beta); + // [c 0 -s] + // [0 1 0] + // [s 0 c] + T(0, 0) = cos_beta; + T(0, 1) = 0.0; + T(0, 2) = -sin_beta; + + T(1, 0) = 0.0; + T(1, 1) = 1.0; + T(1, 2) = 0.0; + + T(2, 0) = sin_beta; + T(2, 1) = 0.0; + T(2, 2) = cos_beta; + + return T; +} + +mat33 transformZ(const idScalar &gamma) { + mat33 T; + const idScalar cos_gamma = std::cos(gamma); + const idScalar sin_gamma = std::sin(gamma); + // [ c s 0] + // [-s c 0] + // [ 0 0 1] + T(0, 0) = cos_gamma; + T(0, 1) = sin_gamma; + T(0, 2) = 0.0; + + T(1, 0) = -sin_gamma; + T(1, 1) = cos_gamma; + T(1, 2) = 0.0; + + T(2, 0) = 0.0; + T(2, 1) = 0.0; + T(2, 2) = 1.0; + + return T; +} + +mat33 tildeOperator(const vec3 &v) { + mat33 m; + m(0, 0) = 0.0; + m(0, 1) = -v(2); + m(0, 2) = v(1); + m(1, 0) = v(2); + m(1, 1) = 0.0; + m(1, 2) = -v(0); + m(2, 0) = -v(1); + m(2, 1) = v(0); + m(2, 2) = 0.0; + return m; +} + +void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { + const idScalar sa = std::sin(alpha); + const idScalar ca = std::cos(alpha); + const idScalar st = std::sin(theta); + const idScalar ct = std::cos(theta); + + (*r)(0) = a; + (*r)(1) = -sa * d; + (*r)(2) = ca * d; + + (*T)(0, 0) = ct; + (*T)(0, 1) = -st; + (*T)(0, 2) = 0.0; + + (*T)(1, 0) = st * ca; + (*T)(1, 1) = ct * ca; + (*T)(1, 2) = -sa; + + (*T)(2, 0) = st * sa; + (*T)(2, 1) = ct * sa; + (*T)(2, 2) = ca; +} + +void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { + const idScalar c = cos(angle); + const idScalar s = -sin(angle); + const idScalar one_m_c = 1.0 - c; + + const idScalar &x = axis(0); + const idScalar &y = axis(1); + const idScalar &z = axis(2); + + (*T)(0, 0) = x * x * one_m_c + c; + (*T)(0, 1) = x * y * one_m_c - z * s; + (*T)(0, 2) = x * z * one_m_c + y * s; + + (*T)(1, 0) = x * y * one_m_c + z * s; + (*T)(1, 1) = y * y * one_m_c + c; + (*T)(1, 2) = y * z * one_m_c - x * s; + + (*T)(2, 0) = x * z * one_m_c - y * s; + (*T)(2, 1) = y * z * one_m_c + x * s; + (*T)(2, 2) = z * z * one_m_c + c; +} + +bool isPositiveDefinite(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) <= 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; +} + +bool isPositiveSemiDefinite(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) < 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; +} + +bool isPositiveSemiDefiniteFuzzy(const mat33 &m) { + // test if all upper left determinants are positive + if (m(0, 0) < -kIsZero) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) { + return false; + } + return true; +} + +idScalar determinant(const mat33 &m) { + return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - + m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2); +} + +bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) { + // TODO(Thomas) do we really want this? + // in cases where the inertia tensor about the center of mass is zero, + // the determinant of the inertia tensor about the joint axis is almost + // zero and can have a very small negative value. + if (!isPositiveSemiDefiniteFuzzy(I)) { + error_message("invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + + return false; + } + + // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) + if (!has_fixed_joint) { + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(1, 1) + I(2, 2) < I(0, 0)) { + error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + } + // check positive/zero diagonal elements + for (int i = 0; i < 3; i++) { + if (I(i, i) < 0) { // accept zero + error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); + return false; + } + } + // check symmetry + if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " + "%e\n", + index, I(1, 0) - I(0, 1)); + return false; + } + if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " + "%e\n", + index, I(2, 0) - I(0, 2)); + return false; + } + if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) { + error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index, + I(1, 2) - I(2, 1)); + return false; + } + return true; +} + +bool isValidTransformMatrix(const mat33 &m) { +#define print_mat(x) \ + error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ + x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) + + // check for unit length column vectors + for (int i = 0; i < 3; i++) { + const idScalar length_minus_1 = + std::fabs(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0); + if (length_minus_1 > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (column %d not unit length)\n" + "column = [%.18e %.18e %.18e]\n" + "length-1.0= %.18e\n", + i, m(0, i), m(1, i), m(2, i), length_minus_1); + print_mat(m); + return false; + } + } + // check for orthogonal column vectors + if (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n"); + print_mat(m); + return false; + } + if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + // check determinant (rotation not reflection) + if (determinant(m) <= 0) { + error_message("Not a valid rotation matrix (determinant <=0)\n"); + print_mat(m); + return false; + } + return true; +} + +bool isUnitVector(const vec3 &vector) { + return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < + kIsZero; +} + +vec3 rpyFromMatrix(const mat33 &rot) { + vec3 rpy; + rpy(2) = std::atan2(-rot(1, 0), rot(0, 0)); + rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0)); + rpy(0) = std::atan2(-rot(2, 0), rot(2, 2)); + return rpy; +} +} diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp new file mode 100644 index 000000000..97e60c120 --- /dev/null +++ b/src/BulletInverseDynamics/IDMath.hpp @@ -0,0 +1,88 @@ +/// @file Math utility functions used in inverse dynamics library. +/// Defined here as they may not be provided by the math library. + +#ifndef IDMATH_HPP_ +#define IDMATH_HPP_ +#include "IDConfig.hpp" + +namespace btInverseDynamics { +/// set all elements to zero +void setZero(vec3& v); +/// set all elements to zero +void setZero(vecx& v); +/// set all elements to zero +void setZero(mat33& m); + +/// return maximum absolute value +idScalar maxAbs(const vecx& v); +#ifndef ID_LINEAR_MATH_USE_EIGEN +/// return maximum absolute value +idScalar maxAbs(const vec3& v); +#endif //ID_LINEAR_MATH_USE_EIGEN +/// get offset vector & transform matrix from DH parameters +/// TODO: add documentation +void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T); + +/// Check if a 3x3 matrix is positive definite +/// @param m a 3x3 matrix +/// @return true if m>0, false otherwise +bool isPositiveDefinite(const mat33& m); + +/// Check if a 3x3 matrix is positive semi definite +/// @param m a 3x3 matrix +/// @return true if m>=0, false otherwise +bool isPositiveSemiDefinite(const mat33& m); +/// Check if a 3x3 matrix is positive semi definite within numeric limits +/// @param m a 3x3 matrix +/// @return true if m>=-eps, false otherwise +bool isPositiveSemiDefiniteFuzzy(const mat33& m); + +/// Determinant of 3x3 matrix +/// NOTE: implemented here for portability, as determinant operation +/// will be implemented differently for various matrix/vector libraries +/// @param m a 3x3 matrix +/// @return det(m) +idScalar determinant(const mat33& m); + +/// Test if a 3x3 matrix satisfies some properties of inertia matrices +/// @param I a 3x3 matrix +/// @param index body index (for error messages) +/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted +/// @return true if I satisfies inertia matrix properties, false otherwise. +bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint); + +/// Check if a 3x3 matrix is a valid transform (rotation) matrix +/// @param m a 3x3 matrix +/// @return true if m is a rotation matrix, false otherwise +bool isValidTransformMatrix(const mat33& m); +/// Transform matrix from parent to child frame, +/// when the child frame is rotated about @param axis by @angle +/// (mathematically positive) +/// @param axis the axis of rotation +/// @param angle rotation angle +/// @param T pointer to transform matrix +void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T); + +/// Check if this is a unit vector +/// @param vector +/// @return true if |vector|=1 within numeric limits +bool isUnitVector(const vec3& vector); + +/// @input a vector in R^3 +/// @returns corresponding spin tensor +mat33 tildeOperator(const vec3& v); +/// @param alpha angle in radians +/// @returns transform matrix for ratation with @param alpha about x-axis +mat33 transformX(const idScalar& alpha); +/// @param beta angle in radians +/// @returns transform matrix for ratation with @param beta about y-axis +mat33 transformY(const idScalar& beta); +/// @param gamma angle in radians +/// @returns transform matrix for ratation with @param gamma about z-axis +mat33 transformZ(const idScalar& gamma); +///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix +/// @param rot rotation matrix +/// @returns x-y-z Euler angles +vec3 rpyFromMatrix(const mat33&rot); +} +#endif // IDMATH_HPP_ diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp new file mode 100644 index 000000000..3de0d8e4f --- /dev/null +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -0,0 +1,331 @@ +#include "MultiBodyTree.hpp" + +#include +#include +#include + +#include "IDMath.hpp" +#include "details/MultiBodyTreeImpl.hpp" +#include "details/MultiBodyTreeInitCache.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::MultiBodyTree() + : m_is_finalized(false), + m_mass_parameters_are_valid(true), + m_accept_invalid_mass_parameters(false), + m_impl(0x0), + m_init_cache(0x0) { + m_init_cache = new InitCache(); +} + +MultiBodyTree::~MultiBodyTree() { + delete m_impl; + delete m_init_cache; +} + +void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) { + m_accept_invalid_mass_parameters = flag; +} + +bool MultiBodyTree::getAcceptInvalidMassProperties() const { + return m_accept_invalid_mass_parameters; +} + +int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const { + return m_impl->getBodyOrigin(body_index, world_origin); +} + +int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const { + return m_impl->getBodyCoM(body_index, world_com); +} + +int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const { + return m_impl->getBodyTransform(body_index, world_T_body); +} +int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const { + return m_impl->getBodyAngularVelocity(body_index, world_omega); +} +int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const { + return m_impl->getBodyLinearVelocity(body_index, world_velocity); +} + +int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const { + return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); +} + +int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const { + return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); +} +int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const { + return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); +} + +void MultiBodyTree::printTree() { m_impl->printTree(); } +void MultiBodyTree::printTreeData() { m_impl->printTreeData(); } + +int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; } + +int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; } + +int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u, + vecx *joint_forces) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { + error_message("error in inverse dynamics calculation\n"); + return -1; + } + return 0; +} + +int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics, + const bool initialize_matrix, + const bool set_lower_triangular_matrix, matxx *mass_matrix) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == + m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, + set_lower_triangular_matrix, mass_matrix)) { + error_message("error in mass matrix calculation\n"); + return -1; + } + return 0; +} +int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { + return calculateMassMatrix(q, true, true, true, mass_matrix); +} +int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type, + const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, + const vec3 &body_axis_of_motion_, idScalar mass, + const vec3 &body_r_body_com, const mat33 &body_I_body, + const int user_int, void *user_ptr) { + if (body_index < 0) { + error_message("body index must be positive (got %d)\n", body_index); + return -1; + } + vec3 body_axis_of_motion(body_axis_of_motion_); + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + // check if axis is unit vector + if (!isUnitVector(body_axis_of_motion)) { + warning_message( + "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", + body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); + idScalar length = std::sqrt(std::pow(body_axis_of_motion(0), 2) + + std::pow(body_axis_of_motion(1), 2) + + std::pow(body_axis_of_motion(2), 2)); + if (length < std::sqrt(std::numeric_limits::min())) { + error_message("axis of motion vector too short (%e)\n", length); + return -1; + } + body_axis_of_motion = (1.0 / length) * body_axis_of_motion; + } + break; + case FIXED: + break; + case FLOATING: + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + // sanity check for mass properties. Zero mass is OK. + if (mass < 0) { + m_mass_parameters_are_valid = false; + error_message("Body %d has invalid mass %e\n", body_index, mass); + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } + + if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) { + m_mass_parameters_are_valid = false; + // error message printed in function call + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } + + if (!isValidTransformMatrix(body_T_parent_ref)) { + return -1; + } + + return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, + body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, + body_I_body, user_int, user_ptr); +} + +int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const { + return m_impl->getParentIndex(body_index, parent_index); +} + +int MultiBodyTree::getUserInt(const int body_index, int *user_int) const { + return m_impl->getUserInt(body_index, user_int); +} + +int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const { + return m_impl->getUserPtr(body_index, user_ptr); +} + +int MultiBodyTree::setUserInt(const int body_index, const int user_int) { + return m_impl->setUserInt(body_index, user_int); +} + +int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) { + return m_impl->setUserPtr(body_index, user_ptr); +} + +int MultiBodyTree::finalize() { + const int &num_bodies = m_init_cache->numBodies(); + const int &num_dofs = m_init_cache->numDoFs(); + + // 1 allocate internal MultiBody structure + m_impl = new MultiBodyImpl(num_bodies, num_dofs); + + // 2 build new index set assuring index(parent) < index(child) + if (-1 == m_init_cache->buildIndexSets()) { + return -1; + } + m_init_cache->getParentIndexArray(&m_impl->m_parent_index); + + // 3 setup internal kinematic and dynamic data + for (int index = 0; index < num_bodies; index++) { + InertiaData inertia; + JointData joint; + if (-1 == m_init_cache->getInertiaData(index, &inertia)) { + return -1; + } + if (-1 == m_init_cache->getJointData(index, &joint)) { + return -1; + } + + RigidBody &rigid_body = m_impl->m_body_list[index]; + + rigid_body.m_mass = inertia.m_mass; + rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; + rigid_body.m_body_I_body = inertia.m_body_I_body; + rigid_body.m_joint_type = joint.m_type; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_joint_type = joint.m_type; + + // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized + // matrices. + switch (rigid_body.m_joint_type) { + case REVOLUTE: + rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case PRISMATIC: + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); + break; + case FIXED: + // NOTE/TODO: dimension really should be zero .. + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case FLOATING: + // NOTE/TODO: this is not really correct. + // the Jacobians should be 3x3 matrices here ! + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + default: + error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + return -1; + } + } + + // 4 assign degree of freedom indices & build per-joint-type index arrays + if (-1 == m_impl->generateIndexSets()) { + error_message("generating index sets\n"); + return -1; + } + + // 5 do some pre-computations .. + m_impl->calculateStaticData(); + + // 6. make sure all user forces are set to zero, as this might not happen + // in the vector ctors. + m_impl->clearAllUserForcesAndMoments(); + + m_is_finalized = true; + return 0; +} + +int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) { + return m_impl->setGravityInWorldFrame(gravity); +} + +int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const { + return m_impl->getJointType(body_index, joint_type); +} + +int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const { + return m_impl->getJointTypeStr(body_index, joint_type); +} + +int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const { + return m_impl->getDoFOffset(body_index, q_offset); +} + +int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { + return m_impl->setBodyMass(body_index, mass); +} + +int MultiBodyTree::setBodyFirstMassMoment(const int body_index, vec3 first_mass_moment) { + return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); +} + +int MultiBodyTree::setBodySecondMassMoment(const int body_index, mat33 second_mass_moment) { + return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); +} + +int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const { + return m_impl->getBodyMass(body_index, mass); +} + +int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const { + return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); +} + +int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const { + return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); +} + +void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); } + +int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) { + return m_impl->addUserForce(body_index, body_force); +} + +int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) { + return m_impl->addUserMoment(body_index, body_moment); +} + +} diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp new file mode 100644 index 000000000..833baf1d3 --- /dev/null +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -0,0 +1,308 @@ +#ifndef MULTIBODYTREE_HPP_ +#define MULTIBODYTREE_HPP_ + +#include "IDMath.hpp" + +namespace btInverseDynamics { + +/// Enumeration of supported joint types +enum JointType { + /// no degree of freedom, moves with parent + FIXED = 0, + /// one rotational degree of freedom relative to parent + REVOLUTE, + /// one translational degree of freedom relative to parent + PRISMATIC, + /// six degrees of freedom relative to parent + FLOATING +}; + +/// Interface class for calculating inverse dynamics for tree structured +/// multibody systems +/// +/// Note on degrees of freedom +/// The q vector contains the generalized coordinate set defining the tree's configuration. +/// Every joint adds elements that define the corresponding link's frame pose relative to +/// its parent. For the joint types that is: +/// - FIXED: none +/// - REVOLUTE: angle of rotation [rad] +/// - PRISMATIC: displacement [m] +/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] +/// (in that order) +/// The u vector contains the generalized speeds, which are +/// - FIXED: none +/// - REVOLUTE: time derivative of angle of rotation [rad/s] +/// - PRISMATIC: time derivative of displacement [m/s] +/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) +/// and time derivative of displacement in parent frame [m/s] +/// +/// The q and u vectors are obtained by stacking contributions of all bodies in one +/// vector in the order of body indices. +/// +/// Note on generalized forces: analogous to u, i.e., +/// - FIXED: none +/// - REVOLUTE: moment [Nm], about joint axis +/// - PRISMATIC: force [N], along joint axis +/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame +/// (in that order) +/// +/// TODO - force element interface (friction, springs, dampers, etc) +/// - gears and motor inertia +class MultiBodyTree { +public: + /// The contructor. + /// Initialization & allocation is via addBody and buildSystem calls. + MultiBodyTree(); + /// the destructor. This also deallocates all memory + ~MultiBodyTree(); + + /// Add body to the system. this allocates memory and not real-time safe. + /// This only adds the data to an initial cache. After all bodies have been + /// added, + /// the system is setup using the buildSystem call + /// @param body_index index of the body to be added. Must >=0, =dim(u) + /// @param dot_u time derivative of u + /// @param joint_forces this is where the resulting joint forces will be + /// stored. dim(joint_forces) = dim(u) + /// @return 0 on success, -1 on error + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + /// Calculate joint space mass matrix + /// @param q generalized coordinates + /// @param initialize_matrix if true, initialize mass matrix with zero. + /// If mass_matrix is initialized to zero externally and only used + /// for mass matrix computations for the same system, it is safe to + /// set this to false. + /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix + /// is also populated, otherwise not. + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + + /// Calculate joint space mass matrix. + /// This version will update kinematics, initialize all mass_matrix elements to zero and + /// populate all mass matrix entries. + /// @param q generalized coordinates + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, matxx* mass_matrix); + + /// set gravitational acceleration + /// the default is [0;0;-9.8] in the world frame + /// @param gravity the gravitational acceleration in world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// returns number of bodies in tree + int numBodies() const; + /// returns number of mechanical degrees of freedom (dimension of q-vector) + int numDoFs() const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// get center of mass of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyCoM(const int body_index, vec3* world_com) const; + /// get transform from of a body-fixed frame to the world frame + /// @param body_index index for frame/body + /// @param world_T_body pointer for return data + /// @return 0 on success, -1 on error + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// get absolute angular velocity for a body, represented in the world frame + /// @param body_index index for frame/body + /// @param world_omega pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// get linear velocity of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_velocity pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// get linear velocity of a body's CoM, represented in world frame + /// (not required for inverse dynamics, provided for convenience) + /// @param body_index index for frame/body + /// @param world_vel_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// get origin of a body-fixed frame, represented in world frame + /// NOTE: this will include the gravitational acceleration, so the actual acceleration is + /// obtainened by setting gravitational acceleration to zero, or subtracting it. + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// returns the (internal) index of body + /// @param body_index is the index of a body (internal: TODO: fix/clarify + /// indexing!) + /// @param parent_index pointer to where parent index will be stored + /// @return 0 on success, -1 on error + int getParentIndex(const int body_index, int* parent_index) const; + /// get joint type + /// @param body_index index of the body + /// @param joint_type the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointType(const int body_index, JointType* joint_type) const; + /// get joint type as string + /// @param body_index index of the body + /// @param joint_type string naming the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointTypeStr(const int body_index, const char** joint_type) const; + /// get offset for degrees of freedom of this body into the q-vector + /// @param body_index index of the body + /// @param q_offset offset the q vector + /// @return -1 on error, 0 on success + int getDoFOffset(const int body_index, int* q_offset) const; + /// get user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int getUserInt(const int body_index, int* user_int) const; + /// get user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int getUserPtr(const int body_index, void** user_ptr) const; + /// set user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int setUserInt(const int body_index, const int user_int); + /// set user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int setUserPtr(const int body_index, void* const user_ptr); + /// set mass for a body + /// @param body_index index of the body + /// @param mass the mass to set + /// @return 0 on success, -1 on failure + int setBodyMass(const int body_index, const idScalar mass); + /// set first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_mass_moment the vector to set + /// @return 0 on success, -1 on failure + int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + /// set second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + /// get mass for a body + /// @param body_index index of the body + /// @param mass the mass + /// @return 0 on success, -1 on failure + int getBodyMass(const int body_index, idScalar* mass) const; + /// get first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_moment the vector + /// @return 0 on success, -1 on failure + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + /// get second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// set all user forces and moments to zero + void clearAllUserForcesAndMoments(); + /// Add an external force to a body, acting at the origin of the body-fixed frame. + /// Calls to addUserForce are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_force the force represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserForce(const int body_index, const vec3& body_force); + /// Add an external moment to a body. + /// Calls to addUserMoment are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_moment the moment represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserMoment(const int body_index, const vec3& body_moment); + +private: + // flag indicating if system has been initialized + bool m_is_finalized; + // flag indicating if mass properties are physically valid + bool m_mass_parameters_are_valid; + // flag defining if unphysical mass parameters are accepted + bool m_accept_invalid_mass_parameters; + // This struct implements the inverse dynamics calculations + class MultiBodyImpl; + MultiBodyImpl* m_impl; + // cache data structure for initialization + class InitCache; + InitCache* m_init_cache; +}; +} // namespace btInverseDynamics + +#endif // MULTIBODYTREE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDEigenInterface.hpp b/src/BulletInverseDynamics/details/IDEigenInterface.hpp new file mode 100644 index 000000000..ec6136aa5 --- /dev/null +++ b/src/BulletInverseDynamics/details/IDEigenInterface.hpp @@ -0,0 +1,17 @@ +#ifndef INVDYNEIGENINTERFACE_HPP_ +#define INVDYNEIGENINTERFACE_HPP_ +#include "../IDConfig.hpp" +namespace btInverseDynamics { +#ifdef BT_USE_DOUBLE_PRECISION +typedef Eigen::VectorXd vecx; +typedef Eigen::Vector3d vec3; +typedef Eigen::Matrix3d mat33; +typedef Eigen::MatrixXd matxx; +#else +typedef Eigen::VectorXf vecx; +typedef Eigen::Vector3f vec3; +typedef Eigen::Matrix3f mat33; +typedef Eigen::MatrixXf matxx; +#endif // +} +#endif // INVDYNEIGENINTERFACE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp new file mode 100644 index 000000000..2209121ba --- /dev/null +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -0,0 +1,112 @@ +#ifndef IDLINEARMATHINTERFACE_HPP_ +#define IDLINEARMATHINTERFACE_HPP_ + +#include + +#include "../IDConfig.hpp" + +#include "../../LinearMath/btMatrix3x3.h" +#include "../../LinearMath/btVector3.h" +#include "../../LinearMath/btMatrixX.h" + +namespace btInverseDynamics { +class vec3; +class vecx; +class mat33; +typedef btMatrixX matxx; + +class vec3 : public btVector3 { +public: + vec3() : btVector3() {} + vec3(const btVector3& btv) { *this = btv; } + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } + const int size() const { return 3; } + const vec3& operator=(const btVector3& rhs) { + *static_cast(this) = rhs; + return *this; + } +}; + +class mat33 : public btMatrix3x3 { +public: + mat33() : btMatrix3x3() {} + mat33(const btMatrix3x3& btm) { *this = btm; } + idScalar& operator()(int i, int j) { return (*this)[i][j]; } + const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } + const mat33& operator=(const btMatrix3x3& rhs) { + *static_cast(this) = rhs; + return *this; + } + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator/(const mat33& a, const idScalar& s); +}; + +inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); } + +inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } + +class vecx : public btVectorX { +public: + vecx(int size) : btVectorX(size) {} + const vecx& operator=(const btVectorX& rhs) { + *static_cast(this) = rhs; + return *this; + } + + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } + + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); + + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); +}; + +inline vecx operator*(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) * s; + } + return result; +} +inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } +inline vecx operator+(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result(i) = a(i) + b(i); + } + + return result; +} + +inline vecx operator-(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result(i) = a(i) - b(i); + } + return result; +} +inline vecx operator/(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) / s; + } + + return result; +} +} + +#endif // IDLINEARMATHINTERFACE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp new file mode 100644 index 000000000..961abade8 --- /dev/null +++ b/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -0,0 +1,326 @@ +/// @file Built-In Matrix-Vector functions +#ifndef IDMATVEC_HPP_ +#define IDMATVEC_HPP_ + +#include + +namespace btInverseDynamics { +class vec3; +class vecx; +class mat33; +class matxx; + +/// This is a very basic implementation to enable stand-alone use of the library. +/// The implementation is not really optimized and misses many features that you would +/// want from a "fully featured" linear math library. +class vec3 { +public: + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int size() const { return 3; } + const vec3& operator=(const vec3& rhs); + const vec3& operator+=(const vec3& b); + const vec3& operator-=(const vec3& b); + vec3 cross(const vec3& b) const; + idScalar dot(const vec3& b) const; + + friend vec3 operator*(const mat33& a, const vec3& b); + friend vec3 operator*(const vec3& a, const idScalar& s); + friend vec3 operator*(const idScalar& s, const vec3& a); + + friend vec3 operator+(const vec3& a, const vec3& b); + friend vec3 operator-(const vec3& a, const vec3& b); + friend vec3 operator/(const vec3& a, const idScalar& s); + +private: + idScalar m_data[3]; +}; + +class mat33 { +public: + idScalar& operator()(int i, int j) { return m_data[3 * i + j]; } + const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; } + const mat33& operator=(const mat33& rhs); + mat33 transpose() const; + const mat33& operator+=(const mat33& b); + const mat33& operator-=(const mat33& b); + + friend mat33 operator*(const mat33& a, const mat33& b); + friend vec3 operator*(const mat33& a, const vec3& b); + friend mat33 operator*(const mat33& a, const idScalar& s); + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator+(const mat33& a, const mat33& b); + friend mat33 operator-(const mat33& a, const mat33& b); + friend mat33 operator/(const mat33& a, const idScalar& s); + +private: + // layout is [0,1,2;3,4,5;6,7,8] + idScalar m_data[9]; +}; + +class vecx { +public: + vecx(int size) : m_size(size) { + m_data = static_cast(idMalloc(sizeof(idScalar) * size)); + } + ~vecx() { idFree(m_data); } + const vecx& operator=(const vecx& rhs); + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int& size() const { return m_size; } + + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); + + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); + +private: + int m_size; + idScalar* m_data; +}; + +class matxx { +public: + matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { + m_data = static_cast(idMalloc(sizeof(idScalar) * rows * cols)); + } + ~matxx() { idFree(m_data); } + const matxx& operator=(const matxx& rhs); + idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; } + const int& rows() const { return m_rows; } + const int& cols() const { return m_cols; } + +private: + int m_rows; + int m_cols; + idScalar* m_data; +}; + +inline const vec3& vec3::operator=(const vec3& rhs) { + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); + } + return *this; +} + +inline vec3 vec3::cross(const vec3& b) const { + vec3 result; + result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1]; + result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2]; + result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; + + return result; +} + +inline idScalar vec3::dot(const vec3& b) const { + return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2]; +} + +inline const mat33& mat33::operator=(const mat33& rhs) { + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); + } + return *this; +} +inline mat33 mat33::transpose() const { + mat33 result; + result.m_data[0] = m_data[0]; + result.m_data[1] = m_data[3]; + result.m_data[2] = m_data[6]; + result.m_data[3] = m_data[1]; + result.m_data[4] = m_data[4]; + result.m_data[5] = m_data[7]; + result.m_data[6] = m_data[2]; + result.m_data[7] = m_data[5]; + result.m_data[8] = m_data[8]; + + return result; +} + +inline mat33 operator*(const mat33& a, const mat33& b) { + mat33 result; + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6]; + result.m_data[1] = + a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; + result.m_data[2] = + a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; + result.m_data[3] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; + result.m_data[4] = + a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; + result.m_data[5] = + a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; + result.m_data[6] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; + result.m_data[7] = + a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; + result.m_data[8] = + a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; + + return result; +} + +inline const mat33& mat33::operator+=(const mat33& b) { + for (int i = 0; i < 9; i++) { + m_data[i] += b.m_data[i]; + } + + return *this; +} + +inline const mat33& mat33::operator-=(const mat33& b) { + for (int i = 0; i < 9; i++) { + m_data[i] -= b.m_data[i]; + } + return *this; +} + +inline vec3 operator*(const mat33& a, const vec3& b) { + vec3 result; + + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; + result.m_data[1] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; + result.m_data[2] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; + + return result; +} + +inline const vec3& vec3::operator+=(const vec3& b) { + for (int i = 0; i < 3; i++) { + m_data[i] += b.m_data[i]; + } + return *this; +} + +inline const vec3& vec3::operator-=(const vec3& b) { + for (int i = 0; i < 3; i++) { + m_data[i] -= b.m_data[i]; + } + return *this; +} + +inline mat33 operator*(const mat33& a, const idScalar& s) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; +} + +inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } + +inline vec3 operator*(const vec3& a, const idScalar& s) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; +} +inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; } + +inline mat33 operator+(const mat33& a, const mat33& b) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + return result; +} +inline vec3 operator+(const vec3& a, const vec3& b) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + return result; +} + +inline mat33 operator-(const mat33& a, const mat33& b) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; +} +inline vec3 operator-(const vec3& a, const vec3& b) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; +} + +inline mat33 operator/(const mat33& a, const idScalar& s) { + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] / s; + } + return result; +} + +inline vec3 operator/(const vec3& a, const idScalar& s) { + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] / s; + } + return result; +} + +inline const vecx& vecx::operator=(const vecx& rhs) { + if (size() != rhs.size()) { + error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); + abort(); + } + if (&rhs != this) { + memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); + } + return *this; +} +inline vecx operator*(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; +} +inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } +inline vecx operator+(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + + return result; +} +inline vecx operator-(const vecx& a, const vecx& b) { + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; +} +inline vecx operator/(const vecx& a, const idScalar& s) { + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] / s; + } + + return result; +} +} +#endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp new file mode 100644 index 000000000..53ef4503d --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -0,0 +1,844 @@ +#include "MultiBodyTreeImpl.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) + : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) { + m_body_list.resize(num_bodies_); + m_parent_index.resize(num_bodies_); + m_child_indices.resize(num_bodies_); + m_user_int.resize(num_bodies_); + m_user_ptr.resize(num_bodies_); + + m_world_gravity(0) = 0.0; + m_world_gravity(1) = 0.0; + m_world_gravity(2) = -9.8; +} + +const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const { + switch (type) { + case FIXED: + return "fixed"; + case REVOLUTE: + return "revolute"; + case PRISMATIC: + return "prismatic"; + case FLOATING: + return "floating"; + } + return "error: invalid"; +} + +inline void indent(const int &level) { + for (int j = 0; j < level; j++) + id_printf(" "); // indent +} + +void MultiBodyTree::MultiBodyImpl::printTree() { + id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); + printTree(0, 0); +} + +void MultiBodyTree::MultiBodyImpl::printTreeData() { + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + id_printf("body: %d\n", static_cast(i)); + id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); + id_printf("q_index= %d\n", body.m_q_index); + id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); + id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); + + id_printf("mass = %f\n", body.m_mass); + id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), + body.m_body_mass_com(2)); + id_printf("I_o= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), + body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), + body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); + + id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), + body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); + } +} +int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + error_message("unknown joint type %d\n", type); + return 0; +} + +void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { + // this is adapted from URDF2Bullet. + // TODO: fix this and print proper graph (similar to git --log --graph) + int num_children = m_child_indices[index].size(); + + indentation += 2; + int count = 0; + + for (int i = 0; i < num_children; i++) { + int child_index = m_child_indices[index][i]; + indent(indentation); + id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, + jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, + m_body_list[index].m_q_index, + m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); + // first grandchild + printTree(child_index, indentation); + } +} + +int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { + m_world_gravity = gravity; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::generateIndexSets() { + m_body_revolute_list.resize(0); + m_body_prismatic_list.resize(0); + int q_index = 0; + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + body.m_q_index = -1; + switch (body.m_joint_type) { + case REVOLUTE: + m_body_revolute_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case PRISMATIC: + m_body_prismatic_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case FIXED: + // do nothing + break; + case FLOATING: + m_body_floating_list.push_back(i); + body.m_q_index = q_index; + q_index += 6; + break; + default: + error_message("unsupported joint type %d\n", body.m_joint_type); + return -1; + } + } + // sanity check + if (q_index != m_num_dofs) { + error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); + return -1; + } + + m_child_indices.resize(m_body_list.size()); + + for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { + const int &parent = m_parent_index[child]; + if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) { + m_child_indices[parent].push_back(child); + } else { + if (-1 == parent) { + // multiple bodies are directly linked to the environment, ie, not a single root + error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + } else { + // should never happen + error_message( + "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", + child, parent, static_cast(m_parent_index.size())); + } + return -1; + } + } + + return 0; +} + +void MultiBodyTree::MultiBodyImpl::calculateStaticData() { + // relative kinematics that are not a function of q, u, dot_u + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + switch (body.m_joint_type) { + case REVOLUTE: + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + break; + case PRISMATIC: + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + break; + case FIXED: + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + break; + case FLOATING: + // no static data + break; + } + } +} + +int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, + const vecx &dot_u, vecx *joint_forces) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || + joint_forces->size() != m_num_dofs) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size()), + static_cast(dot_u.size()), static_cast(joint_forces->size())); + return -1; + } + // 1. update relative kinematics + // 1.1 for revolute + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + mat33 T; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + + // body.m_parent_r_parent_body= fixed + body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); + // body.m_parent_dot_r_rel = fixed; + // NOTE: this assumes that Jac_JR is constant, which is true for revolute + // joints, but not in the general case (eg, slider-crank type mechanisms) + body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); + // body.m_parent_ddot_r_rel = fixed; + } + // 1.2 for prismatic + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // body.m_body_T_parent= fixed + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + // body.m_parent_omega_rel = 0; + body.m_parent_vel_rel = + body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); + // body.parent_dot_omega_rel = 0; + // NOTE: this assumes that Jac_JT is constant, which is true for + // prismatic joints, but not in the general case + body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + + body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); + body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); + body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); + + body.m_parent_vel_rel(0) = u(body.m_q_index + 3); + body.m_parent_vel_rel(1) = u(body.m_q_index + 4); + body.m_parent_vel_rel(2) = u(body.m_q_index + 5); + + body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); + body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); + body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); + + body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); + body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); + body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); + + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } + + // 3. absolute kinematic quantities & dynamic quantities + // NOTE: this should be optimized by specializing for different body types + // (e.g., relative rotation is always zero for prismatic joints, etc.) + + // calculations for root body + { + RigidBody &body = m_body_list[0]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) + // not required right now, added here for debugging purposes + body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_body_T_world = body.m_body_T_parent; + + // 3.2 update absolute velocities + body.m_body_ang_vel = body.m_body_ang_vel_rel; + body.m_body_vel = body.m_parent_vel_rel; + + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = body.m_body_ang_acc_rel; + body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; + // add gravitational acceleration to root body + // this is an efficient way to add gravitational terms, + // but it does mean that the kinematics are no longer + // correct at the acceleration level + // NOTE: To get correct acceleration kinematics, just set world_gravity to zero + body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; + } + + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) not required right now added here for debugging purposes + body.m_body_pos = + body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); + body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; + + // 3.2 update absolute velocities + body.m_body_ang_vel = + body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; + + body.m_body_vel = + body.m_body_T_parent * + (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + + body.m_parent_vel_rel); + + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = + body.m_body_T_parent * parent.m_body_ang_acc - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + + body.m_body_ang_acc_rel; + body.m_body_acc = + body.m_body_T_parent * + (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross( + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); + } + + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + // 3.4 update dynamic terms (rate of change of angular & linear momentum) + body.m_eom_lhs_rotational = + body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + + body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - + body.m_body_moment_user; + body.m_eom_lhs_translational = + body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + + body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - + body.m_body_force_user; + } + + // 4. calculate full set of forces at parent joint + // (not directly calculating the joint force along the free direction + // simplifies inclusion of fixed joints. + // An alternative would be to fuse bodies in a pre-processing step, + // but that would make changing masses online harder (eg, payload masses + // added with fixed joints to a gripper) + // Also, this enables adding zero weight bodies as a way to calculate frame poses + // for force elements, etc. + + for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { + // sum of forces and moments acting on this body from its children + vec3 sum_f_children; + vec3 sum_m_children; + setZero(sum_f_children); + setZero(sum_m_children); + for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); + child_list_idx++) { + const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; + vec3 child_joint_force_in_this_frame = + child.m_body_T_parent.transpose() * child.m_force_at_joint; + sum_f_children -= child_joint_force_in_this_frame; + sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + + child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); + } + RigidBody &body = m_body_list[body_idx]; + + body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; + body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; + } + + // 4. Calculate Joint forces. + // These are the components of force_at_joint/moment_at_joint + // in the free directions given by Jac_JT/Jac_JR + // 4.1 revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); + } + // 4.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); + } + // 4.3 floating bodies (6-DoF joints) + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); + (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); + (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); + + (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); + (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); + (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); + } + + return 0; +} + +static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { + switch (dof) { + // rotational part + case 0: + Jac_JR(0) = 1; + Jac_JR(1) = 0; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 1: + Jac_JR(0) = 0; + Jac_JR(1) = 1; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 2: + Jac_JR(0) = 0; + Jac_JR(1) = 0; + Jac_JR(2) = 1; + setZero(Jac_JT); + break; + // translational part + case 3: + setZero(Jac_JR); + Jac_JT(0) = 1; + Jac_JT(1) = 0; + Jac_JT(2) = 0; + break; + case 4: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 1; + Jac_JT(2) = 0; + break; + case 5: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 0; + Jac_JT(2) = 1; + break; + } +} + +static inline int jointNumDoFs(const JointType &type) { + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + // this should never happen + error_message("invalid joint type\n"); + // TODO add configurable abort/crash function + abort(); +} + +int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, + const bool initialize_matrix, + const bool set_lower_triangular_matrix, + matxx *mass_matrix) { +// This calculates the joint space mass matrix for the multibody system. +// The algorithm is essentially an implementation of "method 3" +// in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982) +// (Later named "Composite Rigid Body Algorithm" by Featherstone). +// +// This implementation, however, handles branched systems and uses a formulation centered +// on the origin of the body-fixed frame to avoid re-computing various quantities at the com. + +// Macro for setting matrix elements depending on underlying math library. +#ifdef ID_LINEAR_MATH_USE_BULLET +#define setMassMatrixElem(row, col, val) mass_matrix->setElem(row, col, val) +#else +#define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val +#endif + + if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || + mass_matrix->cols() != m_num_dofs) { + error_message("Dimension error. System has %d DOFs,\n" + "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", + m_num_dofs, static_cast(q.size()), static_cast(mass_matrix->rows()), + static_cast(mass_matrix->cols())); + return -1; + } + + // TODO add optimized zeroing function? + if (initialize_matrix) { + for (int i = 0; i < m_num_dofs; i++) { + for (int j = 0; j < m_num_dofs; j++) { + setMassMatrixElem(i, j, 0.0); + } + } + } + + if (update_kinematics) { + // 1. update relative kinematics + // 1.1 for revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // from reference orientation (q=0) of body-fixed frame to current orientation + mat33 body_T_body_ref; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); + body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; + } + // 1.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // body.m_body_T_parent= fixed + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * + transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + } + } + for (int i = m_body_list.size() - 1; i >= 0; i--) { + RigidBody &body = m_body_list[i]; + // calculate mass, center of mass and inertia of "composite rigid body", + // ie, sub-tree starting at current body + body.m_subtree_mass = body.m_mass; + body.m_body_subtree_mass_com = body.m_body_mass_com; + body.m_body_subtree_I_body = body.m_body_I_body; + + for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { + RigidBody &child = m_body_list[m_child_indices[i][c]]; + mat33 body_T_child = child.m_body_T_parent.transpose(); + + body.m_subtree_mass += child.m_subtree_mass; + body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + + child.m_parent_pos_parent_body * child.m_subtree_mass; + body.m_body_subtree_I_body += + body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; + + if (child.m_subtree_mass > 0) { + // Shift the reference point for the child subtree inertia using the + // Huygens-Steiner ("parallel axis") theorem. + // (First shift from child origin to child com, then from there to this body's + // origin) + vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; + mat33 tilde_r_child_com = tildeOperator(r_com); + mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); + body.m_body_subtree_I_body += + child.m_subtree_mass * + (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); + } + } + } + + for (int i = m_body_list.size() - 1; i >= 0; i--) { + const RigidBody &body = m_body_list[i]; + + // determine DoF-range for body + const int q_index_min = body.m_q_index; + const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; + // loop over the DoFs used by this body + // local joint jacobians (ok as is for 1-DoF joints) + vec3 Jac_JR = body.m_Jac_JR; + vec3 Jac_JT = body.m_Jac_JT; + for (int col = q_index_max; col >= q_index_min; col--) { + // set jacobians for 6-DoF joints + if (FLOATING == body.m_joint_type) { + setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); + } + + vec3 body_eom_rot = + body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); + vec3 body_eom_trans = + body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); + setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans)); + + // rest of the mass matrix column upwards + { + // 1. for multi-dof joints, rest of the dofs of this body + for (int row = col - 1; row >= q_index_min; row--) { + if (FLOATING != body.m_joint_type) { + error_message("??\n"); + return -1; + } + setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMassMatrixElem(col, row, Mrc); + } + // 2. ancestor dofs + int child_idx = i; + int parent_idx = m_parent_index[i]; + while (parent_idx >= 0) { + const RigidBody &child_body = m_body_list[child_idx]; + const RigidBody &parent_body = m_body_list[parent_idx]; + + const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); + body_eom_rot = parent_T_child * body_eom_rot; + body_eom_trans = parent_T_child * body_eom_trans; + body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); + + const int parent_body_q_index_min = parent_body.m_q_index; + const int parent_body_q_index_max = + parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; + vec3 Jac_JR = parent_body.m_Jac_JR; + vec3 Jac_JT = parent_body.m_Jac_JT; + for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { + // set jacobians for 6-DoF joints + if (FLOATING == parent_body.m_joint_type) { + setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); + } + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMassMatrixElem(col, row, Mrc); + } + + child_idx = parent_idx; + parent_idx = m_parent_index[child_idx]; + } + } + } + } + + if (set_lower_triangular_matrix) { + for (int col = 0; col < m_num_dofs; col++) { + for (int row = 0; row < col; row++) { + setMassMatrixElem(row, col, (*mass_matrix)(col, row)); + } + } + } + +#undef setMassMatrixElem + return 0; +} + +// utility macro +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do { \ + if (index < 0 || index >= m_num_bodies) { \ + error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ + return -1; \ + } \ + } while (0) + +int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *p = m_parent_index[body_index]; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_int = m_user_int[body_index]; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_ptr = m_user_ptr[body_index]; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_int[body_index] = user_int; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_ptr[body_index] = user_ptr; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + if (body.m_mass > 0) { + *world_com = body.m_body_T_world.transpose() * + (body.m_body_pos + body.m_body_mass_com / body.m_mass); + } else { + *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); + } + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_T_body = body.m_body_T_world.transpose(); + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + vec3 com; + if (body.m_mass > 0) { + com = body.m_body_mass_com / body.m_mass; + } else { + com(0) = 0; + com(1) = 0; + com(2) = 0; + } + + *world_velocity = + body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, + vec3 *world_dot_omega) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index, + vec3 *world_acceleration) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = m_body_list[body_index].m_joint_type; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, + const char **joint_type) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); + return 0; +} + +int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *q_index = m_body_list[body_index].m_q_index; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_mass = mass; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, + const vec3 first_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_mass_com = first_mass_moment; + return 0; +} +int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, + const mat33 second_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_I_body = second_mass_moment; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *mass = m_body_list[body_index].m_mass; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index, + vec3 *first_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *first_mass_moment = m_body_list[body_index].m_body_mass_com; + return 0; +} +int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index, + mat33 *second_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *second_mass_moment = m_body_list[body_index].m_body_I_body; + return 0; +} + +void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() { + for (int index = 0; index < m_num_bodies; index++) { + RigidBody &body = m_body_list[index]; + setZero(body.m_body_force_user); + setZero(body.m_body_moment_user); + } +} + +int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_force_user += body_force; + return 0; +} + +int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_moment_user += body_moment; + return 0; +} + +} diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp new file mode 100644 index 000000000..63d8945eb --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -0,0 +1,236 @@ +// The structs and classes defined here provide a basic inverse fynamics implementation used +// by MultiBodyTree +// User interaction should be through MultiBodyTree + +#ifndef MULTI_BODY_REFERENCE_IMPL_HPP_ +#define MULTI_BODY_REFERENCE_IMPL_HPP_ + +#include "../IDConfig.hpp" +#include "../MultiBodyTree.hpp" + +namespace btInverseDynamics { + +/// Structure for for rigid body mass properties, connectivity and kinematic state +/// all vectors and matrices are in body-fixed frame, if not indicated otherwise. +/// The body-fixed frame is located in the joint connecting the body to its parent. +struct RigidBody { + ID_DECLARE_ALIGNED_ALLOCATOR(); + // 1 Inertial properties + /// Mass + idScalar m_mass; + /// Mass times center of gravity in body-fixed frame + vec3 m_body_mass_com; + /// Moment of inertia w.r.t. body-fixed frame + mat33 m_body_I_body; + + // 2 dynamic properties + /// Left-hand side of the body equation of motion, translational part + vec3 m_eom_lhs_translational; + /// Left-hand side of the body equation of motion, rotational part + vec3 m_eom_lhs_rotational; + /// Force acting at the joint when the body is cut from its parent; + /// includes impressed joint force in J_JT direction, + /// as well as constraint force, + /// in body-fixed frame + vec3 m_force_at_joint; + /// Moment acting at the joint when the body is cut from its parent; + /// includes impressed joint moment in J_JR direction, and constraint moment + /// in body-fixed frame + vec3 m_moment_at_joint; + /// external (user provided) force acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_force_user; + /// external (user provided) moment acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_moment_user; + // 3 absolute kinematic properties + /// Position of body-fixed frame relative to world frame + /// this is currently only for debugging purposes + vec3 m_body_pos; + /// Absolute velocity of body-fixed frame + vec3 m_body_vel; + /// Absolute acceleration of body-fixed frame + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_acc; + /// Absolute angular velocity + vec3 m_body_ang_vel; + /// Absolute angular acceleration + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_ang_acc; + + // 4 relative kinematic properties. + // these are in the parent body frame + /// Transform from world to body-fixed frame; + /// this is currently only for debugging purposes + mat33 m_body_T_world; + /// Transform from parent to body-fixed frame + mat33 m_body_T_parent; + /// Vector from parent to child frame in parent frame + vec3 m_parent_pos_parent_body; + /// Relative angular velocity + vec3 m_body_ang_vel_rel; + /// Relative linear velocity + vec3 m_parent_vel_rel; + /// Relative angular acceleration + vec3 m_body_ang_acc_rel; + /// Relative linear acceleration + vec3 m_parent_acc_rel; + + // 5 Data describing the joint type and geometry + /// Type of joint + JointType m_joint_type; + /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame + /// Components are in body-fixed frame of the parent + vec3 m_parent_pos_parent_body_ref; + /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame + mat33 m_body_T_parent_ref; + /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. + /// (NOTE: dimensions will have to be dynamic for additional joint types!) + vec3 m_Jac_JR; + /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. + /// (NOTE: dimensions might have to be dynamic for additional joint types!) + vec3 m_Jac_JT; + /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT + vec3 m_parent_Jac_JT; + /// Start of index range for the position degree(s) of freedom describing this body's motion + /// relative to + /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. + int m_q_index; + + // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" + /// mass of the subtree rooted in this body + idScalar m_subtree_mass; + /// center of mass * mass for subtree rooted in this body, in body-fixed frame + vec3 m_body_subtree_mass_com; + /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame + mat33 m_body_subtree_I_body; +}; + +/// The MBS implements a tree structured multibody system +class MultiBodyTree::MultiBodyImpl { + friend class MultiBodyTree; + +public: + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// constructor + /// @param num_bodies the number of bodies in the system + /// @param num_dofs number of degrees of freedom in the system + MultiBodyImpl(int num_bodies_, int num_dofs_); + + /// \copydoc MultiBodyTree::calculateInverseDynamics + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + ///\copydoc MultiBodyTree::calculateMassMatrix + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + /// generate additional index sets from the parent_index array + /// @return -1 on error, 0 on success + int generateIndexSets(); + /// set gravity acceleration in world frame + /// @param gravity gravity vector in the world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// pretty print tree + void printTree(); + /// print tree data + void printTreeData(); + /// initialize fixed data + void calculateStaticData(); + /// \copydoc MultiBodyTree::getBodyFrame + int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; + /// \copydoc MultiBodyTree::getParentIndex + int getParentIndex(const int body_index, int* m_parent_index); + /// \copydoc MultiBodyTree::getJointType + int getJointType(const int body_index, JointType* joint_type) const; + /// \copydoc MultiBodyTree::getJointTypeStr + int getJointTypeStr(const int body_index, const char** joint_type) const; + /// \copydoc MultiBodyTree:getDoFOffset + int getDoFOffset(const int body_index, int* q_index) const; + /// \copydoc MultiBodyTree::getBodyOrigin + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// \copydoc MultiBodyTree::getBodyCoM + int getBodyCoM(const int body_index, vec3* world_com) const; + /// \copydoc MultiBodyTree::getBodyTransform + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// \copydoc MultiBodyTree::getBodyAngularVelocity + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocity + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyAngularAcceleration + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearAcceleration + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// \copydoc MultiBodyTree::getUserInt + int getUserInt(const int body_index, int* user_int) const; + /// \copydoc MultiBodyTree::getUserPtr + int getUserPtr(const int body_index, void** user_ptr) const; + /// \copydoc MultiBodyTree::setUserInt + int setUserInt(const int body_index, const int user_int); + /// \copydoc MultiBodyTree::setUserPtr + int setUserPtr(const int body_index, void* const user_ptr); + ///\copydoc MultiBodytTree::setBodyMass + int setBodyMass(const int body_index, const idScalar mass); + ///\copydoc MultiBodytTree::setBodyFirstMassMoment + int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + ///\copydoc MultiBodytTree::setBodySecondMassMoment + int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + ///\copydoc MultiBodytTree::getBodyMass + int getBodyMass(const int body_index, idScalar* mass) const; + ///\copydoc MultiBodytTree::getBodyFirstMassMoment + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + ///\copydoc MultiBodytTree::getBodySecondMassMoment + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments + void clearAllUserForcesAndMoments(); + /// \copydoc MultiBodyTree::addUserForce + int addUserForce(const int body_index, const vec3& body_force); + /// \copydoc MultiBodyTree::addUserMoment + int addUserMoment(const int body_index, const vec3& body_moment); + +private: + // debug function. print tree structure to stdout + void printTree(int index, int indentation); + // get string representation of JointType (for debugging) + const char* jointTypeToString(const JointType& type) const; + // get number of degrees of freedom from joint type + int bodyNumDoFs(const JointType& type) const; + // number of bodies in the system + int m_num_bodies; + // number of degrees of freedom + int m_num_dofs; + // Gravitational acceleration (in world frame) + vec3 m_world_gravity; + // vector of bodies in the system + // body 0 is used as an environment body and is allways fixed. + // The bodies are ordered such that a parent body always has an index + // smaller than its child. + idArray::type m_body_list; + // Parent_index[i] is the index for i's parent body in body_list. + // This fully describes the tree. + idArray::type m_parent_index; + // child_indices[i] contains a vector of indices of + // all children of the i-th body + idArray::type>::type m_child_indices; + // Indices of rotary joints + idArray::type m_body_revolute_list; + // Indices of prismatic joints + idArray::type m_body_prismatic_list; + // Indices of floating joints + idArray::type m_body_floating_list; + // a user-provided integer + idArray::type m_user_int; + // a user-provided pointer + idArray::type m_user_ptr; +}; +} +#endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp new file mode 100644 index 000000000..37063bfd7 --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -0,0 +1,113 @@ +#include "MultiBodyTreeInitCache.hpp" + +namespace btInverseDynamics { + +MultiBodyTree::InitCache::InitCache() { + m_inertias.resize(0); + m_joints.resize(0); + m_num_dofs = 0; + m_root_index=-1; +} + +int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, + const JointType joint_type, + const vec3& parent_r_parent_body_ref, + const mat33& body_T_parent_ref, + const vec3& body_axis_of_motion, const idScalar mass, + const vec3& body_r_body_com, const mat33& body_I_body, + const int user_int, void* user_ptr) { + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + m_num_dofs += 1; + break; + case FIXED: + // does not add a degree of freedom + // m_num_dofs+=0; + break; + case FLOATING: + m_num_dofs += 6; + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } + + if(-1 == parent_index) { + if(m_root_index>=0) { + error_message("trying to add body %d as root, but already added %d as root body\n", + body_index, m_root_index); + return -1; + } + m_root_index=body_index; + } + + JointData joint; + joint.m_child = body_index; + joint.m_parent = parent_index; + joint.m_type = joint_type; + joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; + joint.m_child_T_parent_ref = body_T_parent_ref; + joint.m_child_axis_of_motion = body_axis_of_motion; + + InertiaData body; + body.m_mass = mass; + body.m_body_pos_body_com = body_r_body_com; + body.m_body_I_body = body_I_body; + + m_inertias.push_back(body); + m_joints.push_back(joint); + m_user_int.push_back(user_int); + m_user_ptr.push_back(user_ptr); + return 0; +} +int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const { + if (index < 0 || index > static_cast(m_inertias.size())) { + error_message("index out of range\n"); + return -1; + } + + *inertia = m_inertias[index]; + return 0; +} + +int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { + if (index < 0 || index > static_cast(m_user_int.size())) { + error_message("index out of range\n"); + return -1; + } + *user_int = m_user_int[index]; + return 0; +} + +int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { + if (index < 0 || index > static_cast(m_user_ptr.size())) { + error_message("index out of range\n"); + return -1; + } + *user_ptr = m_user_ptr[index]; + return 0; +} + +int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { + if (index < 0 || index > static_cast(m_joints.size())) { + error_message("index out of range\n"); + return -1; + } + *joint = m_joints[index]; + return 0; +} + +int MultiBodyTree::InitCache::buildIndexSets() { + // NOTE: This function assumes that proper indices were provided + // User2InternalIndex from utils can be used to facilitate this. + + m_parent_index.resize(numBodies()); + for (idArrayIdx j = 0; j < m_joints.size(); j++) { + const JointData& joint = m_joints[j]; + m_parent_index[joint.m_child] = joint.m_parent; + } + + return 0; +} +} diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp new file mode 100644 index 000000000..a46cc16bb --- /dev/null +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -0,0 +1,109 @@ +#ifndef MULTIBODYTREEINITCACHE_HPP_ +#define MULTIBODYTREEINITCACHE_HPP_ + +#include "../IDConfig.hpp" +#include "../IDMath.hpp" +#include "../MultiBodyTree.hpp" + +namespace btInverseDynamics { +/// Mass properties of a rigid body +struct InertiaData { + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// mass + idScalar m_mass; + /// vector from body-fixed frame to center of mass, + /// in body-fixed frame, multiplied by the mass + vec3 m_body_pos_body_com; + /// moment of inertia w.r.t. the origin of the body-fixed + /// frame, represented in that frame + mat33 m_body_I_body; +}; + +/// Joint properties +struct JointData { + ID_DECLARE_ALIGNED_ALLOCATOR(); + + /// type of joint + JointType m_type; + /// index of parent body + int m_parent; + /// index of child body + int m_child; + /// vector from parent's body-fixed frame to child's body-fixed + /// frame for q=0, written in the parent's body fixed frame + vec3 m_parent_pos_parent_child_ref; + /// Transform matrix converting vectors written in the parent's frame + /// into vectors written in the child's frame for q=0 + /// ie, child_vector = child_T_parent_ref * parent_vector; + mat33 m_child_T_parent_ref; + /// Axis of motion for 1 degree-of-freedom joints, + /// written in the child's frame + /// For revolute joints, the q-value is positive for a positive + /// rotation about this axis. + /// For prismatic joints, the q-value is positive for a positive + /// translation is this direction. + vec3 m_child_axis_of_motion; +}; + +/// Data structure to store data passed by the user. +/// This is used in MultiBodyTree::finalize to build internal data structures. +class MultiBodyTree::InitCache { +public: + ID_DECLARE_ALIGNED_ALLOCATOR(); + /// constructor + InitCache(); + ///\copydoc MultiBodyTree::addBody + int addBody(const int body_index, const int parent_index, const JointType joint_type, + const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, + const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com, + const mat33 &body_I_body, const int user_int, void *user_ptr); + /// build index arrays + /// @return 0 on success, -1 on failure + int buildIndexSets(); + /// @return number of degrees of freedom + int numDoFs() const { return m_num_dofs; } + /// @return number of bodies + int numBodies() const { return m_inertias.size(); } + /// get inertia data for index + /// @param index of the body + /// @param inertia pointer for return data + /// @return 0 on success, -1 on failure + int getInertiaData(const int index, InertiaData *inertia) const; + /// get joint data for index + /// @param index of the body + /// @param joint pointer for return data + /// @return 0 on success, -1 on failure + int getJointData(const int index, JointData *joint) const; + /// get parent index array (paren_index[i] is the index of the parent of i) + /// @param parent_index pointer for return data + void getParentIndexArray(idArray::type *parent_index) { *parent_index = m_parent_index; } + /// get user integer + /// @param index body index + /// @param user_int user integer + /// @return 0 on success, -1 on failure + int getUserInt(const int index, int *user_int) const; + /// get user pointer + /// @param index body index + /// @param user_int user pointer + /// @return 0 on success, -1 on failure + int getUserPtr(const int index, void **user_ptr) const; + +private: + // vector of bodies + idArray::type m_inertias; + // vector of joints + idArray::type m_joints; + // number of mechanical degrees of freedom + int m_num_dofs; + // parent index array + idArray::type m_parent_index; + // user integers + idArray::type m_user_int; + // user pointers + idArray::type m_user_ptr; + // index of root body (or -1 if not set) + int m_root_index; +}; +} +#endif // MULTIBODYTREEINITCACHE_HPP_ diff --git a/src/BulletInverseDynamics/premake4.lua b/src/BulletInverseDynamics/premake4.lua new file mode 100644 index 000000000..774e037b3 --- /dev/null +++ b/src/BulletInverseDynamics/premake4.lua @@ -0,0 +1,12 @@ + project "BulletInverseDynamics" + + kind "StaticLib" + includedirs { + "..", + } + files { + "IDMath.cpp", + "MultiBodyTree.cpp", + "details/MultiBodyTreeInitCache.cpp", + "details/MultiBodyTreeImpl.cpp", + } diff --git a/test/InverseDynamics/premake4.lua b/test/InverseDynamics/premake4.lua new file mode 100644 index 000000000..73730b0ab --- /dev/null +++ b/test/InverseDynamics/premake4.lua @@ -0,0 +1,34 @@ + + project "Test_InverseDynamicsKinematics" + + kind "ConsoleApp" + +-- defines { } + + + + includedirs + { + ".", + "../../src", + "../../examples/InverseDynamics", + "../../Extras/InverseDynamics", + "../gtest-1.7.0/include" + + } + + + if os.is("Windows") then + --see http://stackoverflow.com/questions/12558327/google-test-in-visual-studio-2012 + defines {"_VARIADIC_MAX=10"} + end + + links {"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common","LinearMath", "gtest"} + + files { + "test_invdyn_kinematics.cpp", + } + + if os.is("Linux") then + links {"pthread"} + end diff --git a/test/InverseDynamics/test_invdyn_bullet.cpp b/test/InverseDynamics/test_invdyn_bullet.cpp new file mode 100644 index 000000000..ab7578a48 --- /dev/null +++ b/test/InverseDynamics/test_invdyn_bullet.cpp @@ -0,0 +1,115 @@ +/// create a bullet btMultiBody model of a tree structured multibody system, +/// convert that model to a MultiBodyTree model. +/// Then - run inverse dynamics on random input data (q, u, dot_u) to get forces +/// - run forward dynamics on (q,u, forces) to get accelerations +/// - compare input accelerations to inverse dynamics to output from forward dynamics + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include <../CommonInterfaces/CommonGUIHelperInterface.h> +#include +#include +#include <../Importers/ImportURDFDemo/BulletUrdfImporter.h> +#include <../Importers/ImportURDFDemo/URDF2Bullet.h> +#include <../Importers/ImportURDFDemo/MyMultiBodyCreator.h> +#include <../Importers/ImportURDFDemo/URDF2Bullet.h> +#include "../../examples/Utils/b3ResourcePath.h" +#include +#include +#include +#include + +using namespace btInverseDynamics; + +DEFINE_bool(verbose, false, "print extra info"); + +static btVector3 gravity(0, 0, -10); +static const bool kBaseFixed = false; +static const char kUrdfFile[] = "r2d2.urdf"; + +/// this test loads the a urdf model with fixed, floating, prismatic and rotational joints, +/// converts in to an inverse dynamics model and compares forward to inverse dynamics for +/// random input +TEST(InvDynCompare, bulletUrdfR2D2) { + MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed); + + char relativeFileName[1024]; + + ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024)); + + mb_load.setFileName(relativeFileName); + mb_load.init(); + + btMultiBodyTreeCreator id_creator; + btMultiBody *btmb = mb_load.getBtMultiBody(); + ASSERT_EQ(id_creator.createFromBtMultiBody(btmb), 0); + + MultiBodyTree *id_tree = CreateMultiBodyTree(id_creator); + ASSERT_EQ(0x0 != id_tree, true); + + vecx q(id_tree->numDoFs()); + vecx u(id_tree->numDoFs()); + vecx dot_u(id_tree->numDoFs()); + vecx joint_forces(id_tree->numDoFs()); + + const int kNLoops = 10; + double max_pos_error = 0; + double max_acc_error = 0; + + std::default_random_engine generator; + std::uniform_real_distribution distribution(-M_PI, M_PI); + auto rnd = std::bind(distribution, generator); + + for (int loop = 0; loop < kNLoops; loop++) { + for (int i = 0; i < q.size(); i++) { + q(i) = rnd(); + u(i) = rnd(); + dot_u(i) = rnd(); + } + + double pos_error; + double acc_error; + btmb->clearForcesAndTorques(); + id_tree->clearAllUserForcesAndMoments(); + // call inverse dynamics once, to get global position & velocity of root body + // (fixed, so q, u, dot_u arbitrary) + EXPECT_EQ(id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces), 0); + + EXPECT_EQ(compareInverseAndForwardDynamics(q, u, dot_u, gravity, FLAGS_verbose, btmb, id_tree, + &pos_error, &acc_error), + 0); + + if (pos_error > max_pos_error) { + max_pos_error = pos_error; + } + if (acc_error > max_acc_error) { + max_acc_error = acc_error; + } + } + + if (FLAGS_verbose) { + printf("max_pos_error= %e\n", max_pos_error); + printf("max_acc_error= %e\n", max_acc_error); + } + + EXPECT_LT(max_pos_error, std::numeric_limits::epsilon()*1e4); + EXPECT_LT(max_acc_error, std::numeric_limits::epsilon()*1e5); +} + +int main(int argc, char **argv) { + gflags::SetUsageMessage("Usage: invdyn_from_btmultibody -verbose = true/false"); + gflags::ParseCommandLineFlags(&argc, &argv, false); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp new file mode 100644 index 000000000..dd5d443df --- /dev/null +++ b/test/InverseDynamics/test_invdyn_kinematics.cpp @@ -0,0 +1,350 @@ +// Test of kinematic consistency: check if finite differences of velocities, accelerations +// match positions + +#include +#include +#include +#include + +#include + +#include "../Extras/InverseDynamics/CoilCreator.hpp" +#include "../Extras/InverseDynamics/DillCreator.hpp" +#include "../Extras/InverseDynamics/SimpleTreeCreator.hpp" +#include "BulletInverseDynamics/MultiBodyTree.hpp" + +using namespace btInverseDynamics; + +const int kLevel = 5; +const int kNumBodies = std::pow(2, kLevel); + +// template function for calculating the norm +template +idScalar calculateNorm(T&); +// only implemented for vec3 +template <> +idScalar calculateNorm(vec3& v) { + return std::sqrt(std::pow(v(0), 2) + std::pow(v(1), 2) + std::pow(v(2), 2)); +} + +// template function to convert a DiffType (finite differences) +// to a ValueType. This is for angular velocity calculations +// via finite differences. +template +DiffType toDiffType(ValueType& fd, ValueType& val); + +// vector case: just return finite difference approximation +template <> +vec3 toDiffType(vec3& fd, vec3& val) { + return fd; +} + +// orientation case: calculate spin tensor and extract angular velocity +template <> +vec3 toDiffType(mat33& fd, mat33& val) { + // spin tensor + mat33 omega_tilde = fd * val.transpose(); + // extract vector from spin tensor + vec3 omega; + omega(0) = 0.5 * (omega_tilde(2, 1) - omega_tilde(1, 2)); + omega(1) = 0.5 * (omega_tilde(0, 2) - omega_tilde(2, 0)); + omega(2) = 0.5 * (omega_tilde(1, 0) - omega_tilde(0, 1)); + return omega; +} + +/// Class for calculating finite difference approximation +/// of time derivatives and comparing it to an analytical solution +/// DiffType and ValueType can be different, to allow comparison +/// of angular velocity vectors and orientations given as transform matrices. +template +class DiffFD { +public: + DiffFD() : m_dt(0.0), m_num_updates(0), m_max_error(0.0), m_max_value(0.0), m_valid_fd(false) {} + + void init(std::string name, idScalar dt) { + m_name = name; + m_dt = dt; + m_num_updates = 0; + m_max_error = 0.0; + m_max_value = 0.0; + m_valid_fd = false; + } + + void update(const ValueType& val, const DiffType& true_diff) { + m_val = val; + if (m_num_updates > 2) { + // 2nd order finite difference approximation for d(value)/dt + ValueType diff_value_fd = (val - m_older_val) / (2.0 * m_dt); + // convert to analytical diff type. This is for angular velocities + m_diff_fd = toDiffType(diff_value_fd, m_old_val); + // now, calculate the error + DiffType error_value_type = m_diff_fd - m_old_true_diff; + idScalar error = calculateNorm(error_value_type); + if (error > m_max_error) { + m_max_error = error; + } + + idScalar value = calculateNorm(m_old_true_diff); + if (value > m_max_value) { + m_max_value = value; + } + + m_valid_fd = true; + } + m_older_val = m_old_val; + m_old_val = m_val; + m_old_true_diff = true_diff; + m_num_updates++; + m_time += m_dt; + } + + void printMaxError() { + printf("max_error: %e dt= %e max_value= %e fraction= %e\n", m_max_error, m_dt, m_max_value, + m_max_value > 0.0 ? m_max_error / m_max_value : 0.0); + } + void printCurrent() { + if (m_valid_fd) { + // note: m_old_true_diff already equals m_true_diff here, so values are not aligned. + // (but error calculation takes this into account) + printf("%s time: %e fd: %e %e %e true: %e %e %e\n", m_name.c_str(), m_time, + m_diff_fd(0), m_diff_fd(1), m_diff_fd(2), m_old_true_diff(0), m_old_true_diff(1), + m_old_true_diff(2)); + } + } + + idScalar getMaxError() const { return m_max_error; } + idScalar getMaxValue() const { return m_max_value; } + +private: + idScalar m_dt; + ValueType m_val; + ValueType m_old_val; + ValueType m_older_val; + DiffType m_old_true_diff; + DiffType m_diff_fd; + int m_num_updates; + idScalar m_max_error; + idScalar m_max_value; + idScalar m_time; + std::string m_name; + bool m_valid_fd; +}; + +template +class VecDiffFD { +public: + VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt) { + for (int i = 0; i < m_fd.size(); i++) { + char buf[256]; + snprintf(buf, 256, "%s-%.2d", name.c_str(), i); + m_fd[i].init(buf, dt); + } + } + void update(int i, ValueType& val, DiffType& true_diff) { m_fd[i].update(val, true_diff); } + idScalar getMaxError() const { + idScalar max_error = 0; + for (int i = 0; i < m_fd.size(); i++) { + const idScalar error = m_fd[i].getMaxError(); + if (error > max_error) { + max_error = error; + } + } + return max_error; + } + + void printMaxError() { + printf("%s: total dt= %e max_error= %e\n", m_name.c_str(), m_dt, getMaxError()); + } + + void printCurrent() { + for (int i = 0; i < m_fd.size(); i++) { + m_fd[i].printCurrent(); + } + } + +private: + std::string m_name; + std::vector > m_fd; + const idScalar m_dt; + idScalar m_max_error; +}; + +// calculate maximum difference between finite difference and analytical differentiation +int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar deltaT, + idScalar endTime, idScalar* max_linear_velocity_error, + idScalar* max_angular_velocity_error, + idScalar* max_linear_acceleration_error, + idScalar* max_angular_acceleration_error) { + // setup system + MultiBodyTree* tree = CreateMultiBodyTree(creator); + if (0x0 == tree) { + return -1; + } + // set gravity to zero, so nothing is added to accelerations in forward kinematics + vec3 gravity_zero; + gravity_zero(0) = 0; + gravity_zero(1) = 0; + gravity_zero(2) = 0; + tree->setGravityInWorldFrame(gravity_zero); + // + const idScalar kAmplitude = 1.0; + const idScalar kFrequency = 1.0; + + vecx q(tree->numDoFs()); + vecx dot_q(tree->numDoFs()); + vecx ddot_q(tree->numDoFs()); + vecx joint_forces(tree->numDoFs()); + + VecDiffFD fd_vel("linear-velocity", tree->numBodies(), deltaT); + VecDiffFD fd_acc("linear-acceleration", tree->numBodies(), deltaT); + VecDiffFD fd_omg("angular-velocity", tree->numBodies(), deltaT); + VecDiffFD fd_omgd("angular-acceleration", tree->numBodies(), deltaT); + + for (idScalar t = 0.0; t < endTime; t += deltaT) { + for (int body = 0; body < tree->numBodies(); body++) { + q(body) = kAmplitude * sin(t * 2.0 * M_PI * kFrequency); + dot_q(body) = kAmplitude * 2.0 * M_PI * kFrequency * cos(t * 2.0 * M_PI * kFrequency); + ddot_q(body) = + -kAmplitude * pow(2.0 * M_PI * kFrequency, 2) * sin(t * 2.0 * M_PI * kFrequency); + } + + if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces)) { + delete tree; + return -1; + } + + // position/velocity + for (int body = 0; body < tree->numBodies(); body++) { + vec3 pos; + vec3 vel; + mat33 world_T_body; + vec3 omega; + vec3 dot_omega; + vec3 acc; + + tree->getBodyOrigin(body, &pos); + tree->getBodyTransform(body, &world_T_body); + tree->getBodyLinearVelocity(body, &vel); + tree->getBodyAngularVelocity(body, &omega); + tree->getBodyLinearAcceleration(body, &acc); + tree->getBodyAngularAcceleration(body, &dot_omega); + + fd_vel.update(body, pos, vel); + fd_omg.update(body, world_T_body, omega); + fd_acc.update(body, vel, acc); + fd_omgd.update(body, omega, dot_omega); + } + } + + *max_linear_velocity_error = fd_vel.getMaxError(); + *max_angular_velocity_error = fd_omg.getMaxError(); + *max_linear_acceleration_error = fd_acc.getMaxError(); + *max_angular_acceleration_error = fd_omgd.getMaxError(); + + delete tree; + return 0; +} + +// first test: absolute difference between numerical and numerial +// differentiation should be small +TEST(InvDynKinematicsDifferentiation, errorAbsolute) { + const idScalar kDeltaT = 1e-7; + const idScalar kDuration = 1e-2; + const idScalar kAcceptableError = 1e-4; + + CoilCreator coil_creator(kNumBodies); + DillCreator dill_creator(kLevel); + SimpleTreeCreator simple_creator(kNumBodies); + + idScalar max_linear_velocity_error; + idScalar max_angular_velocity_error; + idScalar max_linear_acceleration_error; + idScalar max_angular_acceleration_error; + + // test serial chain + calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error, + &max_angular_velocity_error, &max_linear_acceleration_error, + &max_angular_acceleration_error); + + EXPECT_LT(max_linear_velocity_error, kAcceptableError); + EXPECT_LT(max_angular_velocity_error, kAcceptableError); + EXPECT_LT(max_linear_acceleration_error, kAcceptableError); + EXPECT_LT(max_angular_acceleration_error, kAcceptableError); + + // test branched tree + calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error, + &max_angular_velocity_error, &max_linear_acceleration_error, + &max_angular_acceleration_error); + + EXPECT_LT(max_linear_velocity_error, kAcceptableError); + EXPECT_LT(max_angular_velocity_error, kAcceptableError); + EXPECT_LT(max_linear_acceleration_error, kAcceptableError); + EXPECT_LT(max_angular_acceleration_error, kAcceptableError); + + // test system with different joint types + calculateDifferentiationError(simple_creator, kDeltaT, kDuration, &max_linear_velocity_error, + &max_angular_velocity_error, &max_linear_acceleration_error, + &max_angular_acceleration_error); + + EXPECT_LT(max_linear_velocity_error, kAcceptableError); + EXPECT_LT(max_angular_velocity_error, kAcceptableError); + EXPECT_LT(max_linear_acceleration_error, kAcceptableError); + EXPECT_LT(max_angular_acceleration_error, kAcceptableError); +} + +// second test: check if the change in the differentiation error +// is consitent with the second order approximation, ie, error ~ O(dt^2) +TEST(InvDynKinematicsDifferentiation, errorOrder) { + const idScalar kDeltaTs[2] = {1e-4, 1e-5}; + const idScalar kDuration = 1e-2; + + CoilCreator coil_creator(kNumBodies); + // DillCreator dill_creator(kLevel); + // SimpleTreeCreator simple_creator(kNumBodies); + + idScalar max_linear_velocity_error[2]; + idScalar max_angular_velocity_error[2]; + idScalar max_linear_acceleration_error[2]; + idScalar max_angular_acceleration_error[2]; + + // test serial chain + calculateDifferentiationError(coil_creator, kDeltaTs[0], kDuration, + &max_linear_velocity_error[0], &max_angular_velocity_error[0], + &max_linear_acceleration_error[0], + &max_angular_acceleration_error[0]); + + calculateDifferentiationError(coil_creator, kDeltaTs[1], kDuration, + &max_linear_velocity_error[1], &max_angular_velocity_error[1], + &max_linear_acceleration_error[1], + &max_angular_acceleration_error[1]); + + const idScalar expected_linear_velocity_error_1 = + max_linear_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + const idScalar expected_angular_velocity_error_1 = + max_angular_velocity_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + const idScalar expected_linear_acceleration_error_1 = + max_linear_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + const idScalar expected_angular_acceleration_error_1 = + max_angular_acceleration_error[0] * pow(kDeltaTs[1] / kDeltaTs[0], 2); + + printf("linear vel error: %e %e %e\n", max_linear_velocity_error[1], + expected_linear_velocity_error_1, + max_linear_velocity_error[1] - expected_linear_velocity_error_1); + printf("angular vel error: %e %e %e\n", max_angular_velocity_error[1], + expected_angular_velocity_error_1, + max_angular_velocity_error[1] - expected_angular_velocity_error_1); + printf("linear acc error: %e %e %e\n", max_linear_acceleration_error[1], + expected_linear_acceleration_error_1, + max_linear_acceleration_error[1] - expected_linear_acceleration_error_1); + printf("angular acc error: %e %e %e\n", max_angular_acceleration_error[1], + expected_angular_acceleration_error_1, + max_angular_acceleration_error[1] - expected_angular_acceleration_error_1); +} + +int main(int argc, char** argv) { + + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); + + return EXIT_SUCCESS; +} From de763a26e7ef742102252df8da9e8216e5056178 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 17 Nov 2015 21:51:02 -0800 Subject: [PATCH 4/7] fix portability of BulletInverseDynamics (usual issues: std::pow, M_PI, std::vector->idArray::type, snprintf, pass vec3 and mat33 by const reference, not by value) --- Extras/InverseDynamics/CoilCreator.cpp | 4 ++-- Extras/InverseDynamics/DillCreator.cpp | 22 ++++++++++--------- Extras/InverseDynamics/DillCreator.hpp | 14 ++++++------ Extras/InverseDynamics/IDRandomUtil.cpp | 4 ++-- .../btMultiBodyTreeCreator.hpp | 2 +- src/BulletInverseDynamics/IDConfig.hpp | 11 ++++++++++ src/BulletInverseDynamics/MultiBodyTree.cpp | 4 ++-- src/BulletInverseDynamics/MultiBodyTree.hpp | 4 ++-- .../details/MultiBodyTreeImpl.cpp | 4 ++-- .../details/MultiBodyTreeImpl.hpp | 4 ++-- .../test_invdyn_kinematics.cpp | 12 +++++----- 11 files changed, 49 insertions(+), 36 deletions(-) diff --git a/Extras/InverseDynamics/CoilCreator.cpp b/Extras/InverseDynamics/CoilCreator.cpp index 905ba7a57..6ccfbcf28 100644 --- a/Extras/InverseDynamics/CoilCreator.cpp +++ b/Extras/InverseDynamics/CoilCreator.cpp @@ -12,7 +12,7 @@ CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) { const idScalar theta_DH = 0; const idScalar d_DH = 0.0; const idScalar a_DH = 1.0 / m_num_bodies; - const idScalar alpha_DH = 5.0 * M_PI / m_num_bodies; + const idScalar alpha_DH = 5.0 * BT_ID_PI / m_num_bodies; getVecMatFromDH(theta_DH, d_DH, a_DH, alpha_DH, &m_parent_r_parent_body_ref, &m_body_T_parent_ref); // always z-axis @@ -29,7 +29,7 @@ CoilCreator::CoilCreator(int n) : m_num_bodies(n), m_parent(n) { m_body_I_body(0, 1) = 0.0; m_body_I_body(0, 2) = 0.0; m_body_I_body(1, 0) = 0.0; - m_body_I_body(1, 1) = (3e-4 + 4.0 / pow(m_num_bodies, 2)) / (12.0 * m_num_bodies); + m_body_I_body(1, 1) = (3e-4 + 4.0 / BT_ID_POW(m_num_bodies, 2)) / (12.0 * m_num_bodies); m_body_I_body(1, 2) = 0.0; m_body_I_body(2, 0) = 0.0; m_body_I_body(2, 1) = 0.0; diff --git a/Extras/InverseDynamics/DillCreator.cpp b/Extras/InverseDynamics/DillCreator.cpp index cdb3e06a6..8463ab887 100644 --- a/Extras/InverseDynamics/DillCreator.cpp +++ b/Extras/InverseDynamics/DillCreator.cpp @@ -4,14 +4,16 @@ namespace btInverseDynamics { DillCreator::DillCreator(int level) : m_level(level), - m_num_bodies(std::pow(2, level)), - m_parent(m_num_bodies), - m_parent_r_parent_body_ref(m_num_bodies), - m_body_T_parent_ref(m_num_bodies), - m_body_axis_of_motion(m_num_bodies), - m_mass(m_num_bodies), - m_body_r_body_com(m_num_bodies), - m_body_I_body(m_num_bodies) { + m_num_bodies(BT_ID_POW(2, level)) + { + m_parent.resize(m_num_bodies); + m_parent_r_parent_body_ref.resize(m_num_bodies); + m_body_T_parent_ref.resize(m_num_bodies); + m_body_axis_of_motion.resize(m_num_bodies); + m_mass.resize(m_num_bodies); + m_body_r_body_com.resize(m_num_bodies); + m_body_I_body.resize(m_num_bodies); + // generate names (for debugging) for (int i = 0; i < m_num_bodies; i++) { m_parent[i] = i - 1; @@ -85,7 +87,7 @@ int DillCreator::recurseDill(const int level, const int parent, const idScalar d /// these parameters are from the paper ... /// TODO: add proper citation m_parent[body] = parent; - m_mass[body] = 0.1 * std::pow(size, 3); + m_mass[body] = 0.1 * BT_ID_POW(size, 3); m_body_r_body_com[body](0) = 0.05 * size; m_body_r_body_com[body](1) = 0; m_body_r_body_com[body](2) = 0; @@ -112,7 +114,7 @@ int DillCreator::recurseDill(const int level, const int parent, const idScalar d d_DH = 0.0; } const idScalar a_DH = i * 0.1; - const idScalar alpha_DH = i * M_PI / 3.0; + const idScalar alpha_DH = i * BT_ID_PI / 3.0; m_current_body++; recurseDill(i - 1, body, d_DH, a_DH, alpha_DH); } diff --git a/Extras/InverseDynamics/DillCreator.hpp b/Extras/InverseDynamics/DillCreator.hpp index 42f1b1ba0..d52e0d06d 100644 --- a/Extras/InverseDynamics/DillCreator.hpp +++ b/Extras/InverseDynamics/DillCreator.hpp @@ -34,13 +34,13 @@ private: const idScalar a_DH_in, const idScalar alpha_DH_in); int m_level; int m_num_bodies; - std::vector m_parent; - std::vector m_parent_r_parent_body_ref; - std::vector m_body_T_parent_ref; - std::vector m_body_axis_of_motion; - std::vector m_mass; - std::vector m_body_r_body_com; - std::vector m_body_I_body; + idArray::type m_parent; + idArray::type m_parent_r_parent_body_ref; + idArray::type m_body_T_parent_ref; + idArray::type m_body_axis_of_motion; + idArray::type m_mass; + idArray::type m_body_r_body_com; + idArray::type m_body_I_body; int m_current_body; }; } diff --git a/Extras/InverseDynamics/IDRandomUtil.cpp b/Extras/InverseDynamics/IDRandomUtil.cpp index 8896eaf55..c3110e5c7 100644 --- a/Extras/InverseDynamics/IDRandomUtil.cpp +++ b/Extras/InverseDynamics/IDRandomUtil.cpp @@ -39,8 +39,8 @@ mat33 randomInertiaMatrix() { // generate random valid inertia matrix by first getting valid components // along major axes and then rotating by random amount vec3 principal = randomInertiaPrincipal(); - mat33 rot(transformX(randomFloat(-M_PI, M_PI)) * transformY(randomFloat(-M_PI, M_PI)) * - transformZ(randomFloat(-M_PI, M_PI))); + mat33 rot(transformX(randomFloat(-BT_ID_PI, BT_ID_PI)) * transformY(randomFloat(-BT_ID_PI, BT_ID_PI)) * + transformZ(randomFloat(-BT_ID_PI, BT_ID_PI))); mat33 inertia; inertia(0, 0) = principal(0); inertia(0, 1) = 0; diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp index 1ae00ff79..0e2d614f2 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.hpp @@ -42,7 +42,7 @@ private: vec3 body_r_body_com; mat33 body_I_body; }; - std::vector m_data; + idArray::type m_data; bool m_initialized; }; } diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index 6df61071f..4beb75a75 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -5,6 +5,17 @@ // If we have a custom configuration, compile without using other parts of bullet. #ifdef BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H #define BT_ID_WO_BULLET +#define BT_ID_POW(a,b) std::pow(a,b) +#define BT_ID_SNPRINTF snprintf +#define BT_ID_PI M_PI +#else +#define BT_ID_POW(a,b) btPow(a,b) +#define BT_ID_PI SIMD_PI +#ifdef _WIN32 + #define BT_ID_SNPRINTF _snprintf +#else + #define BT_ID_SNPRINTF snprintf +#endif // #endif // error messages #include "IDErrorMessages.hpp" diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index 3de0d8e4f..5c0af64e9 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -298,11 +298,11 @@ int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { return m_impl->setBodyMass(body_index, mass); } -int MultiBodyTree::setBodyFirstMassMoment(const int body_index, vec3 first_mass_moment) { +int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) { return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); } -int MultiBodyTree::setBodySecondMassMoment(const int body_index, mat33 second_mass_moment) { +int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) { return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); } diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp index 833baf1d3..887005c3a 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.hpp +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -250,13 +250,13 @@ public: /// @param body_index index of the body /// @param first_mass_moment the vector to set /// @return 0 on success, -1 on failure - int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); /// set second moment of mass for a body /// (moment of inertia, in body fixed frame, relative to joint) /// @param body_index index of the body /// @param second_mass_moment the inertia matrix /// @return 0 on success, -1 on failure - int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); /// get mass for a body /// @param body_index index of the body /// @param mass the mass diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 53ef4503d..211139459 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -792,13 +792,13 @@ int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScal } int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3 first_mass_moment) { + const vec3& first_mass_moment) { CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_body_list[body_index].m_body_mass_com = first_mass_moment; return 0; } int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, - const mat33 second_mass_moment) { + const mat33& second_mass_moment) { CHECK_IF_BODY_INDEX_IS_VALID(body_index); m_body_list[body_index].m_body_I_body = second_mass_moment; return 0; diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 63d8945eb..286b19d26 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -181,9 +181,9 @@ public: ///\copydoc MultiBodytTree::setBodyMass int setBodyMass(const int body_index, const idScalar mass); ///\copydoc MultiBodytTree::setBodyFirstMassMoment - int setBodyFirstMassMoment(const int body_index, const vec3 first_mass_moment); + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); ///\copydoc MultiBodytTree::setBodySecondMassMoment - int setBodySecondMassMoment(const int body_index, const mat33 second_mass_moment); + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); ///\copydoc MultiBodytTree::getBodyMass int getBodyMass(const int body_index, idScalar* mass) const; ///\copydoc MultiBodytTree::getBodyFirstMassMoment diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp index dd5d443df..4ee728a0d 100644 --- a/test/InverseDynamics/test_invdyn_kinematics.cpp +++ b/test/InverseDynamics/test_invdyn_kinematics.cpp @@ -16,7 +16,7 @@ using namespace btInverseDynamics; const int kLevel = 5; -const int kNumBodies = std::pow(2, kLevel); +const int kNumBodies = BT_ID_POW(2, kLevel); // template function for calculating the norm template @@ -24,7 +24,7 @@ idScalar calculateNorm(T&); // only implemented for vec3 template <> idScalar calculateNorm(vec3& v) { - return std::sqrt(std::pow(v(0), 2) + std::pow(v(1), 2) + std::pow(v(2), 2)); + return std::sqrt(BT_ID_POW(v(0), 2) + BT_ID_POW(v(1), 2) + BT_ID_POW(v(2), 2)); } // template function to convert a DiffType (finite differences) @@ -136,7 +136,7 @@ public: VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt) { for (int i = 0; i < m_fd.size(); i++) { char buf[256]; - snprintf(buf, 256, "%s-%.2d", name.c_str(), i); + BT_ID_SNPRINTF(buf, 256, "%s-%.2d", name.c_str(), i); m_fd[i].init(buf, dt); } } @@ -202,10 +202,10 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar for (idScalar t = 0.0; t < endTime; t += deltaT) { for (int body = 0; body < tree->numBodies(); body++) { - q(body) = kAmplitude * sin(t * 2.0 * M_PI * kFrequency); - dot_q(body) = kAmplitude * 2.0 * M_PI * kFrequency * cos(t * 2.0 * M_PI * kFrequency); + q(body) = kAmplitude * sin(t * 2.0 * BT_ID_PI * kFrequency); + dot_q(body) = kAmplitude * 2.0 * BT_ID_PI * kFrequency * cos(t * 2.0 * BT_ID_PI * kFrequency); ddot_q(body) = - -kAmplitude * pow(2.0 * M_PI * kFrequency, 2) * sin(t * 2.0 * M_PI * kFrequency); + -kAmplitude * pow(2.0 * BT_ID_PI * kFrequency, 2) * sin(t * 2.0 * BT_ID_PI * kFrequency); } if (-1 == tree->calculateInverseDynamics(q, dot_q, ddot_q, &joint_forces)) { From 069936218ac523ff3ddd08041680224437c157ea Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 18 Nov 2015 08:01:32 -0800 Subject: [PATCH 5/7] [InverseDynamics] Add custom namespaces per configuration BulletInverseDynamics can be configured to compile using Eigen instead of Bullet's LinearMath. Adds a preprocessor defined specific namespace for each configuration to produce linker errors if incompatible libraries are linked (eg, builtin core library and utilities for bullet types). --- src/BulletInverseDynamics/IDConfig.hpp | 4 ++++ src/BulletInverseDynamics/IDConfigBuiltin.hpp | 1 + src/BulletInverseDynamics/IDConfigEigen.hpp | 1 + src/BulletInverseDynamics/MultiBodyTree.hpp | 2 +- src/BulletInverseDynamics/details/IDMatVec.hpp | 2 ++ 5 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index 4beb75a75..e7d6c122f 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -24,7 +24,11 @@ #define INVDYN_INCLUDE_HELPER_2(x) #x #define INVDYN_INCLUDE_HELPER(x) INVDYN_INCLUDE_HELPER_2(x) #include INVDYN_INCLUDE_HELPER(BT_CUSTOM_INVERSE_DYNAMICS_CONFIG_H) +#ifndef btInverseDynamics +#error "custom inverse dynamics config, but no custom namespace defined" +#endif #else +#define btInverseDynamics btInverseDynamicsBullet3 // Use default configuration with bullet's types // Use the same scalar type as rest of bullet library #include "LinearMath/btScalar.h" diff --git a/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/src/BulletInverseDynamics/IDConfigBuiltin.hpp index 2427c16f0..3965bc5eb 100644 --- a/src/BulletInverseDynamics/IDConfigBuiltin.hpp +++ b/src/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -1,6 +1,7 @@ ///@file Configuration for Inverse Dynamics Library without external dependencies #ifndef INVDYNCONFIG_BUILTIN_HPP_ #define INVDYNCONFIG_BUILTIN_HPP_ +#define btInverseDynamics btInverseDynamicsBuiltin #ifdef BT_USE_DOUBLE_PRECISION // choose double/single precision version typedef double idScalar; diff --git a/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp index e04b149a6..f0b72b425 100644 --- a/src/BulletInverseDynamics/IDConfigEigen.hpp +++ b/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -1,6 +1,7 @@ ///@file Configuration for Inverse Dynamics Library with Eigen #ifndef INVDYNCONFIG_EIGEN_HPP_ #define INVDYNCONFIG_EIGEN_HPP_ +#define btInverseDynamics btInverseDynamicsEigen #ifdef BT_USE_DOUBLE_PRECISION // choose double/single precision version typedef double idScalar; diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp index 887005c3a..9f3400b40 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.hpp +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -1,6 +1,7 @@ #ifndef MULTIBODYTREE_HPP_ #define MULTIBODYTREE_HPP_ +#include "IDConfig.hpp" #include "IDMath.hpp" namespace btInverseDynamics { @@ -304,5 +305,4 @@ private: InitCache* m_init_cache; }; } // namespace btInverseDynamics - #endif // MULTIBODYTREE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp index 961abade8..a4e2ca5a4 100644 --- a/src/BulletInverseDynamics/details/IDMatVec.hpp +++ b/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -4,6 +4,8 @@ #include +#include "../IDConfig.hpp" + namespace btInverseDynamics { class vec3; class vecx; From 75d657ec85e754d4330e13604ee8e860f2b933d3 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 19 Nov 2015 12:08:04 -0800 Subject: [PATCH 6/7] change 4 spaces to tab for src/BulletInverseDynamics fix unit test in single precision compilation (use delta t of 0.01 if BT_ID_USE_DOUBLE_PRECISION is not defined) --- src/BulletInverseDynamics/IDConfig.hpp | 8 +- src/BulletInverseDynamics/IDConfigBuiltin.hpp | 20 +- src/BulletInverseDynamics/IDConfigEigen.hpp | 20 +- src/BulletInverseDynamics/IDErrorMessages.hpp | 30 +- src/BulletInverseDynamics/IDMath.cpp | 566 +++---- src/BulletInverseDynamics/IDMath.hpp | 4 +- src/BulletInverseDynamics/MultiBodyTree.cpp | 396 ++--- src/BulletInverseDynamics/MultiBodyTree.hpp | 542 +++---- .../details/IDLinearMathInterface.hpp | 122 +- .../details/IDMatVec.hpp | 408 ++--- .../details/MultiBodyTreeImpl.cpp | 1332 ++++++++--------- .../details/MultiBodyTreeImpl.hpp | 406 ++--- .../details/MultiBodyTreeInitCache.cpp | 164 +- .../details/MultiBodyTreeInitCache.hpp | 164 +- .../test_invdyn_kinematics.cpp | 4 + 15 files changed, 2097 insertions(+), 2089 deletions(-) diff --git a/src/BulletInverseDynamics/IDConfig.hpp b/src/BulletInverseDynamics/IDConfig.hpp index e7d6c122f..97646dc43 100644 --- a/src/BulletInverseDynamics/IDConfig.hpp +++ b/src/BulletInverseDynamics/IDConfig.hpp @@ -1,5 +1,5 @@ ///@file Configuration for Inverse Dynamics Library, -/// such as choice of linear algebra library and underlying scalar type +/// such as choice of linear algebra library and underlying scalar type #ifndef IDCONFIG_HPP_ #define IDCONFIG_HPP_ // If we have a custom configuration, compile without using other parts of bullet. @@ -8,6 +8,7 @@ #define BT_ID_POW(a,b) std::pow(a,b) #define BT_ID_SNPRINTF snprintf #define BT_ID_PI M_PI +#define BT_ID_USE_DOUBLE_PRECISION #else #define BT_ID_POW(a,b) btPow(a,b) #define BT_ID_PI SIMD_PI @@ -33,6 +34,9 @@ // Use the same scalar type as rest of bullet library #include "LinearMath/btScalar.h" typedef btScalar idScalar; +#ifdef BT_USE_DOUBLE_PRECISION +#define BT_ID_USE_DOUBLE_PRECISION +#endif // use bullet types for arrays and array indices #include "Bullet3Common/b3AlignedObjectArray.h" // this is to make it work with C++2003, otherwise we could do this: @@ -40,7 +44,7 @@ typedef btScalar idScalar; // using idArray = b3AlignedObjectArray; template struct idArray { - typedef b3AlignedObjectArray type; + typedef b3AlignedObjectArray type; }; typedef int idArrayIdx; #define ID_DECLARE_ALIGNED_ALLOCATOR B3_DECLARE_ALIGNED_ALLOCATOR diff --git a/src/BulletInverseDynamics/IDConfigBuiltin.hpp b/src/BulletInverseDynamics/IDConfigBuiltin.hpp index 3965bc5eb..130c19c6d 100644 --- a/src/BulletInverseDynamics/IDConfigBuiltin.hpp +++ b/src/BulletInverseDynamics/IDConfigBuiltin.hpp @@ -15,7 +15,7 @@ typedef float idScalar; // using idArray = std::vector; template struct idArray { - typedef std::vector type; + typedef std::vector type; }; typedef std::vector::size_type idArrayIdx; // default to standard malloc/free @@ -23,15 +23,15 @@ typedef std::vector::size_type idArrayIdx; #define idMalloc ::malloc #define idFree ::free // currently not aligned at all... -#define ID_DECLARE_ALIGNED_ALLOCATOR() \ - inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete(void* ptr) { idFree(ptr); } \ - inline void* operator new(std::size_t, void* ptr) { return ptr; } \ - inline void operator delete(void*, void*) {} \ - inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete[](void* ptr) { idFree(ptr); } \ - inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ - inline void operator delete[](void*, void*) {} +#define ID_DECLARE_ALIGNED_ALLOCATOR() \ + inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete(void* ptr) { idFree(ptr); } \ + inline void* operator new(std::size_t, void* ptr) { return ptr; } \ + inline void operator delete(void*, void*) {} \ + inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete[](void* ptr) { idFree(ptr); } \ + inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ + inline void operator delete[](void*, void*) {} #include "details/IDMatVec.hpp" #endif diff --git a/src/BulletInverseDynamics/IDConfigEigen.hpp b/src/BulletInverseDynamics/IDConfigEigen.hpp index f0b72b425..a83ee8e3f 100644 --- a/src/BulletInverseDynamics/IDConfigEigen.hpp +++ b/src/BulletInverseDynamics/IDConfigEigen.hpp @@ -16,7 +16,7 @@ typedef float idScalar; // using idArray = std::vector; template struct idArray { - typedef std::vector type; + typedef std::vector type; }; typedef std::vector::size_type idArrayIdx; // default to standard malloc/free @@ -25,15 +25,15 @@ typedef std::vector::size_type idArrayIdx; #define idFree ::free // currently not aligned at all... -#define ID_DECLARE_ALIGNED_ALLOCATOR() \ - inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete(void* ptr) { idFree(ptr); } \ - inline void* operator new(std::size_t, void* ptr) { return ptr; } \ - inline void operator delete(void*, void*) {} \ - inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ - inline void operator delete[](void* ptr) { idFree(ptr); } \ - inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ - inline void operator delete[](void*, void*) {} +#define ID_DECLARE_ALIGNED_ALLOCATOR() \ + inline void* operator new(std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete(void* ptr) { idFree(ptr); } \ + inline void* operator new(std::size_t, void* ptr) { return ptr; } \ + inline void operator delete(void*, void*) {} \ + inline void* operator new[](std::size_t sizeInBytes) { return idMalloc(sizeInBytes); } \ + inline void operator delete[](void* ptr) { idFree(ptr); } \ + inline void* operator new[](std::size_t, void* ptr) { return ptr; } \ + inline void operator delete[](void*, void*) {} // Note on interfaces: // Eigen::Matrix has data(), to get c-array storage diff --git a/src/BulletInverseDynamics/IDErrorMessages.hpp b/src/BulletInverseDynamics/IDErrorMessages.hpp index a66ea8323..a3866edc5 100644 --- a/src/BulletInverseDynamics/IDErrorMessages.hpp +++ b/src/BulletInverseDynamics/IDErrorMessages.hpp @@ -13,22 +13,22 @@ #else // BT_ID_WO_BULLET #include /// print error message with file/line information -#define error_message(...) \ - do { \ - fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - } while (0) +#define error_message(...) \ + do { \ + fprintf(stderr, "[Error:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) /// print warning message with file/line information -#define warning_message(...) \ - do { \ - fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - } while (0) -#define warning_message(...) \ - do { \ - fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - } while (0) +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) +#define warning_message(...) \ + do { \ + fprintf(stderr, "[Warning:%s:%d] ", __INVDYN_FILE_WO_DIR__, __LINE__); \ + fprintf(stderr, __VA_ARGS__); \ + } while (0) #define id_printf(...) printf(__VA_ARGS__) #endif // BT_ID_WO_BULLET #endif diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp index a3daaf1f5..7ce055fdf 100644 --- a/src/BulletInverseDynamics/IDMath.cpp +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -10,362 +10,362 @@ static const idScalar kIsZero = 5 * std::numeric_limits::epsilon(); static const idScalar kAxisLengthEpsilon = 10 * kIsZero; void setZero(vec3 &v) { - v(0) = 0; - v(1) = 0; - v(2) = 0; + v(0) = 0; + v(1) = 0; + v(2) = 0; } void setZero(vecx &v) { - for (int i = 0; i < v.size(); i++) { - v(i) = 0; - } + for (int i = 0; i < v.size(); i++) { + v(i) = 0; + } } void setZero(mat33 &m) { - m(0, 0) = 0; - m(0, 1) = 0; - m(0, 2) = 0; - m(1, 0) = 0; - m(1, 1) = 0; - m(1, 2) = 0; - m(2, 0) = 0; - m(2, 1) = 0; - m(2, 2) = 0; + m(0, 0) = 0; + m(0, 1) = 0; + m(0, 2) = 0; + m(1, 0) = 0; + m(1, 1) = 0; + m(1, 2) = 0; + m(2, 0) = 0; + m(2, 1) = 0; + m(2, 2) = 0; } idScalar maxAbs(const vecx &v) { - idScalar result = 0.0; - for (int i = 0; i < v.size(); i++) { - const idScalar tmp = std::fabs(v(i)); - if (tmp > result) { - result = tmp; - } - } - return result; + idScalar result = 0.0; + for (int i = 0; i < v.size(); i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; } idScalar maxAbs(const vec3 &v) { - idScalar result = 0.0; - for (int i = 0; i < 3; i++) { - const idScalar tmp = std::fabs(v(i)); - if (tmp > result) { - result = tmp; - } - } - return result; + idScalar result = 0.0; + for (int i = 0; i < 3; i++) { + const idScalar tmp = std::fabs(v(i)); + if (tmp > result) { + result = tmp; + } + } + return result; } mat33 transformX(const idScalar &alpha) { - mat33 T; - const idScalar cos_alpha = std::cos(alpha); - const idScalar sin_alpha = std::sin(alpha); - // [1 0 0] - // [0 c s] - // [0 -s c] - T(0, 0) = 1.0; - T(0, 1) = 0.0; - T(0, 2) = 0.0; + mat33 T; + const idScalar cos_alpha = std::cos(alpha); + const idScalar sin_alpha = std::sin(alpha); + // [1 0 0] + // [0 c s] + // [0 -s c] + T(0, 0) = 1.0; + T(0, 1) = 0.0; + T(0, 2) = 0.0; - T(1, 0) = 0.0; - T(1, 1) = cos_alpha; - T(1, 2) = sin_alpha; + T(1, 0) = 0.0; + T(1, 1) = cos_alpha; + T(1, 2) = sin_alpha; - T(2, 0) = 0.0; - T(2, 1) = -sin_alpha; - T(2, 2) = cos_alpha; + T(2, 0) = 0.0; + T(2, 1) = -sin_alpha; + T(2, 2) = cos_alpha; - return T; + return T; } mat33 transformY(const idScalar &beta) { - mat33 T; - const idScalar cos_beta = std::cos(beta); - const idScalar sin_beta = std::sin(beta); - // [c 0 -s] - // [0 1 0] - // [s 0 c] - T(0, 0) = cos_beta; - T(0, 1) = 0.0; - T(0, 2) = -sin_beta; + mat33 T; + const idScalar cos_beta = std::cos(beta); + const idScalar sin_beta = std::sin(beta); + // [c 0 -s] + // [0 1 0] + // [s 0 c] + T(0, 0) = cos_beta; + T(0, 1) = 0.0; + T(0, 2) = -sin_beta; - T(1, 0) = 0.0; - T(1, 1) = 1.0; - T(1, 2) = 0.0; + T(1, 0) = 0.0; + T(1, 1) = 1.0; + T(1, 2) = 0.0; - T(2, 0) = sin_beta; - T(2, 1) = 0.0; - T(2, 2) = cos_beta; + T(2, 0) = sin_beta; + T(2, 1) = 0.0; + T(2, 2) = cos_beta; - return T; + return T; } mat33 transformZ(const idScalar &gamma) { - mat33 T; - const idScalar cos_gamma = std::cos(gamma); - const idScalar sin_gamma = std::sin(gamma); - // [ c s 0] - // [-s c 0] - // [ 0 0 1] - T(0, 0) = cos_gamma; - T(0, 1) = sin_gamma; - T(0, 2) = 0.0; + mat33 T; + const idScalar cos_gamma = std::cos(gamma); + const idScalar sin_gamma = std::sin(gamma); + // [ c s 0] + // [-s c 0] + // [ 0 0 1] + T(0, 0) = cos_gamma; + T(0, 1) = sin_gamma; + T(0, 2) = 0.0; - T(1, 0) = -sin_gamma; - T(1, 1) = cos_gamma; - T(1, 2) = 0.0; + T(1, 0) = -sin_gamma; + T(1, 1) = cos_gamma; + T(1, 2) = 0.0; - T(2, 0) = 0.0; - T(2, 1) = 0.0; - T(2, 2) = 1.0; + T(2, 0) = 0.0; + T(2, 1) = 0.0; + T(2, 2) = 1.0; - return T; + return T; } mat33 tildeOperator(const vec3 &v) { - mat33 m; - m(0, 0) = 0.0; - m(0, 1) = -v(2); - m(0, 2) = v(1); - m(1, 0) = v(2); - m(1, 1) = 0.0; - m(1, 2) = -v(0); - m(2, 0) = -v(1); - m(2, 1) = v(0); - m(2, 2) = 0.0; - return m; + mat33 m; + m(0, 0) = 0.0; + m(0, 1) = -v(2); + m(0, 2) = v(1); + m(1, 0) = v(2); + m(1, 1) = 0.0; + m(1, 2) = -v(0); + m(2, 0) = -v(1); + m(2, 1) = v(0); + m(2, 2) = 0.0; + return m; } void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) { - const idScalar sa = std::sin(alpha); - const idScalar ca = std::cos(alpha); - const idScalar st = std::sin(theta); - const idScalar ct = std::cos(theta); + const idScalar sa = std::sin(alpha); + const idScalar ca = std::cos(alpha); + const idScalar st = std::sin(theta); + const idScalar ct = std::cos(theta); - (*r)(0) = a; - (*r)(1) = -sa * d; - (*r)(2) = ca * d; + (*r)(0) = a; + (*r)(1) = -sa * d; + (*r)(2) = ca * d; - (*T)(0, 0) = ct; - (*T)(0, 1) = -st; - (*T)(0, 2) = 0.0; + (*T)(0, 0) = ct; + (*T)(0, 1) = -st; + (*T)(0, 2) = 0.0; - (*T)(1, 0) = st * ca; - (*T)(1, 1) = ct * ca; - (*T)(1, 2) = -sa; + (*T)(1, 0) = st * ca; + (*T)(1, 1) = ct * ca; + (*T)(1, 2) = -sa; - (*T)(2, 0) = st * sa; - (*T)(2, 1) = ct * sa; - (*T)(2, 2) = ca; + (*T)(2, 0) = st * sa; + (*T)(2, 1) = ct * sa; + (*T)(2, 2) = ca; } void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) { - const idScalar c = cos(angle); - const idScalar s = -sin(angle); - const idScalar one_m_c = 1.0 - c; + const idScalar c = cos(angle); + const idScalar s = -sin(angle); + const idScalar one_m_c = 1.0 - c; - const idScalar &x = axis(0); - const idScalar &y = axis(1); - const idScalar &z = axis(2); + const idScalar &x = axis(0); + const idScalar &y = axis(1); + const idScalar &z = axis(2); - (*T)(0, 0) = x * x * one_m_c + c; - (*T)(0, 1) = x * y * one_m_c - z * s; - (*T)(0, 2) = x * z * one_m_c + y * s; + (*T)(0, 0) = x * x * one_m_c + c; + (*T)(0, 1) = x * y * one_m_c - z * s; + (*T)(0, 2) = x * z * one_m_c + y * s; - (*T)(1, 0) = x * y * one_m_c + z * s; - (*T)(1, 1) = y * y * one_m_c + c; - (*T)(1, 2) = y * z * one_m_c - x * s; + (*T)(1, 0) = x * y * one_m_c + z * s; + (*T)(1, 1) = y * y * one_m_c + c; + (*T)(1, 2) = y * z * one_m_c - x * s; - (*T)(2, 0) = x * z * one_m_c - y * s; - (*T)(2, 1) = y * z * one_m_c + x * s; - (*T)(2, 2) = z * z * one_m_c + c; + (*T)(2, 0) = x * z * one_m_c - y * s; + (*T)(2, 1) = y * z * one_m_c + x * s; + (*T)(2, 2) = z * z * one_m_c + c; } bool isPositiveDefinite(const mat33 &m) { - // test if all upper left determinants are positive - if (m(0, 0) <= 0) { // upper 1x1 - return false; - } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2 - return false; - } - if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { - return false; - } - return true; + // test if all upper left determinants are positive + if (m(0, 0) <= 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; } bool isPositiveSemiDefinite(const mat33 &m) { - // test if all upper left determinants are positive - if (m(0, 0) < 0) { // upper 1x1 - return false; - } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2 - return false; - } - if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { - return false; - } - return true; + // test if all upper left determinants are positive + if (m(0, 0) < 0) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) { + return false; + } + return true; } bool isPositiveSemiDefiniteFuzzy(const mat33 &m) { - // test if all upper left determinants are positive - if (m(0, 0) < -kIsZero) { // upper 1x1 - return false; - } - if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2 - return false; - } - if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - - m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + - m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) { - return false; - } - return true; + // test if all upper left determinants are positive + if (m(0, 0) < -kIsZero) { // upper 1x1 + return false; + } + if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2 + return false; + } + if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) - + m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) + + m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) { + return false; + } + return true; } idScalar determinant(const mat33 &m) { - return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - - m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2); + return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) - + m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2); } bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) { - // TODO(Thomas) do we really want this? - // in cases where the inertia tensor about the center of mass is zero, - // the determinant of the inertia tensor about the joint axis is almost - // zero and can have a very small negative value. - if (!isPositiveSemiDefiniteFuzzy(I)) { - error_message("invalid inertia matrix for body %d, not positive definite " - "(fixed joint)\n", - index); - error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); + // TODO(Thomas) do we really want this? + // in cases where the inertia tensor about the center of mass is zero, + // the determinant of the inertia tensor about the joint axis is almost + // zero and can have a very small negative value. + if (!isPositiveSemiDefiniteFuzzy(I)) { + error_message("invalid inertia matrix for body %d, not positive definite " + "(fixed joint)\n", + index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); - return false; - } + return false; + } - // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) - if (!has_fixed_joint) { - if (I(0, 0) + I(1, 1) < I(2, 2)) { - error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); - error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); - return false; - } - if (I(0, 0) + I(1, 1) < I(2, 2)) { - error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); - error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); - return false; - } - if (I(1, 1) + I(2, 2) < I(0, 0)) { - error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); - error_message("matrix is:\n" - "[%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e;\n" - "%.20e %.20e %.20e]\n", - I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), - I(2, 2)); - return false; - } - } - // check positive/zero diagonal elements - for (int i = 0; i < 3; i++) { - if (I(i, i) < 0) { // accept zero - error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); - return false; - } - } - // check symmetry - if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) { - error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " - "%e\n", - index, I(1, 0) - I(0, 1)); - return false; - } - if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) { - error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " - "%e\n", - index, I(2, 0) - I(0, 2)); - return false; - } - if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) { - error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index, - I(1, 2) - I(2, 1)); - return false; - } - return true; + // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k) + if (!has_fixed_joint) { + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(0, 0) + I(1, 1) < I(2, 2)) { + error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + if (I(1, 1) + I(2, 2) < I(0, 0)) { + error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index); + error_message("matrix is:\n" + "[%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e;\n" + "%.20e %.20e %.20e]\n", + I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1), + I(2, 2)); + return false; + } + } + // check positive/zero diagonal elements + for (int i = 0; i < 3; i++) { + if (I(i, i) < 0) { // accept zero + error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i)); + return false; + } + } + // check symmetry + if (std::fabs(I(1, 0) - I(0, 1)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= " + "%e\n", + index, I(1, 0) - I(0, 1)); + return false; + } + if (std::fabs(I(2, 0) - I(0, 2)) > kIsZero) { + error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= " + "%e\n", + index, I(2, 0) - I(0, 2)); + return false; + } + if (std::fabs(I(1, 2) - I(2, 1)) > kIsZero) { + error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index, + I(1, 2) - I(2, 1)); + return false; + } + return true; } bool isValidTransformMatrix(const mat33 &m) { -#define print_mat(x) \ - error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ - x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) +#define print_mat(x) \ + error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \ + x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2)) - // check for unit length column vectors - for (int i = 0; i < 3; i++) { - const idScalar length_minus_1 = - std::fabs(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0); - if (length_minus_1 > kAxisLengthEpsilon) { - error_message("Not a valid rotation matrix (column %d not unit length)\n" - "column = [%.18e %.18e %.18e]\n" - "length-1.0= %.18e\n", - i, m(0, i), m(1, i), m(2, i), length_minus_1); - print_mat(m); - return false; - } - } - // check for orthogonal column vectors - if (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) { - error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n"); - print_mat(m); - return false; - } - if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) { - error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); - print_mat(m); - return false; - } - if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) { - error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); - print_mat(m); - return false; - } - // check determinant (rotation not reflection) - if (determinant(m) <= 0) { - error_message("Not a valid rotation matrix (determinant <=0)\n"); - print_mat(m); - return false; - } - return true; + // check for unit length column vectors + for (int i = 0; i < 3; i++) { + const idScalar length_minus_1 = + std::fabs(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0); + if (length_minus_1 > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (column %d not unit length)\n" + "column = [%.18e %.18e %.18e]\n" + "length-1.0= %.18e\n", + i, m(0, i), m(1, i), m(2, i), length_minus_1); + print_mat(m); + return false; + } + } + // check for orthogonal column vectors + if (std::fabs(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n"); + print_mat(m); + return false; + } + if (std::fabs(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + if (std::fabs(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) { + error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n"); + print_mat(m); + return false; + } + // check determinant (rotation not reflection) + if (determinant(m) <= 0) { + error_message("Not a valid rotation matrix (determinant <=0)\n"); + print_mat(m); + return false; + } + return true; } bool isUnitVector(const vec3 &vector) { - return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < - kIsZero; + return std::fabs(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) < + kIsZero; } vec3 rpyFromMatrix(const mat33 &rot) { - vec3 rpy; - rpy(2) = std::atan2(-rot(1, 0), rot(0, 0)); - rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0)); - rpy(0) = std::atan2(-rot(2, 0), rot(2, 2)); - return rpy; + vec3 rpy; + rpy(2) = std::atan2(-rot(1, 0), rot(0, 0)); + rpy(1) = std::atan2(rot(2, 0), std::cos(rpy(2)) * rot(0, 0) - std::sin(rpy(0)) * rot(1, 0)); + rpy(0) = std::atan2(-rot(2, 0), rot(2, 2)); + return rpy; } } diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp index 97e60c120..f7526b500 100644 --- a/src/BulletInverseDynamics/IDMath.hpp +++ b/src/BulletInverseDynamics/IDMath.hpp @@ -1,5 +1,5 @@ /// @file Math utility functions used in inverse dynamics library. -/// Defined here as they may not be provided by the math library. +/// Defined here as they may not be provided by the math library. #ifndef IDMATH_HPP_ #define IDMATH_HPP_ @@ -39,7 +39,7 @@ bool isPositiveSemiDefiniteFuzzy(const mat33& m); /// Determinant of 3x3 matrix /// NOTE: implemented here for portability, as determinant operation -/// will be implemented differently for various matrix/vector libraries +/// will be implemented differently for various matrix/vector libraries /// @param m a 3x3 matrix /// @return det(m) idScalar determinant(const mat33& m); diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index 5c0af64e9..f5488b62a 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.cpp +++ b/src/BulletInverseDynamics/MultiBodyTree.cpp @@ -11,54 +11,54 @@ namespace btInverseDynamics { MultiBodyTree::MultiBodyTree() - : m_is_finalized(false), - m_mass_parameters_are_valid(true), - m_accept_invalid_mass_parameters(false), - m_impl(0x0), - m_init_cache(0x0) { - m_init_cache = new InitCache(); + : m_is_finalized(false), + m_mass_parameters_are_valid(true), + m_accept_invalid_mass_parameters(false), + m_impl(0x0), + m_init_cache(0x0) { + m_init_cache = new InitCache(); } MultiBodyTree::~MultiBodyTree() { - delete m_impl; - delete m_init_cache; + delete m_impl; + delete m_init_cache; } void MultiBodyTree::setAcceptInvalidMassParameters(bool flag) { - m_accept_invalid_mass_parameters = flag; + m_accept_invalid_mass_parameters = flag; } bool MultiBodyTree::getAcceptInvalidMassProperties() const { - return m_accept_invalid_mass_parameters; + return m_accept_invalid_mass_parameters; } int MultiBodyTree::getBodyOrigin(const int body_index, vec3 *world_origin) const { - return m_impl->getBodyOrigin(body_index, world_origin); + return m_impl->getBodyOrigin(body_index, world_origin); } int MultiBodyTree::getBodyCoM(const int body_index, vec3 *world_com) const { - return m_impl->getBodyCoM(body_index, world_com); + return m_impl->getBodyCoM(body_index, world_com); } int MultiBodyTree::getBodyTransform(const int body_index, mat33 *world_T_body) const { - return m_impl->getBodyTransform(body_index, world_T_body); + return m_impl->getBodyTransform(body_index, world_T_body); } int MultiBodyTree::getBodyAngularVelocity(const int body_index, vec3 *world_omega) const { - return m_impl->getBodyAngularVelocity(body_index, world_omega); + return m_impl->getBodyAngularVelocity(body_index, world_omega); } int MultiBodyTree::getBodyLinearVelocity(const int body_index, vec3 *world_velocity) const { - return m_impl->getBodyLinearVelocity(body_index, world_velocity); + return m_impl->getBodyLinearVelocity(body_index, world_velocity); } int MultiBodyTree::getBodyLinearVelocityCoM(const int body_index, vec3 *world_velocity) const { - return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); + return m_impl->getBodyLinearVelocityCoM(body_index, world_velocity); } int MultiBodyTree::getBodyAngularAcceleration(const int body_index, vec3 *world_dot_omega) const { - return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); + return m_impl->getBodyAngularAcceleration(body_index, world_dot_omega); } int MultiBodyTree::getBodyLinearAcceleration(const int body_index, vec3 *world_acceleration) const { - return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); + return m_impl->getBodyLinearAcceleration(body_index, world_acceleration); } void MultiBodyTree::printTree() { m_impl->printTree(); } @@ -69,263 +69,263 @@ int MultiBodyTree::numBodies() const { return m_impl->m_num_bodies; } int MultiBodyTree::numDoFs() const { return m_impl->m_num_dofs; } int MultiBodyTree::calculateInverseDynamics(const vecx &q, const vecx &u, const vecx &dot_u, - vecx *joint_forces) { - if (false == m_is_finalized) { - error_message("system has not been initialized\n"); - return -1; - } - if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { - error_message("error in inverse dynamics calculation\n"); - return -1; - } - return 0; + vecx *joint_forces) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == m_impl->calculateInverseDynamics(q, u, dot_u, joint_forces)) { + error_message("error in inverse dynamics calculation\n"); + return -1; + } + return 0; } int MultiBodyTree::calculateMassMatrix(const vecx &q, const bool update_kinematics, - const bool initialize_matrix, - const bool set_lower_triangular_matrix, matxx *mass_matrix) { - if (false == m_is_finalized) { - error_message("system has not been initialized\n"); - return -1; - } - if (-1 == - m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, - set_lower_triangular_matrix, mass_matrix)) { - error_message("error in mass matrix calculation\n"); - return -1; - } - return 0; + const bool initialize_matrix, + const bool set_lower_triangular_matrix, matxx *mass_matrix) { + if (false == m_is_finalized) { + error_message("system has not been initialized\n"); + return -1; + } + if (-1 == + m_impl->calculateMassMatrix(q, update_kinematics, initialize_matrix, + set_lower_triangular_matrix, mass_matrix)) { + error_message("error in mass matrix calculation\n"); + return -1; + } + return 0; } int MultiBodyTree::calculateMassMatrix(const vecx &q, matxx *mass_matrix) { - return calculateMassMatrix(q, true, true, true, mass_matrix); + return calculateMassMatrix(q, true, true, true, mass_matrix); } int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_type, - const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, - const vec3 &body_axis_of_motion_, idScalar mass, - const vec3 &body_r_body_com, const mat33 &body_I_body, - const int user_int, void *user_ptr) { - if (body_index < 0) { - error_message("body index must be positive (got %d)\n", body_index); - return -1; - } - vec3 body_axis_of_motion(body_axis_of_motion_); - switch (joint_type) { - case REVOLUTE: - case PRISMATIC: - // check if axis is unit vector - if (!isUnitVector(body_axis_of_motion)) { - warning_message( - "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", - body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); - idScalar length = std::sqrt(std::pow(body_axis_of_motion(0), 2) + - std::pow(body_axis_of_motion(1), 2) + - std::pow(body_axis_of_motion(2), 2)); - if (length < std::sqrt(std::numeric_limits::min())) { - error_message("axis of motion vector too short (%e)\n", length); - return -1; - } - body_axis_of_motion = (1.0 / length) * body_axis_of_motion; - } - break; - case FIXED: - break; - case FLOATING: - break; - default: - error_message("unknown joint type %d\n", joint_type); - return -1; - } + const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, + const vec3 &body_axis_of_motion_, idScalar mass, + const vec3 &body_r_body_com, const mat33 &body_I_body, + const int user_int, void *user_ptr) { + if (body_index < 0) { + error_message("body index must be positive (got %d)\n", body_index); + return -1; + } + vec3 body_axis_of_motion(body_axis_of_motion_); + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + // check if axis is unit vector + if (!isUnitVector(body_axis_of_motion)) { + warning_message( + "axis of motion not a unit axis ([%f %f %f]), will use normalized vector\n", + body_axis_of_motion(0), body_axis_of_motion(1), body_axis_of_motion(2)); + idScalar length = std::sqrt(std::pow(body_axis_of_motion(0), 2) + + std::pow(body_axis_of_motion(1), 2) + + std::pow(body_axis_of_motion(2), 2)); + if (length < std::sqrt(std::numeric_limits::min())) { + error_message("axis of motion vector too short (%e)\n", length); + return -1; + } + body_axis_of_motion = (1.0 / length) * body_axis_of_motion; + } + break; + case FIXED: + break; + case FLOATING: + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } - // sanity check for mass properties. Zero mass is OK. - if (mass < 0) { - m_mass_parameters_are_valid = false; - error_message("Body %d has invalid mass %e\n", body_index, mass); - if (!m_accept_invalid_mass_parameters) { - return -1; - } - } + // sanity check for mass properties. Zero mass is OK. + if (mass < 0) { + m_mass_parameters_are_valid = false; + error_message("Body %d has invalid mass %e\n", body_index, mass); + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } - if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) { - m_mass_parameters_are_valid = false; - // error message printed in function call - if (!m_accept_invalid_mass_parameters) { - return -1; - } - } + if (!isValidInertiaMatrix(body_I_body, body_index, FIXED == joint_type)) { + m_mass_parameters_are_valid = false; + // error message printed in function call + if (!m_accept_invalid_mass_parameters) { + return -1; + } + } - if (!isValidTransformMatrix(body_T_parent_ref)) { - return -1; - } + if (!isValidTransformMatrix(body_T_parent_ref)) { + return -1; + } - return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, - body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, - body_I_body, user_int, user_ptr); + return m_init_cache->addBody(body_index, parent_index, joint_type, parent_r_parent_body_ref, + body_T_parent_ref, body_axis_of_motion, mass, body_r_body_com, + body_I_body, user_int, user_ptr); } int MultiBodyTree::getParentIndex(const int body_index, int *parent_index) const { - return m_impl->getParentIndex(body_index, parent_index); + return m_impl->getParentIndex(body_index, parent_index); } int MultiBodyTree::getUserInt(const int body_index, int *user_int) const { - return m_impl->getUserInt(body_index, user_int); + return m_impl->getUserInt(body_index, user_int); } int MultiBodyTree::getUserPtr(const int body_index, void **user_ptr) const { - return m_impl->getUserPtr(body_index, user_ptr); + return m_impl->getUserPtr(body_index, user_ptr); } int MultiBodyTree::setUserInt(const int body_index, const int user_int) { - return m_impl->setUserInt(body_index, user_int); + return m_impl->setUserInt(body_index, user_int); } int MultiBodyTree::setUserPtr(const int body_index, void *const user_ptr) { - return m_impl->setUserPtr(body_index, user_ptr); + return m_impl->setUserPtr(body_index, user_ptr); } int MultiBodyTree::finalize() { - const int &num_bodies = m_init_cache->numBodies(); - const int &num_dofs = m_init_cache->numDoFs(); + const int &num_bodies = m_init_cache->numBodies(); + const int &num_dofs = m_init_cache->numDoFs(); - // 1 allocate internal MultiBody structure - m_impl = new MultiBodyImpl(num_bodies, num_dofs); + // 1 allocate internal MultiBody structure + m_impl = new MultiBodyImpl(num_bodies, num_dofs); - // 2 build new index set assuring index(parent) < index(child) - if (-1 == m_init_cache->buildIndexSets()) { - return -1; - } - m_init_cache->getParentIndexArray(&m_impl->m_parent_index); + // 2 build new index set assuring index(parent) < index(child) + if (-1 == m_init_cache->buildIndexSets()) { + return -1; + } + m_init_cache->getParentIndexArray(&m_impl->m_parent_index); - // 3 setup internal kinematic and dynamic data - for (int index = 0; index < num_bodies; index++) { - InertiaData inertia; - JointData joint; - if (-1 == m_init_cache->getInertiaData(index, &inertia)) { - return -1; - } - if (-1 == m_init_cache->getJointData(index, &joint)) { - return -1; - } + // 3 setup internal kinematic and dynamic data + for (int index = 0; index < num_bodies; index++) { + InertiaData inertia; + JointData joint; + if (-1 == m_init_cache->getInertiaData(index, &inertia)) { + return -1; + } + if (-1 == m_init_cache->getJointData(index, &joint)) { + return -1; + } - RigidBody &rigid_body = m_impl->m_body_list[index]; + RigidBody &rigid_body = m_impl->m_body_list[index]; - rigid_body.m_mass = inertia.m_mass; - rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; - rigid_body.m_body_I_body = inertia.m_body_I_body; - rigid_body.m_joint_type = joint.m_type; - rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; - rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; - rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; - rigid_body.m_joint_type = joint.m_type; + rigid_body.m_mass = inertia.m_mass; + rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com; + rigid_body.m_body_I_body = inertia.m_body_I_body; + rigid_body.m_joint_type = joint.m_type; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref; + rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref; + rigid_body.m_joint_type = joint.m_type; - // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized - // matrices. - switch (rigid_body.m_joint_type) { - case REVOLUTE: - rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); - rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); - rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - case PRISMATIC: - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); - rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); - rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); - break; - case FIXED: - // NOTE/TODO: dimension really should be zero .. - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - case FLOATING: - // NOTE/TODO: this is not really correct. - // the Jacobians should be 3x3 matrices here ! - rigid_body.m_Jac_JR(0) = 0.0; - rigid_body.m_Jac_JR(1) = 0.0; - rigid_body.m_Jac_JR(2) = 0.0; - rigid_body.m_Jac_JT(0) = 0.0; - rigid_body.m_Jac_JT(1) = 0.0; - rigid_body.m_Jac_JT(2) = 0.0; - break; - default: - error_message("unsupported joint type %d\n", rigid_body.m_joint_type); - return -1; - } - } + // Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized + // matrices. + switch (rigid_body.m_joint_type) { + case REVOLUTE: + rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2); + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case PRISMATIC: + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0); + rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1); + rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2); + break; + case FIXED: + // NOTE/TODO: dimension really should be zero .. + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + case FLOATING: + // NOTE/TODO: this is not really correct. + // the Jacobians should be 3x3 matrices here ! + rigid_body.m_Jac_JR(0) = 0.0; + rigid_body.m_Jac_JR(1) = 0.0; + rigid_body.m_Jac_JR(2) = 0.0; + rigid_body.m_Jac_JT(0) = 0.0; + rigid_body.m_Jac_JT(1) = 0.0; + rigid_body.m_Jac_JT(2) = 0.0; + break; + default: + error_message("unsupported joint type %d\n", rigid_body.m_joint_type); + return -1; + } + } - // 4 assign degree of freedom indices & build per-joint-type index arrays - if (-1 == m_impl->generateIndexSets()) { - error_message("generating index sets\n"); - return -1; - } + // 4 assign degree of freedom indices & build per-joint-type index arrays + if (-1 == m_impl->generateIndexSets()) { + error_message("generating index sets\n"); + return -1; + } - // 5 do some pre-computations .. - m_impl->calculateStaticData(); + // 5 do some pre-computations .. + m_impl->calculateStaticData(); - // 6. make sure all user forces are set to zero, as this might not happen - // in the vector ctors. - m_impl->clearAllUserForcesAndMoments(); + // 6. make sure all user forces are set to zero, as this might not happen + // in the vector ctors. + m_impl->clearAllUserForcesAndMoments(); - m_is_finalized = true; - return 0; + m_is_finalized = true; + return 0; } int MultiBodyTree::setGravityInWorldFrame(const vec3 &gravity) { - return m_impl->setGravityInWorldFrame(gravity); + return m_impl->setGravityInWorldFrame(gravity); } int MultiBodyTree::getJointType(const int body_index, JointType *joint_type) const { - return m_impl->getJointType(body_index, joint_type); + return m_impl->getJointType(body_index, joint_type); } int MultiBodyTree::getJointTypeStr(const int body_index, const char **joint_type) const { - return m_impl->getJointTypeStr(body_index, joint_type); + return m_impl->getJointTypeStr(body_index, joint_type); } int MultiBodyTree::getDoFOffset(const int body_index, int *q_offset) const { - return m_impl->getDoFOffset(body_index, q_offset); + return m_impl->getDoFOffset(body_index, q_offset); } int MultiBodyTree::setBodyMass(const int body_index, idScalar mass) { - return m_impl->setBodyMass(body_index, mass); + return m_impl->setBodyMass(body_index, mass); } int MultiBodyTree::setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment) { - return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); + return m_impl->setBodyFirstMassMoment(body_index, first_mass_moment); } int MultiBodyTree::setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment) { - return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); + return m_impl->setBodySecondMassMoment(body_index, second_mass_moment); } int MultiBodyTree::getBodyMass(const int body_index, idScalar *mass) const { - return m_impl->getBodyMass(body_index, mass); + return m_impl->getBodyMass(body_index, mass); } int MultiBodyTree::getBodyFirstMassMoment(const int body_index, vec3 *first_mass_moment) const { - return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); + return m_impl->getBodyFirstMassMoment(body_index, first_mass_moment); } int MultiBodyTree::getBodySecondMassMoment(const int body_index, mat33 *second_mass_moment) const { - return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); + return m_impl->getBodySecondMassMoment(body_index, second_mass_moment); } void MultiBodyTree::clearAllUserForcesAndMoments() { m_impl->clearAllUserForcesAndMoments(); } int MultiBodyTree::addUserForce(const int body_index, const vec3 &body_force) { - return m_impl->addUserForce(body_index, body_force); + return m_impl->addUserForce(body_index, body_force); } int MultiBodyTree::addUserMoment(const int body_index, const vec3 &body_moment) { - return m_impl->addUserMoment(body_index, body_moment); + return m_impl->addUserMoment(body_index, body_moment); } } diff --git a/src/BulletInverseDynamics/MultiBodyTree.hpp b/src/BulletInverseDynamics/MultiBodyTree.hpp index 9f3400b40..d33e60e12 100644 --- a/src/BulletInverseDynamics/MultiBodyTree.hpp +++ b/src/BulletInverseDynamics/MultiBodyTree.hpp @@ -8,14 +8,14 @@ namespace btInverseDynamics { /// Enumeration of supported joint types enum JointType { - /// no degree of freedom, moves with parent - FIXED = 0, - /// one rotational degree of freedom relative to parent - REVOLUTE, - /// one translational degree of freedom relative to parent - PRISMATIC, - /// six degrees of freedom relative to parent - FLOATING + /// no degree of freedom, moves with parent + FIXED = 0, + /// one rotational degree of freedom relative to parent + REVOLUTE, + /// one translational degree of freedom relative to parent + PRISMATIC, + /// six degrees of freedom relative to parent + FLOATING }; /// Interface class for calculating inverse dynamics for tree structured @@ -25,284 +25,284 @@ enum JointType { /// The q vector contains the generalized coordinate set defining the tree's configuration. /// Every joint adds elements that define the corresponding link's frame pose relative to /// its parent. For the joint types that is: -/// - FIXED: none -/// - REVOLUTE: angle of rotation [rad] -/// - PRISMATIC: displacement [m] -/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] -/// (in that order) +/// - FIXED: none +/// - REVOLUTE: angle of rotation [rad] +/// - PRISMATIC: displacement [m] +/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m] +/// (in that order) /// The u vector contains the generalized speeds, which are -/// - FIXED: none -/// - REVOLUTE: time derivative of angle of rotation [rad/s] -/// - PRISMATIC: time derivative of displacement [m/s] -/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) -/// and time derivative of displacement in parent frame [m/s] +/// - FIXED: none +/// - REVOLUTE: time derivative of angle of rotation [rad/s] +/// - PRISMATIC: time derivative of displacement [m/s] +/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles) +/// and time derivative of displacement in parent frame [m/s] /// /// The q and u vectors are obtained by stacking contributions of all bodies in one /// vector in the order of body indices. /// /// Note on generalized forces: analogous to u, i.e., -/// - FIXED: none -/// - REVOLUTE: moment [Nm], about joint axis -/// - PRISMATIC: force [N], along joint axis -/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame -/// (in that order) +/// - FIXED: none +/// - REVOLUTE: moment [Nm], about joint axis +/// - PRISMATIC: force [N], along joint axis +/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame +/// (in that order) /// /// TODO - force element interface (friction, springs, dampers, etc) -/// - gears and motor inertia +/// - gears and motor inertia class MultiBodyTree { public: - /// The contructor. - /// Initialization & allocation is via addBody and buildSystem calls. - MultiBodyTree(); - /// the destructor. This also deallocates all memory - ~MultiBodyTree(); + /// The contructor. + /// Initialization & allocation is via addBody and buildSystem calls. + MultiBodyTree(); + /// the destructor. This also deallocates all memory + ~MultiBodyTree(); - /// Add body to the system. this allocates memory and not real-time safe. - /// This only adds the data to an initial cache. After all bodies have been - /// added, - /// the system is setup using the buildSystem call - /// @param body_index index of the body to be added. Must >=0, =dim(u) - /// @param dot_u time derivative of u - /// @param joint_forces this is where the resulting joint forces will be - /// stored. dim(joint_forces) = dim(u) - /// @return 0 on success, -1 on error - int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, - vecx* joint_forces); - /// Calculate joint space mass matrix - /// @param q generalized coordinates - /// @param initialize_matrix if true, initialize mass matrix with zero. - /// If mass_matrix is initialized to zero externally and only used - /// for mass matrix computations for the same system, it is safe to - /// set this to false. - /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix - /// is also populated, otherwise not. - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, const bool update_kinematics, - const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx* mass_matrix); + /// Add body to the system. this allocates memory and not real-time safe. + /// This only adds the data to an initial cache. After all bodies have been + /// added, + /// the system is setup using the buildSystem call + /// @param body_index index of the body to be added. Must >=0, =dim(u) + /// @param dot_u time derivative of u + /// @param joint_forces this is where the resulting joint forces will be + /// stored. dim(joint_forces) = dim(u) + /// @return 0 on success, -1 on error + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + /// Calculate joint space mass matrix + /// @param q generalized coordinates + /// @param initialize_matrix if true, initialize mass matrix with zero. + /// If mass_matrix is initialized to zero externally and only used + /// for mass matrix computations for the same system, it is safe to + /// set this to false. + /// @param set_lower_triangular_matrix if true, the lower triangular section of mass_matrix + /// is also populated, otherwise not. + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); - /// Calculate joint space mass matrix. - /// This version will update kinematics, initialize all mass_matrix elements to zero and - /// populate all mass matrix entries. - /// @param q generalized coordinates - /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) - /// @return -1 on error, 0 on success - int calculateMassMatrix(const vecx& q, matxx* mass_matrix); + /// Calculate joint space mass matrix. + /// This version will update kinematics, initialize all mass_matrix elements to zero and + /// populate all mass matrix entries. + /// @param q generalized coordinates + /// @param mass_matrix matrix for storing the output (should be dim(q)xdim(q)) + /// @return -1 on error, 0 on success + int calculateMassMatrix(const vecx& q, matxx* mass_matrix); - /// set gravitational acceleration - /// the default is [0;0;-9.8] in the world frame - /// @param gravity the gravitational acceleration in world frame - /// @return 0 on success, -1 on error - int setGravityInWorldFrame(const vec3& gravity); - /// returns number of bodies in tree - int numBodies() const; - /// returns number of mechanical degrees of freedom (dimension of q-vector) - int numDoFs() const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyOrigin(const int body_index, vec3* world_origin) const; - /// get center of mass of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyCoM(const int body_index, vec3* world_com) const; - /// get transform from of a body-fixed frame to the world frame - /// @param body_index index for frame/body - /// @param world_T_body pointer for return data - /// @return 0 on success, -1 on error - int getBodyTransform(const int body_index, mat33* world_T_body) const; - /// get absolute angular velocity for a body, represented in the world frame - /// @param body_index index for frame/body - /// @param world_omega pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; - /// get linear velocity of a body, represented in world frame - /// @param body_index index for frame/body - /// @param world_velocity pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; - /// get linear velocity of a body's CoM, represented in world frame - /// (not required for inverse dynamics, provided for convenience) - /// @param body_index index for frame/body - /// @param world_vel_com pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; - /// get origin of a body-fixed frame, represented in world frame - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; - /// get origin of a body-fixed frame, represented in world frame - /// NOTE: this will include the gravitational acceleration, so the actual acceleration is - /// obtainened by setting gravitational acceleration to zero, or subtracting it. - /// @param body_index index for frame/body - /// @param world_origin pointer for return data - /// @return 0 on success, -1 on error - int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; - /// returns the (internal) index of body - /// @param body_index is the index of a body (internal: TODO: fix/clarify - /// indexing!) - /// @param parent_index pointer to where parent index will be stored - /// @return 0 on success, -1 on error - int getParentIndex(const int body_index, int* parent_index) const; - /// get joint type - /// @param body_index index of the body - /// @param joint_type the corresponding joint type - /// @return 0 on success, -1 on failure - int getJointType(const int body_index, JointType* joint_type) const; - /// get joint type as string - /// @param body_index index of the body - /// @param joint_type string naming the corresponding joint type - /// @return 0 on success, -1 on failure - int getJointTypeStr(const int body_index, const char** joint_type) const; - /// get offset for degrees of freedom of this body into the q-vector - /// @param body_index index of the body - /// @param q_offset offset the q vector - /// @return -1 on error, 0 on success - int getDoFOffset(const int body_index, int* q_offset) const; - /// get user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int getUserInt(const int body_index, int* user_int) const; - /// get user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int getUserPtr(const int body_index, void** user_ptr) const; - /// set user integer. not used by the library. - /// @param body_index index of the body - /// @param user_int the user integer - /// @return 0 on success, -1 on error - int setUserInt(const int body_index, const int user_int); - /// set user pointer. not used by the library. - /// @param body_index index of the body - /// @param user_ptr the user pointer - /// @return 0 on success, -1 on error - int setUserPtr(const int body_index, void* const user_ptr); - /// set mass for a body - /// @param body_index index of the body - /// @param mass the mass to set - /// @return 0 on success, -1 on failure - int setBodyMass(const int body_index, const idScalar mass); - /// set first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_mass_moment the vector to set - /// @return 0 on success, -1 on failure - int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); - /// set second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); - /// get mass for a body - /// @param body_index index of the body - /// @param mass the mass - /// @return 0 on success, -1 on failure - int getBodyMass(const int body_index, idScalar* mass) const; - /// get first moment of mass for a body - /// (mass * center of mass, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param first_moment the vector - /// @return 0 on success, -1 on failure - int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; - /// get second moment of mass for a body - /// (moment of inertia, in body fixed frame, relative to joint) - /// @param body_index index of the body - /// @param second_mass_moment the inertia matrix - /// @return 0 on success, -1 on failure - int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; - /// set all user forces and moments to zero - void clearAllUserForcesAndMoments(); - /// Add an external force to a body, acting at the origin of the body-fixed frame. - /// Calls to addUserForce are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_force the force represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserForce(const int body_index, const vec3& body_force); - /// Add an external moment to a body. - /// Calls to addUserMoment are cumulative. Set the user force and moment to zero - /// via clearAllUserForcesAndMoments() - /// @param body_moment the moment represented in the body-fixed frame of reference - /// @return 0 on success, -1 on error - int addUserMoment(const int body_index, const vec3& body_moment); + /// set gravitational acceleration + /// the default is [0;0;-9.8] in the world frame + /// @param gravity the gravitational acceleration in world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// returns number of bodies in tree + int numBodies() const; + /// returns number of mechanical degrees of freedom (dimension of q-vector) + int numDoFs() const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// get center of mass of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyCoM(const int body_index, vec3* world_com) const; + /// get transform from of a body-fixed frame to the world frame + /// @param body_index index for frame/body + /// @param world_T_body pointer for return data + /// @return 0 on success, -1 on error + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// get absolute angular velocity for a body, represented in the world frame + /// @param body_index index for frame/body + /// @param world_omega pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// get linear velocity of a body, represented in world frame + /// @param body_index index for frame/body + /// @param world_velocity pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// get linear velocity of a body's CoM, represented in world frame + /// (not required for inverse dynamics, provided for convenience) + /// @param body_index index for frame/body + /// @param world_vel_com pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// get origin of a body-fixed frame, represented in world frame + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// get origin of a body-fixed frame, represented in world frame + /// NOTE: this will include the gravitational acceleration, so the actual acceleration is + /// obtainened by setting gravitational acceleration to zero, or subtracting it. + /// @param body_index index for frame/body + /// @param world_origin pointer for return data + /// @return 0 on success, -1 on error + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// returns the (internal) index of body + /// @param body_index is the index of a body (internal: TODO: fix/clarify + /// indexing!) + /// @param parent_index pointer to where parent index will be stored + /// @return 0 on success, -1 on error + int getParentIndex(const int body_index, int* parent_index) const; + /// get joint type + /// @param body_index index of the body + /// @param joint_type the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointType(const int body_index, JointType* joint_type) const; + /// get joint type as string + /// @param body_index index of the body + /// @param joint_type string naming the corresponding joint type + /// @return 0 on success, -1 on failure + int getJointTypeStr(const int body_index, const char** joint_type) const; + /// get offset for degrees of freedom of this body into the q-vector + /// @param body_index index of the body + /// @param q_offset offset the q vector + /// @return -1 on error, 0 on success + int getDoFOffset(const int body_index, int* q_offset) const; + /// get user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int getUserInt(const int body_index, int* user_int) const; + /// get user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int getUserPtr(const int body_index, void** user_ptr) const; + /// set user integer. not used by the library. + /// @param body_index index of the body + /// @param user_int the user integer + /// @return 0 on success, -1 on error + int setUserInt(const int body_index, const int user_int); + /// set user pointer. not used by the library. + /// @param body_index index of the body + /// @param user_ptr the user pointer + /// @return 0 on success, -1 on error + int setUserPtr(const int body_index, void* const user_ptr); + /// set mass for a body + /// @param body_index index of the body + /// @param mass the mass to set + /// @return 0 on success, -1 on failure + int setBodyMass(const int body_index, const idScalar mass); + /// set first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_mass_moment the vector to set + /// @return 0 on success, -1 on failure + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); + /// set second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); + /// get mass for a body + /// @param body_index index of the body + /// @param mass the mass + /// @return 0 on success, -1 on failure + int getBodyMass(const int body_index, idScalar* mass) const; + /// get first moment of mass for a body + /// (mass * center of mass, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param first_moment the vector + /// @return 0 on success, -1 on failure + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + /// get second moment of mass for a body + /// (moment of inertia, in body fixed frame, relative to joint) + /// @param body_index index of the body + /// @param second_mass_moment the inertia matrix + /// @return 0 on success, -1 on failure + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// set all user forces and moments to zero + void clearAllUserForcesAndMoments(); + /// Add an external force to a body, acting at the origin of the body-fixed frame. + /// Calls to addUserForce are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_force the force represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserForce(const int body_index, const vec3& body_force); + /// Add an external moment to a body. + /// Calls to addUserMoment are cumulative. Set the user force and moment to zero + /// via clearAllUserForcesAndMoments() + /// @param body_moment the moment represented in the body-fixed frame of reference + /// @return 0 on success, -1 on error + int addUserMoment(const int body_index, const vec3& body_moment); private: - // flag indicating if system has been initialized - bool m_is_finalized; - // flag indicating if mass properties are physically valid - bool m_mass_parameters_are_valid; - // flag defining if unphysical mass parameters are accepted - bool m_accept_invalid_mass_parameters; - // This struct implements the inverse dynamics calculations - class MultiBodyImpl; - MultiBodyImpl* m_impl; - // cache data structure for initialization - class InitCache; - InitCache* m_init_cache; + // flag indicating if system has been initialized + bool m_is_finalized; + // flag indicating if mass properties are physically valid + bool m_mass_parameters_are_valid; + // flag defining if unphysical mass parameters are accepted + bool m_accept_invalid_mass_parameters; + // This struct implements the inverse dynamics calculations + class MultiBodyImpl; + MultiBodyImpl* m_impl; + // cache data structure for initialization + class InitCache; + InitCache* m_init_cache; }; } // namespace btInverseDynamics #endif // MULTIBODYTREE_HPP_ diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index 2209121ba..c77ba3cba 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -17,29 +17,29 @@ typedef btMatrixX matxx; class vec3 : public btVector3 { public: - vec3() : btVector3() {} - vec3(const btVector3& btv) { *this = btv; } - idScalar& operator()(int i) { return (*this)[i]; } - const idScalar& operator()(int i) const { return (*this)[i]; } - const int size() const { return 3; } - const vec3& operator=(const btVector3& rhs) { - *static_cast(this) = rhs; - return *this; - } + vec3() : btVector3() {} + vec3(const btVector3& btv) { *this = btv; } + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } + const int size() const { return 3; } + const vec3& operator=(const btVector3& rhs) { + *static_cast(this) = rhs; + return *this; + } }; class mat33 : public btMatrix3x3 { public: - mat33() : btMatrix3x3() {} - mat33(const btMatrix3x3& btm) { *this = btm; } - idScalar& operator()(int i, int j) { return (*this)[i][j]; } - const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } - const mat33& operator=(const btMatrix3x3& rhs) { - *static_cast(this) = rhs; - return *this; - } - friend mat33 operator*(const idScalar& s, const mat33& a); - friend mat33 operator/(const mat33& a, const idScalar& s); + mat33() : btMatrix3x3() {} + mat33(const btMatrix3x3& btm) { *this = btm; } + idScalar& operator()(int i, int j) { return (*this)[i][j]; } + const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } + const mat33& operator=(const btMatrix3x3& rhs) { + *static_cast(this) = rhs; + return *this; + } + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator/(const mat33& a, const idScalar& s); }; inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); } @@ -48,64 +48,64 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } class vecx : public btVectorX { public: - vecx(int size) : btVectorX(size) {} - const vecx& operator=(const btVectorX& rhs) { - *static_cast(this) = rhs; - return *this; - } + vecx(int size) : btVectorX(size) {} + const vecx& operator=(const btVectorX& rhs) { + *static_cast(this) = rhs; + return *this; + } - idScalar& operator()(int i) { return (*this)[i]; } - const idScalar& operator()(int i) const { return (*this)[i]; } + idScalar& operator()(int i) { return (*this)[i]; } + const idScalar& operator()(int i) const { return (*this)[i]; } - friend vecx operator*(const vecx& a, const idScalar& s); - friend vecx operator*(const idScalar& s, const vecx& a); + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); - friend vecx operator+(const vecx& a, const vecx& b); - friend vecx operator-(const vecx& a, const vecx& b); - friend vecx operator/(const vecx& a, const idScalar& s); + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); }; inline vecx operator*(const vecx& a, const idScalar& s) { - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { - result(i) = a(i) * s; - } - return result; + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) * s; + } + return result; } inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } inline vecx operator+(const vecx& a, const vecx& b) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result(i) = a(i) + b(i); - } + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result(i) = a(i) + b(i); + } - return result; + return result; } inline vecx operator-(const vecx& a, const vecx& b) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result(i) = a(i) - b(i); - } - return result; + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result(i) = a(i) - b(i); + } + return result; } inline vecx operator/(const vecx& a, const idScalar& s) { - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { - result(i) = a(i) / s; - } + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result(i) = a(i) / s; + } - return result; + return result; } } diff --git a/src/BulletInverseDynamics/details/IDMatVec.hpp b/src/BulletInverseDynamics/details/IDMatVec.hpp index a4e2ca5a4..de15e9ed4 100644 --- a/src/BulletInverseDynamics/details/IDMatVec.hpp +++ b/src/BulletInverseDynamics/details/IDMatVec.hpp @@ -17,312 +17,312 @@ class matxx; /// want from a "fully featured" linear math library. class vec3 { public: - idScalar& operator()(int i) { return m_data[i]; } - const idScalar& operator()(int i) const { return m_data[i]; } - const int size() const { return 3; } - const vec3& operator=(const vec3& rhs); - const vec3& operator+=(const vec3& b); - const vec3& operator-=(const vec3& b); - vec3 cross(const vec3& b) const; - idScalar dot(const vec3& b) const; + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int size() const { return 3; } + const vec3& operator=(const vec3& rhs); + const vec3& operator+=(const vec3& b); + const vec3& operator-=(const vec3& b); + vec3 cross(const vec3& b) const; + idScalar dot(const vec3& b) const; - friend vec3 operator*(const mat33& a, const vec3& b); - friend vec3 operator*(const vec3& a, const idScalar& s); - friend vec3 operator*(const idScalar& s, const vec3& a); + friend vec3 operator*(const mat33& a, const vec3& b); + friend vec3 operator*(const vec3& a, const idScalar& s); + friend vec3 operator*(const idScalar& s, const vec3& a); - friend vec3 operator+(const vec3& a, const vec3& b); - friend vec3 operator-(const vec3& a, const vec3& b); - friend vec3 operator/(const vec3& a, const idScalar& s); + friend vec3 operator+(const vec3& a, const vec3& b); + friend vec3 operator-(const vec3& a, const vec3& b); + friend vec3 operator/(const vec3& a, const idScalar& s); private: - idScalar m_data[3]; + idScalar m_data[3]; }; class mat33 { public: - idScalar& operator()(int i, int j) { return m_data[3 * i + j]; } - const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; } - const mat33& operator=(const mat33& rhs); - mat33 transpose() const; - const mat33& operator+=(const mat33& b); - const mat33& operator-=(const mat33& b); + idScalar& operator()(int i, int j) { return m_data[3 * i + j]; } + const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; } + const mat33& operator=(const mat33& rhs); + mat33 transpose() const; + const mat33& operator+=(const mat33& b); + const mat33& operator-=(const mat33& b); - friend mat33 operator*(const mat33& a, const mat33& b); - friend vec3 operator*(const mat33& a, const vec3& b); - friend mat33 operator*(const mat33& a, const idScalar& s); - friend mat33 operator*(const idScalar& s, const mat33& a); - friend mat33 operator+(const mat33& a, const mat33& b); - friend mat33 operator-(const mat33& a, const mat33& b); - friend mat33 operator/(const mat33& a, const idScalar& s); + friend mat33 operator*(const mat33& a, const mat33& b); + friend vec3 operator*(const mat33& a, const vec3& b); + friend mat33 operator*(const mat33& a, const idScalar& s); + friend mat33 operator*(const idScalar& s, const mat33& a); + friend mat33 operator+(const mat33& a, const mat33& b); + friend mat33 operator-(const mat33& a, const mat33& b); + friend mat33 operator/(const mat33& a, const idScalar& s); private: - // layout is [0,1,2;3,4,5;6,7,8] - idScalar m_data[9]; + // layout is [0,1,2;3,4,5;6,7,8] + idScalar m_data[9]; }; class vecx { public: - vecx(int size) : m_size(size) { - m_data = static_cast(idMalloc(sizeof(idScalar) * size)); - } - ~vecx() { idFree(m_data); } - const vecx& operator=(const vecx& rhs); - idScalar& operator()(int i) { return m_data[i]; } - const idScalar& operator()(int i) const { return m_data[i]; } - const int& size() const { return m_size; } + vecx(int size) : m_size(size) { + m_data = static_cast(idMalloc(sizeof(idScalar) * size)); + } + ~vecx() { idFree(m_data); } + const vecx& operator=(const vecx& rhs); + idScalar& operator()(int i) { return m_data[i]; } + const idScalar& operator()(int i) const { return m_data[i]; } + const int& size() const { return m_size; } - friend vecx operator*(const vecx& a, const idScalar& s); - friend vecx operator*(const idScalar& s, const vecx& a); + friend vecx operator*(const vecx& a, const idScalar& s); + friend vecx operator*(const idScalar& s, const vecx& a); - friend vecx operator+(const vecx& a, const vecx& b); - friend vecx operator-(const vecx& a, const vecx& b); - friend vecx operator/(const vecx& a, const idScalar& s); + friend vecx operator+(const vecx& a, const vecx& b); + friend vecx operator-(const vecx& a, const vecx& b); + friend vecx operator/(const vecx& a, const idScalar& s); private: - int m_size; - idScalar* m_data; + int m_size; + idScalar* m_data; }; class matxx { public: - matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { - m_data = static_cast(idMalloc(sizeof(idScalar) * rows * cols)); - } - ~matxx() { idFree(m_data); } - const matxx& operator=(const matxx& rhs); - idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; } - const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; } - const int& rows() const { return m_rows; } - const int& cols() const { return m_cols; } + matxx(int rows, int cols) : m_rows(rows), m_cols(cols) { + m_data = static_cast(idMalloc(sizeof(idScalar) * rows * cols)); + } + ~matxx() { idFree(m_data); } + const matxx& operator=(const matxx& rhs); + idScalar& operator()(int row, int col) { return m_data[row * m_rows + col]; } + const idScalar& operator()(int row, int col) const { return m_data[row * m_rows + col]; } + const int& rows() const { return m_rows; } + const int& cols() const { return m_cols; } private: - int m_rows; - int m_cols; - idScalar* m_data; + int m_rows; + int m_cols; + idScalar* m_data; }; inline const vec3& vec3::operator=(const vec3& rhs) { - if (&rhs != this) { - memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); - } - return *this; + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar)); + } + return *this; } inline vec3 vec3::cross(const vec3& b) const { - vec3 result; - result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1]; - result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2]; - result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; + vec3 result; + result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1]; + result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2]; + result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0]; - return result; + return result; } inline idScalar vec3::dot(const vec3& b) const { - return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2]; + return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2]; } inline const mat33& mat33::operator=(const mat33& rhs) { - if (&rhs != this) { - memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); - } - return *this; + if (&rhs != this) { + memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar)); + } + return *this; } inline mat33 mat33::transpose() const { - mat33 result; - result.m_data[0] = m_data[0]; - result.m_data[1] = m_data[3]; - result.m_data[2] = m_data[6]; - result.m_data[3] = m_data[1]; - result.m_data[4] = m_data[4]; - result.m_data[5] = m_data[7]; - result.m_data[6] = m_data[2]; - result.m_data[7] = m_data[5]; - result.m_data[8] = m_data[8]; + mat33 result; + result.m_data[0] = m_data[0]; + result.m_data[1] = m_data[3]; + result.m_data[2] = m_data[6]; + result.m_data[3] = m_data[1]; + result.m_data[4] = m_data[4]; + result.m_data[5] = m_data[7]; + result.m_data[6] = m_data[2]; + result.m_data[7] = m_data[5]; + result.m_data[8] = m_data[8]; - return result; + return result; } inline mat33 operator*(const mat33& a, const mat33& b) { - mat33 result; - result.m_data[0] = - a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6]; - result.m_data[1] = - a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; - result.m_data[2] = - a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; - result.m_data[3] = - a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; - result.m_data[4] = - a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; - result.m_data[5] = - a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; - result.m_data[6] = - a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; - result.m_data[7] = - a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; - result.m_data[8] = - a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; + mat33 result; + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6]; + result.m_data[1] = + a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7]; + result.m_data[2] = + a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8]; + result.m_data[3] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6]; + result.m_data[4] = + a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7]; + result.m_data[5] = + a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8]; + result.m_data[6] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6]; + result.m_data[7] = + a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7]; + result.m_data[8] = + a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8]; - return result; + return result; } inline const mat33& mat33::operator+=(const mat33& b) { - for (int i = 0; i < 9; i++) { - m_data[i] += b.m_data[i]; - } + for (int i = 0; i < 9; i++) { + m_data[i] += b.m_data[i]; + } - return *this; + return *this; } inline const mat33& mat33::operator-=(const mat33& b) { - for (int i = 0; i < 9; i++) { - m_data[i] -= b.m_data[i]; - } - return *this; + for (int i = 0; i < 9; i++) { + m_data[i] -= b.m_data[i]; + } + return *this; } inline vec3 operator*(const mat33& a, const vec3& b) { - vec3 result; + vec3 result; - result.m_data[0] = - a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; - result.m_data[1] = - a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; - result.m_data[2] = - a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; + result.m_data[0] = + a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2]; + result.m_data[1] = + a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2]; + result.m_data[2] = + a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2]; - return result; + return result; } inline const vec3& vec3::operator+=(const vec3& b) { - for (int i = 0; i < 3; i++) { - m_data[i] += b.m_data[i]; - } - return *this; + for (int i = 0; i < 3; i++) { + m_data[i] += b.m_data[i]; + } + return *this; } inline const vec3& vec3::operator-=(const vec3& b) { - for (int i = 0; i < 3; i++) { - m_data[i] -= b.m_data[i]; - } - return *this; + for (int i = 0; i < 3; i++) { + m_data[i] -= b.m_data[i]; + } + return *this; } inline mat33 operator*(const mat33& a, const idScalar& s) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] * s; - } - return result; + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; } inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } inline vec3 operator*(const vec3& a, const idScalar& s) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] * s; - } - return result; + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; } inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; } inline mat33 operator+(const mat33& a, const mat33& b) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } - return result; + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + return result; } inline vec3 operator+(const vec3& a, const vec3& b) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } - return result; + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } + return result; } inline mat33 operator-(const mat33& a, const mat33& b) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; } inline vec3 operator-(const vec3& a, const vec3& b) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; } inline mat33 operator/(const mat33& a, const idScalar& s) { - mat33 result; - for (int i = 0; i < 9; i++) { - result.m_data[i] = a.m_data[i] / s; - } - return result; + mat33 result; + for (int i = 0; i < 9; i++) { + result.m_data[i] = a.m_data[i] / s; + } + return result; } inline vec3 operator/(const vec3& a, const idScalar& s) { - vec3 result; - for (int i = 0; i < 3; i++) { - result.m_data[i] = a.m_data[i] / s; - } - return result; + vec3 result; + for (int i = 0; i < 3; i++) { + result.m_data[i] = a.m_data[i] / s; + } + return result; } inline const vecx& vecx::operator=(const vecx& rhs) { - if (size() != rhs.size()) { - error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); - abort(); - } - if (&rhs != this) { - memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); - } - return *this; + if (size() != rhs.size()) { + error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); + abort(); + } + if (&rhs != this) { + memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar)); + } + return *this; } inline vecx operator*(const vecx& a, const idScalar& s) { - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { - result.m_data[i] = a.m_data[i] * s; - } - return result; + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] * s; + } + return result; } inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } inline vecx operator+(const vecx& a, const vecx& b) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result.m_data[i] = a.m_data[i] + b.m_data[i]; - } + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result.m_data[i] = a.m_data[i] + b.m_data[i]; + } - return result; + return result; } inline vecx operator-(const vecx& a, const vecx& b) { - vecx result(a.size()); - // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); - abort(); - } - for (int i = 0; i < a.size(); i++) { - result.m_data[i] = a.m_data[i] - b.m_data[i]; - } - return result; + vecx result(a.size()); + // TODO: error handling for a.size() != b.size()?? + if (a.size() != b.size()) { + error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); + abort(); + } + for (int i = 0; i < a.size(); i++) { + result.m_data[i] = a.m_data[i] - b.m_data[i]; + } + return result; } inline vecx operator/(const vecx& a, const idScalar& s) { - vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { - result.m_data[i] = a.m_data[i] / s; - } + vecx result(a.size()); + for (int i = 0; i < result.size(); i++) { + result.m_data[i] = a.m_data[i] / s; + } - return result; + return result; } } #endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 211139459..06a3595be 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -3,482 +3,482 @@ namespace btInverseDynamics { MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_) - : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) { - m_body_list.resize(num_bodies_); - m_parent_index.resize(num_bodies_); - m_child_indices.resize(num_bodies_); - m_user_int.resize(num_bodies_); - m_user_ptr.resize(num_bodies_); + : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_) { + m_body_list.resize(num_bodies_); + m_parent_index.resize(num_bodies_); + m_child_indices.resize(num_bodies_); + m_user_int.resize(num_bodies_); + m_user_ptr.resize(num_bodies_); - m_world_gravity(0) = 0.0; - m_world_gravity(1) = 0.0; - m_world_gravity(2) = -9.8; + m_world_gravity(0) = 0.0; + m_world_gravity(1) = 0.0; + m_world_gravity(2) = -9.8; } const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const { - switch (type) { - case FIXED: - return "fixed"; - case REVOLUTE: - return "revolute"; - case PRISMATIC: - return "prismatic"; - case FLOATING: - return "floating"; - } - return "error: invalid"; + switch (type) { + case FIXED: + return "fixed"; + case REVOLUTE: + return "revolute"; + case PRISMATIC: + return "prismatic"; + case FLOATING: + return "floating"; + } + return "error: invalid"; } inline void indent(const int &level) { - for (int j = 0; j < level; j++) - id_printf(" "); // indent + for (int j = 0; j < level; j++) + id_printf(" "); // indent } void MultiBodyTree::MultiBodyImpl::printTree() { - id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); - printTree(0, 0); + id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type)); + printTree(0, 0); } void MultiBodyTree::MultiBodyImpl::printTreeData() { - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - id_printf("body: %d\n", static_cast(i)); - id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); - id_printf("q_index= %d\n", body.m_q_index); - id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); - id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + id_printf("body: %d\n", static_cast(i)); + id_printf("type: %s\n", jointTypeToString(body.m_joint_type)); + id_printf("q_index= %d\n", body.m_q_index); + id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2)); + id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2)); - id_printf("mass = %f\n", body.m_mass); - id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), - body.m_body_mass_com(2)); - id_printf("I_o= [%f %f %f;\n" - " %f %f %f;\n" - " %f %f %f]\n", - body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), - body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), - body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); + id_printf("mass = %f\n", body.m_mass); + id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1), + body.m_body_mass_com(2)); + id_printf("I_o= [%f %f %f;\n" + " %f %f %f;\n" + " %f %f %f]\n", + body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2), + body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2), + body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2)); - id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), - body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); - } + id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0), + body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2)); + } } int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { - switch (type) { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - } - error_message("unknown joint type %d\n", type); - return 0; + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + error_message("unknown joint type %d\n", type); + return 0; } void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation) { - // this is adapted from URDF2Bullet. - // TODO: fix this and print proper graph (similar to git --log --graph) - int num_children = m_child_indices[index].size(); + // this is adapted from URDF2Bullet. + // TODO: fix this and print proper graph (similar to git --log --graph) + int num_children = m_child_indices[index].size(); - indentation += 2; - int count = 0; + indentation += 2; + int count = 0; - for (int i = 0; i < num_children; i++) { - int child_index = m_child_indices[index][i]; - indent(indentation); - id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, - jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, - m_body_list[index].m_q_index, - m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); - // first grandchild - printTree(child_index, indentation); - } + for (int i = 0; i < num_children; i++) { + int child_index = m_child_indices[index][i]; + indent(indentation); + id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index, + jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1, + m_body_list[index].m_q_index, + m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type)); + // first grandchild + printTree(child_index, indentation); + } } int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity) { - m_world_gravity = gravity; - return 0; + m_world_gravity = gravity; + return 0; } int MultiBodyTree::MultiBodyImpl::generateIndexSets() { - m_body_revolute_list.resize(0); - m_body_prismatic_list.resize(0); - int q_index = 0; - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - body.m_q_index = -1; - switch (body.m_joint_type) { - case REVOLUTE: - m_body_revolute_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case PRISMATIC: - m_body_prismatic_list.push_back(i); - body.m_q_index = q_index; - q_index++; - break; - case FIXED: - // do nothing - break; - case FLOATING: - m_body_floating_list.push_back(i); - body.m_q_index = q_index; - q_index += 6; - break; - default: - error_message("unsupported joint type %d\n", body.m_joint_type); - return -1; - } - } - // sanity check - if (q_index != m_num_dofs) { - error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); - return -1; - } + m_body_revolute_list.resize(0); + m_body_prismatic_list.resize(0); + int q_index = 0; + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + body.m_q_index = -1; + switch (body.m_joint_type) { + case REVOLUTE: + m_body_revolute_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case PRISMATIC: + m_body_prismatic_list.push_back(i); + body.m_q_index = q_index; + q_index++; + break; + case FIXED: + // do nothing + break; + case FLOATING: + m_body_floating_list.push_back(i); + body.m_q_index = q_index; + q_index += 6; + break; + default: + error_message("unsupported joint type %d\n", body.m_joint_type); + return -1; + } + } + // sanity check + if (q_index != m_num_dofs) { + error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); + return -1; + } - m_child_indices.resize(m_body_list.size()); + m_child_indices.resize(m_body_list.size()); - for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { - const int &parent = m_parent_index[child]; - if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) { - m_child_indices[parent].push_back(child); - } else { - if (-1 == parent) { - // multiple bodies are directly linked to the environment, ie, not a single root - error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); - } else { - // should never happen - error_message( - "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", - child, parent, static_cast(m_parent_index.size())); - } - return -1; - } - } + for (idArrayIdx child = 1; child < m_parent_index.size(); child++) { + const int &parent = m_parent_index[child]; + if (parent >= 0 && parent < (static_cast(m_parent_index.size()) - 1)) { + m_child_indices[parent].push_back(child); + } else { + if (-1 == parent) { + // multiple bodies are directly linked to the environment, ie, not a single root + error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + } else { + // should never happen + error_message( + "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", + child, parent, static_cast(m_parent_index.size())); + } + return -1; + } + } - return 0; + return 0; } void MultiBodyTree::MultiBodyImpl::calculateStaticData() { - // relative kinematics that are not a function of q, u, dot_u - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - switch (body.m_joint_type) { - case REVOLUTE: - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - break; - case PRISMATIC: - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - break; - case FIXED: - body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; - body.m_body_T_parent = body.m_body_T_parent_ref; - body.m_body_ang_vel_rel(0) = 0; - body.m_body_ang_vel_rel(1) = 0; - body.m_body_ang_vel_rel(2) = 0; - body.m_parent_vel_rel(0) = 0; - body.m_parent_vel_rel(1) = 0; - body.m_parent_vel_rel(2) = 0; - body.m_body_ang_acc_rel(0) = 0; - body.m_body_ang_acc_rel(1) = 0; - body.m_body_ang_acc_rel(2) = 0; - body.m_parent_acc_rel(0) = 0; - body.m_parent_acc_rel(1) = 0; - body.m_parent_acc_rel(2) = 0; - break; - case FLOATING: - // no static data - break; - } - } + // relative kinematics that are not a function of q, u, dot_u + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + switch (body.m_joint_type) { + case REVOLUTE: + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + break; + case PRISMATIC: + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + break; + case FIXED: + body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref; + body.m_body_T_parent = body.m_body_T_parent_ref; + body.m_body_ang_vel_rel(0) = 0; + body.m_body_ang_vel_rel(1) = 0; + body.m_body_ang_vel_rel(2) = 0; + body.m_parent_vel_rel(0) = 0; + body.m_parent_vel_rel(1) = 0; + body.m_parent_vel_rel(2) = 0; + body.m_body_ang_acc_rel(0) = 0; + body.m_body_ang_acc_rel(1) = 0; + body.m_body_ang_acc_rel(2) = 0; + body.m_parent_acc_rel(0) = 0; + body.m_parent_acc_rel(1) = 0; + body.m_parent_acc_rel(2) = 0; + break; + case FLOATING: + // no static data + break; + } + } } int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u, - const vecx &dot_u, vecx *joint_forces) { - if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || - joint_forces->size() != m_num_dofs) { - error_message("wrong vector dimension. system has %d DOFs,\n" - "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", - m_num_dofs, static_cast(q.size()), static_cast(u.size()), - static_cast(dot_u.size()), static_cast(joint_forces->size())); - return -1; - } - // 1. update relative kinematics - // 1.1 for revolute - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - mat33 T; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); - body.m_body_T_parent = T * body.m_body_T_parent_ref; + const vecx &dot_u, vecx *joint_forces) { + if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || + joint_forces->size() != m_num_dofs) { + error_message("wrong vector dimension. system has %d DOFs,\n" + "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", + m_num_dofs, static_cast(q.size()), static_cast(u.size()), + static_cast(dot_u.size()), static_cast(joint_forces->size())); + return -1; + } + // 1. update relative kinematics + // 1.1 for revolute + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + mat33 T; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T); + body.m_body_T_parent = T * body.m_body_T_parent_ref; - // body.m_parent_r_parent_body= fixed - body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); - // body.m_parent_dot_r_rel = fixed; - // NOTE: this assumes that Jac_JR is constant, which is true for revolute - // joints, but not in the general case (eg, slider-crank type mechanisms) - body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); - // body.m_parent_ddot_r_rel = fixed; - } - // 1.2 for prismatic - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // body.m_body_T_parent= fixed - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - // body.m_parent_omega_rel = 0; - body.m_parent_vel_rel = - body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); - // body.parent_dot_omega_rel = 0; - // NOTE: this assumes that Jac_JT is constant, which is true for - // prismatic joints, but not in the general case - body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; + // body.m_parent_r_parent_body= fixed + body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index); + // body.m_parent_dot_r_rel = fixed; + // NOTE: this assumes that Jac_JR is constant, which is true for revolute + // joints, but not in the general case (eg, slider-crank type mechanisms) + body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index); + // body.m_parent_ddot_r_rel = fixed; + } + // 1.2 for prismatic + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // body.m_body_T_parent= fixed + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + // body.m_parent_omega_rel = 0; + body.m_parent_vel_rel = + body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index); + // body.parent_dot_omega_rel = 0; + // NOTE: this assumes that Jac_JT is constant, which is true for + // prismatic joints, but not in the general case + body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index); + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); - body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); - body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); + body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0); + body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1); + body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2); - body.m_parent_vel_rel(0) = u(body.m_q_index + 3); - body.m_parent_vel_rel(1) = u(body.m_q_index + 4); - body.m_parent_vel_rel(2) = u(body.m_q_index + 5); + body.m_parent_vel_rel(0) = u(body.m_q_index + 3); + body.m_parent_vel_rel(1) = u(body.m_q_index + 4); + body.m_parent_vel_rel(2) = u(body.m_q_index + 5); - body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); - body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); - body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); + body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0); + body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1); + body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2); - body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); - body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); - body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); + body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3); + body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4); + body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5); - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; - body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; - } + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel; + body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel; + } - // 3. absolute kinematic quantities & dynamic quantities - // NOTE: this should be optimized by specializing for different body types - // (e.g., relative rotation is always zero for prismatic joints, etc.) + // 3. absolute kinematic quantities & dynamic quantities + // NOTE: this should be optimized by specializing for different body types + // (e.g., relative rotation is always zero for prismatic joints, etc.) - // calculations for root body - { - RigidBody &body = m_body_list[0]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) - // not required right now, added here for debugging purposes - body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; - body.m_body_T_world = body.m_body_T_parent; + // calculations for root body + { + RigidBody &body = m_body_list[0]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) + // not required right now, added here for debugging purposes + body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body; + body.m_body_T_world = body.m_body_T_parent; - // 3.2 update absolute velocities - body.m_body_ang_vel = body.m_body_ang_vel_rel; - body.m_body_vel = body.m_parent_vel_rel; + // 3.2 update absolute velocities + body.m_body_ang_vel = body.m_body_ang_vel_rel; + body.m_body_vel = body.m_parent_vel_rel; - // 3.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = body.m_body_ang_acc_rel; - body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; - // add gravitational acceleration to root body - // this is an efficient way to add gravitational terms, - // but it does mean that the kinematics are no longer - // correct at the acceleration level - // NOTE: To get correct acceleration kinematics, just set world_gravity to zero - body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; - } + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = body.m_body_ang_acc_rel; + body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel; + // add gravitational acceleration to root body + // this is an efficient way to add gravitational terms, + // but it does mean that the kinematics are no longer + // correct at the acceleration level + // NOTE: To get correct acceleration kinematics, just set world_gravity to zero + body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity; + } - for (idArrayIdx i = 1; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - RigidBody &parent = m_body_list[m_parent_index[i]]; - // 3.1 update absolute positions and orientations: - // will be required if we add force elements (eg springs between bodies, - // or contacts) not required right now added here for debugging purposes - body.m_body_pos = - body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); - body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; + for (idArrayIdx i = 1; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + RigidBody &parent = m_body_list[m_parent_index[i]]; + // 3.1 update absolute positions and orientations: + // will be required if we add force elements (eg springs between bodies, + // or contacts) not required right now added here for debugging purposes + body.m_body_pos = + body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body); + body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world; - // 3.2 update absolute velocities - body.m_body_ang_vel = - body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; + // 3.2 update absolute velocities + body.m_body_ang_vel = + body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel; - body.m_body_vel = - body.m_body_T_parent * - (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + - body.m_parent_vel_rel); + body.m_body_vel = + body.m_body_T_parent * + (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) + + body.m_parent_vel_rel); - // 3.3 update absolute accelerations - // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints - body.m_body_ang_acc = - body.m_body_T_parent * parent.m_body_ang_acc - - body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + - body.m_body_ang_acc_rel; - body.m_body_acc = - body.m_body_T_parent * - (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + - parent.m_body_ang_vel.cross( - parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + - 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); - } + // 3.3 update absolute accelerations + // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints + body.m_body_ang_acc = + body.m_body_T_parent * parent.m_body_ang_acc - + body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) + + body.m_body_ang_acc_rel; + body.m_body_acc = + body.m_body_T_parent * + (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) + + parent.m_body_ang_vel.cross( + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) + + 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel); + } - for (idArrayIdx i = 0; i < m_body_list.size(); i++) { - RigidBody &body = m_body_list[i]; - // 3.4 update dynamic terms (rate of change of angular & linear momentum) - body.m_eom_lhs_rotational = - body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + - body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - - body.m_body_moment_user; - body.m_eom_lhs_translational = - body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + - body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - - body.m_body_force_user; - } + for (idArrayIdx i = 0; i < m_body_list.size(); i++) { + RigidBody &body = m_body_list[i]; + // 3.4 update dynamic terms (rate of change of angular & linear momentum) + body.m_eom_lhs_rotational = + body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) + + body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) - + body.m_body_moment_user; + body.m_eom_lhs_translational = + body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc + + body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) - + body.m_body_force_user; + } - // 4. calculate full set of forces at parent joint - // (not directly calculating the joint force along the free direction - // simplifies inclusion of fixed joints. - // An alternative would be to fuse bodies in a pre-processing step, - // but that would make changing masses online harder (eg, payload masses - // added with fixed joints to a gripper) - // Also, this enables adding zero weight bodies as a way to calculate frame poses - // for force elements, etc. + // 4. calculate full set of forces at parent joint + // (not directly calculating the joint force along the free direction + // simplifies inclusion of fixed joints. + // An alternative would be to fuse bodies in a pre-processing step, + // but that would make changing masses online harder (eg, payload masses + // added with fixed joints to a gripper) + // Also, this enables adding zero weight bodies as a way to calculate frame poses + // for force elements, etc. - for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { - // sum of forces and moments acting on this body from its children - vec3 sum_f_children; - vec3 sum_m_children; - setZero(sum_f_children); - setZero(sum_m_children); - for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); - child_list_idx++) { - const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; - vec3 child_joint_force_in_this_frame = - child.m_body_T_parent.transpose() * child.m_force_at_joint; - sum_f_children -= child_joint_force_in_this_frame; - sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + - child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); - } - RigidBody &body = m_body_list[body_idx]; + for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--) { + // sum of forces and moments acting on this body from its children + vec3 sum_f_children; + vec3 sum_m_children; + setZero(sum_f_children); + setZero(sum_m_children); + for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size(); + child_list_idx++) { + const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]]; + vec3 child_joint_force_in_this_frame = + child.m_body_T_parent.transpose() * child.m_force_at_joint; + sum_f_children -= child_joint_force_in_this_frame; + sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint + + child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame); + } + RigidBody &body = m_body_list[body_idx]; - body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; - body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; - } + body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children; + body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children; + } - // 4. Calculate Joint forces. - // These are the components of force_at_joint/moment_at_joint - // in the free directions given by Jac_JT/Jac_JR - // 4.1 revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); - } - // 4.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; - (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); - } - // 4.3 floating bodies (6-DoF joints) - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; - (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); - (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); - (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); + // 4. Calculate Joint forces. + // These are the components of force_at_joint/moment_at_joint + // in the free directions given by Jac_JT/Jac_JR + // 4.1 revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint); + } + // 4.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint; + (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint); + } + // 4.3 floating bodies (6-DoF joints) + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; + (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0); + (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1); + (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2); - (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); - (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); - (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); - } + (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0); + (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1); + (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2); + } - return 0; + return 0; } static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT) { - switch (dof) { - // rotational part - case 0: - Jac_JR(0) = 1; - Jac_JR(1) = 0; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 1: - Jac_JR(0) = 0; - Jac_JR(1) = 1; - Jac_JR(2) = 0; - setZero(Jac_JT); - break; - case 2: - Jac_JR(0) = 0; - Jac_JR(1) = 0; - Jac_JR(2) = 1; - setZero(Jac_JT); - break; - // translational part - case 3: - setZero(Jac_JR); - Jac_JT(0) = 1; - Jac_JT(1) = 0; - Jac_JT(2) = 0; - break; - case 4: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 1; - Jac_JT(2) = 0; - break; - case 5: - setZero(Jac_JR); - Jac_JT(0) = 0; - Jac_JT(1) = 0; - Jac_JT(2) = 1; - break; - } + switch (dof) { + // rotational part + case 0: + Jac_JR(0) = 1; + Jac_JR(1) = 0; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 1: + Jac_JR(0) = 0; + Jac_JR(1) = 1; + Jac_JR(2) = 0; + setZero(Jac_JT); + break; + case 2: + Jac_JR(0) = 0; + Jac_JR(1) = 0; + Jac_JR(2) = 1; + setZero(Jac_JT); + break; + // translational part + case 3: + setZero(Jac_JR); + Jac_JT(0) = 1; + Jac_JT(1) = 0; + Jac_JT(2) = 0; + break; + case 4: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 1; + Jac_JT(2) = 0; + break; + case 5: + setZero(Jac_JR); + Jac_JT(0) = 0; + Jac_JT(1) = 0; + Jac_JT(2) = 1; + break; + } } static inline int jointNumDoFs(const JointType &type) { - switch (type) { - case FIXED: - return 0; - case REVOLUTE: - case PRISMATIC: - return 1; - case FLOATING: - return 6; - } - // this should never happen - error_message("invalid joint type\n"); - // TODO add configurable abort/crash function - abort(); + switch (type) { + case FIXED: + return 0; + case REVOLUTE: + case PRISMATIC: + return 1; + case FLOATING: + return 6; + } + // this should never happen + error_message("invalid joint type\n"); + // TODO add configurable abort/crash function + abort(); } int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics, - const bool initialize_matrix, - const bool set_lower_triangular_matrix, - matxx *mass_matrix) { + const bool initialize_matrix, + const bool set_lower_triangular_matrix, + matxx *mass_matrix) { // This calculates the joint space mass matrix for the multibody system. // The algorithm is essentially an implementation of "method 3" // in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982) @@ -494,351 +494,351 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool #define setMassMatrixElem(row, col, val) (*mass_matrix)(row, col) = val #endif - if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || - mass_matrix->cols() != m_num_dofs) { - error_message("Dimension error. System has %d DOFs,\n" - "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", - m_num_dofs, static_cast(q.size()), static_cast(mass_matrix->rows()), - static_cast(mass_matrix->cols())); - return -1; - } + if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || + mass_matrix->cols() != m_num_dofs) { + error_message("Dimension error. System has %d DOFs,\n" + "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", + m_num_dofs, static_cast(q.size()), static_cast(mass_matrix->rows()), + static_cast(mass_matrix->cols())); + return -1; + } - // TODO add optimized zeroing function? - if (initialize_matrix) { - for (int i = 0; i < m_num_dofs; i++) { - for (int j = 0; j < m_num_dofs; j++) { - setMassMatrixElem(i, j, 0.0); - } - } - } + // TODO add optimized zeroing function? + if (initialize_matrix) { + for (int i = 0; i < m_num_dofs; i++) { + for (int j = 0; j < m_num_dofs; j++) { + setMassMatrixElem(i, j, 0.0); + } + } + } - if (update_kinematics) { - // 1. update relative kinematics - // 1.1 for revolute joints - for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { - RigidBody &body = m_body_list[m_body_revolute_list[i]]; - // from reference orientation (q=0) of body-fixed frame to current orientation - mat33 body_T_body_ref; - bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); - body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; - } - // 1.2 for prismatic joints - for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { - RigidBody &body = m_body_list[m_body_prismatic_list[i]]; - // body.m_body_T_parent= fixed - body.m_parent_pos_parent_body = - body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); - } - // 1.3 fixed joints: nothing to do - // 1.4 6dof joints: - for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { - RigidBody &body = m_body_list[m_body_floating_list[i]]; + if (update_kinematics) { + // 1. update relative kinematics + // 1.1 for revolute joints + for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++) { + RigidBody &body = m_body_list[m_body_revolute_list[i]]; + // from reference orientation (q=0) of body-fixed frame to current orientation + mat33 body_T_body_ref; + bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref); + body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref; + } + // 1.2 for prismatic joints + for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++) { + RigidBody &body = m_body_list[m_body_prismatic_list[i]]; + // body.m_body_T_parent= fixed + body.m_parent_pos_parent_body = + body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index); + } + // 1.3 fixed joints: nothing to do + // 1.4 6dof joints: + for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++) { + RigidBody &body = m_body_list[m_body_floating_list[i]]; - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * - transformX(q(body.m_q_index)); - body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); - body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); - body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); + body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * + transformY(q(body.m_q_index + 1)) * + transformX(q(body.m_q_index)); + body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3); + body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4); + body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5); - body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; - } - } - for (int i = m_body_list.size() - 1; i >= 0; i--) { - RigidBody &body = m_body_list[i]; - // calculate mass, center of mass and inertia of "composite rigid body", - // ie, sub-tree starting at current body - body.m_subtree_mass = body.m_mass; - body.m_body_subtree_mass_com = body.m_body_mass_com; - body.m_body_subtree_I_body = body.m_body_I_body; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + } + } + for (int i = m_body_list.size() - 1; i >= 0; i--) { + RigidBody &body = m_body_list[i]; + // calculate mass, center of mass and inertia of "composite rigid body", + // ie, sub-tree starting at current body + body.m_subtree_mass = body.m_mass; + body.m_body_subtree_mass_com = body.m_body_mass_com; + body.m_body_subtree_I_body = body.m_body_I_body; - for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { - RigidBody &child = m_body_list[m_child_indices[i][c]]; - mat33 body_T_child = child.m_body_T_parent.transpose(); + for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++) { + RigidBody &child = m_body_list[m_child_indices[i][c]]; + mat33 body_T_child = child.m_body_T_parent.transpose(); - body.m_subtree_mass += child.m_subtree_mass; - body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + - child.m_parent_pos_parent_body * child.m_subtree_mass; - body.m_body_subtree_I_body += - body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; + body.m_subtree_mass += child.m_subtree_mass; + body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com + + child.m_parent_pos_parent_body * child.m_subtree_mass; + body.m_body_subtree_I_body += + body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent; - if (child.m_subtree_mass > 0) { - // Shift the reference point for the child subtree inertia using the - // Huygens-Steiner ("parallel axis") theorem. - // (First shift from child origin to child com, then from there to this body's - // origin) - vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; - mat33 tilde_r_child_com = tildeOperator(r_com); - mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); - body.m_body_subtree_I_body += - child.m_subtree_mass * - (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); - } - } - } + if (child.m_subtree_mass > 0) { + // Shift the reference point for the child subtree inertia using the + // Huygens-Steiner ("parallel axis") theorem. + // (First shift from child origin to child com, then from there to this body's + // origin) + vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass; + mat33 tilde_r_child_com = tildeOperator(r_com); + mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com); + body.m_body_subtree_I_body += + child.m_subtree_mass * + (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com); + } + } + } - for (int i = m_body_list.size() - 1; i >= 0; i--) { - const RigidBody &body = m_body_list[i]; + for (int i = m_body_list.size() - 1; i >= 0; i--) { + const RigidBody &body = m_body_list[i]; - // determine DoF-range for body - const int q_index_min = body.m_q_index; - const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; - // loop over the DoFs used by this body - // local joint jacobians (ok as is for 1-DoF joints) - vec3 Jac_JR = body.m_Jac_JR; - vec3 Jac_JT = body.m_Jac_JT; - for (int col = q_index_max; col >= q_index_min; col--) { - // set jacobians for 6-DoF joints - if (FLOATING == body.m_joint_type) { - setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); - } + // determine DoF-range for body + const int q_index_min = body.m_q_index; + const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1; + // loop over the DoFs used by this body + // local joint jacobians (ok as is for 1-DoF joints) + vec3 Jac_JR = body.m_Jac_JR; + vec3 Jac_JT = body.m_Jac_JT; + for (int col = q_index_max; col >= q_index_min; col--) { + // set jacobians for 6-DoF joints + if (FLOATING == body.m_joint_type) { + setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT); + } - vec3 body_eom_rot = - body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); - vec3 body_eom_trans = - body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); - setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans)); + vec3 body_eom_rot = + body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT); + vec3 body_eom_trans = + body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR); + setMassMatrixElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans)); - // rest of the mass matrix column upwards - { - // 1. for multi-dof joints, rest of the dofs of this body - for (int row = col - 1; row >= q_index_min; row--) { - if (FLOATING != body.m_joint_type) { - error_message("??\n"); - return -1; - } - setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMassMatrixElem(col, row, Mrc); - } - // 2. ancestor dofs - int child_idx = i; - int parent_idx = m_parent_index[i]; - while (parent_idx >= 0) { - const RigidBody &child_body = m_body_list[child_idx]; - const RigidBody &parent_body = m_body_list[parent_idx]; + // rest of the mass matrix column upwards + { + // 1. for multi-dof joints, rest of the dofs of this body + for (int row = col - 1; row >= q_index_min; row--) { + if (FLOATING != body.m_joint_type) { + error_message("??\n"); + return -1; + } + setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMassMatrixElem(col, row, Mrc); + } + // 2. ancestor dofs + int child_idx = i; + int parent_idx = m_parent_index[i]; + while (parent_idx >= 0) { + const RigidBody &child_body = m_body_list[child_idx]; + const RigidBody &parent_body = m_body_list[parent_idx]; - const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); - body_eom_rot = parent_T_child * body_eom_rot; - body_eom_trans = parent_T_child * body_eom_trans; - body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); + const mat33 parent_T_child = child_body.m_body_T_parent.transpose(); + body_eom_rot = parent_T_child * body_eom_rot; + body_eom_trans = parent_T_child * body_eom_trans; + body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans); - const int parent_body_q_index_min = parent_body.m_q_index; - const int parent_body_q_index_max = - parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; - vec3 Jac_JR = parent_body.m_Jac_JR; - vec3 Jac_JT = parent_body.m_Jac_JT; - for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { - // set jacobians for 6-DoF joints - if (FLOATING == parent_body.m_joint_type) { - setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); - } - const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); - setMassMatrixElem(col, row, Mrc); - } + const int parent_body_q_index_min = parent_body.m_q_index; + const int parent_body_q_index_max = + parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1; + vec3 Jac_JR = parent_body.m_Jac_JR; + vec3 Jac_JT = parent_body.m_Jac_JT; + for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--) { + // set jacobians for 6-DoF joints + if (FLOATING == parent_body.m_joint_type) { + setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT); + } + const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans); + setMassMatrixElem(col, row, Mrc); + } - child_idx = parent_idx; - parent_idx = m_parent_index[child_idx]; - } - } - } - } + child_idx = parent_idx; + parent_idx = m_parent_index[child_idx]; + } + } + } + } - if (set_lower_triangular_matrix) { - for (int col = 0; col < m_num_dofs; col++) { - for (int row = 0; row < col; row++) { - setMassMatrixElem(row, col, (*mass_matrix)(col, row)); - } - } - } + if (set_lower_triangular_matrix) { + for (int col = 0; col < m_num_dofs; col++) { + for (int row = 0; row < col; row++) { + setMassMatrixElem(row, col, (*mass_matrix)(col, row)); + } + } + } #undef setMassMatrixElem - return 0; + return 0; } // utility macro -#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ - do { \ - if (index < 0 || index >= m_num_bodies) { \ - error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ - return -1; \ - } \ - } while (0) +#define CHECK_IF_BODY_INDEX_IS_VALID(index) \ + do { \ + if (index < 0 || index >= m_num_bodies) { \ + error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ + return -1; \ + } \ + } while (0) int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *p = m_parent_index[body_index]; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *p = m_parent_index[body_index]; + return 0; } int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *user_int = m_user_int[body_index]; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_int = m_user_int[body_index]; + return 0; } int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *user_ptr = m_user_ptr[body_index]; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *user_ptr = m_user_ptr[body_index]; + return 0; } int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_user_int[body_index] = user_int; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_int[body_index] = user_int; + return 0; } int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_user_ptr[body_index] = user_ptr; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_user_ptr[body_index] = user_ptr; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_origin = body.m_body_T_world.transpose() * body.m_body_pos; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - if (body.m_mass > 0) { - *world_com = body.m_body_T_world.transpose() * - (body.m_body_pos + body.m_body_mass_com / body.m_mass); - } else { - *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); - } - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + if (body.m_mass > 0) { + *world_com = body.m_body_T_world.transpose() * + (body.m_body_pos + body.m_body_mass_com / body.m_mass); + } else { + *world_com = body.m_body_T_world.transpose() * (body.m_body_pos); + } + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_T_body = body.m_body_T_world.transpose(); - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_T_body = body.m_body_T_world.transpose(); + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index, - vec3 *world_velocity) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; - return 0; + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index, - vec3 *world_velocity) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - vec3 com; - if (body.m_mass > 0) { - com = body.m_body_mass_com / body.m_mass; - } else { - com(0) = 0; - com(1) = 0; - com(2) = 0; - } + vec3 *world_velocity) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + vec3 com; + if (body.m_mass > 0) { + com = body.m_body_mass_com / body.m_mass; + } else { + com(0) = 0; + com(1) = 0; + com(2) = 0; + } - *world_velocity = - body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); - return 0; + *world_velocity = + body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com)); + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index, - vec3 *world_dot_omega) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; - return 0; + vec3 *world_dot_omega) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index, - vec3 *world_acceleration) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - const RigidBody &body = m_body_list[body_index]; - *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; - return 0; + vec3 *world_acceleration) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + const RigidBody &body = m_body_list[body_index]; + *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc; + return 0; } int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *joint_type = m_body_list[body_index].m_joint_type; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = m_body_list[body_index].m_joint_type; + return 0; } int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index, - const char **joint_type) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); - return 0; + const char **joint_type) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type); + return 0; } int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *q_index = m_body_list[body_index].m_q_index; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *q_index = m_body_list[body_index].m_q_index; + return 0; } int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_mass = mass; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_mass = mass; + return 0; } int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index, - const vec3& first_mass_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_mass_com = first_mass_moment; - return 0; + const vec3& first_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_mass_com = first_mass_moment; + return 0; } int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index, - const mat33& second_mass_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_I_body = second_mass_moment; - return 0; + const mat33& second_mass_moment) { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_I_body = second_mass_moment; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *mass = m_body_list[body_index].m_mass; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *mass = m_body_list[body_index].m_mass; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index, - vec3 *first_mass_moment) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *first_mass_moment = m_body_list[body_index].m_body_mass_com; - return 0; + vec3 *first_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *first_mass_moment = m_body_list[body_index].m_body_mass_com; + return 0; } int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index, - mat33 *second_mass_moment) const { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - *second_mass_moment = m_body_list[body_index].m_body_I_body; - return 0; + mat33 *second_mass_moment) const { + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + *second_mass_moment = m_body_list[body_index].m_body_I_body; + return 0; } void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments() { - for (int index = 0; index < m_num_bodies; index++) { - RigidBody &body = m_body_list[index]; - setZero(body.m_body_force_user); - setZero(body.m_body_moment_user); - } + for (int index = 0; index < m_num_bodies; index++) { + RigidBody &body = m_body_list[index]; + setZero(body.m_body_force_user); + setZero(body.m_body_moment_user); + } } int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_force_user += body_force; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_force_user += body_force; + return 0; } int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment) { - CHECK_IF_BODY_INDEX_IS_VALID(body_index); - m_body_list[body_index].m_body_moment_user += body_moment; - return 0; + CHECK_IF_BODY_INDEX_IS_VALID(body_index); + m_body_list[body_index].m_body_moment_user += body_moment; + return 0; } } diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp index 286b19d26..7787a4665 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp @@ -14,223 +14,223 @@ namespace btInverseDynamics { /// all vectors and matrices are in body-fixed frame, if not indicated otherwise. /// The body-fixed frame is located in the joint connecting the body to its parent. struct RigidBody { - ID_DECLARE_ALIGNED_ALLOCATOR(); - // 1 Inertial properties - /// Mass - idScalar m_mass; - /// Mass times center of gravity in body-fixed frame - vec3 m_body_mass_com; - /// Moment of inertia w.r.t. body-fixed frame - mat33 m_body_I_body; + ID_DECLARE_ALIGNED_ALLOCATOR(); + // 1 Inertial properties + /// Mass + idScalar m_mass; + /// Mass times center of gravity in body-fixed frame + vec3 m_body_mass_com; + /// Moment of inertia w.r.t. body-fixed frame + mat33 m_body_I_body; - // 2 dynamic properties - /// Left-hand side of the body equation of motion, translational part - vec3 m_eom_lhs_translational; - /// Left-hand side of the body equation of motion, rotational part - vec3 m_eom_lhs_rotational; - /// Force acting at the joint when the body is cut from its parent; - /// includes impressed joint force in J_JT direction, - /// as well as constraint force, - /// in body-fixed frame - vec3 m_force_at_joint; - /// Moment acting at the joint when the body is cut from its parent; - /// includes impressed joint moment in J_JR direction, and constraint moment - /// in body-fixed frame - vec3 m_moment_at_joint; - /// external (user provided) force acting at the body-fixed frame's origin, written in that - /// frame - vec3 m_body_force_user; - /// external (user provided) moment acting at the body-fixed frame's origin, written in that - /// frame - vec3 m_body_moment_user; - // 3 absolute kinematic properties - /// Position of body-fixed frame relative to world frame - /// this is currently only for debugging purposes - vec3 m_body_pos; - /// Absolute velocity of body-fixed frame - vec3 m_body_vel; - /// Absolute acceleration of body-fixed frame - /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational - /// acceleration! - vec3 m_body_acc; - /// Absolute angular velocity - vec3 m_body_ang_vel; - /// Absolute angular acceleration - /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational - /// acceleration! - vec3 m_body_ang_acc; + // 2 dynamic properties + /// Left-hand side of the body equation of motion, translational part + vec3 m_eom_lhs_translational; + /// Left-hand side of the body equation of motion, rotational part + vec3 m_eom_lhs_rotational; + /// Force acting at the joint when the body is cut from its parent; + /// includes impressed joint force in J_JT direction, + /// as well as constraint force, + /// in body-fixed frame + vec3 m_force_at_joint; + /// Moment acting at the joint when the body is cut from its parent; + /// includes impressed joint moment in J_JR direction, and constraint moment + /// in body-fixed frame + vec3 m_moment_at_joint; + /// external (user provided) force acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_force_user; + /// external (user provided) moment acting at the body-fixed frame's origin, written in that + /// frame + vec3 m_body_moment_user; + // 3 absolute kinematic properties + /// Position of body-fixed frame relative to world frame + /// this is currently only for debugging purposes + vec3 m_body_pos; + /// Absolute velocity of body-fixed frame + vec3 m_body_vel; + /// Absolute acceleration of body-fixed frame + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_acc; + /// Absolute angular velocity + vec3 m_body_ang_vel; + /// Absolute angular acceleration + /// NOTE: if gravitational acceleration is not zero, this is the accelation PLUS gravitational + /// acceleration! + vec3 m_body_ang_acc; - // 4 relative kinematic properties. - // these are in the parent body frame - /// Transform from world to body-fixed frame; - /// this is currently only for debugging purposes - mat33 m_body_T_world; - /// Transform from parent to body-fixed frame - mat33 m_body_T_parent; - /// Vector from parent to child frame in parent frame - vec3 m_parent_pos_parent_body; - /// Relative angular velocity - vec3 m_body_ang_vel_rel; - /// Relative linear velocity - vec3 m_parent_vel_rel; - /// Relative angular acceleration - vec3 m_body_ang_acc_rel; - /// Relative linear acceleration - vec3 m_parent_acc_rel; + // 4 relative kinematic properties. + // these are in the parent body frame + /// Transform from world to body-fixed frame; + /// this is currently only for debugging purposes + mat33 m_body_T_world; + /// Transform from parent to body-fixed frame + mat33 m_body_T_parent; + /// Vector from parent to child frame in parent frame + vec3 m_parent_pos_parent_body; + /// Relative angular velocity + vec3 m_body_ang_vel_rel; + /// Relative linear velocity + vec3 m_parent_vel_rel; + /// Relative angular acceleration + vec3 m_body_ang_acc_rel; + /// Relative linear acceleration + vec3 m_parent_acc_rel; - // 5 Data describing the joint type and geometry - /// Type of joint - JointType m_joint_type; - /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame - /// Components are in body-fixed frame of the parent - vec3 m_parent_pos_parent_body_ref; - /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame - mat33 m_body_T_parent_ref; - /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute - /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. - /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. - /// (NOTE: dimensions will have to be dynamic for additional joint types!) - vec3 m_Jac_JR; - /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute - /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. - /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. - /// (NOTE: dimensions might have to be dynamic for additional joint types!) - vec3 m_Jac_JT; - /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT - vec3 m_parent_Jac_JT; - /// Start of index range for the position degree(s) of freedom describing this body's motion - /// relative to - /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. - int m_q_index; + // 5 Data describing the joint type and geometry + /// Type of joint + JointType m_joint_type; + /// Position of joint frame (body-fixed frame at q=0) relative to the parent frame + /// Components are in body-fixed frame of the parent + vec3 m_parent_pos_parent_body_ref; + /// Orientation of joint frame (body-fixed frame at q=0) relative to the parent frame + mat33 m_body_T_parent_ref; + /// Joint rotational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// angular velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For revolute joints this is the joint axis, for prismatic joints it is a null matrix. + /// (NOTE: dimensions will have to be dynamic for additional joint types!) + vec3 m_Jac_JR; + /// Joint translational Jacobian, ie, the partial derivative of the body-fixed frames absolute + /// linear velocity w.r.t. the generalized velocity of this body's relative degree of freedom. + /// For prismatic joints this is the joint axis, for revolute joints it is a null matrix. + /// (NOTE: dimensions might have to be dynamic for additional joint types!) + vec3 m_Jac_JT; + /// m_Jac_JT in the parent frame, it, m_body_T_parent_ref.transpose()*m_Jac_JT + vec3 m_parent_Jac_JT; + /// Start of index range for the position degree(s) of freedom describing this body's motion + /// relative to + /// its parent. The indices are wrt the multibody system's q-vector of generalized coordinates. + int m_q_index; - // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" - /// mass of the subtree rooted in this body - idScalar m_subtree_mass; - /// center of mass * mass for subtree rooted in this body, in body-fixed frame - vec3 m_body_subtree_mass_com; - /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame - mat33 m_body_subtree_I_body; + // 6 Scratch data for mass matrix computation using "composite rigid body algorithm" + /// mass of the subtree rooted in this body + idScalar m_subtree_mass; + /// center of mass * mass for subtree rooted in this body, in body-fixed frame + vec3 m_body_subtree_mass_com; + /// moment of inertia of subtree rooted in this body, w.r.t. body origin, in body-fixed frame + mat33 m_body_subtree_I_body; }; /// The MBS implements a tree structured multibody system class MultiBodyTree::MultiBodyImpl { - friend class MultiBodyTree; + friend class MultiBodyTree; public: - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); - /// constructor - /// @param num_bodies the number of bodies in the system - /// @param num_dofs number of degrees of freedom in the system - MultiBodyImpl(int num_bodies_, int num_dofs_); + /// constructor + /// @param num_bodies the number of bodies in the system + /// @param num_dofs number of degrees of freedom in the system + MultiBodyImpl(int num_bodies_, int num_dofs_); - /// \copydoc MultiBodyTree::calculateInverseDynamics - int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, - vecx* joint_forces); - ///\copydoc MultiBodyTree::calculateMassMatrix - int calculateMassMatrix(const vecx& q, const bool update_kinematics, - const bool initialize_matrix, const bool set_lower_triangular_matrix, - matxx* mass_matrix); - /// generate additional index sets from the parent_index array - /// @return -1 on error, 0 on success - int generateIndexSets(); - /// set gravity acceleration in world frame - /// @param gravity gravity vector in the world frame - /// @return 0 on success, -1 on error - int setGravityInWorldFrame(const vec3& gravity); - /// pretty print tree - void printTree(); - /// print tree data - void printTreeData(); - /// initialize fixed data - void calculateStaticData(); - /// \copydoc MultiBodyTree::getBodyFrame - int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; - /// \copydoc MultiBodyTree::getParentIndex - int getParentIndex(const int body_index, int* m_parent_index); - /// \copydoc MultiBodyTree::getJointType - int getJointType(const int body_index, JointType* joint_type) const; - /// \copydoc MultiBodyTree::getJointTypeStr - int getJointTypeStr(const int body_index, const char** joint_type) const; - /// \copydoc MultiBodyTree:getDoFOffset - int getDoFOffset(const int body_index, int* q_index) const; - /// \copydoc MultiBodyTree::getBodyOrigin - int getBodyOrigin(const int body_index, vec3* world_origin) const; - /// \copydoc MultiBodyTree::getBodyCoM - int getBodyCoM(const int body_index, vec3* world_com) const; - /// \copydoc MultiBodyTree::getBodyTransform - int getBodyTransform(const int body_index, mat33* world_T_body) const; - /// \copydoc MultiBodyTree::getBodyAngularVelocity - int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; - /// \copydoc MultiBodyTree::getBodyLinearVelocity - int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; - /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM - int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; - /// \copydoc MultiBodyTree::getBodyAngularAcceleration - int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; - /// \copydoc MultiBodyTree::getBodyLinearAcceleration - int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; - /// \copydoc MultiBodyTree::getUserInt - int getUserInt(const int body_index, int* user_int) const; - /// \copydoc MultiBodyTree::getUserPtr - int getUserPtr(const int body_index, void** user_ptr) const; - /// \copydoc MultiBodyTree::setUserInt - int setUserInt(const int body_index, const int user_int); - /// \copydoc MultiBodyTree::setUserPtr - int setUserPtr(const int body_index, void* const user_ptr); - ///\copydoc MultiBodytTree::setBodyMass - int setBodyMass(const int body_index, const idScalar mass); - ///\copydoc MultiBodytTree::setBodyFirstMassMoment - int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); - ///\copydoc MultiBodytTree::setBodySecondMassMoment - int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); - ///\copydoc MultiBodytTree::getBodyMass - int getBodyMass(const int body_index, idScalar* mass) const; - ///\copydoc MultiBodytTree::getBodyFirstMassMoment - int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; - ///\copydoc MultiBodytTree::getBodySecondMassMoment - int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; - /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments - void clearAllUserForcesAndMoments(); - /// \copydoc MultiBodyTree::addUserForce - int addUserForce(const int body_index, const vec3& body_force); - /// \copydoc MultiBodyTree::addUserMoment - int addUserMoment(const int body_index, const vec3& body_moment); + /// \copydoc MultiBodyTree::calculateInverseDynamics + int calculateInverseDynamics(const vecx& q, const vecx& u, const vecx& dot_u, + vecx* joint_forces); + ///\copydoc MultiBodyTree::calculateMassMatrix + int calculateMassMatrix(const vecx& q, const bool update_kinematics, + const bool initialize_matrix, const bool set_lower_triangular_matrix, + matxx* mass_matrix); + /// generate additional index sets from the parent_index array + /// @return -1 on error, 0 on success + int generateIndexSets(); + /// set gravity acceleration in world frame + /// @param gravity gravity vector in the world frame + /// @return 0 on success, -1 on error + int setGravityInWorldFrame(const vec3& gravity); + /// pretty print tree + void printTree(); + /// print tree data + void printTreeData(); + /// initialize fixed data + void calculateStaticData(); + /// \copydoc MultiBodyTree::getBodyFrame + int getBodyFrame(const int index, vec3* world_origin, mat33* body_T_world) const; + /// \copydoc MultiBodyTree::getParentIndex + int getParentIndex(const int body_index, int* m_parent_index); + /// \copydoc MultiBodyTree::getJointType + int getJointType(const int body_index, JointType* joint_type) const; + /// \copydoc MultiBodyTree::getJointTypeStr + int getJointTypeStr(const int body_index, const char** joint_type) const; + /// \copydoc MultiBodyTree:getDoFOffset + int getDoFOffset(const int body_index, int* q_index) const; + /// \copydoc MultiBodyTree::getBodyOrigin + int getBodyOrigin(const int body_index, vec3* world_origin) const; + /// \copydoc MultiBodyTree::getBodyCoM + int getBodyCoM(const int body_index, vec3* world_com) const; + /// \copydoc MultiBodyTree::getBodyTransform + int getBodyTransform(const int body_index, mat33* world_T_body) const; + /// \copydoc MultiBodyTree::getBodyAngularVelocity + int getBodyAngularVelocity(const int body_index, vec3* world_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocity + int getBodyLinearVelocity(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyLinearVelocityCoM + int getBodyLinearVelocityCoM(const int body_index, vec3* world_velocity) const; + /// \copydoc MultiBodyTree::getBodyAngularAcceleration + int getBodyAngularAcceleration(const int body_index, vec3* world_dot_omega) const; + /// \copydoc MultiBodyTree::getBodyLinearAcceleration + int getBodyLinearAcceleration(const int body_index, vec3* world_acceleration) const; + /// \copydoc MultiBodyTree::getUserInt + int getUserInt(const int body_index, int* user_int) const; + /// \copydoc MultiBodyTree::getUserPtr + int getUserPtr(const int body_index, void** user_ptr) const; + /// \copydoc MultiBodyTree::setUserInt + int setUserInt(const int body_index, const int user_int); + /// \copydoc MultiBodyTree::setUserPtr + int setUserPtr(const int body_index, void* const user_ptr); + ///\copydoc MultiBodytTree::setBodyMass + int setBodyMass(const int body_index, const idScalar mass); + ///\copydoc MultiBodytTree::setBodyFirstMassMoment + int setBodyFirstMassMoment(const int body_index, const vec3& first_mass_moment); + ///\copydoc MultiBodytTree::setBodySecondMassMoment + int setBodySecondMassMoment(const int body_index, const mat33& second_mass_moment); + ///\copydoc MultiBodytTree::getBodyMass + int getBodyMass(const int body_index, idScalar* mass) const; + ///\copydoc MultiBodytTree::getBodyFirstMassMoment + int getBodyFirstMassMoment(const int body_index, vec3* first_mass_moment) const; + ///\copydoc MultiBodytTree::getBodySecondMassMoment + int getBodySecondMassMoment(const int body_index, mat33* second_mass_moment) const; + /// \copydoc MultiBodyTree::clearAllUserForcesAndMoments + void clearAllUserForcesAndMoments(); + /// \copydoc MultiBodyTree::addUserForce + int addUserForce(const int body_index, const vec3& body_force); + /// \copydoc MultiBodyTree::addUserMoment + int addUserMoment(const int body_index, const vec3& body_moment); private: - // debug function. print tree structure to stdout - void printTree(int index, int indentation); - // get string representation of JointType (for debugging) - const char* jointTypeToString(const JointType& type) const; - // get number of degrees of freedom from joint type - int bodyNumDoFs(const JointType& type) const; - // number of bodies in the system - int m_num_bodies; - // number of degrees of freedom - int m_num_dofs; - // Gravitational acceleration (in world frame) - vec3 m_world_gravity; - // vector of bodies in the system - // body 0 is used as an environment body and is allways fixed. - // The bodies are ordered such that a parent body always has an index - // smaller than its child. - idArray::type m_body_list; - // Parent_index[i] is the index for i's parent body in body_list. - // This fully describes the tree. - idArray::type m_parent_index; - // child_indices[i] contains a vector of indices of - // all children of the i-th body - idArray::type>::type m_child_indices; - // Indices of rotary joints - idArray::type m_body_revolute_list; - // Indices of prismatic joints - idArray::type m_body_prismatic_list; - // Indices of floating joints - idArray::type m_body_floating_list; - // a user-provided integer - idArray::type m_user_int; - // a user-provided pointer - idArray::type m_user_ptr; + // debug function. print tree structure to stdout + void printTree(int index, int indentation); + // get string representation of JointType (for debugging) + const char* jointTypeToString(const JointType& type) const; + // get number of degrees of freedom from joint type + int bodyNumDoFs(const JointType& type) const; + // number of bodies in the system + int m_num_bodies; + // number of degrees of freedom + int m_num_dofs; + // Gravitational acceleration (in world frame) + vec3 m_world_gravity; + // vector of bodies in the system + // body 0 is used as an environment body and is allways fixed. + // The bodies are ordered such that a parent body always has an index + // smaller than its child. + idArray::type m_body_list; + // Parent_index[i] is the index for i's parent body in body_list. + // This fully describes the tree. + idArray::type m_parent_index; + // child_indices[i] contains a vector of indices of + // all children of the i-th body + idArray::type>::type m_child_indices; + // Indices of rotary joints + idArray::type m_body_revolute_list; + // Indices of prismatic joints + idArray::type m_body_prismatic_list; + // Indices of floating joints + idArray::type m_body_floating_list; + // a user-provided integer + idArray::type m_user_int; + // a user-provided pointer + idArray::type m_user_ptr; }; } #endif diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index 37063bfd7..47b4ab389 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -3,111 +3,111 @@ namespace btInverseDynamics { MultiBodyTree::InitCache::InitCache() { - m_inertias.resize(0); - m_joints.resize(0); - m_num_dofs = 0; - m_root_index=-1; + m_inertias.resize(0); + m_joints.resize(0); + m_num_dofs = 0; + m_root_index=-1; } int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, - const JointType joint_type, - const vec3& parent_r_parent_body_ref, - const mat33& body_T_parent_ref, - const vec3& body_axis_of_motion, const idScalar mass, - const vec3& body_r_body_com, const mat33& body_I_body, - const int user_int, void* user_ptr) { - switch (joint_type) { - case REVOLUTE: - case PRISMATIC: - m_num_dofs += 1; - break; - case FIXED: - // does not add a degree of freedom - // m_num_dofs+=0; - break; - case FLOATING: - m_num_dofs += 6; - break; - default: - error_message("unknown joint type %d\n", joint_type); - return -1; - } + const JointType joint_type, + const vec3& parent_r_parent_body_ref, + const mat33& body_T_parent_ref, + const vec3& body_axis_of_motion, const idScalar mass, + const vec3& body_r_body_com, const mat33& body_I_body, + const int user_int, void* user_ptr) { + switch (joint_type) { + case REVOLUTE: + case PRISMATIC: + m_num_dofs += 1; + break; + case FIXED: + // does not add a degree of freedom + // m_num_dofs+=0; + break; + case FLOATING: + m_num_dofs += 6; + break; + default: + error_message("unknown joint type %d\n", joint_type); + return -1; + } - if(-1 == parent_index) { - if(m_root_index>=0) { - error_message("trying to add body %d as root, but already added %d as root body\n", - body_index, m_root_index); - return -1; - } - m_root_index=body_index; - } + if(-1 == parent_index) { + if(m_root_index>=0) { + error_message("trying to add body %d as root, but already added %d as root body\n", + body_index, m_root_index); + return -1; + } + m_root_index=body_index; + } - JointData joint; - joint.m_child = body_index; - joint.m_parent = parent_index; - joint.m_type = joint_type; - joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; - joint.m_child_T_parent_ref = body_T_parent_ref; - joint.m_child_axis_of_motion = body_axis_of_motion; + JointData joint; + joint.m_child = body_index; + joint.m_parent = parent_index; + joint.m_type = joint_type; + joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref; + joint.m_child_T_parent_ref = body_T_parent_ref; + joint.m_child_axis_of_motion = body_axis_of_motion; - InertiaData body; - body.m_mass = mass; - body.m_body_pos_body_com = body_r_body_com; - body.m_body_I_body = body_I_body; + InertiaData body; + body.m_mass = mass; + body.m_body_pos_body_com = body_r_body_com; + body.m_body_I_body = body_I_body; - m_inertias.push_back(body); - m_joints.push_back(joint); - m_user_int.push_back(user_int); - m_user_ptr.push_back(user_ptr); - return 0; + m_inertias.push_back(body); + m_joints.push_back(joint); + m_user_int.push_back(user_int); + m_user_ptr.push_back(user_ptr); + return 0; } int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const { - if (index < 0 || index > static_cast(m_inertias.size())) { - error_message("index out of range\n"); - return -1; - } + if (index < 0 || index > static_cast(m_inertias.size())) { + error_message("index out of range\n"); + return -1; + } - *inertia = m_inertias[index]; - return 0; + *inertia = m_inertias[index]; + return 0; } int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { - if (index < 0 || index > static_cast(m_user_int.size())) { - error_message("index out of range\n"); - return -1; - } - *user_int = m_user_int[index]; - return 0; + if (index < 0 || index > static_cast(m_user_int.size())) { + error_message("index out of range\n"); + return -1; + } + *user_int = m_user_int[index]; + return 0; } int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { - if (index < 0 || index > static_cast(m_user_ptr.size())) { - error_message("index out of range\n"); - return -1; - } - *user_ptr = m_user_ptr[index]; - return 0; + if (index < 0 || index > static_cast(m_user_ptr.size())) { + error_message("index out of range\n"); + return -1; + } + *user_ptr = m_user_ptr[index]; + return 0; } int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { - if (index < 0 || index > static_cast(m_joints.size())) { - error_message("index out of range\n"); - return -1; - } - *joint = m_joints[index]; - return 0; + if (index < 0 || index > static_cast(m_joints.size())) { + error_message("index out of range\n"); + return -1; + } + *joint = m_joints[index]; + return 0; } int MultiBodyTree::InitCache::buildIndexSets() { - // NOTE: This function assumes that proper indices were provided - // User2InternalIndex from utils can be used to facilitate this. + // NOTE: This function assumes that proper indices were provided + // User2InternalIndex from utils can be used to facilitate this. - m_parent_index.resize(numBodies()); - for (idArrayIdx j = 0; j < m_joints.size(); j++) { - const JointData& joint = m_joints[j]; - m_parent_index[joint.m_child] = joint.m_parent; - } + m_parent_index.resize(numBodies()); + for (idArrayIdx j = 0; j < m_joints.size(); j++) { + const JointData& joint = m_joints[j]; + m_parent_index[joint.m_child] = joint.m_parent; + } - return 0; + return 0; } } diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp index a46cc16bb..0d2aa4a07 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.hpp @@ -8,102 +8,102 @@ namespace btInverseDynamics { /// Mass properties of a rigid body struct InertiaData { - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); - /// mass - idScalar m_mass; - /// vector from body-fixed frame to center of mass, - /// in body-fixed frame, multiplied by the mass - vec3 m_body_pos_body_com; - /// moment of inertia w.r.t. the origin of the body-fixed - /// frame, represented in that frame - mat33 m_body_I_body; + /// mass + idScalar m_mass; + /// vector from body-fixed frame to center of mass, + /// in body-fixed frame, multiplied by the mass + vec3 m_body_pos_body_com; + /// moment of inertia w.r.t. the origin of the body-fixed + /// frame, represented in that frame + mat33 m_body_I_body; }; /// Joint properties struct JointData { - ID_DECLARE_ALIGNED_ALLOCATOR(); + ID_DECLARE_ALIGNED_ALLOCATOR(); - /// type of joint - JointType m_type; - /// index of parent body - int m_parent; - /// index of child body - int m_child; - /// vector from parent's body-fixed frame to child's body-fixed - /// frame for q=0, written in the parent's body fixed frame - vec3 m_parent_pos_parent_child_ref; - /// Transform matrix converting vectors written in the parent's frame - /// into vectors written in the child's frame for q=0 - /// ie, child_vector = child_T_parent_ref * parent_vector; - mat33 m_child_T_parent_ref; - /// Axis of motion for 1 degree-of-freedom joints, - /// written in the child's frame - /// For revolute joints, the q-value is positive for a positive - /// rotation about this axis. - /// For prismatic joints, the q-value is positive for a positive - /// translation is this direction. - vec3 m_child_axis_of_motion; + /// type of joint + JointType m_type; + /// index of parent body + int m_parent; + /// index of child body + int m_child; + /// vector from parent's body-fixed frame to child's body-fixed + /// frame for q=0, written in the parent's body fixed frame + vec3 m_parent_pos_parent_child_ref; + /// Transform matrix converting vectors written in the parent's frame + /// into vectors written in the child's frame for q=0 + /// ie, child_vector = child_T_parent_ref * parent_vector; + mat33 m_child_T_parent_ref; + /// Axis of motion for 1 degree-of-freedom joints, + /// written in the child's frame + /// For revolute joints, the q-value is positive for a positive + /// rotation about this axis. + /// For prismatic joints, the q-value is positive for a positive + /// translation is this direction. + vec3 m_child_axis_of_motion; }; /// Data structure to store data passed by the user. /// This is used in MultiBodyTree::finalize to build internal data structures. class MultiBodyTree::InitCache { public: - ID_DECLARE_ALIGNED_ALLOCATOR(); - /// constructor - InitCache(); - ///\copydoc MultiBodyTree::addBody - int addBody(const int body_index, const int parent_index, const JointType joint_type, - const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, - const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com, - const mat33 &body_I_body, const int user_int, void *user_ptr); - /// build index arrays - /// @return 0 on success, -1 on failure - int buildIndexSets(); - /// @return number of degrees of freedom - int numDoFs() const { return m_num_dofs; } - /// @return number of bodies - int numBodies() const { return m_inertias.size(); } - /// get inertia data for index - /// @param index of the body - /// @param inertia pointer for return data - /// @return 0 on success, -1 on failure - int getInertiaData(const int index, InertiaData *inertia) const; - /// get joint data for index - /// @param index of the body - /// @param joint pointer for return data - /// @return 0 on success, -1 on failure - int getJointData(const int index, JointData *joint) const; - /// get parent index array (paren_index[i] is the index of the parent of i) - /// @param parent_index pointer for return data - void getParentIndexArray(idArray::type *parent_index) { *parent_index = m_parent_index; } - /// get user integer - /// @param index body index - /// @param user_int user integer - /// @return 0 on success, -1 on failure - int getUserInt(const int index, int *user_int) const; - /// get user pointer - /// @param index body index - /// @param user_int user pointer - /// @return 0 on success, -1 on failure - int getUserPtr(const int index, void **user_ptr) const; + ID_DECLARE_ALIGNED_ALLOCATOR(); + /// constructor + InitCache(); + ///\copydoc MultiBodyTree::addBody + int addBody(const int body_index, const int parent_index, const JointType joint_type, + const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref, + const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com, + const mat33 &body_I_body, const int user_int, void *user_ptr); + /// build index arrays + /// @return 0 on success, -1 on failure + int buildIndexSets(); + /// @return number of degrees of freedom + int numDoFs() const { return m_num_dofs; } + /// @return number of bodies + int numBodies() const { return m_inertias.size(); } + /// get inertia data for index + /// @param index of the body + /// @param inertia pointer for return data + /// @return 0 on success, -1 on failure + int getInertiaData(const int index, InertiaData *inertia) const; + /// get joint data for index + /// @param index of the body + /// @param joint pointer for return data + /// @return 0 on success, -1 on failure + int getJointData(const int index, JointData *joint) const; + /// get parent index array (paren_index[i] is the index of the parent of i) + /// @param parent_index pointer for return data + void getParentIndexArray(idArray::type *parent_index) { *parent_index = m_parent_index; } + /// get user integer + /// @param index body index + /// @param user_int user integer + /// @return 0 on success, -1 on failure + int getUserInt(const int index, int *user_int) const; + /// get user pointer + /// @param index body index + /// @param user_int user pointer + /// @return 0 on success, -1 on failure + int getUserPtr(const int index, void **user_ptr) const; private: - // vector of bodies - idArray::type m_inertias; - // vector of joints - idArray::type m_joints; - // number of mechanical degrees of freedom - int m_num_dofs; - // parent index array - idArray::type m_parent_index; - // user integers - idArray::type m_user_int; - // user pointers - idArray::type m_user_ptr; - // index of root body (or -1 if not set) - int m_root_index; + // vector of bodies + idArray::type m_inertias; + // vector of joints + idArray::type m_joints; + // number of mechanical degrees of freedom + int m_num_dofs; + // parent index array + idArray::type m_parent_index; + // user integers + idArray::type m_user_int; + // user pointers + idArray::type m_user_ptr; + // index of root body (or -1 if not set) + int m_root_index; }; } #endif // MULTIBODYTREEINITCACHE_HPP_ diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp index 4ee728a0d..d067123e9 100644 --- a/test/InverseDynamics/test_invdyn_kinematics.cpp +++ b/test/InverseDynamics/test_invdyn_kinematics.cpp @@ -248,7 +248,11 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar // first test: absolute difference between numerical and numerial // differentiation should be small TEST(InvDynKinematicsDifferentiation, errorAbsolute) { +#ifdef BT_ID_USE_DOUBLE_PRECISION const idScalar kDeltaT = 1e-7; +#else + const idScalar kDeltaT = 1e-2; +#endif const idScalar kDuration = 1e-2; const idScalar kAcceptableError = 1e-4; From 7651d89b980e6c52fc290545dbff11fe00a565db Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 19 Nov 2015 14:33:14 -0800 Subject: [PATCH 7/7] fix InverseDynamics/test_invdyn_kinematics.cpp for single/double precision builds use dill_creator for tree structure (not coil_creator) --- .../test_invdyn_kinematics.cpp | 38 ++++++++++++++----- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/test/InverseDynamics/test_invdyn_kinematics.cpp b/test/InverseDynamics/test_invdyn_kinematics.cpp index d067123e9..06e7c999e 100644 --- a/test/InverseDynamics/test_invdyn_kinematics.cpp +++ b/test/InverseDynamics/test_invdyn_kinematics.cpp @@ -136,7 +136,7 @@ public: VecDiffFD(std::string name, int dim, idScalar dt) : m_name(name), m_fd(dim), m_dt(dt) { for (int i = 0; i < m_fd.size(); i++) { char buf[256]; - BT_ID_SNPRINTF(buf, 256, "%s-%.2d", name.c_str(), i); + BT_ID_SNPRINTF(buf, 256, "%s-%.2d", name.c_str(), i); m_fd[i].init(buf, dt); } } @@ -151,7 +151,16 @@ public: } return max_error; } - + idScalar getMaxValue() const { + idScalar max_value = 0; + for (int i = 0; i < m_fd.size(); i++) { + const idScalar value = m_fd[i].getMaxValue(); + if (value > max_value) { + max_value= value; + } + } + return max_value; + } void printMaxError() { printf("%s: total dt= %e max_error= %e\n", m_name.c_str(), m_dt, getMaxError()); } @@ -233,13 +242,19 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar fd_omg.update(body, world_T_body, omega); fd_acc.update(body, vel, acc); fd_omgd.update(body, omega, dot_omega); + + +// fd_vel.printCurrent(); +//fd_acc.printCurrent(); +//fd_omg.printCurrent(); +//fd_omgd.printCurrent(); } } - *max_linear_velocity_error = fd_vel.getMaxError(); - *max_angular_velocity_error = fd_omg.getMaxError(); - *max_linear_acceleration_error = fd_acc.getMaxError(); - *max_angular_acceleration_error = fd_omgd.getMaxError(); + *max_linear_velocity_error = fd_vel.getMaxError()/fd_vel.getMaxValue(); + *max_angular_velocity_error = fd_omg.getMaxError()/fd_omg.getMaxValue(); + *max_linear_acceleration_error = fd_acc.getMaxError()/fd_acc.getMaxValue(); + *max_angular_acceleration_error = fd_omgd.getMaxError()/fd_omgd.getMaxValue(); delete tree; return 0; @@ -248,13 +263,16 @@ int calculateDifferentiationError(const MultiBodyTreeCreator& creator, idScalar // first test: absolute difference between numerical and numerial // differentiation should be small TEST(InvDynKinematicsDifferentiation, errorAbsolute) { + //CAVEAT:these values are hand-tuned to work for the specific trajectory defined above. #ifdef BT_ID_USE_DOUBLE_PRECISION const idScalar kDeltaT = 1e-7; + const idScalar kAcceptableError = 1e-4; #else - const idScalar kDeltaT = 1e-2; + const idScalar kDeltaT = 1e-4; + const idScalar kAcceptableError = 5e-3; #endif - const idScalar kDuration = 1e-2; - const idScalar kAcceptableError = 1e-4; + const idScalar kDuration = 0.01; + CoilCreator coil_creator(kNumBodies); DillCreator dill_creator(kLevel); @@ -276,7 +294,7 @@ TEST(InvDynKinematicsDifferentiation, errorAbsolute) { EXPECT_LT(max_angular_acceleration_error, kAcceptableError); // test branched tree - calculateDifferentiationError(coil_creator, kDeltaT, kDuration, &max_linear_velocity_error, + calculateDifferentiationError(dill_creator, kDeltaT, kDuration, &max_linear_velocity_error, &max_angular_velocity_error, &max_linear_acceleration_error, &max_angular_acceleration_error);