Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Expose Jacobian computation to RobotSimAPI.

Browse Source
This commit is contained in:
yunfeibai
2016-09-07 17:37:38 -07:00
parent c198029cb9
commit f635c64205
7 changed files with 28 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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++)

4
examples/SharedMemory/PhysicsClientC_API.h
View File

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

2
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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];