Expose Jacobian computation to RobotSimAPI.

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -148,6 +148,11 @@ public:
         if (numJoints==7)
         {
             double q_current[7]={0,0,0,0,0,0,0};
+            double qdot_current[7]={0,0,0,0,0,0,0};
+            double qddot_current[7]={0,0,0,0,0,0,0};
+            double local_position[3]={0,0,0};
+            double jacobian_linear[21];
+            double jacobian_angular[21];
             b3JointStates jointStates;
             if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
             {
@@ -158,6 +163,13 @@ public:
                     q_current[i] = jointStates.m_actualStateQ[i+7];
                 }
             }
+            
+            // compute body Jacobian
+            m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular);
+            for (int i = 0; i < 21; ++i) {
+                printf("j%d: %f\n", i, jacobian_linear[i]);
+            }
+            
             // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
             double q_new[7];
             int ikMethod=IK2_SDLS;

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -912,3 +912,14 @@ void b3RobotSimAPI::renderScene()
 	}
 	m_data->m_physicsServer.renderScene();
 }
+
+void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
+{
+    b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClient, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
+    b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+    
+    if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
+    {
+        b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
+    }
+}

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -115,6 +115,8 @@ public:
 
 	void renderScene();
 	void debugDraw(int debugDrawMode);
+    
+    void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
 };
 
 #endif //B3_ROBOT_SIM_API_H

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1279,21 +1279,13 @@ b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle
     return (b3SharedMemoryCommandHandle)command;
 }
 
-int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* bodyIndex, int* linkIndex, double* linearJacobian, double* angularJacobian)
+int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian)
 {
     const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
     btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
     if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
         return false;
     
-    if (bodyIndex)
-    {
-        *bodyIndex = status->m_jacobianResultArgs.m_bodyUniqueId;
-    }
-    if (linkIndex)
-    {
-        *linkIndex = status->m_jacobianResultArgs.m_linkIndex;
-    }
     if (linearJacobian)
     {
         for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)

examples/SharedMemory/PhysicsClientC_API.h

@@ -115,9 +115,9 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl
 	int* dofCount,
 	double* jointForces);
 
-b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition);
+b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
 
-int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* bodyIndex, int* linkIndex, double* linearJacobian, double* angularJacobian);
+int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
 
 b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2372,8 +2372,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                             if (-1 != tree->setGravityInWorldFrame(id_grav) &&
                                 -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
                             {
-                                serverCmd.m_jacobianResultArgs.m_bodyUniqueId = clientCmd.m_calculateJacobianArguments.m_bodyUniqueId;
-                                serverCmd.m_jacobianResultArgs.m_linkIndex = clientCmd.m_calculateJacobianArguments.m_linkIndex;
                                 serverCmd.m_jacobianResultArgs.m_dofCount = num_dofs;
                                 // Set jacobian value
                                 tree->calculateJacobians(q);

examples/SharedMemory/SharedMemoryCommands.h

@@ -384,8 +384,6 @@ struct CalculateJacobianArgs
 
 struct CalculateJacobianResultArgs
 {
-    int m_bodyUniqueId;
-    int m_linkIndex;
     int m_dofCount;
     double m_linearJacobian[3*MAX_DEGREE_OF_FREEDOM];
     double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];