diff --git a/data/multibody.bullet b/data/multibody.bullet index 36a570fe5..8d5019a78 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index c81ec21fc..07d4fd26b 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -7,6 +7,7 @@ #include "../../Utils/b3ResourcePath.h" #include #include +#include "../ImportURDFDemo/URDF2Bullet.h" #include "../ImportURDFDemo/UrdfParser.h" #include "../ImportURDFDemo/urdfStringSplit.h" #include "../ImportURDFDemo/urdfLexicalCast.h" @@ -190,10 +191,13 @@ struct BulletMJCFImporterInternalData btAlignedObjectArray m_allocatedCollisionShapes; mutable btAlignedObjectArray m_allocatedMeshInterfaces; + int m_flags; + BulletMJCFImporterInternalData() :m_inertiaFromGeom(true), m_activeModel(-1), - m_activeBodyUniqueId(-1) + m_activeBodyUniqueId(-1), + m_flags(0) { m_pathPrefix[0] = 0; } @@ -1370,11 +1374,12 @@ struct BulletMJCFImporterInternalData }; -BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter) +BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, int flags) { m_data = new BulletMJCFImporterInternalData(); m_data->m_guiHelper = helper; m_data->m_customVisualShapesConverter = customConverter; + m_data->m_flags = flags; } BulletMJCFImporter::~BulletMJCFImporter() @@ -1897,43 +1902,45 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes( int linkI { if (col->m_geometry.m_hasFromTo) { -#if 0 - btVector3 f = col->m_geometry.m_capsuleFrom; - btVector3 t = col->m_geometry.m_capsuleTo; - btVector3 fromto[2] = {f,t}; - btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius) - ,btScalar(col->m_geometry.m_capsuleRadius)}; - - btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2); - childShape = ms; -#else - btVector3 f = col->m_geometry.m_capsuleFrom; - btVector3 t = col->m_geometry.m_capsuleTo; - - //compute the local 'fromto' transform - btVector3 localPosition = btScalar(0.5)*(t+f); - btQuaternion localOrn; - localOrn = btQuaternion::getIdentity(); - - btVector3 diff = t-f; - btScalar lenSqr = diff.length2(); - btScalar height = 0.f; - - if (lenSqr > SIMD_EPSILON) + if (m_data->m_flags&CUF_USE_IMPLICIT_CYLINDER) { - height = btSqrt(lenSqr); - btVector3 ax = diff / height; + btVector3 f = col->m_geometry.m_capsuleFrom; + btVector3 t = col->m_geometry.m_capsuleTo; + + //compute the local 'fromto' transform + btVector3 localPosition = btScalar(0.5)*(t+f); + btQuaternion localOrn; + localOrn = btQuaternion::getIdentity(); - btVector3 zAxis(0,0,1); - localOrn = shortestArcQuat(zAxis,ax); + btVector3 diff = t-f; + btScalar lenSqr = diff.length2(); + btScalar height = 0.f; + + if (lenSqr > SIMD_EPSILON) + { + height = btSqrt(lenSqr); + btVector3 ax = diff / height; + + btVector3 zAxis(0,0,1); + localOrn = shortestArcQuat(zAxis,ax); + } + btCapsuleShapeZ* capsule= new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius,height); + + btCompoundShape* compound = new btCompoundShape(); + btTransform localTransform(localOrn,localPosition); + compound->addChildShape(localTransform,capsule); + childShape = compound; + } else + { + btVector3 f = col->m_geometry.m_capsuleFrom; + btVector3 t = col->m_geometry.m_capsuleTo; + btVector3 fromto[2] = {f,t}; + btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius) + ,btScalar(col->m_geometry.m_capsuleRadius)}; + + btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2); + childShape = ms; } - btCapsuleShapeZ* capsule= new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius,height); - - btCompoundShape* compound = new btCompoundShape(); - btTransform localTransform(localOrn,localPosition); - compound->addChildShape(localTransform,capsule); - childShape = compound; -#endif } else { btCapsuleShapeZ* cap = new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius, diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 05e5dc2fe..39be50898 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -19,7 +19,7 @@ class BulletMJCFImporter : public URDFImporterInterface struct BulletMJCFImporterInternalData* m_data; public: - BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter); + BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, int flags); virtual ~BulletMJCFImporter(); virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger); diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index e224e7efe..53a30ea57 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -216,7 +216,8 @@ void ImportMJCFSetup::initPhysics() m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - BulletMJCFImporter importer(m_guiHelper, 0); + int flags=0; + BulletMJCFImporter importer(m_guiHelper, 0,flags); MyMJCFLogger logger; bool result = importer.loadMJCF(m_fileName,&logger); if (result) diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index d06460347..5382682a2 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -344,9 +344,9 @@ void ConvertURDF2BulletInternal( bool isFixedBase = (mass==0);//todo: figure out when base is fixed int totalNumJoints = cache.m_totalNumJoints1; cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep); - if (flags & CUF_NO_GLOBAL_VELOCITIES_MB) + if (flags & CUF_GLOBAL_VELOCITIES_MB) { - cache.m_bulletMultiBody->useGlobalVelocities(false); + cache.m_bulletMultiBody->useGlobalVelocities(true); } if (flags & CUF_USE_MJCF) { diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 40c8803c5..abc4f3573 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -24,7 +24,7 @@ enum ConvertURDFFlags { CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, CUF_RESERVED=64, CUF_USE_IMPLICIT_CYLINDER=128, - CUF_NO_GLOBAL_VELOCITIES_MB=256, + CUF_GLOBAL_VELOCITIES_MB=256, }; void ConvertURDF2Bullet(const URDFImporterInterface& u2b, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 670ae1ac9..e76c4bc3d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2704,7 +2704,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS m_data->m_sdfRecentLoadedBodies.clear(); - BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); + BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, flags); bool useFixedBase = false; MyMJCFLogger2 logger; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 9761519b7..05a81c8c1 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -673,7 +673,7 @@ enum eURDF_Flags URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS=32, URDF_RESERVED=64, URDF_USE_IMPLICIT_CYLINDER =128, - URDF_NO_GLOBAL_VELOCITIES_MB =256, + URDF_GLOBAL_VELOCITIES_MB =256, }; enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index d5b83017c..eb8ca3aad 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -55,7 +55,9 @@ class MJCFBasedRobot: parts[part_name] = BodyPart(part_name, bodies, i, -1) for j in range(p.getNumJoints(bodies[i])): p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0) - _,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j) + jointInfo = p.getJointInfo(bodies[i], j) + joint_name=jointInfo[1] + part_name=jointInfo[12] joint_name = joint_name.decode("utf8") part_name = part_name.decode("utf8") @@ -172,7 +174,11 @@ class Joint: self.bodyIndex = bodyIndex self.jointIndex = jointIndex self.joint_name = joint_name - _,_,_,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex) + + jointInfo = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex) + self.lowerLimit = jointInfo[8] + self.upperLimit = jointInfo[9] + self.power_coeff = 0 def set_state(self, x, vx): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 6d5124de4..c719bda5c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8817,7 +8817,7 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); PyModule_AddIntConstant(m, "URDF_USE_IMPLICIT_CYLINDER", URDF_USE_IMPLICIT_CYLINDER); - PyModule_AddIntConstant(m, "URDF_NO_GLOBAL_VELOCITIES_MB", URDF_NO_GLOBAL_VELOCITIES_MB); + PyModule_AddIntConstant(m, "URDF_GLOBAL_VELOCITIES_MB", URDF_GLOBAL_VELOCITIES_MB); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a750f033d..6571822c9 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -122,7 +122,7 @@ btMultiBody::btMultiBody(int n_links, m_dofCount(0), m_posVarCnt(0), m_useRK4(false), - m_useGlobalVelocities(true), + m_useGlobalVelocities(false), m_internalNeedsJointFeedback(false) { m_cachedInertiaTopLeft.setValue(0,0,0,0,0,0,0,0,0);