From c198029cb9dd86d05042c230413b070c9b3002e6 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 7 Sep 2016 16:00:38 -0700 Subject: [PATCH 1/5] Expose body Jacobian in shared memory API. --- examples/SharedMemory/PhysicsClientC_API.cpp | 60 ++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 79 ++++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 21 +++++ examples/SharedMemory/SharedMemoryPublic.h | 3 + examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 6 ++ examples/ThirdPartyLibs/BussIK/Jacobian.h | 1 + 7 files changed, 172 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d57113e3e..a62a0c16e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; } \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4d6522576..67fe44eb0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 03357e59f..c86ae9535 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b718d40c7..25a4fee57 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 28038a3ad..5a43bb598 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index bd8f49e67..0a42ad3d4 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/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. diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 502ba0413..5f9bd08c5 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/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(); From f635c64205f8530cf8d4295b26667b220433769b Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 7 Sep 2016 17:37:38 -0700 Subject: [PATCH 2/5] 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]; From c94a8e0d35b4891ff47133cd7bcc8093cdeb1915 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 7 Sep 2016 23:14:23 -0700 Subject: [PATCH 3/5] Use body Jacobian from Bullet for IK. --- .../RoboticsLearning/IKTrajectoryHelper.cpp | 17 ++++++++++++++++- examples/RoboticsLearning/IKTrajectoryHelper.h | 2 +- examples/RoboticsLearning/KukaGraspExample.cpp | 5 +---- examples/ThirdPartyLibs/BussIK/Jacobian.h | 3 +++ 4 files changed, 21 insertions(+), 6 deletions(-) diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/RoboticsLearning/IKTrajectoryHelper.cpp index 2e97653f6..9c8ba195d 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp +++ b/examples/RoboticsLearning/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" @@ -86,7 +87,7 @@ void IKTrajectoryHelper::createKukaIIWA() bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[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) @@ -110,6 +111,20 @@ 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 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: diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/RoboticsLearning/IKTrajectoryHelper.h index 5b436057f..06ccfe0d8 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.h +++ b/examples/RoboticsLearning/IKTrajectoryHelper.h @@ -24,7 +24,7 @@ public: bool computeIK(const double endEffectorTargetPosition[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 diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 31ae807eb..5c73c7900 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -166,16 +166,13 @@ public: // 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; b3Vector3DoubleData dataOut; m_targetPos.serializeDouble(dataOut); - m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod); + m_ikHelper.computeIK(dataOut.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]); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 5f9bd08c5..99ecd19a0 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -83,6 +83,9 @@ 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 ); + int GetNumRows() {return nRow;} + int GetNumCols() {return nCol;} + private: Tree* tree; // tree associated with this Jacobian matrix int nEffector; // Number of end effectors From 1b72b91bcfedf51230b23d826cdc830c1b80b051 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sat, 10 Sep 2016 18:48:57 -0700 Subject: [PATCH 4/5] Expose link state in RobotSimAPI. --- examples/RoboticsLearning/IKTrajectoryHelper.cpp | 3 +++ examples/RoboticsLearning/IKTrajectoryHelper.h | 1 + examples/RoboticsLearning/KukaGraspExample.cpp | 10 +++++++++- examples/RoboticsLearning/b3RobotSimAPI.cpp | 15 +++++++++++++++ examples/RoboticsLearning/b3RobotSimAPI.h | 2 ++ 5 files changed, 30 insertions(+), 1 deletion(-) diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/RoboticsLearning/IKTrajectoryHelper.cpp index 9c8ba195d..a1be4d831 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp +++ b/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(); diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/RoboticsLearning/IKTrajectoryHelper.h index 06ccfe0d8..b57a17ce9 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.h +++ b/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); diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 5c73c7900..3dcae9271 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/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]); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 62d98f810..cb7f231f1 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/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]; + } +} \ No newline at end of file diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index ef7a38d6f..b57a8a7ca 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/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 From a00841a498cee2979edc0c89aaf3e1e5281370a7 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sat, 10 Sep 2016 19:18:29 -0700 Subject: [PATCH 5/5] Compute delta S from Bullet link state. --- examples/RoboticsLearning/IKTrajectoryHelper.cpp | 10 ++++++++-- examples/RoboticsLearning/KukaGraspExample.cpp | 2 +- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 5 +++++ examples/ThirdPartyLibs/BussIK/Jacobian.h | 3 ++- 4 files changed, 16 insertions(+), 4 deletions(-) diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/RoboticsLearning/IKTrajectoryHelper.cpp index a1be4d831..94f6600c9 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp +++ b/examples/RoboticsLearning/IKTrajectoryHelper.cpp @@ -112,8 +112,14 @@ 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 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(); diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 3dcae9271..cb4b6c521 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -175,7 +175,7 @@ public: // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo); double q_new[7]; - int ikMethod=IK2_SDLS; + int ikMethod=IK2_DLS; b3Vector3DoubleData dataOut; m_targetPos.serializeDouble(dataOut); b3Vector3DoubleData worldPosDataOut; diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 0a42ad3d4..68f31158b 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -144,6 +144,11 @@ void Jacobian::SetJendTrans(MatrixRmn& J) 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. diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 99ecd19a0..3f62b2da2 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -60,6 +60,7 @@ public: 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(); @@ -86,7 +87,7 @@ public: int GetNumRows() {return nRow;} int GetNumCols() {return nCol;} -private: +public: Tree* tree; // tree associated with this Jacobian matrix int nEffector; // Number of end effectors int nJoint; // Number of joints