Expose body Jacobian in shared memory API.

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1251,4 +1251,64 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl
 
 	
 	return true;
+}
+
+b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
+{
+    PhysicsClient* cl = (PhysicsClient*)physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    
+    command->m_type = CMD_CALCULATE_JACOBIAN;
+    command->m_updateFlags = 0;
+    command->m_calculateJacobianArguments.m_bodyUniqueId = bodyIndex;
+    command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
+    command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
+    command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
+    command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
+    int numJoints = cl->getNumJoints(bodyIndex);
+    for (int i = 0; i < numJoints;i++)
+    {
+        command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
+        command->m_calculateJacobianArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
+        command->m_calculateJacobianArguments.m_jointAccelerations[i] = jointAccelerations[i];
+    }
+    
+    return (b3SharedMemoryCommandHandle)command;
+}
+
+int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* bodyIndex, int* linkIndex, 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++)
+        {
+            linearJacobian[i] = status->m_jacobianResultArgs.m_linearJacobian[i];
+        }
+    }
+    if (angularJacobian)
+    {
+        for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)
+        {
+            angularJacobian[i] = status->m_jacobianResultArgs.m_angularJacobian[i];
+        }
+
+    }
+    
+    return true;
 }

examples/SharedMemory/PhysicsClientC_API.h

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

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2325,7 +2325,84 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					hasStatus = true;
 					break;
 				}
-
+                case CMD_CALCULATE_JACOBIAN:
+                {
+                    SharedMemoryStatus& serverCmd = serverStatusOut;
+                    InternalBodyHandle* bodyHandle = m_data->getHandle(clientCmd.m_calculateJacobianArguments.m_bodyUniqueId);
+                    if (bodyHandle && bodyHandle->m_multiBody)
+                    {
+                        btInverseDynamics::MultiBodyTree** treePtrPtr =
+                        m_data->m_inverseDynamicsBodies.find(bodyHandle->m_multiBody);
+                        btInverseDynamics::MultiBodyTree* tree = 0;
+                        serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
+                        
+                        if (treePtrPtr)
+                        {
+                            tree = *treePtrPtr;
+                        }
+                        else
+                        {
+                            btInverseDynamics::btMultiBodyTreeCreator id_creator;
+                            if (-1 == id_creator.createFromBtMultiBody(bodyHandle->m_multiBody, false))
+                            {
+                                b3Error("error creating tree\n");
+                                serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
+                            }
+                            else
+                            {
+                                tree = btInverseDynamics::CreateMultiBodyTree(id_creator);
+                                m_data->m_inverseDynamicsBodies.insert(bodyHandle->m_multiBody, tree);
+                            }
+                        }
+                        
+                        if (tree)
+                        {
+                            int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+                            const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
+                            btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
+                            for (int i = 0; i < num_dofs; i++)
+                            {
+                                q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i];
+                                qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i];
+                                nu[i+baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
+                            }
+                            // Set the gravity to correspond to the world gravity
+                            btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
+                            
+                            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);
+                                btInverseDynamics::mat3x jac_t(3, num_dofs);
+                                tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex, &jac_t);
+                                for (int i = 0; i < 3; ++i)
+                                {
+                                    for (int j = 0; j < num_dofs; ++j)
+                                    {
+                                        serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j);
+                                    }
+                                }
+                                serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED;
+                            }
+                            else
+                            {
+                                serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
+                            }
+                        }
+                        
+                    }
+                    else
+                    {
+                        serverCmd.m_type = CMD_CALCULATED_JACOBIAN_FAILED;
+                    }
+                    
+                    hasStatus = true;
+                    break;
+                }
                 case CMD_APPLY_EXTERNAL_FORCE:
                 {
                 	if (m_data->m_verboseOutput)

examples/SharedMemory/SharedMemoryCommands.h

@@ -372,6 +372,25 @@ struct CalculateInverseDynamicsResultArgs
 	double m_jointForces[MAX_DEGREE_OF_FREEDOM];
 };
 
+struct CalculateJacobianArgs
+{
+    int m_bodyUniqueId;
+    int m_linkIndex;
+    double m_localPosition[3];
+    double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+    double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
+    double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
+};
+
+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];
+};
+
 struct CreateJointArgs
 {
     int m_parentBodyIndex;
@@ -411,6 +430,7 @@ struct SharedMemoryCommand
 		struct PickBodyArgs m_pickBodyArguments;
         struct ExternalForceArgs m_externalForceArguments;
 		struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
+        struct CalculateJacobianArgs m_calculateJacobianArguments;
         struct CreateJointArgs m_createJointArguments;
         struct RequestContactDataArgs m_requestContactPointArguments;
     };
@@ -444,6 +464,7 @@ struct SharedMemoryStatus
 		struct SendPixelDataArgs m_sendPixelDataArguments;
 		struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
 		struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
+        struct CalculateJacobianResultArgs m_jacobianResultArgs;
 		struct SendContactDataArgs m_sendContactPointArgs;
 	};
 };

examples/SharedMemory/SharedMemoryPublic.h

@@ -29,6 +29,7 @@ enum EnumSharedMemoryClientCommand
     CMD_APPLY_EXTERNAL_FORCE,
 	CMD_CALCULATE_INVERSE_DYNAMICS,
     CMD_CALCULATE_INVERSE_KINEMATICS,
+    CMD_CALCULATE_JACOBIAN,
     CMD_CREATE_JOINT,
     CMD_REQUEST_CONTACT_POINT_INFORMATION,
     //don't go beyond this command!
@@ -67,6 +68,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_INVALID_STATUS,
 		CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
 		CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
+        CMD_CALCULATED_JACOBIAN_COMPLETED,
+        CMD_CALCULATED_JACOBIAN_FAILED,
 		CMD_CONTACT_POINT_INFORMATION_COMPLETED,
 		CMD_CONTACT_POINT_INFORMATION_FAILED,
         CMD_MAX_SERVER_COMMANDS

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -138,6 +138,12 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
 	}
 }
 
+void Jacobian::SetJendTrans(MatrixRmn& J)
+{
+    Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
+    Jend.LoadAsSubmatrix(J);
+}
+
 // The delta theta values have been computed in dTheta array
 // Apply the delta theta values to the joints
 // Nothing is done about joint limits for now.

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -59,6 +59,7 @@ public:
 	const MatrixRmn& ActiveJacobian() const { return *Jactive; } 
 	void SetJendActive() { Jactive = &Jend; }						// The default setting is Jend.
 	void SetJtargetActive() { Jactive = &Jtarget; }
+    void SetJendTrans(MatrixRmn& J);
 
 	void CalcDeltaThetas();			// Use this only if the Current Mode has been set.
 	void ZeroDeltaThetas();