From 7698d5f95c3b08ea6e7ff1b9b7b5847e06806bb0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 15 Jul 2015 17:39:43 -0700 Subject: [PATCH] fix some out-of-date comments in btMultiBody, thanks to Thomas Buschmann disable disactivation for btRigidBody in import urdf demo, increase strength of joint motor for btMultiBody --- .../ImportURDFDemo/ImportURDFSetup.cpp | 18 ++++++++++++------ .../ImportURDFDemo/MyMultiBodyCreator.cpp | 2 ++ .../Featherstone/btMultiBody.cpp | 4 ++-- src/BulletDynamics/Featherstone/btMultiBody.h | 2 +- .../Featherstone/btMultiBodyLink.h | 13 ++++++++----- 5 files changed, 25 insertions(+), 14 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 966ffce94..236899847 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -130,7 +130,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, if (gFileNameArray.size()==0) { - gFileNameArray.push_back("r2d2.urdf");//husky/husky.urdf"); + gFileNameArray.push_back("prismatic.urdf"); } @@ -223,9 +223,11 @@ void ImportUrdfSetup::initPhysics() #endif//USE_ROS_URDF URDFImporterInterface& u2b = *bla; bool loadOk = u2b.loadURDF(m_fileName); - + +#ifdef TEST_MULTIBODY_SERIALIZATION //test to serialize a multibody to disk or shared memory, with base, link and joint names btSerializer* s = new btDefaultSerializer; +#endif //TEST_MULTIBODY_SERIALIZATION if (loadOk) { @@ -255,8 +257,9 @@ void ImportUrdfSetup::initPhysics() { std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_nameMemory.push_back(name); +#ifdef TEST_MULTIBODY_SERIALIZATION s->registerNameForPointer(name->c_str(),name->c_str()); - +#endif//TEST_MULTIBODY_SERIALIZATION mb->setBaseName(name->c_str()); //create motors for each btMultiBody joint int numLinks = mb->getNumLinks(); @@ -267,9 +270,10 @@ void ImportUrdfSetup::initPhysics() std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex)); std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); +#ifdef TEST_MULTIBODY_SERIALIZATION s->registerNameForPointer(jointName->c_str(),jointName->c_str()); s->registerNameForPointer(linkName->c_str(),linkName->c_str()); - +#endif//TEST_MULTIBODY_SERIALIZATION m_nameMemory.push_back(jointName); m_nameMemory.push_back(linkName); @@ -291,7 +295,7 @@ void ImportUrdfSetup::initPhysics() slider.m_minVal=-4; slider.m_maxVal=4; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - float maxMotorImpulse = 0.1f; + float maxMotorImpulse = 10.1f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse); //motor->setMaxAppliedImpulse(0); m_data->m_jointMotors[m_data->m_numMotors]=motor; @@ -376,7 +380,7 @@ void ImportUrdfSetup::initPhysics() m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.); } - +#ifdef TEST_MULTIBODY_SERIALIZATION m_dynamicsWorld->serialize(s); b3ResourcePath p; char resourcePath[1024]; @@ -386,6 +390,8 @@ void ImportUrdfSetup::initPhysics() fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); fclose(f); } +#endif//TEST_MULTIBODY_SERIALIZATION + } void ImportUrdfSetup::stepSimulation(float deltaTime) diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 75b0fa212..51316eb58 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -32,6 +32,8 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); rbci.m_startWorldTransform = initialWorldTrans; btRigidBody* body = new btRigidBody(rbci); + body->forceActivationState(DISABLE_DEACTIVATION); + return body; } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index dfc66725f..e81a5846b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -182,7 +182,7 @@ void btMultiBody::setupPrismatic(int i, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, - const btVector3 &parentComToThisComOffset, + const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { @@ -198,7 +198,7 @@ void btMultiBody::setupPrismatic(int i, m_links[i].m_zeroRotParentToThis = rotParentToThis; m_links[i].setAxisTop(0, 0., 0., 0.); m_links[i].setAxisBottom(0, jointAxis); - m_links[i].m_eVector = parentComToThisComOffset; + m_links[i].m_eVector = parentComToThisPivotOffset; m_links[i].m_dVector = thisPivotToThisComOffset; m_links[i].m_cachedRotParentToThis = rotParentToThis; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 0a2032113..9938149eb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -83,7 +83,7 @@ public: int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, - const btVector3 &parentComToThisComOffset, + const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index ec590fad2..88eda8eeb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -56,11 +56,14 @@ struct btMultibodyLink btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. - btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. set for revolute joints only. - - // m_eVector is constant, but depends on the joint type - // prismatic: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) - // revolute: vector from parent's COM to the pivot point, in PARENT's frame. + btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. + //this is set to zero for planar joint (see also m_eVector comment) + + // m_eVector is constant, but depends on the joint type: + // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame. + // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) + // todo: fix the planar so it is consistent with the other joints + btVector3 m_eVector; btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;