From 6005e54aa1e72c56c65e323ad4c4b682debb61d4 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 18 Aug 2016 09:44:33 -0700 Subject: [PATCH] Fix some inconsistencies in URDF file handling between btRigidBody and btMultiBody (rotation order and application of root-inertial-frame offset) --- .../ImportURDFDemo/ImportURDFSetup.cpp | 33 ++++++++++++------- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 10 +++--- .../SharedMemory/PhysicsClientExample.cpp | 2 +- 3 files changed, 29 insertions(+), 16 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 46b69cc08..de25c1a66 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -36,7 +36,8 @@ class ImportUrdfSetup : public CommonMultiBodyBase struct ImportUrdfInternalData* m_data; bool m_useMultiBody; btAlignedObjectArray m_nameMemory; - + btScalar m_grav; + int m_upAxis; public: ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName); virtual ~ImportUrdfSetup(); @@ -87,7 +88,9 @@ struct ImportUrdfInternalData ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName) - :CommonMultiBodyBase(helper) + :CommonMultiBodyBase(helper), + m_grav(0), + m_upAxis(2) { m_data = new ImportUrdfInternalData; @@ -186,9 +189,9 @@ void ImportUrdfSetup::setFileName(const char* urdfFileName) void ImportUrdfSetup::initPhysics() { - int upAxis = 2; - m_guiHelper->setUpAxis(upAxis); - + + m_guiHelper->setUpAxis(m_upAxis); + this->createEmptyDynamicsWorld(); //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -199,10 +202,14 @@ void ImportUrdfSetup::initPhysics() );//+btIDebugDraw::DBG_DrawConstraintLimits); - btVector3 gravity(0,0,0); - gravity[upAxis]=-9.8; - - m_dynamicsWorld->setGravity(gravity); + if (m_guiHelper->getParameterInterface()) + { + SliderParams slider("Gravity", &m_grav); + slider.m_minVal = -10; + slider.m_maxVal = 10; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + BulletURDFImporter u2b(m_guiHelper, 0); @@ -350,7 +357,7 @@ void ImportUrdfSetup::initPhysics() if (createGround) { btVector3 groundHalfExtents(20,20,20); - groundHalfExtents[upAxis]=1.f; + groundHalfExtents[m_upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); m_collisionShapes.push_back(box); box->initializePolyhedralFeatures(); @@ -358,7 +365,7 @@ void ImportUrdfSetup::initPhysics() m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(0,0,0); - groundOrigin[upAxis]=-2.5; + groundOrigin[m_upAxis]=-2.5; start.setOrigin(groundOrigin); btRigidBody* body = createRigidBody(0,start,box); //m_dynamicsWorld->removeRigidBody(body); @@ -388,6 +395,10 @@ void ImportUrdfSetup::stepSimulation(float deltaTime) { if (m_dynamicsWorld) { + btVector3 gravity(0, 0, 0); + gravity[m_upAxis] = m_grav; + m_dynamicsWorld->setGravity(gravity); + for (int i=0;im_numMotors;i++) { if (m_data->m_jointMotors[i]) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 367e674da..8503dd41d 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -299,7 +299,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat { //b3Printf("Fixed joint\n"); - btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB); + btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA); if (enableConstraints) world1->addConstraint(dof6,true); @@ -324,7 +324,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); if (enableConstraints) world1->addConstraint(dof6,true); @@ -350,7 +350,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); + btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); if (enableConstraints) @@ -455,7 +455,9 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b, MultiBodyCreationInter mb->setHasSelfCollision(false); mb->finalizeMultiDof(); - mb->setBaseWorldTransform(rootTransformInWorldSpace); + btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex]; + + mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot); btAlignedObjectArray scratch_q; btAlignedObjectArray scratch_m; mb->forwardKinematics(scratch_q,scratch_m); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 8dc2f962d..034215bd2 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -239,7 +239,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; - static double startPosY = 1; + static double startPosY = 0; double startPosZ = 0; b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); startPosY += 2.f;