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();