diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
index 6ae5f4b7c..7bda1a949 100644
--- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
@@ -25,6 +25,8 @@ m_guiHelper(guiHelper)
     m_mb2urdfLink.resize(totalNumJoints+1,-2);
 
     m_bulletMultiBody = new btMultiBody(totalNumJoints,mass,localInertiaDiagonal,isFixedBase,canSleep);
+	//if (canSleep)
+	//	m_bulletMultiBody->goToSleep();
     return m_bulletMultiBody;
 }
 
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index 00f1f77ab..6e02c7b05 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -595,6 +595,11 @@ void ConvertURDF2BulletInternal(
 					}
                 } else
                 {
+					//todo: fix the crash it can cause
+					//if (cache.m_bulletMultiBody->getBaseMass()==0)
+					//{
+					//	col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);//:CF_STATIC_OBJECT);
+					//}
                     cache.m_bulletMultiBody->setBaseCollider(col);
                 }
             }
diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
index cc32a3913..c025631a6 100644
--- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp
@@ -1270,6 +1270,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int link
 		status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
 		status_type = b3GetStatusType(status_handle);
 		if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) {
+			b3GetDynamicsInfo(status_handle, dynamicsInfo);
 			return true;
 		} else {
 			b3Warning("getDynamicsInfo did not complete");
@@ -1396,7 +1397,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) {
 }
 
 
-int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
+int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
 {
 	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
 	if (sm == 0) {
@@ -1407,7 +1408,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;
 
-	commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);
+	commandHandle = b3InitUserDebugDrawAddText3D(sm, text, textPosition, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);
 
 	if (args.m_parentObjectUniqueId>=0) {
 		b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
@@ -1428,12 +1429,12 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ
 	return -1;
 }
 
-int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
+int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args)
 {
 	double dposXYZ[3];
-	dposXYZ[0] = posXYZ.x();
-	dposXYZ[1] = posXYZ.y();
-	dposXYZ[2] = posXYZ.z();
+	dposXYZ[0] = textPosition.x();
+	dposXYZ[1] = textPosition.y();
+	dposXYZ[2] = textPosition.z();
 
 	return addUserDebugText(text, &dposXYZ[0], args);
 }
@@ -1626,6 +1627,10 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi
 		b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP);
 	}
 
+	if (args.m_solverResidualThreshold >= 0) {
+		b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold);
+	}
+
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	return true;
 }
@@ -2060,4 +2065,53 @@ void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* gu
 struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper()
 {
 	return m_data->m_guiHelper;
-}
\ No newline at end of file
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, int linkIndex,
+															b3CollisionShapeInformation &collisionShapeInfo)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	{
+		command = b3InitRequestCollisionShapeInformation(sm, bodyUniqueId, linkIndex);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+		statusType = b3GetStatusType(statusHandle);
+	}
+
+	btAssert(statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED);
+	if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED) {
+		b3GetCollisionShapeInformation(sm, &collisionShapeInfo);
+	}
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI_NoGUI::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo)
+{
+	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
+	if (sm == 0) {
+		b3Warning("Not connected");
+		return false;
+	}
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	{
+		commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+		statusType = b3GetStatusType(statusHandle);
+
+		btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED);
+		if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) {
+			b3GetVisualShapeInformation(sm, &visualShapeInfo);
+		}
+		return true;
+	}
+}
diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
index 02594cf40..2a3dcc328 100644
--- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h
@@ -152,7 +152,14 @@ struct b3RobotSimulatorJointMotorArrayArgs
 	double *m_forces;
 
 	b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs)
-		: m_controlMode(controlMode), m_numControlledDofs(numControlledDofs)
+		: m_controlMode(controlMode),
+		  m_numControlledDofs(numControlledDofs),
+		  m_jointIndices(NULL),
+		  m_targetPositions(NULL),
+		  m_kps(NULL),
+		  m_targetVelocities(NULL),
+		  m_kds(NULL),
+		  m_forces(NULL)
 	{
 	}
 };
@@ -204,6 +211,7 @@ struct b3RobotSimulatorSetPhysicsEngineParameters
 	double m_erp;
 	double m_contactERP;
 	double m_frictionERP;
+	double m_solverResidualThreshold;
 
 	b3RobotSimulatorSetPhysicsEngineParameters()
 		: m_fixedTimeStep(-1),
@@ -218,7 +226,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters
 		m_restitutionVelocityThreshold(-1),
 		m_erp(-1),
 		m_contactERP(-1),
-		m_frictionERP(-1)
+		m_frictionERP(-1),
+	  	m_solverResidualThreshold(-1)
 	{}
 };
 
@@ -566,6 +575,10 @@ public:
 	
 	virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
 	virtual struct GUIHelperInterface* getGuiHelper();
+
+	bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo);
+
+	bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
 	
 };
 
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index c108aa77b..afa1da448 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1516,7 +1516,7 @@ B3_SHARED_API	int	b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHan
     return 0;
 }
 
-B3_SHARED_API	b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
+B3_SHARED_API	b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
     
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1526,7 +1526,7 @@ B3_SHARED_API	b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClien
     b3Assert(command);
     command->m_type = CMD_INIT_POSE;
     command->m_updateFlags =0;
-	command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
+	command->m_initPoseArgs.m_bodyUniqueId = bodyUniqueId;
 	//a bit slow, initialing the full range to zero...
 	for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
     {
@@ -2037,16 +2037,54 @@ B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUnique
 
 
 
-B3_SHARED_API	int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
+B3_SHARED_API	int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
-	return cl->getNumJoints(bodyId);
+	return cl->getNumJoints(bodyUniqueId);
 }
 
-B3_SHARED_API	int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
+B3_SHARED_API	int	b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId)
+{
+	int nj = b3GetNumJoints(physClient, bodyUniqueId);
+	int j=0;
+	int dofCountOrg = 0;
+	for (j=0;j<nj;j++)
+	{
+		struct b3JointInfo info;
+		b3GetJointInfo(physClient, bodyUniqueId, j, &info);
+		switch (info.m_jointType)
+		{
+		case eRevoluteType:
+			{
+				dofCountOrg+=1;
+				break;
+			}
+		case ePrismaticType:
+			{
+				dofCountOrg+=1;
+				break;
+			}
+		case eSphericalType:
+			{
+				return -1;
+			}
+		case ePlanarType:
+			{
+				return -2;
+			}
+			default:
+			{
+				//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
+			}
+		}
+	}
+	return dofCountOrg;
+}
+
+B3_SHARED_API	int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info)
 {
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
-	return cl->getJointInfo(bodyIndex, jointIndex, *info);
+	return cl->getJointInfo(bodyUniqueId, jointIndex, *info);
 }
 
 
@@ -2389,7 +2427,7 @@ B3_SHARED_API	int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo
 
 
 
-B3_SHARED_API	b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
     b3Assert(cl);
@@ -2400,9 +2438,9 @@ B3_SHARED_API	b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3Ph
     command->m_type = CMD_USER_CONSTRAINT;
 	command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
 
-    command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex;
+    command->m_userConstraintArguments.m_parentBodyIndex = parentBodyUniqueId;
     command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
-    command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex;
+    command->m_userConstraintArguments.m_childBodyIndex = childBodyUniqueId;
     command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
     for (int i = 0; i < 7; ++i) {
         command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
@@ -3742,7 +3780,7 @@ B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHand
 
 
 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
-B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
 	const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -3753,8 +3791,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
 
 	command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
 	command->m_updateFlags = 0;
-	command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex;
-	int numJoints = cl->getNumJoints(bodyIndex);
+	command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
+	int numJoints = cl->getNumJoints(bodyUniqueId);
 	for (int i = 0; i < numJoints;i++)
 	{
 		command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3799,7 +3837,7 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
 	return true;
 }
 
-B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
+B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
 {
     PhysicsClient* cl = (PhysicsClient*)physClient;
     b3Assert(cl);
@@ -3809,12 +3847,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
     
     command->m_type = CMD_CALCULATE_JACOBIAN;
     command->m_updateFlags = 0;
-    command->m_calculateJacobianArguments.m_bodyUniqueId = bodyIndex;
+    command->m_calculateJacobianArguments.m_bodyUniqueId = bodyUniqueId;
     command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
     command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
     command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
     command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
-    int numJoints = cl->getNumJoints(bodyIndex);
+    int numJoints = cl->getNumJoints(bodyUniqueId);
     for (int i = 0; i < numJoints;i++)
     {
         command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3859,7 +3897,7 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
     return true;
 }
 
-B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ)
+B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ)
 {
     PhysicsClient* cl = (PhysicsClient*)physClient;
     b3Assert(cl);
@@ -3869,7 +3907,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
     
     command->m_type = CMD_CALCULATE_MASS_MATRIX;
     command->m_updateFlags = 0;
-    int numJoints = cl->getNumJoints(bodyIndex);
+    int numJoints = cl->getNumJoints(bodyUniqueId);
     for (int i = 0; i < numJoints; i++)
     {
         command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3905,7 +3943,7 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
 }
 
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
-B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
+B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
 	b3Assert(cl);
@@ -3915,7 +3953,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandIni
 
 	command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
 	command->m_updateFlags = 0;
-	command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
+	command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyUniqueId;
 
 	return (b3SharedMemoryCommandHandle)command;
 
@@ -4008,6 +4046,36 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMe
 
 }
 
+B3_SHARED_API	void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+    command->m_updateFlags |= IK_HAS_CURRENT_JOINT_POSITIONS;
+	for (int i = 0; i < numDof; ++i)
+    {
+		command->m_calculateInverseKinematicsArguments.m_currentPositions[i] = currentJointPositions[i];
+    }
+}
+
+B3_SHARED_API	void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+    command->m_updateFlags |= IK_HAS_MAX_ITERATIONS;
+	command->m_calculateInverseKinematicsArguments.m_maxNumIterations = maxNumIterations;
+}
+
+B3_SHARED_API	void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+    command->m_updateFlags |= IK_HAS_RESIDUAL_THRESHOLD;
+	command->m_calculateInverseKinematicsArguments.m_residualThreshold = residualThreshold;
+}
+
 B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index ebf83c677..3c23c2003 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -104,10 +104,14 @@ B3_SHARED_API	int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serial
 B3_SHARED_API	int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
 
 ///give a unique body index (after loading the body) return the number of joints.
-B3_SHARED_API	int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId);
+B3_SHARED_API	int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId);
+
+///compute the number of degrees of freedom for this body.
+///Return -1 for unsupported spherical joint, -2 for unsupported planar joint.
+B3_SHARED_API	int	b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId);
 
 ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
-B3_SHARED_API	int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
+B3_SHARED_API	int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info);
 
 B3_SHARED_API	b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
 ///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
@@ -128,7 +132,7 @@ B3_SHARED_API	int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHan
 B3_SHARED_API	int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius);
 B3_SHARED_API	int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold);
 
-B3_SHARED_API	b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
+B3_SHARED_API	b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);
 
 ///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
 B3_SHARED_API	int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
@@ -350,26 +354,26 @@ B3_SHARED_API	void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
 
 
 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
-B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
 	const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
 B3_SHARED_API	int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
 	int* bodyUniqueId,
 	int* dofCount,
 	double* jointForces);
 
-B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
 B3_SHARED_API	int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
 	int* dofCount,
 	double* linearJacobian,
 	double* angularJacobian);
 
-B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ);
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ);
 ///the mass matrix is stored in column-major layout of size dofCount*dofCount
 B3_SHARED_API	int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle,	int* dofCount,	double* massMatrix);
 
 
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
-B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
 B3_SHARED_API	void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
 B3_SHARED_API	void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]);
 B3_SHARED_API	void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
@@ -381,6 +385,11 @@ B3_SHARED_API	int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
 	int* dofCount,
 	double* jointPositions);
 
+B3_SHARED_API	void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions);
+B3_SHARED_API	void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations);
+B3_SHARED_API	void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold);
+
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
 B3_SHARED_API	int	b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
 B3_SHARED_API	int	b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
@@ -476,7 +485,7 @@ B3_SHARED_API	int	b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle com
 ///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
 ///base orientation and joint angles. This will set all velocities of base and joints to zero.
 ///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
-B3_SHARED_API	b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
+B3_SHARED_API	b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
 B3_SHARED_API	int	b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
 B3_SHARED_API	int	b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
 B3_SHARED_API	int	b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]);
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index bd3285351..d35d425cc 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -695,6 +695,9 @@ struct CalculateInverseKinematicsArgs
     double m_jointRange[MAX_DEGREE_OF_FREEDOM];
     double m_restPose[MAX_DEGREE_OF_FREEDOM];
     double m_jointDamping[MAX_DEGREE_OF_FREEDOM];
+	double m_currentPositions[MAX_DEGREE_OF_FREEDOM];
+	int m_maxNumIterations;
+	double m_residualThreshold;
 };
 
 struct CalculateInverseKinematicsResultArgs
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 1373bd570..bfabbfb0c 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -628,7 +628,9 @@ enum EnumCalculateInverseKinematicsFlags
 	IK_HAS_TARGET_ORIENTATION=32,
 	IK_HAS_NULL_SPACE_VELOCITY=64,
 	IK_HAS_JOINT_DAMPING=128,
-	//IK_HAS_CURRENT_JOINT_POSITIONS=256,//not used yet
+	IK_HAS_CURRENT_JOINT_POSITIONS=256,
+	IK_HAS_MAX_ITERATIONS=512,
+	IK_HAS_RESIDUAL_THRESHOLD = 1024,
 };
 
 enum b3ConfigureDebugVisualizerEnum
diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py
index 928389542..7293c15c2 100644
--- a/examples/pybullet/examples/inverse_kinematics.py
+++ b/examples/pybullet/examples/inverse_kinematics.py
@@ -34,12 +34,12 @@ t=0.
 prevPose=[0,0,0]
 prevPose1=[0,0,0]
 hasPrevPose = 0
-useNullSpace = 0
+useNullSpace = 1
 
 useOrientation = 1
 #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
 #This can be used to test the IK result accuracy.
-useSimulation = 1
+useSimulation = 0
 useRealTimeSimulation = 1
 ikSolver = 0
 p.setRealTimeSimulation(useRealTimeSimulation)
@@ -69,7 +69,7 @@ while 1:
 				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
 		else:
 			if (useOrientation==1):
-				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver)
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver, maxNumIterations=100, residualThreshold=.01)
 			else:
 				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)
 	
diff --git a/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py b/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py
index e413d3971..90f80a754 100644
--- a/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py
+++ b/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py
@@ -24,8 +24,8 @@ ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SH
 p.setPhysicsEngineParameter(solverResidualThreshold=1e-2)
 
 index = 0
-numX = 3 
-numY = 3
+numX = 10 
+numY = 10
 
 for i in range (numX):
 	for j in range (numY):
diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py
index 25d079831..f9b39b347 100644
--- a/examples/pybullet/gym/pybullet_envs/robot_bases.py
+++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py
@@ -1,4 +1,4 @@
-import pybullet
+import pybullet 
 import gym, gym.spaces, gym.utils
 import numpy as np
 import os, inspect
diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py
index 4db807f5e..7ed7a1272 100644
--- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py
+++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py
@@ -1,6 +1,6 @@
 from robot_bases import XmlBasedRobot, MJCFBasedRobot, URDFBasedRobot
 import numpy as np
-import pybullet
+import pybullet 
 import os
 import pybullet_data
 from robot_bases import BodyPart
diff --git a/examples/pybullet/gym/pybullet_utils/bullet_client.py b/examples/pybullet/gym/pybullet_utils/bullet_client.py
index 8fc1fcd5c..9b7f907f3 100644
--- a/examples/pybullet/gym/pybullet_utils/bullet_client.py
+++ b/examples/pybullet/gym/pybullet_utils/bullet_client.py
@@ -3,7 +3,7 @@ from __future__ import absolute_import
 from __future__ import division
 import functools
 import inspect
-import pybullet
+import pybullet 
 
 
 class BulletClient(object):
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index e3952e083..d6954e0b8 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -8055,9 +8055,6 @@ static PyObject* pybullet_executePluginCommand(PyObject* self,
 	return PyInt_FromLong(statusType);
 }
 
-
-
-
 ///Inverse Kinematics binding
 static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 													 PyObject* args, PyObject* keywds)
@@ -8077,9 +8074,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 	PyObject* jointRangesObj = 0;
 	PyObject* restPosesObj = 0;
 	PyObject* jointDampingObj = 0;
+	PyObject* currentPositionsObj = 0;
+	int maxNumIterations = -1;
+	double residualThreshold=-1;
 
-	static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId",  NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId))
+	static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "currentPositions", "maxNumIterations", "residualThreshold", "physicsClientId",  NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &currentPositionsObj, &maxNumIterations, &residualThreshold, &physicsClientId))
 	{
 		//backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version
 		static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId",  NULL};
@@ -8107,53 +8107,69 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 		int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0;
 		int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0;
 
+		int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0;
+
+
 		int numJoints = b3GetNumJoints(sm, bodyUniqueId);
+		int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
 
 		int hasNullSpace = 0;
 		int hasJointDamping = 0;
+		int hasCurrentPositions = 0;
 		double* lowerLimits = 0;
 		double* upperLimits = 0;
 		double* jointRanges = 0;
 		double* restPoses = 0;
 		double* jointDamping = 0;
-
-		if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
-			(szJointRanges == numJoints) && (szRestPoses == numJoints))
+		double* currentPositions = 0;
+		
+		if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) &&
+			(szJointRanges == dofCount) && (szRestPoses == dofCount))
 		{
-			int szInBytes = sizeof(double) * numJoints;
+			int szInBytes = sizeof(double) * dofCount;
 			int i;
 			lowerLimits = (double*)malloc(szInBytes);
 			upperLimits = (double*)malloc(szInBytes);
 			jointRanges = (double*)malloc(szInBytes);
 			restPoses = (double*)malloc(szInBytes);
 
-			for (i = 0; i < numJoints; i++)
+			for (i = 0; i < dofCount; i++)
 			{
-				lowerLimits[i] =
-					pybullet_internalGetFloatFromSequence(lowerLimitsObj, i);
-				upperLimits[i] =
-					pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
-				jointRanges[i] =
-					pybullet_internalGetFloatFromSequence(jointRangesObj, i);
-				restPoses[i] =
-					pybullet_internalGetFloatFromSequence(restPosesObj, i);
+				lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i);
+				upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
+				jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i);
+				restPoses[i] =	pybullet_internalGetFloatFromSequence(restPosesObj, i);
 			}
 			hasNullSpace = 1;
 		}
 
-		if (szJointDamping > 0)
+		if (szCurrentPositions > 0)
 		{
-			// We allow the number of joint damping values to be larger than
-			// the number of degrees of freedom (DOFs). On the server side, it does
-			// the check and only sets joint damping for all DOFs.
-			// We can use the number of DOFs here when that is exposed. Since the
-			// number of joints is larger than the number of DOFs (we assume the
-			// joint is either fixed joint or one DOF joint), it is safe to use
-			// number of joints here.
-			if (szJointDamping < numJoints)
+			if (szCurrentPositions != dofCount)
 			{
 				PyErr_SetString(SpamError,
-								"calculateInverseKinematics the size of input joint damping values is smaller than the number of joints.");
+								"calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom.");
+				return NULL;
+			}
+			else
+			{
+				int szInBytes = sizeof(double) * szCurrentPositions;
+				int i;
+				currentPositions = (double*)malloc(szInBytes);
+				for (i = 0; i < szCurrentPositions; i++)
+				{
+					currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i);
+				}
+				hasCurrentPositions = 1;
+			}
+		}
+
+		if (szJointDamping > 0)
+		{
+			if (szJointDamping != dofCount)
+			{
+				PyErr_SetString(SpamError,
+								"calculateInverseKinematics the size of input joint damping values is unequal to the number of degrees of freedom.");
 				return NULL;
 			}
 			else
@@ -8179,15 +8195,28 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 			b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
 			b3CalculateInverseKinematicsSelectSolver(command, solver);
 
+			if (hasCurrentPositions)
+			{
+				b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions);
+			}
+			if (maxNumIterations>0)
+			{
+				b3CalculateInverseKinematicsSetMaxNumIterations(command,maxNumIterations);
+			}
+			if (residualThreshold>=0)
+			{
+				b3CalculateInverseKinematicsSetResidualThreshold(command,residualThreshold);
+			}
+
 			if (hasNullSpace)
 			{
 				if (hasOrn)
 				{
-					b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
+					b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses);
 				}
 				else
 				{
-					b3CalculateInverseKinematicsPosWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
+					b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses);
 				}
 			}
 			else
@@ -8204,8 +8233,9 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 
 			if (hasJointDamping)
 			{
-				b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
+				b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping);
 			}
+			free(currentPositions);
 			free(jointDamping);
 
 			statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -8294,44 +8324,8 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
 			int szObPos = PySequence_Size(objPositionsQ);
 			int szObVel = PySequence_Size(objVelocitiesQdot);
 			int szObAcc = PySequence_Size(objAccelerations);
-			int nj = b3GetNumJoints(sm, bodyUniqueId);
-			int j=0;
-			int dofCountOrg = 0;
-			for (j=0;j<nj;j++)
-			{
-				struct b3JointInfo info;
-				b3GetJointInfo(sm, bodyUniqueId, j, &info);
-				switch (info.m_jointType)
-				{
-				case eRevoluteType:
-					{
-						dofCountOrg+=1;
-						break;
-					}
-				case ePrismaticType:
-					{
-						dofCountOrg+=1;
-						break;
-					}
-				case eSphericalType:
-					{
-						PyErr_SetString(SpamError,
-								"Spherirical joints are not supported in the pybullet binding");
-						return NULL;
-					}
-				case ePlanarType:
-					{
-						PyErr_SetString(SpamError,
-								"Planar joints are not supported in the pybullet binding");
-						return NULL;
-					}
-					default:
-					{
-						//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
-					}
-				}
-			}
-			
+		
+			int dofCountOrg = b3ComputeDofCount(sm, bodyUniqueId);
 
 			if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) &&
 				(szObAcc == dofCountOrg))
diff --git a/setup.py b/setup.py
index 7a3b2f443..90c0875fe 100644
--- a/setup.py
+++ b/setup.py
@@ -450,7 +450,7 @@ print("-----")
 
 setup(
 	name = 'pybullet',
-	version='2.0.1',
+	version='2.0.2',
 	description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index 578814c36..580775060 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -37,7 +37,7 @@ bool gJointFeedbackInJointFrame = false;
 
 namespace {
     const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
-    const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
+    const btScalar SLEEP_TIMEOUT = btScalar(0.3);     // in seconds
 }
 
 namespace {
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index 23ea5533f..04cb5ac0a 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -485,7 +485,7 @@ void addJointTorque(int i, btScalar Q);
 	}
 	void setCompanionId(int id)
 	{
-		//printf("for %p setCompanionId(%d)\n",this, id);
+		printf("for %p setCompanionId(%d)\n",this, id);
 		m_companionId = id;
 	}
 
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 8c6cb6aa8..68ba8d68f 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -26,6 +26,7 @@ subject to the following restrictions:
 
 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
 {
+	printf("btSequentialImpulseConstraintSolver::solveSingleIteration\n");
 	btScalar leastSquaredResidual  = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
 	
 	//solve featherstone non-contact constraints
@@ -38,6 +39,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 
 		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
 		
+		printf("m_multiBodyNonContactConstraints resolveSingleConstraintRowGeneric\n");
 		btScalar residual = resolveSingleConstraintRowGeneric(constraint);
 		leastSquaredResidual += residual*residual;
 
@@ -46,7 +48,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		if(constraint.m_multiBodyB) 
 			constraint.m_multiBodyB->setPosUpdated(false);
 	}
-
+	printf("featherstone normal contact\n");
+	printf("numContact=%d\n", m_multiBodyNormalContactConstraints.size());
 	//solve featherstone normal contact
 	for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
 	{
@@ -57,6 +60,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 
 		if (iteration < infoGlobal.m_numIterations)
 		{
+			printf("featherstone normal contact resolveSingleConstraintRowGeneric\n");
 			residual = resolveSingleConstraintRowGeneric(constraint);
 		}
 
@@ -68,6 +72,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 			constraint.m_multiBodyB->setPosUpdated(false);
 	}
 
+	printf("featherstone frictional contact\n");
 	//solve featherstone frictional contact
 	if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
 	{
@@ -158,6 +163,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 			}
 		}
 	}
+	printf("end featjerstp\n");
 	return leastSquaredResidual;
 }
 
@@ -203,6 +209,19 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
 	int ndofA = 0;
 	int ndofB = 0;
 
+	printf("c.m_solverBodyIdA=%d\n", c.m_solverBodyIdA);
+	printf("c.m_solverBodyIdB=%d\n", c.m_solverBodyIdB);
+
+	printf("c.m_multiBodyA=%p\n", c.m_multiBodyA);
+	printf("c.m_multiBodyB=%p\n", c.m_multiBodyB);
+	printf("c.m_jacAindex=%d\n",c.m_jacAindex);
+	printf("c.m_jacBindex=%d\n",c.m_jacBindex);
+	printf("c.m_deltaVelAindex=%d\n",c.m_deltaVelAindex);
+	printf("c.m_deltaVelBindex=%d\n",c.m_deltaVelBindex);
+	if (c.m_deltaVelAindex>500)
+	{
+		printf("???\n");
+	}
 	if (c.m_multiBodyA)
 	{
 		ndofA = c.m_multiBodyA->getNumDofs() + 6;
@@ -539,7 +558,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 		const int ndofA  = multiBodyA->getNumDofs() + 6;
 
+
 		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+		if (solverConstraint.m_deltaVelAindex>10000)
+		{
+			printf("????????????????????????????????????????\n");
+			printf("solverConstraint.m_deltaVelAindex==%d\n", solverConstraint.m_deltaVelAindex);
+		}
 
 		if (solverConstraint.m_deltaVelAindex <0)
 		{
@@ -1657,7 +1682,9 @@ void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodie
 	//printf("solveMultiBodyGroup start\n");
 	m_tmpMultiBodyConstraints = multiBodyConstraints;
 	m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-	
+
+	printf("m_tmpNumMultiBodyConstraints =%d\n",m_tmpNumMultiBodyConstraints );
+	printf("solveGroup\n");
 	btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 
 	m_tmpMultiBodyConstraints = 0;