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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans
2017-09-24 09:49:53 -07:00
parent 63ee841bea 17709ae690
commit 70364445b8
7 changed files with 243 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3310,13 +3310,17 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
}
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian)
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, 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 (dofCount)
{
*dofCount = status->m_jacobianResultArgs.m_dofCount;
}
if (linearJacobian)
{
for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -316,7 +316,10 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
int* dofCount,
double* linearJacobian,
double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);

29
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6618,30 +6618,37 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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++)
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx q(numDofs + baseDofs);
btInverseDynamics::vecx qdot(numDofs + baseDofs);
btInverseDynamics::vecx nu(numDofs + baseDofs);
btInverseDynamics::vecx joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; 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];
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;
serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs;
// Set jacobian value
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex+1, &jac_t);
btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t);
tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < num_dofs; ++j)
for (int j = 0; j < (numDofs + baseDofs); ++j)
{
serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j);
int element = (numDofs + baseDofs) * i + j;
serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j);
serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j);
}
}
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED;
@@ -7365,7 +7372,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)