Merge branch 'master' of https://github.com/erwincoumans/bullet3

# Conflicts: # examples/SharedMemory/PhysicsServerCommandProcessor.cpp
2018-06-01 09:35:15 -07:00
parent 2d40a18315 00c8f8f68e
commit 92579f9196
18 changed files with 296 additions and 119 deletions
--- 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;
 }

--- 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);
                }
            }
--- 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;
-}
+}
+
+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;
+	}
+}
--- 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);
 	
 };

--- 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;
--- 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*/]);
--- 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
--- 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
--- 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)
 	
--- 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):
--- 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
--- 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
--- 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):
--- 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))