From f635c64205f8530cf8d4295b26667b220433769b Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 7 Sep 2016 17:37:38 -0700 Subject: [PATCH] Expose Jacobian computation to RobotSimAPI. --- examples/RoboticsLearning/KukaGraspExample.cpp | 12 ++++++++++++ examples/RoboticsLearning/b3RobotSimAPI.cpp | 11 +++++++++++ examples/RoboticsLearning/b3RobotSimAPI.h | 2 ++ examples/SharedMemory/PhysicsClientC_API.cpp | 10 +--------- examples/SharedMemory/PhysicsClientC_API.h | 4 ++-- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 2 -- examples/SharedMemory/SharedMemoryCommands.h | 2 -- 7 files changed, 28 insertions(+), 15 deletions(-) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 52cf68160..31ae807eb 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/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; diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index e7daae828..62d98f810 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/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); + } +} diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 142d406cf..ef7a38d6f 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/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 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a62a0c16e..c49b88c04 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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++) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 67fe44eb0..931a1d9d9 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c86ae9535..8d6fcd70c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 25a4fee57..64089286b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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];