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

Expose body Jacobian in shared memory API.

Browse Source
This commit is contained in:
yunfeibai
2016-09-07 16:00:38 -07:00
parent 21c60c66ec
commit c198029cb9
7 changed files with 172 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1250,5 +1250,65 @@ 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;
}

3
examples/SharedMemory/PhysicsClientC_API.h
View File

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

77
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

21
examples/SharedMemory/SharedMemoryCommands.h
View File

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

3
examples/SharedMemory/SharedMemoryPublic.h
View File

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

6
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -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.

1
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

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