add quadruped.py script to load and initialize the a Minitaur-like quadruped

pybullet removeConstraint, createConstraint
rename b3CreateJoint to b3InitCreateUserConstraintCommand
add int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -960,7 +960,9 @@ int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
 	return cl->getJointInfo(bodyIndex, jointIndex, *info);
 }
 
-b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
+
+
+b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
     b3Assert(cl);
@@ -968,22 +970,52 @@ b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int
     struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
     b3Assert(command);
     
-    command->m_type = CMD_CREATE_JOINT;
+    command->m_type = CMD_USER_CONSTRAINT;
-    command->m_createJointArguments.m_parentBodyIndex = parentBodyIndex;
+	command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
-    command->m_createJointArguments.m_parentJointIndex = parentJointIndex;
+
-    command->m_createJointArguments.m_childBodyIndex = childBodyIndex;
+    command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex;
-    command->m_createJointArguments.m_childJointIndex = childJointIndex;
+    command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
+    command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex;
+    command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
     for (int i = 0; i < 7; ++i) {
-        command->m_createJointArguments.m_parentFrame[i] = info->m_parentFrame[i];
+        command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
-        command->m_createJointArguments.m_childFrame[i] = info->m_childFrame[i];
+        command->m_userConstraintArguments.m_childFrame[i] = info->m_childFrame[i];
     }
     for (int i = 0; i < 3; ++i) {
-        command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i];
+        command->m_userConstraintArguments.m_jointAxis[i] = info->m_jointAxis[i];
     }
-    command->m_createJointArguments.m_jointType = info->m_jointType;
+    command->m_userConstraintArguments.m_jointType = info->m_jointType;
     return (b3SharedMemoryCommandHandle)command;
 }
 
+b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
+{
+	 PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    
+    command->m_type = CMD_USER_CONSTRAINT;
+	command->m_updateFlags = USER_CONSTRAINT_REMOVE_CONSTRAINT;
+	command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
+	return (b3SharedMemoryCommandHandle)command;
+}
+int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+	b3Assert(status);
+	b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
+	if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
+	{
+		return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
+	}
+
+	return -1;
+
+}
+
+
 b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
                                        double rayFromWorldY, double rayFromWorldZ,
                                        double rayToWorldX, double rayToWorldY, double rayToWorldZ)

examples/SharedMemory/PhysicsClientC_API.h

@@ -72,7 +72,9 @@ int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
 ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
 int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
     
-b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
+b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
+int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
+b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
 
 ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
 ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -731,7 +731,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 				b3Warning("User debug draw failed");
 				break;
 			}
-
+			case CMD_USER_CONSTRAINT_COMPLETED:
+			{
+				break;
+			}
+			case CMD_USER_CONSTRAINT_FAILED:
+			{
+				b3Warning("createConstraint failed");
+				break;
+			}
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);

examples/SharedMemory/PhysicsDirect.cpp

@@ -671,10 +671,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 	{
 		break;
 	}
-
+	case CMD_USER_CONSTRAINT_COMPLETED:
+	{
+		break;
+	}
+	case CMD_USER_CONSTRAINT_FAILED:
+	{
+		b3Warning("createConstraint failed");
+		break;
+	}
 	default:
 	{
-		b3Warning("Unknown server status type");
+		//b3Warning("Unknown server status type");
 	}
 	};
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -123,6 +123,17 @@ struct InteralBodyData
 	}
 };
 
+struct InteralUserConstraintData
+{
+	btTypedConstraint* m_rbConstraint;
+	btMultiBodyConstraint* m_mbConstraint;
+	InteralUserConstraintData()
+		:m_rbConstraint(0),
+		m_mbConstraint(0)
+	{
+	}
+};
+
 ///todo: templatize this
 struct InternalBodyHandle : public InteralBodyData
 {
@@ -468,6 +479,10 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
 	btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
 	btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
+	
+	int m_userConstraintUIDGenerator;
+	btHashMap<btHashInt, InteralUserConstraintData> m_userConstraints;
+
 	b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
 
 
@@ -532,6 +547,7 @@ struct PhysicsServerCommandProcessorInternalData
 		m_logPlayback(0),
 		m_physicsDeltaTime(1./240.),
         m_numSimulationSubSteps(0),
+		m_userConstraintUIDGenerator(1),
 		m_dynamicsWorld(0),
 		m_remoteDebugDrawer(0),
 		m_guiHelper(0),
@@ -3097,72 +3113,144 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     hasStatus = true;
                     break;
                 }
-                case CMD_CREATE_JOINT:
+                case CMD_USER_CONSTRAINT:
                 {
-                    InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_createJointArguments.m_parentBodyIndex);
-                    if (parentBody && parentBody->m_multiBody)
-                    {
-                        InteralBodyData* childBody = m_data->getHandle(clientCmd.m_createJointArguments.m_childBodyIndex);
-                        if (childBody)
-                        {
-                            btVector3 pivotInParent(clientCmd.m_createJointArguments.m_parentFrame[0], clientCmd.m_createJointArguments.m_parentFrame[1], clientCmd.m_createJointArguments.m_parentFrame[2]);
-                            btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]);
-                            btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6]));
-                            btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6]));
-                            btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]);
-                            if (clientCmd.m_createJointArguments.m_jointType == eFixedType)
-                            {
-                                if (childBody->m_multiBody)
-                                {
-                                    btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
-                                    multibodyFixed->setMaxAppliedImpulse(500.0);
-                                    m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
-                                }
-                                else
-                                {
-                                    btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
-                                    rigidbodyFixed->setMaxAppliedImpulse(500.0);
-                                    btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
-                                    world->addMultiBodyConstraint(rigidbodyFixed);
-                                }
-                            }
-                            else if (clientCmd.m_createJointArguments.m_jointType == ePrismaticType)
-                            {
-                                if (childBody->m_multiBody)
-                                {
-                                    btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
-                                    multibodySlider->setMaxAppliedImpulse(500.0);
-                                    m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
-                                }
-                                else
-                                {
-                                    btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
-                                    rigidbodySlider->setMaxAppliedImpulse(500.0);
-                                    btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
-                                    world->addMultiBodyConstraint(rigidbodySlider);
-                                }
-                            } else if (clientCmd.m_createJointArguments.m_jointType == ePoint2PointType)
-                            {
-                                if (childBody->m_multiBody)
-                                {
-									btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild);
-                                    p2p->setMaxAppliedImpulse(500);
-                                    m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
-                                }
-                                else
-                                {
-                                    btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
-                                    p2p->setMaxAppliedImpulse(500);
-                                    btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
-                                    world->addMultiBodyConstraint(p2p);
-                                }
-                            }
-
-                        }
-                    }
 					SharedMemoryStatus& serverCmd =serverStatusOut;
-                    serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+                    serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
                     hasStatus = true;
+
+					if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
+					{
+						InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
+						if (parentBody && parentBody->m_multiBody)
+						{
+							InteralBodyData* childBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex);
+							if (childBody)
+							{
+								btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
+								btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
+								btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
+								btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
+								btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
+								if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
+								{
+									if (childBody->m_multiBody)
+									{
+										btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
+										multibodyFixed->setMaxAppliedImpulse(500.0);
+										m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
+										InteralUserConstraintData userConstraintData;
+										userConstraintData.m_mbConstraint = multibodyFixed;
+										int uid = m_data->m_userConstraintUIDGenerator++;
+										m_data->m_userConstraints.insert(uid,userConstraintData);
+										serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+										serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+									
+									}
+									else
+									{
+										btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
+										rigidbodyFixed->setMaxAppliedImpulse(500.0);
+										btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+										world->addMultiBodyConstraint(rigidbodyFixed);
+										InteralUserConstraintData userConstraintData;
+										userConstraintData.m_mbConstraint = rigidbodyFixed;
+										int uid = m_data->m_userConstraintUIDGenerator++;
+										m_data->m_userConstraints.insert(uid,userConstraintData);
+										serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+										serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+									}
+								
+								}
+								else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
+								{
+									if (childBody->m_multiBody)
+									{
+										btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
+										multibodySlider->setMaxAppliedImpulse(500.0);
+										m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
+										InteralUserConstraintData userConstraintData;
+										userConstraintData.m_mbConstraint = multibodySlider;
+										int uid = m_data->m_userConstraintUIDGenerator++;
+										m_data->m_userConstraints.insert(uid,userConstraintData);
+										serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+										serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+									}
+									else
+									{
+										btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
+										rigidbodySlider->setMaxAppliedImpulse(500.0);
+										btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+										world->addMultiBodyConstraint(rigidbodySlider);
+										InteralUserConstraintData userConstraintData;
+										userConstraintData.m_mbConstraint = rigidbodySlider;
+										int uid = m_data->m_userConstraintUIDGenerator++;
+										m_data->m_userConstraints.insert(uid,userConstraintData);
+										serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+										serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+									}
+								
+								} else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
+								{
+									if (childBody->m_multiBody)
+									{
+										btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
+										p2p->setMaxAppliedImpulse(500);
+										m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
+										InteralUserConstraintData userConstraintData;
+										userConstraintData.m_mbConstraint = p2p;
+										int uid = m_data->m_userConstraintUIDGenerator++;
+										m_data->m_userConstraints.insert(uid,userConstraintData);
+										serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+										serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+									}
+									else
+									{
+										btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
+										p2p->setMaxAppliedImpulse(500);
+										btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+										world->addMultiBodyConstraint(p2p);
+										InteralUserConstraintData userConstraintData;
+										userConstraintData.m_mbConstraint = p2p;
+										int uid = m_data->m_userConstraintUIDGenerator++;
+										m_data->m_userConstraints.insert(uid,userConstraintData);
+										serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+										serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+									}
+								
+								} else
+								{
+									b3Warning("unknown constraint type");
+								}
+
+							
+							}
+						}
+					}
+
+					if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
+					{
+						int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
+						InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
+						if (userConstraintPtr)
+						{
+							if (userConstraintPtr->m_mbConstraint)
+							{
+								m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint);
+								delete userConstraintPtr->m_mbConstraint;
+								m_data->m_userConstraints.remove(userConstraintUidRemove);
+							}
+							if (userConstraintPtr->m_rbConstraint)
+							{
+
+							}
+							serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
+							serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+						}
+
+						
+					}
+					
                     break;
                 }
 				case CMD_CALCULATE_INVERSE_KINEMATICS:

examples/SharedMemory/SharedMemoryCommands.h

@@ -488,7 +488,14 @@ struct CalculateInverseKinematicsResultArgs
 	double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
 };
 
-struct CreateJointArgs
+enum EnumUserConstraintFlags
+{
+    USER_CONSTRAINT_ADD_CONSTRAINT=1,
+	USER_CONSTRAINT_REMOVE_CONSTRAINT=2,
+	USER_CONSTRAINT_CHANGE_CONSTRAINT=4
+};
+
+struct UserConstraintArgs
 {
     int m_parentBodyIndex;
     int m_parentJointIndex;
@@ -498,9 +505,13 @@ struct CreateJointArgs
     double m_childFrame[7];
     double m_jointAxis[3];
     int m_jointType;
+	int m_userConstraintUniqueId;
 };
 
-
+struct UserConstraintResultArgs
+{
+	int m_userConstraintUniqueId;
+};
 
 enum EnumUserDebugDrawFlags
 {
@@ -563,7 +574,7 @@ struct SharedMemoryCommand
         struct ExternalForceArgs m_externalForceArguments;
 		struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
         struct CalculateJacobianArgs m_calculateJacobianArguments;
-        struct CreateJointArgs m_createJointArguments;
+        struct UserConstraintArgs m_userConstraintArguments;
         struct RequestContactDataArgs m_requestContactPointArguments;
 		struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
         struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
@@ -620,6 +631,7 @@ struct SharedMemoryStatus
 		struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
 		struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
 		struct UserDebugDrawResultArgs m_userDebugDrawArgs;
+		struct UserConstraintResultArgs m_userConstraintResultArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -33,7 +33,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_CALCULATE_INVERSE_DYNAMICS,
     CMD_CALCULATE_INVERSE_KINEMATICS,
     CMD_CALCULATE_JACOBIAN,
-    CMD_CREATE_JOINT,
+    CMD_USER_CONSTRAINT,
     CMD_REQUEST_CONTACT_POINT_INFORMATION,
 	CMD_REQUEST_AABB_OVERLAP,
 	CMD_SAVE_WORLD,
@@ -104,6 +104,8 @@ enum EnumSharedMemoryServerStatus
         CMD_LOAD_TEXTURE_FAILED,
 		CMD_USER_DEBUG_DRAW_COMPLETED,
 		CMD_USER_DEBUG_DRAW_FAILED,
+		CMD_USER_CONSTRAINT_COMPLETED,
+		CMD_USER_CONSTRAINT_FAILED,
         //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };

examples/pybullet/pybullet.c

@@ -263,7 +263,8 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
 	}
 	command = b3SaveBulletCommandInit(sm, bulletFileName);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-	if (statusHandle != CMD_BULLET_SAVING_COMPLETED)
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_BULLET_SAVING_COMPLETED)
 	{
 		PyErr_SetString(SpamError, "Couldn't save .bullet file.");
 		return NULL;
@@ -291,7 +292,8 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
 	}
 	command = b3LoadMJCFCommandInit(sm, mjcfjFileName);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-	if (statusHandle != CMD_MJCF_LOADING_COMPLETED)
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_MJCF_LOADING_COMPLETED)
 	{
 		PyErr_SetString(SpamError, "Couldn't load .mjcf file.");
 		return NULL;
@@ -1433,7 +1435,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
 }
 
 //  vector - double[3] which will be set by values from obVec
-static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
+static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
     int i, len;
     PyObject* seq;
     if (obVec==NULL)
@@ -1459,7 +1461,6 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
   int size = PySequence_Size(args);
   
   b3SharedMemoryCommandHandle commandHandle;
-  struct b3ContactInformation contactPointData;
   b3SharedMemoryStatusHandle statusHandle;
   int statusType;
   int res = 0;
@@ -1526,7 +1527,6 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
   int size = PySequence_Size(args);
   
   b3SharedMemoryCommandHandle commandHandle;
-  struct b3ContactInformation contactPointData;
   b3SharedMemoryStatusHandle statusHandle;
   int statusType;
   int res = 0;
@@ -1594,7 +1594,6 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
 static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds) 
 {
 	 b3SharedMemoryCommandHandle commandHandle;
-  struct b3ContactInformation contactPointData;
   b3SharedMemoryStatusHandle statusHandle;
   int statusType;
   int itemUniqueId;
@@ -1621,7 +1620,6 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py
 static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds) 
 {
 	 b3SharedMemoryCommandHandle commandHandle;
-  struct b3ContactInformation contactPointData;
   b3SharedMemoryStatusHandle statusHandle;
   int statusType;
 
@@ -2016,6 +2014,144 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
   return Py_None;
 }
 
+static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	static char *kwlist[] = { "userConstraintUniqueId",NULL};
+	int userConstraintUniqueId=-1;
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+
+	if (0 == sm) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", kwlist,&userConstraintUniqueId))
+	{
+		return NULL;
+	}
+
+	commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	Py_INCREF(Py_None);
+	return Py_None;
+};
+
+
+/*
+static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	return NULL;
+}
+*/
+
+static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	int size = PySequence_Size(args);
+	int bodyUniqueIdA = -1;
+	int bodyUniqueIdB = -1;
+  
+	b3SharedMemoryCommandHandle commandHandle;
+	int parentBodyUniqueId=-1;
+	int parentLinkIndex=-1;
+	int childBodyUniqueId=-1;
+	int childLinkIndex=-1;
+	int jointType=ePoint2PointType;
+	PyObject* jointAxisObj=0;
+	double jointAxis[3]={0,0,0};
+	PyObject* parentFramePositionObj = 0;
+	double parentFramePosition[3]={0,0,0};
+	PyObject* childFramePositionObj = 0;
+	double childFramePosition[3]={0,0,0};
+	PyObject* parentFrameOrientationObj = 0;
+	double parentFrameOrientation[4]={0,0,0,1};
+	PyObject* childFrameOrientationObj = 0;
+	double childFrameOrientation[4]={0,0,0,1};
+
+	struct b3JointInfo jointInfo;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	PyObject* pyResultList = 0;
+
+
+	static char *kwlist[] = { "parentBodyUniqueId", "parentLinkIndex",
+							  "childBodyUniqueId", "childLinkIndex",
+							"jointType",
+							"jointAxis",
+							"parentFramePosition",
+							"childFramePosition",
+							"parentFrameOrientation",
+		"childFrameOrientation",
+		NULL };
+
+	if (0 == sm) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OO", kwlist,&parentBodyUniqueId,&parentLinkIndex,
+										&childBodyUniqueId,&childLinkIndex,
+										&jointType,&jointAxisObj,
+										&parentFramePositionObj,
+										&childFramePositionObj,
+										&parentFrameOrientationObj,
+										&childFrameOrientationObj
+	))
+	{
+		return NULL;
+	}
+	
+	pybullet_internalSetVectord(jointAxisObj,jointAxis);
+	pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition);
+	pybullet_internalSetVectord(childFramePositionObj,childFramePosition);
+	pybullet_internalSetVector4d(parentFrameOrientationObj,parentFrameOrientation);
+	pybullet_internalSetVector4d(childFrameOrientationObj,childFrameOrientation);
+	
+	jointInfo.m_jointType = jointType;
+	jointInfo.m_parentFrame[0] = parentFramePosition[0];
+	jointInfo.m_parentFrame[1] = parentFramePosition[1];
+	jointInfo.m_parentFrame[2] = parentFramePosition[2];
+	jointInfo.m_parentFrame[3] = parentFrameOrientation[0];
+	jointInfo.m_parentFrame[4] = parentFrameOrientation[1];
+	jointInfo.m_parentFrame[5] = parentFrameOrientation[2];
+	jointInfo.m_parentFrame[6] = parentFrameOrientation[3];
+
+	jointInfo.m_childFrame[0] = childFramePosition[0];
+	jointInfo.m_childFrame[1] = childFramePosition[1];
+	jointInfo.m_childFrame[2] = childFramePosition[2];
+	jointInfo.m_childFrame[3] = childFrameOrientation[0];
+	jointInfo.m_childFrame[4] = childFrameOrientation[1];
+	jointInfo.m_childFrame[5] = childFrameOrientation[2];
+	jointInfo.m_childFrame[6] = childFrameOrientation[3];
+	
+	jointInfo.m_jointAxis[0] = jointAxis[0];
+	jointInfo.m_jointAxis[1] = jointAxis[1];
+	jointInfo.m_jointAxis[2] = jointAxis[2];
+
+	commandHandle = b3InitCreateUserConstraintCommand(sm,  parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType==CMD_USER_CONSTRAINT_COMPLETED)
+	{
+		int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
+		PyObject* ob = PyLong_FromLong(userConstraintUid);
+		return ob;
+	} else
+	{
+		PyErr_SetString(SpamError, "createConstraint failed.");
+		return NULL;
+	}
+
+  Py_INCREF(Py_None);
+  return Py_None;
+
+}
 
 static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
   int size = PySequence_Size(args);
@@ -2594,7 +2730,7 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
             double pos[3];
             double ori[4]={0,1.0,0,0};
             
-            if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4(targetOrnObj,ori))
+            if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori))
             {
                 b3SharedMemoryStatusHandle statusHandle;
                 int numPos=0;
@@ -2820,6 +2956,14 @@ static PyMethodDef SpamMethods[] = {
 	 { "loadMJCF", pybullet_loadMJCF, METH_VARARGS,
 	"Load multibodies from an MJCF file." },
 
+	  {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS,
+	"Create a constraint between two bodies. Returns a (int) unique id, if successfull."
+     },
+
+	{"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS,
+     "Remove a constraint using its unique id."
+     },
+	
 	{"saveWorld", pybullet_saveWorld, METH_VARARGS,
      "Save a approximate Python file to reproduce the current state of the world: saveWorld"
 	"(filename). (very preliminary and approximately)"},

examples/pybullet/quadruped.py

@@ -0,0 +1,35 @@
+import pybullet as p
+p.connect(p.GUI)
+p.loadURDF("plane.urdf")
+p.loadURDF("quadruped/quadruped.urdf",0,0,0.2)
+#p.getNumJoints(1)
+
+#right front leg
+p.resetJointState(1,0,1.57)
+p.resetJointState(1,2,-2.2)
+p.resetJointState(1,3,-1.57)
+p.resetJointState(1,5,2.2)
+p.createConstraint(1,2,1,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
+
+#left front leg
+p.resetJointState(1,6,1.57)
+p.resetJointState(1,8,-2.2)
+p.resetJointState(1,9,-1.57)
+p.resetJointState(1,11,2.2)
+p.createConstraint(1,8,1,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
+
+#right back leg
+p.resetJointState(1,12,1.57)
+p.resetJointState(1,14,-2.2)
+p.resetJointState(1,15,-1.57)
+p.resetJointState(1,17,2.2)
+p.createConstraint(1,14,1,17,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
+
+#left back leg
+p.resetJointState(1,18,1.57)
+p.resetJointState(1,20,-2.2)
+p.resetJointState(1,21,-1.57)
+p.resetJointState(1,23,2.2)
+p.createConstraint(1,20,1,23,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
+
+