From 53a0772257e2b1bb144df2f3129a784502652ab4 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwincoumans@google.com>
Date: Thu, 16 Jun 2016 18:46:34 -0700
Subject: [PATCH] fix some issues related to controlling a robot/multibody
 beyond body index 0 (most testing happened with a single robot/multibody so
 far) preliminary pybullet.setJointControl implementation

---
 examples/SharedMemory/PhysicsClientC_API.cpp  |   8 +-
 examples/SharedMemory/PhysicsClientC_API.h    |   4 +-
 .../SharedMemory/PhysicsClientExample.cpp     |  82 ++++++--
 .../PhysicsServerCommandProcessor.cpp         |  24 +--
 examples/pybullet/pybullet.c                  | 195 +++++++++++++++---
 .../Featherstone/btMultiBodyLink.h            |   3 +
 test/SharedMemory/test.c                      |   2 +-
 7 files changed, 248 insertions(+), 70 deletions(-)

diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index a627e08af..77e6060bf 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -150,7 +150,7 @@ b3SharedMemoryCommandHandle     b3InitResetSimulationCommand(b3PhysicsClientHand
 }
 
 
-b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
+b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
     b3Assert(cl);
@@ -159,7 +159,7 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
     b3Assert(command);
 	command->m_type = CMD_SEND_DESIRED_STATE;
     command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
-	command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
+	command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
 	command->m_updateFlags = 0;
     return (b3SharedMemoryCommandHandle) command;
 }
@@ -433,7 +433,7 @@ int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
 
 
 
-b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
+b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
     b3Assert(cl);
@@ -444,7 +444,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
     command->m_type = CMD_CREATE_SENSOR;
     command->m_updateFlags = 0;
     command->m_createSensorArguments.m_numJointSensorChanges = 0;
-	command->m_createSensorArguments.m_bodyUniqueId = 0;
+	command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
     return (b3SharedMemoryCommandHandle) command;
     
 }
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 4ae1a5235..3813fab88 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -95,7 +95,7 @@ b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
 
 ///Set joint control variables such as desired position/angle, desired velocity,
 ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
-b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
+b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
 ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
 int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
 int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
@@ -132,7 +132,7 @@ int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
 
 ///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
 ///This is rather inconsistent, to mix programmatical creation with loading from file.
-b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
+b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
 int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
 ///b3CreateSensorEnableIMUForLink is not implemented yet.
 ///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp
index 691dbb9c6..e0338153a 100644
--- a/examples/SharedMemory/PhysicsClientExample.cpp
+++ b/examples/SharedMemory/PhysicsClientExample.cpp
@@ -37,6 +37,9 @@ protected:
 	
 	bool m_wantsTermination;
     btAlignedObjectArray<int> m_userCommandRequests;
+    btAlignedObjectArray<int> m_bodyUniqueIds;
+    
+    
     int m_sharedMemoryKey;
     int m_selectedBody;
 	int m_prevSelectedBody;
@@ -66,7 +69,8 @@ protected:
 	{
 		if (m_guiHelper && m_guiHelper->getParameterInterface())
 		{
-			int bodyIndex = comboIndex;
+            int itemIndex = int(atoi(name));
+			int bodyIndex = m_bodyUniqueIds[itemIndex];
 			if (m_selectedBody != bodyIndex)
 			{
 				m_selectedBody = bodyIndex;
@@ -154,14 +158,20 @@ protected:
 
 	void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
 	{
+        
         for (int i=0;i<m_numMotors;i++)
         {
             
             btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
             int qIndex = m_motorTargetPositions[i].m_qIndex;
             int uIndex = m_motorTargetPositions[i].m_uIndex;
+            static int serial=0;
+            serial++;
+          //  b3Printf("# motors = %d, cmd[%d] qIndex = %d, uIndex = %d, targetPos = %f", m_numMotors, serial, qIndex,uIndex,targetPos);
+            
             b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
-            b3JointControlSetKp(commandHandle, uIndex, 0.1);
+            b3JointControlSetKp(commandHandle, qIndex, 0.1);
+            b3JointControlSetKd(commandHandle, uIndex, 0);
             
             b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
         }
@@ -187,7 +197,7 @@ protected:
 
 void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
 {
-	b3Printf("Item selected %s", item);
+	//b3Printf("Item selected %s", item);
 
 	PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
 	b3Assert(cl);
@@ -364,11 +374,18 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
         }
         case CMD_SEND_DESIRED_STATE:
         {
-            // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
-            b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
+	   if (m_selectedBody>=0)
+           {
+            // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
+            b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
+          //  b3Printf("prepare control command for body %d", m_selectedBody);
+               
             prepareControlCommand(command);
+            
+               
             b3SubmitClientCommand(m_physicsClientHandle, command);
-            break;
+        }   
+	 break;
         }
         case CMD_RESET_SIMULATION:
         {
@@ -471,9 +488,36 @@ void	PhysicsClientExample::createButtons()
 		createButton("Initialize Pose",CMD_INIT_POSE,  isTrigger);
         createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
 
+        if (m_bodyUniqueIds.size())
+        {
+            if (m_selectedBody<0)
+                m_selectedBody = 0;
+            
+            ComboBoxParams comboParams;
+            comboParams.m_comboboxId = 0;
+            comboParams.m_numItems = m_bodyUniqueIds.size();
+            comboParams.m_startItem = m_selectedBody;
+            comboParams.m_callback = MyComboBoxCallback;
+            comboParams.m_userPointer = this;
+            //todo: get the real object name
+
+            const char** blarray = new const char*[m_bodyUniqueIds.size()];
+            
+            for (int i=0;i<m_bodyUniqueIds.size();i++)
+            {
+                char* bla = new char[16];
+                sprintf(bla,"%d", i);
+                blarray[i] = bla;
+                comboParams.m_items=blarray;//{&bla};
+            }
+            m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+        }
+        
 
 		if (m_physicsClientHandle && m_selectedBody>=0)
 		{
+            m_numMotors = 0;
+            
 			int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
 			for (int i=0;i<numJoints;i++)
 			{
@@ -664,11 +708,16 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
                     b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity);
                 } else
                 {
-                    /*
                     for (int i=0;i<numBodies;i++)
                     {
                         int bodyUniqueId = bodyIndicesOut[i];
+                        m_bodyUniqueIds.push_back(bodyUniqueId);
                         int numJoints =  b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
+                        if (numJoints>0)
+                        {
+                            m_selectedBody = bodyUniqueId;
+                        }
+/*                        int numJoints =  b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
                         b3Printf("numJoints = %d", numJoints);
                         for (int i=0;i<numJoints;i++)
                         {
@@ -676,8 +725,9 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
                             b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,i,&info);
                             b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
                         }
+ 
+ */
                     }
-                    */
                 }
 			    
 			    //int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
@@ -715,6 +765,8 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 				int bodyIndex = b3GetStatusBodyIndex(status);
 				if (bodyIndex>=0)
 				{
+                    m_bodyUniqueIds.push_back(bodyIndex);
+                    m_selectedBody = bodyIndex;
 					int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
             
 					for (int i=0;i<numJoints;i++)
@@ -723,18 +775,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 						b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
 						b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
 					}
-					ComboBoxParams comboParams;
-					comboParams.m_comboboxId = bodyIndex;
-					comboParams.m_numItems = 1;
-					comboParams.m_startItem = 0;
-					comboParams.m_callback = MyComboBoxCallback;
-					comboParams.m_userPointer = this;
-					const char* bla = "bla";
-					const char* blarray[1];
-					blarray[0] = bla;
-				
-					comboParams.m_items=blarray;//{&bla};
-					m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+					
 				}
 			}
 		}
@@ -759,6 +800,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
             {
 				m_selectedBody = -1;
                 m_numMotors=0;
+                m_bodyUniqueIds.clear();
                 createButtons();
 				b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
 				if (m_options == eCLIENTEXAMPLE_SERVER)
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index e3a9b7f48..33d7d3153 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -382,7 +382,6 @@ struct PhysicsServerCommandProcessorInternalData
 
 	btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
 	btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
-	btHashMap<btHashInt, btMultiBodyJointMotor*>	m_multiBodyJointMotorMap;
 	btAlignedObjectArray<std::string*> m_strings;
 
 	btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;
@@ -560,7 +559,6 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 	}
 	m_data->m_urdfLinkNameMapper.clear();
 	
-	m_data->m_multiBodyJointMotorMap.clear();
 	
 	for (int i=0;i<m_data->m_strings.size();i++)
 	{
@@ -672,7 +670,7 @@ void	PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
 			btScalar desiredVelocity = 0.f;
 			btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
 			//motor->setMaxAppliedImpulse(0);
-			m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor);
+            mb->getLink(mbLinkIndex).m_userPtr = motor;
 			m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
             motor->finalizeMultiDof();
             
@@ -1436,10 +1434,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 												if (supportsJointMotor(mb,link))
 												{
 													
-													btMultiBodyJointMotor** motorPtr  = m_data->m_multiBodyJointMotorMap[link];
-													if (motorPtr)
+                                                    btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
+                                                    
+													
+                                                    if (motor)
 													{
-														btMultiBodyJointMotor* motor = *motorPtr;
 														btScalar desiredVelocity = 0.f;
 														if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0)
 															desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
@@ -1479,10 +1478,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 												if (supportsJointMotor(mb,link))
 												{
 													
-													btMultiBodyJointMotor** motorPtr  = m_data->m_multiBodyJointMotorMap[link];
-													if (motorPtr)
+
+                                                    btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
+                                                    
+                                                	if (motor)
 													{
-														btMultiBodyJointMotor* motor = *motorPtr;
 													
                                                         btScalar desiredVelocity = 0.f;
 														if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0)
@@ -1644,10 +1644,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 if (supportsJointMotor(mb,l))
                                 {
                                     
-                                    btMultiBodyJointMotor** motorPtr  = m_data->m_multiBodyJointMotorMap[l];
-                                    if (motorPtr && m_data->m_physicsDeltaTime>btScalar(0))
+                                    btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
+                                    
+                                    if (motor && m_data->m_physicsDeltaTime>btScalar(0))
                                     {
-                                        btMultiBodyJointMotor* motor = *motorPtr;
                                         btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime;
                                         serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] =
                                         force;
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 17c092b2d..fbd070ff9 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -1,6 +1,6 @@
-#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
 #include "../SharedMemory/PhysicsClientC_API.h"
 #include "../SharedMemory/PhysicsDirectC_API.h"
+#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
 
 
 #ifdef __APPLE__
@@ -16,6 +16,8 @@ enum eCONNECT_METHOD
 	eCONNECT_SHARED_MEMORY=3,
 };
 
+
+
 static PyObject *SpamError;
 static  b3PhysicsClientHandle sm=0;
 
@@ -182,6 +184,25 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
 	return PyLong_FromLong(bodyIndex);
 }
 
+static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index)
+{
+    float v = 0.f;
+    PyObject* item;
+    
+    if (PyList_Check(seq))
+    {
+        item = PyList_GET_ITEM(seq, index);
+        v = PyFloat_AsDouble(item);
+    }
+    else
+    {
+        item = PyTuple_GET_ITEM(seq,index);
+        v = PyFloat_AsDouble(item);
+    }
+    return v;
+}
+
+
 #define MAX_SDF_BODIES 512
 
 static PyObject*
@@ -255,6 +276,109 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
 	return Py_None;
 }
 
+static int pybullet_setJointControl(PyObject* self, PyObject* args)
+{
+    //todo(erwincoumans): set max forces, kp, kd
+
+    int size;
+    
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+    
+    size= PySequence_Size(args);
+    
+    if (size==3)
+    {
+        int bodyIndex, controlMode;
+        PyObject* targetValues;
+        if (PyArg_ParseTuple(args, "iiO", &bodyIndex, &controlMode, &targetValues))
+        {
+            PyObject* seq;
+            int numJoints, len;
+            seq = PySequence_Fast(targetValues, "expected a sequence");
+            len = PySequence_Size(targetValues);
+            numJoints = b3GetNumJoints(sm,bodyIndex);
+            b3SharedMemoryCommandHandle commandHandle;
+            
+            if (len!=numJoints)
+            {
+                PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints.");
+                Py_DECREF(seq);
+                return NULL;
+            }
+            
+            if ((controlMode != CONTROL_MODE_VELOCITY) &&
+                (controlMode != CONTROL_MODE_TORQUE) &&
+                (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
+            {
+                PyErr_SetString(SpamError, "Illegral control mode.");
+                Py_DECREF(seq);
+                return NULL;
+            }
+            
+            commandHandle = b3JointControlCommandInit(sm, bodyIndex,controlMode);
+            
+            for (int qIndex=0;qIndex<numJoints;qIndex++)
+            {
+                float value = pybullet_internalGetFloatFromSequence(seq,qIndex);
+            
+                switch (controlMode)
+                {
+                    case CONTROL_MODE_VELOCITY:
+                    {
+                        b3JointControlSetDesiredVelocity(commandHandle, qIndex, value);
+                        break;
+                    }
+
+                    case CONTROL_MODE_TORQUE:
+                    {
+                        b3JointControlSetDesiredForceTorque(commandHandle, qIndex, value);
+                        break;
+                    }
+                        
+                    case CONTROL_MODE_POSITION_VELOCITY_PD:
+                    {
+                        b3JointControlSetDesiredPosition( commandHandle, qIndex, value);
+                        break;
+                    }
+                    default:
+                    {
+                    
+                    }
+                };
+            }
+            Py_DECREF(seq);
+            Py_INCREF(Py_None);
+            return Py_None;
+        }
+        
+    }
+    PyErr_SetString(SpamError, "error in setJointControl.");
+    return NULL;
+}
+
+                /*
+                 ///Set joint control variables such as desired position/angle, desired velocity,
+                 ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
+                 b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
+                 ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
+                 int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
+                 int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+                 int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+                 //Only use when controlMode is CONTROL_MODE_VELOCITY
+                 int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); 
+                 // find a better name for dof/q/u indices, point to b3JointInfo
+                int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex,  double value);
+                ///Only use if when controlMode is CONTROL_MODE_TORQUE,
+                int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int  dofIndex, double value);
+                
+                 */
+         
+
+
 // Set the gravity of the world with (x, y, z) arguments
 static PyObject *
 pybullet_setGravity(PyObject* self, PyObject* args)
@@ -448,6 +572,23 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args)
 	}
   // TODO(hellojas): initialize all joint positions given a pylist of values
 
+    //
+    //
+    /*
+     ///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.
+     b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
+     int     b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
+     int     b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
+     int     b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
+     int     b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle,  int jointIndex, double jointPosition);
+     
+
+     */
+    //
+    //
+    
 	Py_INCREF(Py_None);
         return Py_None;
 }
@@ -765,6 +906,7 @@ pybullet_getJointState(PyObject* self, PyObject* args)
 }
 
 
+
 // internal function to set a float matrix[16]
 // used to initialize camera position with
 // a view and projection matrix in renderImage()
@@ -773,39 +915,22 @@ pybullet_getJointState(PyObject* self, PyObject* args)
 //  matrix - float[16] which will be set by values from objMat
 static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
 {
+    int i, len;
+    PyObject* seq;
 
-  int i, len;
-  PyObject* item;
-  PyObject* seq;
-
-  seq = PySequence_Fast(objMat, "expected a sequence");
-  len = PySequence_Size(objMat);
-  if (len==16)
-  {
-
-    if (PyList_Check(seq))
+    seq = PySequence_Fast(objMat, "expected a sequence");
+    len = PySequence_Size(objMat);
+    if (len==16)
     {
-      for (i = 0; i < len; i++)
-      {
-        item = PyList_GET_ITEM(seq, i);
-        matrix[i] = PyFloat_AsDouble(item);
-      }
+        for (i = 0; i < len; i++)
+        {
+            matrix[i] = pybullet_internalGetFloatFromSequence(seq,i);
+        }
+        Py_DECREF(seq);
+        return 1;
     }
-    else
-    {
-      for (i = 0; i < len; i++)
-      {
-          item = PyTuple_GET_ITEM(seq,i);
-          matrix[i] = PyFloat_AsDouble(item);
-      }
-    }
-     Py_DECREF(seq);
-     return 1;
-  } else
-  {
     Py_DECREF(seq);
     return 0;
-  }
 }
 
 // Render an image from the current timestep of the simulation
@@ -959,7 +1084,11 @@ static PyMethodDef SpamMethods[] = {
   
   {"getJointState", pybullet_getJointState, METH_VARARGS,
   "Get the joint metadata info for a joint on a body."},
-  
+
+    {"setJointControl", pybullet_setJointControl, METH_VARARGS,
+        "Set the joint control mode and desired target values."},
+
+    
 	{"renderImage", pybullet_renderImage, METH_VARARGS,
 	"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},	
 	
@@ -1017,7 +1146,11 @@ initpybullet(void)
 	PyModule_AddIntConstant (m, "SHARED_MEMORY", eCONNECT_SHARED_MEMORY); // user read
 	PyModule_AddIntConstant (m, "DIRECT", eCONNECT_DIRECT); // user read 
 	PyModule_AddIntConstant (m, "GUI", eCONNECT_GUI); // user read 
-
+    
+    PyModule_AddIntConstant (m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
+    PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read
+    PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read
+    
     SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
     Py_INCREF(SpamError);
     PyModule_AddObject(m, "error", SpamError);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index b6a4a4a03..8b9036303 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -132,6 +132,8 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 
 	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
 	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
+    const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!
+    
 	btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
 	btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
 
@@ -149,6 +151,7 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 			m_jointFeedback(0),
 			m_linkName(0),
 			m_jointName(0),
+            m_userPtr(0),
 			m_jointDamping(0),
 			m_jointFriction(0)
 	{
diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c
index 01d0a16a8..9d80eed63 100644
--- a/test/SharedMemory/test.c
+++ b/test/SharedMemory/test.c
@@ -125,7 +125,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
         
 			if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
 			{
-				b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
+				b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
 				b3SharedMemoryStatusHandle statusHandle;
 				if (imuLinkIndex>=0)
 				{