Merge branch 'master' of github.com:jyc-n/bullet3

30 files changed, 728 insertions, 201 deletions

diff --git a/Extras/obj2sdf/obj2sdf.cpp b/Extras/obj2sdf/obj2sdf.cpp
index a59cd68f5..60881d50e 100644
--- a/Extras/obj2sdf/obj2sdf.cpp
+++ b/Extras/obj2sdf/obj2sdf.cpp
@@ -30,13 +30,13 @@
 #include "Bullet3Common/b3HashMap.h"
 #include "../Utils/b3BulletDefaultFileIO.h"
 
-using tinyobj::index_t;
+using bt_tinyobj::index_t;
 
 struct ShapeContainer
 {
 	std::string m_matName;
 	std::string m_shapeName;
-	tinyobj::material_t material;
+	bt_tinyobj::material_t material;
 	std::vector<float> positions;
 	std::vector<float> normals;
 	std::vector<float> texcoords;
@@ -91,11 +91,11 @@ int main(int argc, char* argv[])
 	char materialPrefixPath[MAX_PATH_LEN];
 	b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN);
 
-	std::vector<tinyobj::shape_t> shapes;
-	tinyobj::attrib_t attribute;
+	std::vector<bt_tinyobj::shape_t> shapes;
+	bt_tinyobj::attrib_t attribute;
 
 	b3BulletDefaultFileIO fileIO;
-	std::string err = tinyobj::LoadObj(attribute, shapes, fileNameWithPath, materialPrefixPath,&fileIO);
+	std::string err = bt_tinyobj::LoadObj(attribute, shapes, fileNameWithPath, materialPrefixPath, &fileIO);
 
 	char sdfFileName[MAX_PATH_LEN];
 	sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf");
@@ -110,8 +110,8 @@ int main(int argc, char* argv[])
 
 	for (int s = 0; s < (int)shapes.size(); s++)
 	{
-		tinyobj::shape_t& shape = shapes[s];
-		tinyobj::material_t mat = shape.material;
+		bt_tinyobj::shape_t& shape = shapes[s];
+		bt_tinyobj::material_t mat = shape.material;
 
 		b3HashString key = mat.name.length() ? mat.name.c_str() : "";
 		if (!gMaterialNames.find(key))
@@ -212,7 +212,7 @@ int main(int argc, char* argv[])
 
 			int faceCount = shapeCon->indices.size();
 			int vertexCount = shapeCon->positions.size();
-			tinyobj::material_t mat = shapeCon->material;
+			bt_tinyobj::material_t mat = shapeCon->material;
 			if (shapeCon->m_matName.length())
 			{
 				const char* objName = shapeCon->m_matName.c_str();
@@ -317,7 +317,7 @@ int main(int argc, char* argv[])
 	{
 		for (int s = 0; s < (int)shapes.size(); s++)
 		{
-			tinyobj::shape_t& shape = shapes[s];
+			bt_tinyobj::shape_t& shape = shapes[s];
 
 			if (shape.name.length())
 			{
@@ -351,7 +351,7 @@ int main(int argc, char* argv[])
 
 			int faceCount = shape.mesh.indices.size();
 			int vertexCount = attribute.vertices.size();
-			tinyobj::material_t mat = shape.material;
+			bt_tinyobj::material_t mat = shape.material;
 			if (shape.name.length())
 			{
 				const char* objName = shape.name.c_str();
diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf
index e4d591cec..eaaf62c24 100644
--- a/docs/pybullet_quickstartguide.pdf
+++ b/docs/pybullet_quickstartguide.pdf
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index b8142d90a..7ae38f4d3 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -2273,7 +2273,7 @@ int BulletMJCFImporter::getBodyUniqueId() const
 	return m_data->m_activeBodyUniqueId;
 }
 
-static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
+static btCollisionShape* MjcfCreateConvexHullFromShapes(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
 {
 	btCompoundShape* compound = new btCompoundShape();
 	compound->setMargin(collisionMargin);
@@ -2285,7 +2285,7 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t&
 	{
 		btConvexHullShape* convexHull = new btConvexHullShape();
 		convexHull->setMargin(collisionMargin);
-		tinyobj::shape_t& shape = shapes[s];
+		bt_tinyobj::shape_t& shape = shapes[s];
 
 		int faceCount = shape.mesh.indices.size();
 
@@ -2399,9 +2399,9 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
 							}
 							else
 							{
-								std::vector<tinyobj::shape_t> shapes;
-								tinyobj::attrib_t attribute;
-								std::string err = tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
+								std::vector<bt_tinyobj::shape_t> shapes;
+								bt_tinyobj::attrib_t attribute;
+								std::string err = bt_tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
 								//create a convex hull for each shape, and store it in a btCompoundShape
 
 								childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
index 6f13d86b2..954f0bb06 100644
--- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
@@ -64,8 +64,8 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
 		b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
 		btVector3 shift(0, 0, 0);
 
-		std::vector<tinyobj::shape_t> shapes;
-		tinyobj::attrib_t attribute;
+		std::vector<bt_tinyobj::shape_t> shapes;
+		bt_tinyobj::attrib_t attribute;
 		{
 			B3_PROFILE("tinyobj::LoadObj");
 			std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO);
@@ -79,7 +79,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
 			//try to load some texture
 			for (int i = 0; meshData.m_textureImage1 == 0 && i < shapes.size(); i++)
 			{
-				const tinyobj::shape_t& shape = shapes[i];
+				const bt_tinyobj::shape_t& shape = shapes[i];
 				meshData.m_rgbaColor[0] = shape.material.diffuse[0];
 				meshData.m_rgbaColor[1] = shape.material.diffuse[1];
 				meshData.m_rgbaColor[2] = shape.material.diffuse[2];
diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
index e5bd6633c..b78272eb8 100644
--- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
@@ -11,8 +11,8 @@
 struct CachedObjResult
 {
 	std::string m_msg;
-	std::vector<tinyobj::shape_t> m_shapes;
-	tinyobj::attrib_t m_attribute;
+	std::vector<bt_tinyobj::shape_t> m_shapes;
+	bt_tinyobj::attrib_t m_attribute;
 };
 
 static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
@@ -32,8 +32,8 @@ void b3EnableFileCaching(int enable)
 }
 
 std::string LoadFromCachedOrFromObj(
-	tinyobj::attrib_t& attribute,
-	std::vector<tinyobj::shape_t>& shapes,  // [output]
+	bt_tinyobj::attrib_t& attribute,
+	std::vector<bt_tinyobj::shape_t>& shapes,  // [output]
 	const char* filename,
 	const char* mtl_basepath,
 	struct CommonFileIOInterface* fileIO)
@@ -47,7 +47,7 @@ std::string LoadFromCachedOrFromObj(
 		return result.m_msg;
 	}
 
-	std::string err = tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO);
+	std::string err = bt_tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO);
 	CachedObjResult result;
 	result.m_msg = err;
 	result.m_shapes = shapes;
@@ -62,10 +62,10 @@ std::string LoadFromCachedOrFromObj(
 GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO)
 {
 	B3_PROFILE("LoadMeshFromObj");
-	std::vector<tinyobj::shape_t> shapes;
-	tinyobj::attrib_t attribute;
+	std::vector<bt_tinyobj::shape_t> shapes;
+	bt_tinyobj::attrib_t attribute;
 	{
-		B3_PROFILE("tinyobj::LoadObj2");
+		B3_PROFILE("bt_tinyobj::LoadObj2");
 		std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO);
 	}
 
diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h
index 54bc66312..08f5f727e 100644
--- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h
+++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h
@@ -9,8 +9,8 @@ int b3IsFileCachingEnabled();
 void b3EnableFileCaching(int enable);
 
 std::string LoadFromCachedOrFromObj(
-	tinyobj::attrib_t& attribute,
-	std::vector<tinyobj::shape_t>& shapes,  // [output]
+	bt_tinyobj::attrib_t& attribute,
+	std::vector<bt_tinyobj::shape_t>& shapes,  // [output]
 	const char* filename,
 	const char* mtl_basepath,
 	struct CommonFileIOInterface* fileIO);
diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
index f44030f21..aa3a23817 100644
--- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
@@ -9,7 +9,7 @@
 #include "../../OpenGLWindow/GLInstancingRenderer.h"
 #include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
 
-GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading)
+GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, bool flatShading)
 {
 	b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
 	{
@@ -19,7 +19,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
 
 		for (int s = 0; s < (int)shapes.size(); s++)
 		{
-			tinyobj::shape_t& shape = shapes[s];
+			bt_tinyobj::shape_t& shape = shapes[s];
 			int faceCount = shape.mesh.indices.size();
 
 			for (int f = 0; f < faceCount; f += 3)
@@ -36,7 +36,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
 					}
 
 					GLInstanceVertex vtx0;
-					tinyobj::index_t v_0 = shape.mesh.indices[f];
+					bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
 					vtx0.xyzw[0] = attribute.vertices[3 * v_0.vertex_index];
 					vtx0.xyzw[1] = attribute.vertices[3 * v_0.vertex_index + 1];
 					vtx0.xyzw[2] = attribute.vertices[3 * v_0.vertex_index + 2];
@@ -65,7 +65,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
 					}
 
 					GLInstanceVertex vtx1;
-					tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
+					bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
 					vtx1.xyzw[0] = attribute.vertices[3 * v_1.vertex_index];
 					vtx1.xyzw[1] = attribute.vertices[3 * v_1.vertex_index + 1];
 					vtx1.xyzw[2] = attribute.vertices[3 * v_1.vertex_index + 2];
@@ -94,7 +94,7 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::a
 					}
 
 					GLInstanceVertex vtx2;
-					tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
+					bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
 					vtx2.xyzw[0] = attribute.vertices[3 * v_2.vertex_index];
 					vtx2.xyzw[1] = attribute.vertices[3 * v_2.vertex_index + 1];
 					vtx2.xyzw[2] = attribute.vertices[3 * v_2.vertex_index + 2];
diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
index 4054b4dab..4e9a8d52c 100644
--- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
+++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h
@@ -4,6 +4,6 @@
 #include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
 #include <vector>
 
-struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, bool flatShading = false);
+struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, bool flatShading = false);
 
 #endif  //WAVEFRONT2GRAPHICS_H
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index 71411bf9b..e24437d7d 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
@@ -540,7 +547,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
 	return true;
 }
 
-static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
+static btCollisionShape* createConvexHullFromShapes(const bt_tinyobj::attrib_t& attribute, std::vector<bt_tinyobj::shape_t>& shapes, const btVector3& geomScale, int flags)
 {
 	B3_PROFILE("createConvexHullFromShapes");
 	btCompoundShape* compound = new btCompoundShape();
@@ -553,7 +560,7 @@ static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& att
 	{
 		btConvexHullShape* convexHull = new btConvexHullShape();
 		convexHull->setMargin(gUrdfDefaultCollisionMargin);
-		tinyobj::shape_t& shape = shapes[s];
+		bt_tinyobj::shape_t& shape = shapes[s];
 		int faceCount = shape.mesh.indices.size();
 
 		for (int f = 0; f < faceCount; f += 3)
@@ -747,9 +754,9 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
 					}
 					else
 					{
-						std::vector<tinyobj::shape_t> shapes;
-						tinyobj::attrib_t attribute;
-						std::string err = tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
+						std::vector<bt_tinyobj::shape_t> shapes;
+						bt_tinyobj::attrib_t attribute;
+						std::string err = bt_tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO);
 						//create a convex hull for each shape, and store it in a btCompoundShape
 						shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
 						m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
index 6483e8998..5fe7f8c5f 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 f03c7610f..aea391592 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -1455,6 +1455,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)
 	{
@@ -1469,13 +1470,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)
 		{
@@ -1502,6 +1509,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)
@@ -1509,6 +1524,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 126285fc2..8256099a5 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/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 813b3ff86..fcf04c778 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -3455,34 +3455,46 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
 			}
 		}
 
-		// Because the link order between UrdfModel and MultiBody may be different,
-		// create a mapping from link name to link index in order to apply the user
-		// data to the correct link in the MultiBody.
-		btHashMap<btHashString, int> linkNameToIndexMap;
-		if (bodyHandle->m_multiBody)
-		{
-			btMultiBody* mb = bodyHandle->m_multiBody;
-			linkNameToIndexMap.insert(mb->getBaseName(), -1);
-			for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
-			{
-				linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
-			}
-		}
-
+		// Add user data specified in URDF to the added body.
 		const UrdfModel* urdfModel = u2b.getUrdfModel();
 		if (urdfModel)
 		{
 			addUserData(urdfModel->m_userData, bodyUniqueId);
-			for (int i = 0; i < urdfModel->m_links.size(); ++i)
+			if (bodyHandle->m_multiBody)
 			{
-				const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
-				int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
-				if (linkIndex)
+				btMultiBody* mb = bodyHandle->m_multiBody;
+				// Because the link order between UrdfModel and MultiBody may be different,
+				// create a mapping from link name to link index in order to apply the user
+				// data to the correct link in the MultiBody.
+				btHashMap<btHashString, int> linkNameToIndexMap;
+				linkNameToIndexMap.insert(mb->getBaseName(), -1);
+				for (int linkIndex = 0; linkIndex < mb->getNumLinks(); ++linkIndex)
+				{
+					linkNameToIndexMap.insert(mb->getLink(linkIndex).m_linkName, linkIndex);
+				}
+				for (int i = 0; i < urdfModel->m_links.size(); ++i)
 				{
-					addUserData(link->m_userData, bodyUniqueId, *linkIndex);
+					const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
+					int* linkIndex = linkNameToIndexMap.find(link->m_name.c_str());
+					if (linkIndex)
+					{
+						addUserData(link->m_userData, bodyUniqueId, *linkIndex);
+						for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
+						{
+							addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex);
+						}
+					}
+				}
+			}
+			else if (bodyHandle->m_rigidBody)
+			{
+				for (int i = 0; i < urdfModel->m_links.size(); ++i)
+				{
+					const UrdfLink* link = *urdfModel->m_links.getAtIndex(i);
+					addUserData(link->m_userData, bodyUniqueId, -1);
 					for (int visualShapeIndex = 0; visualShapeIndex < link->m_visualArray.size(); ++visualShapeIndex)
 					{
-						addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, *linkIndex, visualShapeIndex);
+						addUserData(link->m_visualArray.at(visualShapeIndex).m_userData, bodyUniqueId, -1, visualShapeIndex);
 					}
 				}
 			}
@@ -5302,12 +5314,12 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 						{
 							//create a convex hull for each shape, and store it in a btCompoundShape
 
-							std::vector<tinyobj::shape_t> shapes;
-							tinyobj::attrib_t attribute;
-							std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
+							std::vector<bt_tinyobj::shape_t> shapes;
+							bt_tinyobj::attrib_t attribute;
+							std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
 
 							//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
-							//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
+							//static btCollisionShape* createConvexHullFromShapes(std::vector<bt_tinyobj::shape_t>& shapes, const btVector3& geomScale)
 							B3_PROFILE("createConvexHullFromShapes");
 							if (compound == 0)
 							{
@@ -5319,7 +5331,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
 							{
 								btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
 								convexHull->setMargin(m_data->m_defaultCollisionMargin);
-								tinyobj::shape_t& shape = shapes[s];
+								bt_tinyobj::shape_t& shape = shapes[s];
 								int faceCount = shape.mesh.indices.size();
 
 								for (int f = 0; f < faceCount; f += 3)
@@ -8100,26 +8112,21 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 	{
 		return false;
 	}
+	int numSoftbodyContact = 0;
+	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
+	{
+		numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size();
+	}
+	int num_contact_points = m_data->m_cachedContactPoints.size();
+	m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact);
 
 	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
 	{
 		btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
-		btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
-		btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
 		for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
 		{
 			const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
-			// calculate normal and tangent impulse
-			btVector3 impulse = contact->m_cti.m_impulse;
-			btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal;
-			btVector3 impulseTangent = impulse - impulseNormal;
-			// get node in contact
-			int contactNodeIdx = contact->m_bary.maxAxis();
-			btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx];
-			// check if node is already in the list
-			int idx = nodesInContact.findLinearSearch2(node);
-
-			//apply the filter, if the user provides it
+			//convert rigidbody contact
 			int linkIndexA = -1;
 			int linkIndexB = -1;
 			int objectIndexA = psb->getUserIndex2();
@@ -8136,6 +8143,8 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 				linkIndexB = mblB->m_link;
 				objectIndexB = mblB->m_multiBody->getUserIndex2();
 			}
+
+			//apply the filter, if the user provides it
 			bool swap = false;
 			if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
 			{
@@ -8181,87 +8190,37 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 			{
 				continue;
 			}
-
-			if (idx < 0)
+			b3ContactPointData pt;
+			pt.m_bodyUniqueIdA = objectIndexA;
+			pt.m_bodyUniqueIdB = objectIndexB;
+			pt.m_contactDistance = contact->m_cti.m_offset;
+			pt.m_contactFlags = 0;
+			pt.m_linkIndexA = linkIndexA;
+			pt.m_linkIndexB = linkIndexB;
+			for (int j = 0; j < 3; j++)
 			{
-				// add new node and contact point
-				nodesInContact.push_back(node);
-				b3ContactPointData pt;
-				pt.m_bodyUniqueIdA = objectIndexA;
-				pt.m_bodyUniqueIdB = objectIndexB;
-				pt.m_contactDistance = -contact->m_cti.m_offset;
-				pt.m_contactFlags = 0;
-				pt.m_linkIndexA = linkIndexA;
-				pt.m_linkIndexB = linkIndexB;
-				for (int j = 0; j < 3; j++)
+				if (swap)
 				{
-					if (swap)
-					{
-						pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
-						pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
-						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
-						pt.m_positionOnBInWS[j] = node->m_x[j];
-					}
-					else
-					{
-						pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
-						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
-						pt.m_positionOnAInWS[j] = node->m_x[j];
-						pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
-					}
+					pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
+					pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j];
+					pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j];
 				}
-				pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm();
-				pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm();
-				pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm();
-				for (int j = 0; j < 3; j++)
+				else
 				{
-					pt.m_linearFrictionDirection1[j] = contact->t1[j];
-					pt.m_linearFrictionDirection2[j] = contact->t2[j];
+					pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
+					pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j];
+					pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j];
 				}
-				distinctContactPoints.push_back(pt);
 			}
-			else
+			pt.m_normalForce = 1;
+			pt.m_linearFrictionForce1 = 0;
+			pt.m_linearFrictionForce2 = 0;
+			for (int j = 0; j < 3; j++)
 			{
-				// add values to existing contact point
-				b3ContactPointData* pt = &distinctContactPoints[idx];
-				// current normal force of node
-				btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]),
-												  btScalar(pt->m_contactNormalOnBInWS[1]),
-												  btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce;
-				// add normal force of additional node contact
-				btScalar swapFactor = swap ? -1.0 : 1.0;
-				normalForce += swapFactor * contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm();
-				// get magnitude of normal force
-				pt->m_normalForce = normalForce.norm();
-				// get direction of normal force
-				if (!normalForce.fuzzyZero())
-				{
-					// normalize for unit vectors if above numerical threshold
-					normalForce.normalize();
-					for (int j = 0; j < 3; j++)
-					{
-						pt->m_contactNormalOnBInWS[j] = normalForce[j];
-					}
-				}
-
-				// add magnitudes of tangential forces in existing directions
-				btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]),
-															   btScalar(pt->m_linearFrictionDirection1[1]),
-															   btScalar(pt->m_linearFrictionDirection1[2]));
-				btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]),
-															   btScalar(pt->m_linearFrictionDirection2[1]),
-															   btScalar(pt->m_linearFrictionDirection2[2]));
-				pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm();
-				pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm();
+				pt.m_linearFrictionDirection1[j] = 0;
+				pt.m_linearFrictionDirection2[j] = 0;
 			}
-		}
-
-		int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size();
-		m_data->m_cachedContactPoints.reserve(num_contact_points);
-		// add points to contact points cache
-		for (int p = 0; p < distinctContactPoints.size(); p++)
-		{
-			m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]);
+			m_data->m_cachedContactPoints.push_back(pt);
 		}
 	}
 #endif
@@ -9055,12 +9014,12 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 	}
 	if (out_sim_type == UrdfGeometry::FILE_OBJ)
 	{
-		std::vector<tinyobj::shape_t> shapes;
-		tinyobj::attrib_t attribute;
-		std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
+		std::vector<bt_tinyobj::shape_t> shapes;
+		bt_tinyobj::attrib_t attribute;
+		std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
 		if (!shapes.empty())
 		{
-			const tinyobj::shape_t& shape = shapes[0];
+			const bt_tinyobj::shape_t& shape = shapes[0];
 			btAlignedObjectArray<btScalar> vertices;
 			btAlignedObjectArray<int> indices;
 			for (int i = 0; i < attribute.vertices.size(); i++)
@@ -9204,14 +9163,14 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 			}
 			else
 			{
-				tinyobj::attrib_t attribute;
-				std::vector<tinyobj::shape_t> shapes;
+				bt_tinyobj::attrib_t attribute;
+				std::vector<bt_tinyobj::shape_t> shapes;
 
-				std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
+				std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
 
 				for (int s = 0; s < (int)shapes.size(); s++)
 				{
-					tinyobj::shape_t& shape = shapes[s];
+					bt_tinyobj::shape_t& shape = shapes[s];
 					int faceCount = shape.mesh.indices.size();
 					int vertexCount = attribute.vertices.size() / 3;
 					for (int v = 0; v < vertexCount; v++)
@@ -9226,9 +9185,9 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 						{
 							continue;
 						}
-						tinyobj::index_t v_0 = shape.mesh.indices[f];
-						tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
-						tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
+						bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
+						bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
+						bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
 						btSoftBody::RenderFace ff;
 						ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
 						ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];
@@ -9653,14 +9612,14 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 			}
 			else
 			{
-				tinyobj::attrib_t attribute;
-				std::vector<tinyobj::shape_t> shapes;
+				bt_tinyobj::attrib_t attribute;
+				std::vector<bt_tinyobj::shape_t> shapes;
 
-				std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
+				std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
 
 				for (int s = 0; s < (int)shapes.size(); s++)
 				{
-					tinyobj::shape_t& shape = shapes[s];
+					bt_tinyobj::shape_t& shape = shapes[s];
 					int faceCount = shape.mesh.indices.size();
 					int vertexCount = attribute.vertices.size() / 3;
 					for (int v = 0; v < vertexCount; v++)
@@ -9675,9 +9634,9 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 						{
 							continue;
 						}
-						tinyobj::index_t v_0 = shape.mesh.indices[f];
-						tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
-						tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
+						bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
+						bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
+						bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
 						btSoftBody::RenderFace ff;
 						ff.m_n[0] = &rsb->m_renderNodes[v_0.vertex_index];
 						ff.m_n[1] = &rsb->m_renderNodes[v_1.vertex_index];
diff --git a/examples/ThirdPartyLibs/Wavefront/main.cpp b/examples/ThirdPartyLibs/Wavefront/main.cpp
index 5f8695953..cbf9f7582 100644
--- a/examples/ThirdPartyLibs/Wavefront/main.cpp
+++ b/examples/ThirdPartyLibs/Wavefront/main.cpp
@@ -38,8 +38,8 @@ TestLoadObj(
 
 	std::cout << "Loading " << fullPath << std::endl;
 
-	std::vector<tinyobj::shape_t> shapes;
-	std::string err = tinyobj::LoadObj(shapes, fullPath, prefix[index]);
+	std::vector<bt_tinyobj::shape_t> shapes;
+	std::string err = bt_tinyobj::LoadObj(shapes, fullPath, prefix[index]);
 
 	if (!err.empty())
 	{
diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
index d1b9c6362..3ad7a6f0a 100644
--- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
@@ -31,7 +31,7 @@
 #include "tiny_obj_loader.h"
 #include <stdio.h>
 
-namespace tinyobj
+namespace bt_tinyobj
 {
 #ifdef USE_STREAM
 //See http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
@@ -880,4 +880,4 @@ LoadObj(
 	return err.str();
 }
 
-};  // namespace tinyobj
+};  // namespace bt_tinyobj
diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
index 1fb15b34a..f05da52c7 100644
--- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
@@ -3,8 +3,8 @@
 //
 // Licensed under 2-clause BSD liecense.
 //
-#ifndef _TINY_OBJ_LOADER_H
-#define _TINY_OBJ_LOADER_H
+#ifndef _BT_TINY_OBJ_LOADER_H
+#define _BT_TINY_OBJ_LOADER_H
 
 #include <string>
 #include <vector>
@@ -12,7 +12,7 @@
 
 struct CommonFileIOInterface;
 
-namespace tinyobj
+namespace bt_tinyobj
 {
 struct vertex_index_t
 {
@@ -94,6 +94,6 @@ LoadObj(
 	CommonFileIOInterface* fileIO);
 #endif
 
-};  // namespace tinyobj
+};  // namespace bt_tinyobj
 
-#endif  // _TINY_OBJ_LOADER_H
+#endif  // _BT_TINY_OBJ_LOADER_H
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/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
index 8ab49b103..e07c8d505 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizer_base.py
@@ -25,11 +25,28 @@ class EnvRandomizerBase(object):
     pass
 
   def randomize_step(self, env):
-    """Randomize simulation steps.
+    """Randomize environment steps.
 
-    Will be called at every timestep. May add random forces/torques to Minitaur.
+    Will be called at every environment step.
+
+    It is NOT recommended to use this for force / torque disturbance because
+    pybullet applyExternalForce/Torque only persist for single simulation step
+    not the entire env step which can contain multiple simulation steps.
+
+    Args:
+      env: The Minitaur gym environment to be randomized.
+    """
+    pass
+
+  def randomize_sub_step(self, env, sub_step_index, num_sub_steps):
+    """Randomize simulation sub steps.
+
+    Will be called at every simulation step. This is the correct place to add
+    random forces/torques.
 
     Args:
       env: The Minitaur gym environment to be randomized.
+      sub_step_index: Index of sub step, from 0 to N-1. N is the action repeat.
+      num_sub_steps: Number of sub steps, equals to action repeat.
     """
     pass
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
index 8d9aa8e64..aa4adb7d4 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs/env_randomizers/minitaur_push_randomizer.py
@@ -10,7 +10,9 @@ parentdir = os.path.dirname(os.path.dirname(currentdir))
 parentdir = os.path.dirname(os.path.dirname(parentdir))
 os.sys.path.insert(0, parentdir)
 
+import functools
 import math
+import gin
 import numpy as np
 from pybullet_envs.minitaur.envs import env_randomizer_base
 
@@ -23,6 +25,7 @@ _VERTICAL_FORCE_UPPER_BOUND = 300
 _VERTICAL_FORCE_LOWER_BOUND = 500
 
 
+@gin.configurable
 class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
   """Applies a random impulse to the base of Minitaur."""
 
@@ -54,6 +57,7 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
                                     [_HORIZONTAL_FORCE_LOWER_BOUND, _HORIZONTAL_FORCE_UPPER_BOUND])
     self._vertical_force_bound = (vertical_force_bound if vertical_force_bound else
                                   [_VERTICAL_FORCE_LOWER_BOUND, _VERTICAL_FORCE_UPPER_BOUND])
+    self._perturbation_parameter_dict = None
 
   def randomize_env(self, env):
     """Randomizes the simulation environment.
@@ -64,9 +68,10 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
     pass
 
   def randomize_step(self, env):
-    """Randomizes simulation steps.
+    """Randomizes env steps.
 
-    Will be called at every timestep. May add random forces/torques to Minitaur.
+    Will be called at every env step. Called to generate randomized  force and
+    torque to apply. Application of forces are done in randomize_sub_step.
 
     Args:
       env: The Minitaur gym environment to be randomized.
@@ -85,8 +90,25 @@ class MinitaurPushRandomizer(env_randomizer_base.EnvRandomizerBase):
     if (env.env_step_counter % self._perturbation_interval_steps <
         self._perturbation_duration_steps) and (env.env_step_counter >=
                                                 self._perturbation_start_step):
-      env.pybullet_client.applyExternalForce(objectUniqueId=env.minitaur.quadruped,
-                                             linkIndex=self._applied_link_id,
-                                             forceObj=self._applied_force,
-                                             posObj=[0.0, 0.0, 0.0],
-                                             flags=env.pybullet_client.LINK_FRAME)
+      # Parameter of pybullet_client.applyExternalForce()
+      self._perturbation_parameter_dict = dict(objectUniqueId=env.minitaur.quadruped,
+                                               linkIndex=self._applied_link_id,
+                                               forceObj=self._applied_force,
+                                               posObj=[0.0, 0.0, 0.0],
+                                               flags=env.pybullet_client.LINK_FRAME)
+    else:
+      self._perturbation_parameter_dict = None
+
+  def randomize_sub_step(self, env, sub_step_index, num_sub_steps):
+    """Randomize simulation steps per sub steps (simulation step).
+
+    Will be called at every simulation step. This is the correct place to add
+    random forces/torques to Minitaur.
+
+    Args:
+      env: The Minitaur gym environment to be randomized.
+      sub_step_index: Index of sub step, from 0 to N-1. N is the action repeat.
+      num_sub_steps: Number of sub steps, equals to action repeat.
+    """
+    if self._perturbation_parameter_dict is not None:
+      env.pybullet_client.applyExternalForce(**self._perturbation_parameter_dict)
diff --git a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
index 0c30e5b8e..b98d3b005 100644
--- a/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
+++ b/examples/pybullet/gym/pybullet_envs/minitaur/envs_v2/locomotion_gym_env.py
@@ -457,6 +457,8 @@ class LocomotionGymEnv(gym.Env):
     for obj in self._dynamic_objects():
       obj.pre_control_step(autonomous_object.AUTONOMOUS_ACTION)
     for _ in range(self._num_action_repeat):
+      for env_randomizer in self._env_randomizers:
+        env_randomizer.randomize_sub_step(self, i, self._num_action_repeat)
       self._robot.apply_action(action)
       for obj in self._dynamic_objects():
         obj.update(self.get_time_since_reset(), self._observation_dict)
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/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp
index 9c8e72f50..09398d79a 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 	{
 		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 	}
-	// dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
 	if (!infoGlobal.m_splitImpulse)
 	{
@@ -487,9 +487,6 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 	btVector3 dv = impulse * contact->m_c2;
 	btSoftBody::Face* face = contact->m_face;
 
-	// save applied impulse
-	contact->m_cti.m_impulse = impulse;
-
 	btVector3& v0 = face->m_n[0]->m_v;
 	btVector3& v1 = face->m_n[1]->m_v;
 	btVector3& v2 = face->m_n[2]->m_v;
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 827295d78..1016a9aa3 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -223,12 +223,10 @@ public:
 	/* sCti is Softbody contact info	*/
 	struct sCti
 	{
-		const btCollisionObject* m_colObj; /* Rigid body			        */
-		btVector3 m_normal;                /* Outward normal		        */
-		mutable btVector3 m_impulse;	   /* Applied impulse        	    */
-		btScalar m_offset;                 /* Offset from origin	        */
+		const btCollisionObject* m_colObj; /* Rigid body			*/
+		btVector3 m_normal;                /* Outward normal		*/
+		btScalar m_offset;                 /* Offset from origin	*/
 		btVector3 m_bary;                  /* Barycentric weights for faces */
-		sCti() : m_impulse(0, 0, 0) {}
 	};
 
 	/* sMedium		*/
@@ -897,7 +895,7 @@ public:
 				   int node1) const;
 	bool checkLink(const Node* node0,
 				   const Node* node1) const;
-	/* Check for existing face												*/
+	/* Check for existring face												*/
 	bool checkFace(int node0,
 				   int node1,
 				   int node2) const;
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"