Merge pull request #4151 from erwincoumans/master

add btMultiBody spherical joint limit

13 files changed, 537 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/examples/spherical_joint_limit.py b/examples/pybullet/examples/spherical_joint_limit.py
new file mode 100644
index 000000000..c8ee8af01
--- /dev/null
+++ b/examples/pybullet/examples/spherical_joint_limit.py
@@ -0,0 +1,56 @@
+import pybullet as p
+import pybullet_data as pd
+
+#see spherical_joint_limit.urdf
+#lower is the swing range in the joint local X axis
+#upper is the swing range in the joint local Y axis
+#twist is the twist range rotation around the joint local Z axis
+#effort is the maximum force impulse to enforce the joint limit
+#<limit effort="1000.0" lower="0.2" upper=".8" twist=".3"/>
+
+import time
+dt = 1./240.
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+
+p.setTimeStep(dt)
+
+humanoid = p.loadURDF("spherical_joint_limit.urdf",[0,0,2], useFixedBase=True)
+
+gravxId = p.addUserDebugParameter("grav_x",-20,20,0.3)
+gravyId = p.addUserDebugParameter("grav_y",-20,20,0.3)
+gravzId = p.addUserDebugParameter("grav_y",-20,20,-10)
+
+index= 0
+spherical_index = -1
+
+for j in range(p.getNumJoints(humanoid)):
+  if index<7:
+    ji = p.getJointInfo(humanoid, j)
+    jointType = ji[2]
+    if (jointType == p.JOINT_SPHERICAL):
+      index+=4
+      p.resetJointStateMultiDof(humanoid, j, targetValue=[0,0,0,1], targetVelocity=[0,0,0])
+      #p.changeDynamics(humanoid,j,angularDamping=0, linearDamping=0)
+      spherical_index = j
+      p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL, 
+          targetPosition=[0,0,0,1],  positionGain=0.2,
+          targetVelocity=[0,0,0], velocityGain=0,
+          force=[0,0,0])
+                                   
+    if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
+      index+=1
+      p.resetJointStateMultiDof(humanoid, j, targetValue=[0], targetVelocity=[0])
+      p.setJointMotorControlMultiDof(humanoid, j, controlMode=p.POSITION_CONTROL, 
+          targetPosition=[0], targetVelocity=[0], force=[0])
+
+p.loadURDF("plane.urdf")
+
+p.setRealTimeSimulation(1)
+while p.isConnected():
+  gravX = p.readUserDebugParameter(gravxId)
+  gravY = p.readUserDebugParameter(gravyId)
+  gravZ = p.readUserDebugParameter(gravzId)
+  p.setGravity(gravX,gravY,gravZ)
+  #p.stepSimulation()
+  time.sleep(dt/10.)
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/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp
new file mode 100644
index 000000000..27c7520dc
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.cpp
@@ -0,0 +1,270 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#include "btMultiBodySphericalJointLimit.h"
+#include "btMultiBody.h"
+#include "btMultiBodyLinkCollider.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "LinearMath/btTransformUtil.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+#include "LinearMath/btIDebugDraw.h"
+
+btMultiBodySphericalJointLimit::btMultiBodySphericalJointLimit(btMultiBody* body, int link, 
+	btScalar swingxRange,
+	btScalar swingyRange,
+	btScalar twistRange,
+	btScalar maxAppliedImpulse)
+	: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true, MULTIBODY_CONSTRAINT_SPHERICAL_LIMIT),
+	m_desiredVelocity(0, 0, 0),
+	m_desiredPosition(0,0,0,1),
+	m_use_multi_dof_params(false),
+	m_kd(1., 1., 1.),
+	m_kp(0.2, 0.2, 0.2),
+	m_erp(1),
+	m_rhsClamp(SIMD_INFINITY),
+	m_maxAppliedImpulseMultiDof(maxAppliedImpulse, maxAppliedImpulse, maxAppliedImpulse),
+	m_pivotA(m_bodyA->getLink(link).m_eVector),
+	m_pivotB(m_bodyB->getLink(link).m_eVector),
+	m_swingxRange(swingxRange),
+	m_swingyRange(swingyRange),
+	m_twistRange(twistRange)
+
+{
+
+	m_maxAppliedImpulse = maxAppliedImpulse;
+}
+
+
+void btMultiBodySphericalJointLimit::finalizeMultiDof()
+{
+	allocateJacobiansMultiDof();
+	// note: we rely on the fact that data.m_jacobians are
+	// always initialized to zero by the Constraint ctor
+	int linkDoF = 0;
+	unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
+
+	// row 0: the lower bound
+	// row 0: the lower bound
+	jacobianA(0)[offset] = 1;
+
+	jacobianB(1)[offset] = -1;
+
+	m_numDofsFinalized = m_jacSizeBoth;
+}
+
+
+btMultiBodySphericalJointLimit::~btMultiBodySphericalJointLimit()
+{
+}
+
+int btMultiBodySphericalJointLimit::getIslandIdA() const
+{
+	if (this->m_linkA < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyA->getLink(m_linkA).m_collider)
+		{
+			return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+int btMultiBodySphericalJointLimit::getIslandIdB() const
+{
+	if (m_linkB < 0)
+	{
+		btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
+		if (col)
+			return col->getIslandTag();
+	}
+	else
+	{
+		if (m_bodyB->getLink(m_linkB).m_collider)
+		{
+			return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
+		}
+	}
+	return -1;
+}
+
+void btMultiBodySphericalJointLimit::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+												 btMultiBodyJacobianData& data,
+												 const btContactSolverInfo& infoGlobal)
+{
+	// only positions need to be updated -- data.m_jacobians and force
+	// directions were set in the ctor and never change.
+
+	if (m_numDofsFinalized != m_jacSizeBoth)
+	{
+		finalizeMultiDof();
+	}
+
+	//don't crash
+	if (m_numDofsFinalized != m_jacSizeBoth)
+		return;
+	
+
+	if (m_maxAppliedImpulse == 0.f)
+		return;
+
+	const btScalar posError = 0;
+	const btVector3 zero(0, 0, 0);
+
+	
+	btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) };
+	
+	btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0],
+		m_bodyA->getJointPosMultiDof(m_linkA)[1],
+		m_bodyA->getJointPosMultiDof(m_linkA)[2],
+		m_bodyA->getJointPosMultiDof(m_linkA)[3]);
+
+	btQuaternion refQuat = m_desiredPosition;
+	btVector3 vTwist(0,0,1);
+	
+	btVector3 vConeNoTwist = quatRotate(currentQuat, vTwist);
+	vConeNoTwist.normalize();
+	btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist);
+	qABCone.normalize();
+	btQuaternion qABTwist = qABCone.inverse() * currentQuat;
+	qABTwist.normalize();
+	btQuaternion desiredQuat = qABTwist;
+
+
+	btQuaternion relRot = currentQuat.inverse() * desiredQuat;
+	btVector3 angleDiff;
+	btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff);
+	
+	btScalar limitRanges[3] = {m_swingxRange, m_swingyRange, m_twistRange};
+	
+	/// twist axis/angle
+	btQuaternion qMinTwist = qABTwist;
+	btScalar twistAngle = qABTwist.getAngle();
+
+	if (twistAngle > SIMD_PI)  // long way around. flip quat and recalculate.
+	{
+		qMinTwist = -(qABTwist);
+		twistAngle = qMinTwist.getAngle();
+	}
+	btVector3 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
+	if (twistAngle > SIMD_EPSILON)
+		vTwistAxis.normalize();
+	
+	if (vTwistAxis.dot(vTwist)<0)
+		twistAngle*=-1.;
+
+	angleDiff[2] = twistAngle;
+
+
+	for (int row = 0; row < getNumRows(); row++)
+	{
+		btScalar allowed = limitRanges[row];
+		btScalar damp = 1;
+		if((angleDiff[row]>-allowed)&&(angleDiff[row]<allowed))
+		{
+			angleDiff[row]=0;
+			damp=0;
+
+		} else
+		{
+			if (angleDiff[row]>allowed)
+			{
+				angleDiff[row]-=allowed;
+				
+			}
+			if (angleDiff[row]<-allowed)
+			{
+				angleDiff[row]+=allowed;
+				
+			} 
+		}
+		
+
+		int dof = row;
+		
+		btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar desiredVelocity = this->m_desiredVelocity[row];
+		
+		double kd = m_use_multi_dof_params ? m_kd[row % 3] : m_kd[0];
+		btScalar velocityError = (desiredVelocity - currentVelocity) * kd;
+
+		btMatrix3x3 frameAworld;
+		frameAworld.setIdentity();
+		frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
+		btScalar posError = 0;
+		{
+			btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical);
+			switch (m_bodyA->getLink(m_linkA).m_jointType)
+			{
+				case btMultibodyLink::eSpherical:
+				{
+					btVector3 constraintNormalAng = frameAworld.getColumn(row % 3);
+					double kp = m_use_multi_dof_params ? m_kp[row % 3] : m_kp[0];
+					posError = kp*angleDiff[row % 3];
+					double max_applied_impulse = m_use_multi_dof_params ? m_maxAppliedImpulseMultiDof[row % 3] : m_maxAppliedImpulse;
+					//should multiply by time step
+					//max_applied_impulse *= infoGlobal.m_timeStep
+
+					double min_applied_impulse = -max_applied_impulse;
+					
+
+					if (posError>0)
+						max_applied_impulse=0;
+					else
+						min_applied_impulse=0;
+
+					if (btFabs(posError)>SIMD_EPSILON)
+					{
+						btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+						fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
+							zero, zero, zero,//pure angular, so zero out linear parts
+							posError,
+							infoGlobal,
+							min_applied_impulse, max_applied_impulse, true,
+							1.0, false, 0, 0,
+							damp);
+						constraintRow.m_orgConstraint = this;
+						constraintRow.m_orgDofIndex = row;
+					}
+					break;
+				}
+				default:
+				{
+					btAssert(0);
+				}
+			};
+		}
+	}
+}
+
+
+void btMultiBodySphericalJointLimit::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h
new file mode 100644
index 000000000..b82db6a99
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointLimit.h
@@ -0,0 +1,115 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2018 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///This file was written by Erwin Coumans
+
+#ifndef BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H
+#define BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H
+
+#include "btMultiBodyConstraint.h"
+struct btSolverInfo;
+
+class btMultiBodySphericalJointLimit : public btMultiBodyConstraint
+{
+protected:
+	btVector3 m_desiredVelocity;
+	btQuaternion m_desiredPosition;
+	bool m_use_multi_dof_params;
+	btVector3 m_kd;
+	btVector3 m_kp;
+	btScalar m_erp;
+	btScalar m_rhsClamp;  //maximum error
+	btVector3 m_maxAppliedImpulseMultiDof;
+	btVector3 m_pivotA;
+	btVector3 m_pivotB;
+	btScalar m_swingxRange;
+	btScalar m_swingyRange;
+	btScalar m_twistRange;
+
+public:
+	btMultiBodySphericalJointLimit(btMultiBody* body, int link, 
+		btScalar swingxRange,
+		btScalar swingyRange,
+		btScalar twistRange,
+		btScalar maxAppliedImpulse);
+	
+	virtual ~btMultiBodySphericalJointLimit();
+	virtual void finalizeMultiDof();
+
+	virtual int getIslandIdA() const;
+	virtual int getIslandIdB() const;
+
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+									  btMultiBodyJacobianData& data,
+									  const btContactSolverInfo& infoGlobal);
+
+	virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.0)
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = btVector3(kd, kd, kd);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setVelocityTargetMultiDof(const btVector3& velTarget, const btVector3& kd = btVector3(1.0, 1.0, 1.0))
+	{
+		m_desiredVelocity = velTarget;
+		m_kd = kd;
+		m_use_multi_dof_params = true;
+	}
+
+	virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp =1.f)
+	{
+		m_desiredPosition = posTarget;
+		m_kp = btVector3(kp, kp, kp);
+		m_use_multi_dof_params = false;
+	}
+
+	virtual void setPositionTargetMultiDof(const btQuaternion& posTarget, const btVector3& kp = btVector3(1.f, 1.f, 1.f))
+	{
+		m_desiredPosition = posTarget;
+		m_kp = kp;
+		m_use_multi_dof_params = true;
+	}
+
+	virtual void setErp(btScalar erp)
+	{
+		m_erp = erp;
+	}
+	virtual btScalar getErp() const
+	{
+		return m_erp;
+	}
+	virtual void setRhsClamp(btScalar rhsClamp)
+	{
+		m_rhsClamp = rhsClamp;
+	}
+
+	btScalar getMaxAppliedImpulseMultiDof(int i) const
+	{
+		return m_maxAppliedImpulseMultiDof[i];
+	}
+
+	void setMaxAppliedImpulseMultiDof(const btVector3& maxImp)
+	{
+		m_maxAppliedImpulseMultiDof = maxImp;
+		m_use_multi_dof_params = true;
+	}
+
+
+	virtual void debugDraw(class btIDebugDraw* drawer);
+
+};
+
+#endif  //BT_MULTIBODY_SPHERICAL_JOINT_LIMIT_H
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"