From 55e185fe39c6eab527e1235b0f57972ba59ea387 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 22 Jun 2018 09:28:07 -0700 Subject: [PATCH] PyBullet loadURDF in useMaximalCoordinated=True (btRigidBody), support lower/upper limits for revolute joints. --- .../ImportURDFDemo/MyMultiBodyCreator.cpp | 14 +++++++----- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 22 +++++++++++++++---- 2 files changed, 26 insertions(+), 10 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 7bda1a949..6c62cd9c2 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -119,8 +119,8 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(-1,0,0)); - dof6->setAngularLowerLimit(btVector3(1,0,0)); + dof6->setAngularLowerLimit(btVector3(jointLowerLimit,0,0)); + dof6->setAngularUpperLimit(btVector3(jointUpperLimit,0,0)); break; } @@ -130,8 +130,9 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,-1,0)); - dof6->setAngularLowerLimit(btVector3(0,1,0)); + + dof6->setAngularLowerLimit(btVector3(0,jointLowerLimit,0)); + dof6->setAngularUpperLimit(btVector3(0,jointUpperLimit,0)); break; } case 2: @@ -141,8 +142,9 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in dof6->setLinearLowerLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0)); - dof6->setAngularUpperLimit(btVector3(0,0,-1)); - dof6->setAngularLowerLimit(btVector3(0,0,1)); + + dof6->setAngularLowerLimit(btVector3(0,0,jointLowerLimit)); + dof6->setAngularUpperLimit(btVector3(0,0,jointUpperLimit)); } }; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index b336659f2..6cfb83106 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -462,14 +462,28 @@ void ConvertURDF2BulletInternal( { btGeneric6DofSpring2Constraint* dof6 = 0; - //backwards compatibility - if (flags & CUF_RESERVED ) + if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) { - dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + //backwards compatibility + if (flags & CUF_RESERVED ) + { + dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + } else + { + dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + } } else { - dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + //disable joint limits + if (flags & CUF_RESERVED ) + { + dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,1,-1); + } else + { + dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,1,-1); + } } + if (enableConstraints) world1->addConstraint(dof6,true); //b3Printf("Revolute/Continuous joint\n");