diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 68be8acee..3e0de985d 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() @@ -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; - - m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod); - // printf("q_current = %f,%f,%f,%f,%f,%f,%f\n", q_current[0],q_current[1],q_current[2], - // q_current[3],q_current[4],q_current[5],q_current[6]); - - // 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]); + int ikMethod=IK2_DLS; + b3Vector3DoubleData dataOut; + m_targetPos.serializeDouble(dataOut); + 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]); //set the for (int i=0;im_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]; + } +} \ No newline at end of file diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 16c379609..54ebec698 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/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 diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 4954f6b10..735cf389f 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/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: diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index 6bbd53815..bfbc4cdec 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/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 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e4f027695..d6d869a4d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; -} +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 60a647e9a..08a179680 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8f6f19e0b..f45610c15 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 2039cd40a..ee7f2e592 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index de76b2a98..b9a5c9eb5 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_CALCULATE_INVERSE_KINEMATICS_COMPLETED, diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index bd8f49e67..68f31158b 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/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. diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 502ba0413..3f62b2da2 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/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