Implement joint limit for btMultiBody spherical joint. In URDF, add

<limit effort="1000.0" lower="0.2" upper=".8" twist=".3"/>
effort is max force/impulse
lower = swing range in X
upper = swing range in Y
twist = twist range around Z
(ranges all in radians)
lower, upper, twist and effort need to be > 0, otherwise no limit is created

See examples/pybullet/examples/spherical_joint_limit.py and
    examples/pybullet/gym/pybullet_data/spherical_joint_limit.urdf

10 files changed, 96 insertions, 5 deletions

diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 42cc102f4..5a697b8a8 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -484,6 +484,12 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass, btVect
 
 bool BulletURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
 {
+	btScalar twistLimit;
+	return getJointInfo3(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity, twistLimit);
+}
+
+bool BulletURDFImporter::getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const
+{
 	jointLowerLimit = 0.f;
 	jointUpperLimit = 0.f;
 	jointDamping = 0.f;
@@ -510,6 +516,7 @@ bool BulletURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2jo
 			jointFriction = pj->m_jointFriction;
 			jointMaxForce = pj->m_effortLimit;
 			jointMaxVelocity = pj->m_velocityLimit;
+			twistLimit = pj->m_twistLimit;
 			return true;
 		}
 		else
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
index eb890c016..66bb908de 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
@@ -60,6 +60,7 @@ public:
 
 	virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
 	virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
+	virtual bool getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const;
 
 	virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
 	virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 907ed55be..571cd2199 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -10,6 +10,9 @@
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h"
+
+
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "URDF2Bullet.h"
 #include "URDFImporterInterface.h"
@@ -259,8 +262,8 @@ btTransform ConvertURDF2BulletInternal(
 	btScalar jointFriction;
 	btScalar jointMaxForce;
 	btScalar jointMaxVelocity;
-
-	bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
+	btScalar twistLimit;
+	bool hasParentJoint = u2b.getJointInfo3(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity, twistLimit);
 	std::string linkName = u2b.getLinkName(urdfLinkIndex);
 
 	if (flags & CUF_USE_SDF)
@@ -422,6 +425,20 @@ btTransform ConvertURDF2BulletInternal(
 						cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
 							parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(),
 							disableParentCollision);
+
+
+						//create a spherical joint limit, swing_x,. swing_y and twist
+						//jointLowerLimit <= jointUpperLimit)
+						if (jointUpperLimit > 0 && jointLowerLimit> 0 && twistLimit > 0 && jointMaxForce>0)
+						{
+							btMultiBodySphericalJointLimit* con = new btMultiBodySphericalJointLimit(cache.m_bulletMultiBody, mbLinkIndex, 
+								jointLowerLimit,
+								jointUpperLimit,
+								twistLimit,
+								jointMaxForce);
+							world1->addMultiBodyConstraint(con);
+						}
+
 					}
 					else
 					{
diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
index 0bf494b08..847452cd8 100644
--- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
+++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h
@@ -64,6 +64,13 @@ public:
 		return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
 	};
 
+	virtual bool getJointInfo3(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity, btScalar& twistLimit) const
+	{
+		//backwards compatibility for custom file importers
+		twistLimit = 0;
+		return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
+	};
+
 	virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const = 0;
 	virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) {}
 
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index 23f36a5e6..96f7538a2 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -1290,6 +1290,7 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
 	joint.m_velocityLimit = 0.f;
 	joint.m_jointDamping = 0.f;
 	joint.m_jointFriction = 0.f;
+	joint.m_twistLimit = -1;
 
 	if (m_parseSDF)
 	{
@@ -1304,13 +1305,19 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
 		{
 			joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText());
 		}
+		
+		XMLElement* twist_xml = config->FirstChildElement("twist");
+		if (twist_xml)
+		{
+			joint.m_twistLimit = urdfLexicalCast<double>(twist_xml->GetText());
+		}
 
 		XMLElement* effort_xml = config->FirstChildElement("effort");
 		if (effort_xml)
 		{
 			joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText());
 		}
-
+		
 		XMLElement* velocity_xml = config->FirstChildElement("velocity");
 		if (velocity_xml)
 		{
@@ -1337,6 +1344,14 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
 			joint.m_upperLimit *= m_urdfScaling;
 		}
 
+		
+		const char* twist_str = config->Attribute("twist");
+		if (twist_str)
+		{
+			joint.m_twistLimit = urdfLexicalCast<double>(twist_str);
+		}
+
+
 		// Get joint effort limit
 		const char* effort_str = config->Attribute("effort");
 		if (effort_str)
@@ -1344,6 +1359,7 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLog
 			joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
 		}
 
+
 		// Get joint velocity limit
 		const char* velocity_str = config->Attribute("velocity");
 		if (velocity_str)
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index efe51b704..b234fa66b 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -189,13 +189,15 @@ struct UrdfJoint
 
 	double m_jointDamping;
 	double m_jointFriction;
+	double m_twistLimit;
 	UrdfJoint()
 		: m_lowerLimit(0),
 		  m_upperLimit(-1),
 		  m_effortLimit(0),
 		  m_velocityLimit(0),
 		  m_jointDamping(0),
-		  m_jointFriction(0)
+		  m_jointFriction(0),
+		  m_twistLimit(-1)
 	{
 	}
 };
diff --git a/examples/pybullet/gym/pybullet_data/spherical_joint_limit.urdf b/examples/pybullet/gym/pybullet_data/spherical_joint_limit.urdf
new file mode 100644
index 000000000..a3ee524f0
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/spherical_joint_limit.urdf
@@ -0,0 +1,36 @@
+<robot name="dumpUrdf">
+	<link name="chest" >
+		<inertial>
+				<origin rpy = "0 0 0" xyz = "0.000000 0.00000 0.000000" />
+			 <mass value = "4.000000" />
+			<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
+		</inertial>
+		<collision>
+				<origin rpy = "0 0 0" xyz = "0.000000 0.00000 0.000000" />
+			<geometry>
+				<box size="0.6 0.6 0.6"/>
+			</geometry>
+		</collision>
+	</link>
+	<link name="neck" >
+		<inertial>
+				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.30000" />
+			 <mass value = "1.00000" />
+			<inertia ixx = "0.01" ixy = "0" ixz = "0" iyy = "0.001" iyz = "0" izz = "0.001" />
+		</inertial>
+		<collision>
+				<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.30000" />
+			<geometry>
+				<box size="0.1 0.2 0.6"/>
+			</geometry>
+		</collision>
+	</link>
+	<joint name="neck" type="spherical" >
+		<parent link="chest" />
+		<limit effort="1000.0" lower="0.2" upper=".8" twist=".3"/>
+		<child link="neck" />
+				<origin rpy = "0 0 0" xyz = "0.000000 0.0 1.000000" />
+				<axis xyz="0 0 1"/>
+	</joint>
+	
+</robot>
diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt
index 3332440f2..cfd49e906 100644
--- a/src/BulletDynamics/CMakeLists.txt
+++ b/src/BulletDynamics/CMakeLists.txt
@@ -42,6 +42,7 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBodyPoint2Point.cpp
 	Featherstone/btMultiBodySliderConstraint.cpp
 	Featherstone/btMultiBodySphericalJointMotor.cpp
+	Featherstone/btMultiBodySphericalJointLimit.cpp
 	MLCPSolvers/btDantzigLCP.cpp
 	MLCPSolvers/btMLCPSolver.cpp
 	MLCPSolvers/btLemkeAlgorithm.cpp
@@ -105,6 +106,9 @@ SET(Featherstone_HDRS
 	Featherstone/btMultiBodyPoint2Point.h
 	Featherstone/btMultiBodySliderConstraint.h
 	Featherstone/btMultiBodySolverConstraint.h
+  Featherstone/btMultiBodySphericalJointMotor.h
+	Featherstone/btMultiBodySphericalJointLimit.h
+
 )
 
 SET(MLCPSolvers_HDRS
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 1aaa07b69..7287ccc89 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -31,7 +31,7 @@ enum btTypedMultiBodyConstraintType
 	MULTIBODY_CONSTRAINT_SLIDER,
 	MULTIBODY_CONSTRAINT_SPHERICAL_MOTOR,
 	MULTIBODY_CONSTRAINT_FIXED,
-	
+	MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT,
 	MAX_MULTIBODY_CONSTRAINT_TYPE,
 };
 
diff --git a/src/btBulletDynamicsAll.cpp b/src/btBulletDynamicsAll.cpp
index a8069e30a..6e73880dc 100644
--- a/src/btBulletDynamicsAll.cpp
+++ b/src/btBulletDynamicsAll.cpp
@@ -36,6 +36,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"
 #include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"
+#include "BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp"
 #include "BulletDynamics/Vehicle/btRaycastVehicle.cpp"
 #include "BulletDynamics/Vehicle/btWheelInfo.cpp"
 #include "BulletDynamics/Character/btKinematicCharacterController.cpp"