Merge pull request #775 from YunfeiBai/master

Compute IK with Bullet body Jacobian

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -26,6 +26,7 @@ class KukaGraspExample : public CommonExampleInterface
     IKTrajectoryHelper m_ikHelper;
     int m_targetSphereInstance;
     b3Vector3 m_targetPos;
+    b3Vector3 m_worldPos;
     double m_time;
 	int m_options;
 	
@@ -45,6 +46,7 @@ public:
     m_time(0)
     {
         m_targetPos.setValue(0.5,0,1);
+        m_worldPos.setValue(0, 0, 0);
 		m_app->setUpAxis(2);
     }
     virtual ~KukaGraspExample()
@@ -151,6 +153,12 @@ 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];
+            double world_position[3]={0,0,0};
             b3JointStates jointStates;
             if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
             {
@@ -161,10 +169,13 @@ public:
                     q_current[i] = jointStates.m_actualStateQ[i+7];
                 }
             }
-			 b3Vector3DoubleData dataOut;
+            
-			 m_targetPos.serializeDouble(dataOut);
+            // compute body position
-
+            m_robotSim.getLinkState(0, 6, world_position);
-
+            m_worldPos.setValue(world_position[0], world_position[1], world_position[2]);
+            // compute body Jacobian
+            m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular);
+            
             // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
 /*
 			b3RobotSimInverseKinematicArgs ikargs;
@@ -189,14 +200,14 @@ public:
 			}
 */
             double q_new[7];
-            int ikMethod=IK2_SDLS;
+            int ikMethod=IK2_DLS;
-           
+            b3Vector3DoubleData dataOut;
-            m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod);
+            m_targetPos.serializeDouble(dataOut);
-        //    printf("q_current = %f,%f,%f,%f,%f,%f,%f\n", q_current[0],q_current[1],q_current[2],
+            b3Vector3DoubleData worldPosDataOut;
-            //       q_current[3],q_current[4],q_current[5],q_current[6]);
+            m_worldPos.serializeDouble(worldPosDataOut);
-            
+            m_ikHelper.computeIK(dataOut.m_floats,worldPosDataOut.m_floats,q_current, numJoints, q_new, ikMethod,jacobian_linear,(sizeof(jacobian_linear)/sizeof(*jacobian_linear)));
-          //  printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
+            printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2],
-              //     q_new[3],q_new[4],q_new[5],q_new[6]);
+                   q_new[3],q_new[4],q_new[5],q_new[6]);
         
             //set the
             for (int i=0;i<numJoints;i++)

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -939,3 +939,29 @@ 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);
+    }
+}
+
+void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition)
+{
+    b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId);
+    b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+    
+    if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+    {
+        b3LinkState linkState;
+        b3GetLinkState(m_data->m_physicsClient, statusHandle, linkIndex, &linkState);
+        worldPosition[0] = linkState.m_worldPosition[0];
+        worldPosition[1] = linkState.m_worldPosition[1];
+        worldPosition[2] = linkState.m_worldPosition[2];
+    }
+}

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -147,6 +147,10 @@ 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);
+    
+    void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition);
 };
 
 #endif //B3_ROBOT_SIM_API_H

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -3,6 +3,7 @@
 #include "BussIK/Tree.h"
 #include "BussIK/Jacobian.h"
 #include "BussIK/VectorRn.h"
+#include "BussIK/MatrixRmn.h"
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "BulletDynamics/Featherstone/btMultiBody.h"
 
@@ -96,8 +97,9 @@ void IKTrajectoryHelper::createKukaIIWA()
 }
 
 bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
+                                   const double endEffectorWorldPosition[3],
                const double* q_current, int numQ,
-               double* q_new, int ikMethod)
+               double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size)
 {
 
     if (numQ != 7)
@@ -121,6 +123,28 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
     targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
     m_data->m_ikJacobian->ComputeJacobian(&targets);						// Set up Jacobian and deltaS vectors
     
+    // Set one end effector world position from Bullet
+    VectorRn deltaS(3);
+    for (int i = 0; i < 3; ++i)
+    {
+        deltaS.Set(i,endEffectorTargetPosition[i]-endEffectorWorldPosition[i]);
+    }
+    m_data->m_ikJacobian->SetDeltaS(deltaS);
+
+    // Set Jacobian from Bullet body Jacobian
+    int nRow = m_data->m_ikJacobian->GetNumRows();
+    int nCol = m_data->m_ikJacobian->GetNumCols();
+    b3Assert(jacobian_size==nRow*nCol);
+    MatrixRmn linearJacobian(nRow,nCol);
+    for (int i = 0; i < nRow; ++i)
+    {
+        for (int j = 0; j < nCol; ++j)
+        {
+            linearJacobian.Set(i,j,linear_jacobian[i*nCol+j]);
+        }
+    }
+    m_data->m_ikJacobian->SetJendTrans(linearJacobian);
+    
     // Calculate the change in theta values
     switch (ikMethod) {
         case IK2_JACOB_TRANS:

examples/SharedMemory/IKTrajectoryHelper.h

@@ -25,8 +25,9 @@ public:
 	bool createFromMultiBody(class btMultiBody* mb);
 
     bool computeIK(const double endEffectorTargetPosition[3],
+                   const double endEffectorWorldPosition[3],
                    const double* q_old, int numQ,
-                   double* q_new, int ikMethod);
+                   double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size);
     
 };
 #endif //IK_TRAJECTORY_HELPER_H

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1266,6 +1266,57 @@ 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, 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 (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;
+}
 
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
 b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
@@ -1322,4 +1373,4 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
 	}
 
 	return false;
-}
+}

examples/SharedMemory/PhysicsClientC_API.h

@@ -116,6 +116,10 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl
 	int* dofCount,
 	double* jointForces);
 
+b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
+
+int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
+
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
 b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
 	const double* jointPositionsQ, const double targetPosition[3]);
@@ -125,9 +129,6 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
 	int* dofCount,
 	double* jointPositions);
 
-
-
-
 b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
 
 ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2344,7 +2344,82 @@ 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_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

@@ -374,6 +374,23 @@ 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_dofCount;
+    double m_linearJacobian[3*MAX_DEGREE_OF_FREEDOM];
+    double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
+};
+
 struct CalculateInverseKinematicsArgs
 {
 	int m_bodyUniqueId;
@@ -389,7 +406,6 @@ struct CalculateInverseKinematicsResultArgs
 	double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
 };
 
-
 struct CreateJointArgs
 {
     int m_parentBodyIndex;
@@ -429,6 +445,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;
 		struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
@@ -463,6 +480,7 @@ struct SharedMemoryStatus
 		struct SendPixelDataArgs m_sendPixelDataArguments;
 		struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
 		struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
+        struct CalculateJacobianResultArgs m_jacobianResultArgs;
 		struct SendContactDataArgs m_sendContactPointArgs;
 		struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
 	};

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_CALCULATE_INVERSE_KINEMATICS_COMPLETED,

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -138,6 +138,17 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
 	}
 }
 
+void Jacobian::SetJendTrans(MatrixRmn& J)
+{
+    Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
+    Jend.LoadAsSubmatrix(J);
+}
+
+void Jacobian::SetDeltaS(VectorRn& S)
+{
+    dS.Set(S);
+}
+
 // 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,8 @@ 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 SetDeltaS(VectorRn& S);
 
 	void CalcDeltaThetas();			// Use this only if the Current Mode has been set.
 	void ZeroDeltaThetas();
@@ -82,7 +84,10 @@ public:
 	static void CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 );
 	static void CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies );
 
-private:
+    int GetNumRows() {return nRow;}
+    int GetNumCols() {return nCol;}
+    
+public:
 	Tree* tree;			// tree associated with this Jacobian matrix
 	int nEffector;		// Number of end effectors
 	int nJoint;			// Number of joints