Expose link state in RobotSimAPI.

examples/RoboticsLearning/IKTrajectoryHelper.cpp

@@ -86,6 +86,7 @@ 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, const double* linear_jacobian, int jacobian_size)
 {
@@ -111,6 +112,8 @@ 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 end effector world position from Bullet
+    
     // Set Jacobian from Bullet body Jacobian
     int nRow = m_data->m_ikJacobian->GetNumRows();
     int nCol = m_data->m_ikJacobian->GetNumCols();

examples/RoboticsLearning/IKTrajectoryHelper.h

@@ -23,6 +23,7 @@ public:
     void createKukaIIWA();
     
     bool computeIK(const double endEffectorTargetPosition[3],
+                   const double endEffectorWorldPosition[3],
                    const double* q_old, int numQ,
                    double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size);
     

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()
@@ -153,6 +155,7 @@ public:
             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))
             {
@@ -164,6 +167,9 @@ public:
                 }
             }
             
+            // 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);
             
@@ -172,7 +178,9 @@ public:
             int ikMethod=IK2_SDLS;
             b3Vector3DoubleData dataOut;
             m_targetPos.serializeDouble(dataOut);
-            m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod,jacobian_linear,(sizeof(jacobian_linear)/sizeof(*jacobian_linear)));
+            b3Vector3DoubleData worldPosDataOut;
+            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],
                    q_new[3],q_new[4],q_new[5],q_new[6]);
         

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -923,3 +923,18 @@ void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const doubl
         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

@@ -117,6 +117,8 @@ public:
 	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