Merge pull request #673 from erwincoumans/master

implement a few more pybullet methods:

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -903,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
 	}
 }
 
+b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
+{
+    PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    
+    command->m_type = CMD_APPLY_EXTERNAL_FORCE;
+    command->m_updateFlags = 0;
+    command->m_externalForceArguments.m_numForcesAndTorques = 0;
+    return (b3SharedMemoryCommandHandle) command;
+}
+
+void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
+    int index = command->m_externalForceArguments.m_numForcesAndTorques;
+    command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
+    command->m_externalForceArguments.m_linkIds[index] = linkId;
+    command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
+    for (int i = 0; i < 3; ++i) {
+        command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
+        command->m_externalForceArguments.m_positions[index+i] = position[i];
+    }
+    
+    command->m_externalForceArguments.m_numForcesAndTorques++;
+}
+
+void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
+    int index = command->m_externalForceArguments.m_numForcesAndTorques;
+    command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
+    command->m_externalForceArguments.m_linkIds[index] = linkId;
+    command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
+
+    for (int i = 0; i < 3; ++i) {
+        command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
+    }
+    command->m_externalForceArguments.m_numForcesAndTorques++;
+}
+

examples/SharedMemory/PhysicsClientC_API.h

@@ -159,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
                                              double rayToWorldZ);
 b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
 
+/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
+b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
+void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
+void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
+
 #ifdef __cplusplus
 }
 #endif

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2056,6 +2056,70 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						hasStatus = true;
                         break;
                     }
+                case CMD_APPLY_EXTERNAL_FORCE:
+                {
+                    for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
+                    {
+                        InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
+                        if (body && body->m_multiBody)
+                        {
+                            btMultiBody* mb = body->m_multiBody;
+                            bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
+                                
+                            if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
+                            {
+                                btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
+                                                clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
+                                                clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
+                                btVector3 positionLocal(
+                                                        clientCmd.m_externalForceArguments.m_positions[i*3+0],
+                                                        clientCmd.m_externalForceArguments.m_positions[i*3+1],
+                                                        clientCmd.m_externalForceArguments.m_positions[i*3+2]);
+                                
+                                if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
+                                {
+                                    btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
+                                    btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal;
+                                    mb->addBaseForce(forceWorld);
+                                    mb->addBaseTorque(relPosWorld.cross(forceWorld));
+                                    //b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
+                                } else
+                                {
+                                    int link = clientCmd.m_externalForceArguments.m_linkIds[i];
+                                    btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal;
+                                    btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal;
+                                    mb->addLinkForce(link, forceWorld);
+                                    mb->addLinkTorque(link,relPosWorld.cross(forceWorld));
+                                    //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
+                                }
+                            }
+                            if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0)
+                            {
+                                btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
+                                                      clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
+                                                      clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
+                                
+                                if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
+                                {
+                                    btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
+                                    mb->addBaseTorque(torqueWorld);
+                                    //b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
+                                } else
+                                {
+                                    int link = clientCmd.m_externalForceArguments.m_linkIds[i];
+                                    btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal;
+                                    mb->addLinkTorque(link, torqueWorld);
+                                    //b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
+                                }
+                            }
+                        }
+                    }
+                    
+                    SharedMemoryStatus& serverCmd =serverStatusOut;
+                    serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+                    hasStatus = true;
+                    break;
+                }
                 default:
                 {
                     b3Error("Unknown command encountered");

examples/SharedMemory/SharedMemoryCommands.h

@@ -319,6 +319,27 @@ struct SdfRequestInfoArgs
     int m_bodyUniqueId;
 };
 
+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForcePrivateFlags
+{
+//    EF_LINK_FRAME=1,
+//    EF_WORLD_FRAME=2,
+    EF_TORQUE=4,
+    EF_FORCE=8,
+};
+
+
+
+struct ExternalForceArgs
+{
+    int m_numForcesAndTorques;
+    int m_bodyUniqueIds[MAX_SDF_BODIES];
+    int m_linkIds[MAX_SDF_BODIES];
+    double m_forcesAndTorques[3*MAX_SDF_BODIES];
+    double m_positions[3*MAX_SDF_BODIES];
+    int m_forceFlags[MAX_SDF_BODIES];
+};
+
 enum EnumSdfRequestInfoFlags
 {
     SDF_REQUEST_INFO_BODY=1,
@@ -350,6 +371,7 @@ struct SharedMemoryCommand
 		struct RequestDebugLinesArgs m_requestDebugLinesArguments;
 		struct RequestPixelDataArgs m_requestPixelDataArguments;
 		struct PickBodyArgs m_pickBodyArguments;
+        struct ExternalForceArgs m_externalForceArguments;
     };
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -26,6 +26,7 @@ enum EnumSharedMemoryClientCommand
     CMD_MOVE_PICKED_BODY,
     CMD_REMOVE_PICKING_CONSTRAINT_BODY,
     CMD_REQUEST_CAMERA_IMAGE_DATA,
+    CMD_APPLY_EXTERNAL_FORCE,
     CMD_MAX_CLIENT_COMMANDS
 };
 
@@ -143,5 +144,11 @@ enum {
     CONTROL_MODE_POSITION_VELOCITY_PD,
 };
 
+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForceFlags
+{
+    EF_LINK_FRAME=1,
+    EF_WORLD_FRAME=2,
+};
 
 #endif//SHARED_MEMORY_PUBLIC_H

examples/SharedMemory/premake4.lua

@@ -26,6 +26,8 @@ files {
 	"PhysicsServer.h",
 	"main.cpp",
 	"PhysicsClientC_API.cpp",
+	"SharedMemoryCommands.h",
+	"SharedMemoryPublic.h",
 	"PhysicsServer.cpp",
 	"PosixSharedMemory.cpp",
 	"Win32SharedMemory.cpp",

examples/pybullet/pybullet.c

@@ -437,6 +437,36 @@ pybullet_setGravity(PyObject* self, PyObject* args)
 	return Py_None;
 }
 
+static PyObject *
+pybullet_setTimeStep(PyObject* self, PyObject* args)
+{
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+    
+    {
+        double timeStep=0.001;
+        int ret;
+        
+        b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+        b3SharedMemoryStatusHandle statusHandle;
+        
+        if (!PyArg_ParseTuple(args, "d", &timeStep))
+        {
+            PyErr_SetString(SpamError, "setTimeStep expected a single value (double).");
+            return NULL;
+        }
+        ret = b3PhysicsParamSetTimeStep(command,  timeStep);
+        statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+        //ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
+    }
+    
+    Py_INCREF(Py_None);
+    return Py_None;
+}
+
 
 
 // Internal function used to get the base position and orientation
@@ -632,140 +662,88 @@ pybullet_resetJointState(PyObject* self, PyObject* args)
     return NULL;
 }
 
-
+// Reset the position and orientation of the base/root link, position [x,y,z] and orientation quaternion [x,y,z,w]
-
+static PyObject*
-// CURRENTLY NOT SUPPORTED 
+pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args)
-// Initalize a single joint position for a specific body index
-// 
-// This method skips any physics simulation and 
-// teleports all joints to the new positions.
-
-// TODO(hellojas): initializing one joint currently not supported
-// static PyObject*
-// pybullet_initializeJointPosition(PyObject* self, PyObject* args)
-// {
-// 	if (0==sm)
-// 	{
-// 		PyErr_SetString(SpamError, "Not connected to physics server.");
-// 		return NULL;
-// 	}
-//   
-//   int i;
-//   int bodyIndex = -1;
-//   int jointIndex = -1;
-//   double jointPos = 0.0;
-//   
-//   int size= PySequence_Size(args);
-// 
-//   if (size==3) // get body index, joint index, and joint position value
-//   {
-//    if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos))
-//     {
-//       b3SharedMemoryCommandHandle cmd_handle =  b3CreatePoseCommandInit(sm, bodyIndex);
-// 
-//       // printf("initializing joint %d at %f\n", jointIndex, jointPos);
-//       b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos);
-// 
-//       b3SharedMemoryStatusHandle status_handle =
-//               b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
-// 
-//     const int status_type = b3GetStatusType(status_handle);
-// 
-//     }
-//   }
-//   
-// 	Py_INCREF(Py_None);
-//         return Py_None;
-// }
-#if 0
-static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) {
-  int i, j;
-  int numDegreeQ;
-  int numDegreeU;
-  int arrSizeOfPosAndOrn = 7;
-  
-  for (i =0;i <numJoints; i++){
-    jointPositions[i] = .5;
-  }
-  
-  {
-  b3SharedMemoryCommandHandle cmd_handle =
-          b3RequestActualStateCommandInit(sm, bodyIndex);
-      b3SharedMemoryStatusHandle status_handle =
-              b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
-              
-      const int status_type = b3GetStatusType(status_handle);            
-      const double* actualStateQ;
-
-      b3GetStatusActualState(status_handle, 0/* body_unique_id */,
-                         &numDegreeQ ,
-                         &numDegreeU/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/,
-                         &actualStateQ , 0 /* actual_state_q_dot */,
-                         0 /* joint_reaction_forces */);
-
-      
-      // printf("actual state Q, size = %lu\n", sizeof(actualStateQ.));
-      // printf("numjoints = %d\n", numJoints);
-      // printf("numDegreeQ = %d\n", numDegreeQ);
-      // printf("numDegreeU = %d\n", numDegreeU);
-      // printf("actualStateQ[0] = %f\n",actualStateQ[0]);
-      for (j = arrSizeOfPosAndOrn; j < numJoints + arrSizeOfPosAndOrn; j++) {
-        jointPositions[j - arrSizeOfPosAndOrn] = actualStateQ[j];
-        // printf("%d=%f\n", j, jointPositions[j - arrSizeOfPosAndOrn]);
-
-      }     
-    }
-}
-
-// Get a list of all joint positions for a given body index
-//
-// Args:
-//  bodyIndex - integer indicating body in simulation
-// Returns:
-//  pyListJointPos - list of positions for each joint index
-//
-static PyObject *
-pybullet_getJointPositions(PyObject* self, PyObject* args)
 {
+    int size;
     if (0==sm)
     {
         PyErr_SetString(SpamError, "Not connected to physics server.");
         return NULL;
     }
     
-    int bodyIndex = -1;
     
-    if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
+    size= PySequence_Size(args);
+    
+    if (size==3)
     {
-        PyErr_SetString(SpamError, "Expected a body index (integer).");
+        int bodyIndex;
-        return NULL;
+        PyObject* posObj;
-    }
+        PyObject* ornObj;
-    
+        double pos[3];
-    {
+        double orn[4];//as a quaternion
-      PyObject *item;   
+        
-      PyObject *pyListJointPos; 
+        if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj))
-      
+        {
-      int i;
+            b3SharedMemoryCommandHandle commandHandle;
-      int numJoints = b3GetNumJoints(sm,bodyIndex);
+            b3SharedMemoryStatusHandle statusHandle;
-      double* jointPositions = malloc(numJoints*sizeof(double));
+            
-      pyListJointPos = PyTuple_New(numJoints);
+            {
+                PyObject* seq;
+                int len,i;
+                seq = PySequence_Fast(posObj, "expected a sequence");
+                len = PySequence_Size(posObj);
+                if (len==3)
+                {
+                    for (i = 0; i < 3; i++)
+                    {
+                        pos[i] = pybullet_internalGetFloatFromSequence(seq,i);
+                    }
+                } else
+                {
+                    PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
+                    Py_DECREF(seq);
+                    return NULL;
+                }
+                 Py_DECREF(seq);
+            }
 
-
+            {
-      // printf("joint positions size = %lu\n", sizeof(jointPositions)/sizeof(double));
+                PyObject* seq;
-      pybullet_internalGetJointPositions(bodyIndex, numJoints,jointPositions);
+                int len,i;
-      
+                seq = PySequence_Fast(ornObj, "expected a sequence");
-      for (i =0;i <numJoints; i++){
+                len = PySequence_Size(ornObj);
-        item = PyFloat_FromDouble(jointPositions[i]);
+                if (len==4)
-        PyTuple_SetItem(pyListJointPos, i, item);
+                {
-      }
+                    for (i = 0; i < 4; i++)
-      free(jointPositions);
+                    {
-      return pyListJointPos;
+                        orn[i] = pybullet_internalGetFloatFromSequence(seq,i);
-    }
+                    }
+                } else
+                {
+                    PyErr_SetString(SpamError, "orientation needs a 4 coordinates, quaternion [x,y,z,w].");
+                    Py_DECREF(seq);
+                    return NULL;
+                }
+                Py_DECREF(seq);
+            }
     
-    Py_INCREF(Py_None);
+            commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
-    return Py_None;
+            
+            b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]);
+            b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]);
+
+            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+            Py_INCREF(Py_None);
+            return Py_None;
+        }
+        
+    }
+    PyErr_SetString(SpamError, "error in resetJointState.");
+    return NULL;
 }
-#endif 
+
 
 // Get the a single joint info for a specific bodyIndex
 //
@@ -890,9 +868,6 @@ pybullet_getJointState(PyObject* self, PyObject* args)
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-  
-    
-
 
   if (size==2) // get body index and joint index
   {
@@ -908,28 +883,15 @@ pybullet_getJointState(PyObject* self, PyObject* args)
    pyListJointForceTorque = PyTuple_New(forceTorqueSize);
 
     
-    // double m_jointPosition;
-    // double m_jointVelocity;
-    // double m_jointForceTorque[6];  /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
-    // double m_jointMotorTorque;
     b3GetJointState(sm, status_handle, jointIndex, &sensorState);
-    // printf("Joint%d: position=%f velocity=%f motortorque=%f\n", 
-    // jointIndex, 
-    // sensorState.m_jointPosition,
-    // sensorState.m_jointVelocity,
-    // sensorState.m_jointMotorTorque);
     
     PyTuple_SetItem(pyListJointState, 0, 
       PyFloat_FromDouble(sensorState.m_jointPosition));
     PyTuple_SetItem(pyListJointState, 1, 
       PyFloat_FromDouble(sensorState.m_jointVelocity));
       
-    // joint force torque is list of 6
-    /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
-    // printf(" jointForceTorque = ");
     for (j = 0; j < forceTorqueSize; j++) {
-      // printf("%f ", sensorState.m_jointForceTorque[j]);
+      PyTuple_SetItem(pyListJointForceTorque, j,
-      PyTuple_SetItem(pyListJointForceTorque, j, 
         PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
     }
     
@@ -941,6 +903,10 @@ pybullet_getJointState(PyObject* self, PyObject* args)
     
     return pyListJointState;
     }
+  } else
+  {
+      PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index).");
+      return NULL;
   }
       
   	Py_INCREF(Py_None);
@@ -1091,6 +1057,274 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
 	return Py_None;
 }
 
+static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args)
+{
+    
+   if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+    {
+        int objectUniqueId, linkIndex, flags;
+        double force[3];
+        double position[3]={0,0,0};
+        PyObject* forceObj, *posObj;
+        
+        b3SharedMemoryCommandHandle command;
+        b3SharedMemoryStatusHandle statusHandle;
+        int size= PySequence_Size(args);
+  
+        if (size==5)
+        {
+            if(!PyArg_ParseTuple(args, "iiOOi", &objectUniqueId, &linkIndex, &forceObj, &posObj, &flags))
+            {
+                PyErr_SetString(SpamError, "applyBaseForce couldn't parse arguments");
+                return NULL;
+            }
+        } else
+        {
+            PyErr_SetString(SpamError, "applyBaseForce needs 5 arguments: objectUniqueId, linkIndex (-1 for base/root link), force [x,y,z], position [x,y,z], flags");
+            
+            return NULL;
+        }
+        
+        {
+            PyObject* seq;
+            int len,i;
+            seq = PySequence_Fast(forceObj, "expected a sequence");
+            len = PySequence_Size(forceObj);
+            if (len==3)
+            {
+                for (i = 0; i < 3; i++)
+                {
+                    force[i] = pybullet_internalGetFloatFromSequence(seq,i);
+                }
+            } else
+            {
+                PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z].");
+                Py_DECREF(seq);
+                return NULL;
+            }
+             Py_DECREF(seq);
+        }
+        {
+            PyObject* seq;
+            int len,i;
+            seq = PySequence_Fast(posObj, "expected a sequence");
+            len = PySequence_Size(posObj);
+            if (len==3)
+            {
+                for (i = 0; i < 3; i++)
+                {
+                    position[i] = pybullet_internalGetFloatFromSequence(seq,i);
+                }
+            } else
+            {
+                PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
+                Py_DECREF(seq);
+                return NULL;
+            }
+             Py_DECREF(seq);
+
+        }
+        if ((flags !=EF_WORLD_FRAME) && (flags != EF_LINK_FRAME))
+        {
+            PyErr_SetString(SpamError, "flag has to be either WORLD_FRAME or LINK_FRAME");
+            return NULL;
+        }
+        command = b3ApplyExternalForceCommandInit(sm);
+        b3ApplyExternalForce(command,objectUniqueId,-1,force,position, flags);
+        statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+    }
+    Py_INCREF(Py_None);
+    return Py_None;
+}
+
+static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args)
+{
+    
+    if (0==sm)
+    {
+        PyErr_SetString(SpamError, "Not connected to physics server.");
+        return NULL;
+    }
+    {
+        int objectUniqueId, linkIndex, flags;
+        double torque[3];
+        PyObject* torqueObj;
+        
+        if (PyArg_ParseTuple(args, "iiOi", &objectUniqueId, &linkIndex, &torqueObj, &flags))
+        {
+            
+            PyObject* seq;
+            int len,i;
+            seq = PySequence_Fast(torqueObj, "expected a sequence");
+            len = PySequence_Size(torqueObj);
+            if (len==3)
+            {
+                for (i = 0; i < 3; i++)
+                {
+                    torque[i] = pybullet_internalGetFloatFromSequence(seq,i);
+                }
+            } else
+            {
+                PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z].");
+                Py_DECREF(seq);
+                return NULL;
+            }
+            Py_DECREF(seq);
+            
+            if (linkIndex <-1)
+            {
+                PyErr_SetString(SpamError, "Invalid link index, has to be -1 or larger");
+                return NULL;
+            }
+            if ((flags !=EF_WORLD_FRAME) && (flags != EF_LINK_FRAME))
+            {
+                PyErr_SetString(SpamError, "flag has to be either WORLD_FRAME or LINK_FRAME");
+                return NULL;
+            }
+            b3SharedMemoryStatusHandle statusHandle;
+            b3SharedMemoryCommandHandle command = b3ApplyExternalForceCommandInit(sm);
+            b3ApplyExternalTorque(command,objectUniqueId,-1,torque, flags);
+            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+
+        }
+    }
+    
+    Py_INCREF(Py_None);
+    return Py_None;
+}
+
+static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, PyObject* args)
+{
+    double rpy[3];
+   
+    PyObject* eulerObj;
+ 
+    if (PyArg_ParseTuple(args, "O", &eulerObj))
+    {
+        PyObject* seq;
+        int len,i;
+        seq = PySequence_Fast(eulerObj, "expected a sequence");
+        len = PySequence_Size(eulerObj);
+        if (len==3)
+        {
+            for (i = 0; i < 3; i++)
+            {
+                rpy[i] = pybullet_internalGetFloatFromSequence(seq,i);
+            }
+        } else
+        {
+            PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw].");
+            Py_DECREF(seq);
+            return NULL;
+        }
+        Py_DECREF(seq);
+    } else
+    {
+        PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw].");
+        return NULL;
+    }
+    
+    {
+        double phi, the, psi;
+        double roll = rpy[0];
+        double pitch = rpy[1];
+        double yaw = rpy[2];
+        phi = roll / 2.0;
+        the = pitch / 2.0;
+        psi = yaw / 2.0;
+        {
+            double quat[4] = {
+                sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
+                cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
+                cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
+                cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
+
+            //normalize the quaternion
+            double len = sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
+            quat[0] /= len;
+            quat[1] /= len;
+            quat[2] /= len;
+            quat[3] /= len;
+            {
+                PyObject *pylist;
+                int i;
+                pylist = PyTuple_New(4);
+                for (i=0;i<4;i++)
+                    PyTuple_SetItem(pylist,i,PyFloat_FromDouble(quat[i]));
+                return pylist;
+            }
+        }
+     }
+    Py_INCREF(Py_None);
+    return Py_None;
+    
+}
+///quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
+///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
+static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args)
+{
+    
+    double squ;
+    double sqx;
+    double sqy;
+    double sqz;
+    
+    double quat[4];
+
+    PyObject* quatObj;
+    
+    if (PyArg_ParseTuple(args, "O", &quatObj))
+    {
+        PyObject* seq;
+        int len,i;
+        seq = PySequence_Fast(quatObj, "expected a sequence");
+        len = PySequence_Size(quatObj);
+        if (len==4)
+        {
+            for (i = 0; i < 4; i++)
+            {
+                quat[i] = pybullet_internalGetFloatFromSequence(seq,i);
+            }
+        } else
+        {
+            PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
+            Py_DECREF(seq);
+            return NULL;
+        }
+        Py_DECREF(seq);
+    } else
+    {
+         PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
+        return NULL;
+    }
+
+    {
+        double rpy[3];
+        sqx = quat[0] * quat[0];
+        sqy = quat[1] * quat[1];
+        sqz = quat[2] * quat[2];
+        squ = quat[3] * quat[3];
+        rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz);
+        double sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]);
+        rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg));
+        rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz);
+        {
+            PyObject *pylist;
+            int i;
+            pylist = PyTuple_New(3);
+            for (i=0;i<3;i++)
+                PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i]));
+            return pylist;
+        }
+    }
+    Py_INCREF(Py_None);
+    return Py_None;
+}
+
 
 static PyMethodDef SpamMethods[] = {
   
@@ -1109,6 +1343,9 @@ static PyMethodDef SpamMethods[] = {
 	{"setGravity",  pybullet_setGravity, METH_VARARGS,
         "Set the gravity acceleration (x,y,z)."},
 
+    {"setTimeStep",  pybullet_setTimeStep, METH_VARARGS,
+        "Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"},
+    
     {"loadURDF",  pybullet_loadURDF, METH_VARARGS,
       "Create a multibody by loading a URDF file."},
       
@@ -1118,8 +1355,8 @@ static PyMethodDef SpamMethods[] = {
 	{"getBasePositionAndOrientation",  pybullet_getBasePositionAndOrientation, METH_VARARGS,
         "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
 
-//    {"resetBasePositionAndOrientation",  pybullet_getBasePositionAndOrientation, METH_VARARGS,
+    {"resetBasePositionAndOrientation",  pybullet_resetBasePositionAndOrientation, METH_VARARGS,
-//        "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
+        "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
 
 	{"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
         "Get the number of joints for an object."},
@@ -1136,18 +1373,29 @@ static PyMethodDef SpamMethods[] = {
     {"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS,
         "Set a single joint motor control mode and desired target value. There is no immediate state change, stepSimulation will process the motors."},
     
+    {"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS,
+        "for objectUniqueId, linkIndex (-1 for base/root link), apply a force [x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or FORCE_IN_WORLD_FRAME coordinates"},
+   
+    {"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS,
+        "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque [x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or TORQUE_IN_WORLD_FRAME coordinates"},
+    
+	{"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"},	
+
+    {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
+        "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"},
+    
+    {"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
+        "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"},
+    
+    //todo(erwincoumans)
     //saveSnapshot
     //loadSnapshot
     
+    ////todo(erwincoumans)
     //collision info
     //raycast info
-        
+
-    //applyBaseForce
-    //applyLinkForce
-     
-	{"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"},	
-	
     {NULL, NULL, 0, NULL}        /* Sentinel */
 };
 
@@ -1155,7 +1403,7 @@ static PyMethodDef SpamMethods[] = {
     static struct PyModuleDef moduledef = {
         PyModuleDef_HEAD_INIT,
         "pybullet",     /* m_name */
-        "Python bindings for Bullet",  /* m_doc */
+        "Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)",  /* m_doc */
         -1,                  /* m_size */
         SpamMethods,    /* m_methods */
         NULL,                /* m_reload */
@@ -1198,6 +1446,9 @@ initpybullet(void)
     PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read
     PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read
     
+    PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME);
+    PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME);
+    
     SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
     Py_INCREF(SpamError);
     PyModule_AddObject(m, "error", SpamError);