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

Merge pull request #2007 from erwincoumans/master

Browse Source 
enable pybullet.calculateInverseDynamics for floating bodies
This commit is contained in:
erwincoumans
2018-11-28 14:37:26 -08:00
committed by GitHub
parent 554208c98d dc642c6e65
commit 1be34588a9
11 changed files with 244 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -4369,13 +4369,47 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints; i++)
int dofCount = b3ComputeDofCount(physClient, bodyUniqueId);
for (int i = 0; i < dofCount; i++)
{
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCount;
command->m_calculateInverseDynamicsArguments.m_dofCountQdot = dofCount;
return (b3SharedMemoryCommandHandle)command;
}
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCountQ;
for (int i = 0; i < dofCountQ; i++)
{
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
}
command->m_calculateInverseDynamicsArguments.m_dofCountQdot = dofCountQdot;
for (int i=0;i<dofCountQdot;i++)
{
command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
return (b3SharedMemoryCommandHandle)command;
}

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -384,6 +384,9 @@ extern "C"
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot);
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

67
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -8493,23 +8493,56 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
BT_PROFILE("CMD_CALCULATE_INVERSE_DYNAMICS");
SharedMemoryStatus& serverCmd = serverStatusOut;
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId);
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
if (bodyHandle && bodyHandle->m_multiBody)
{
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;
int baseDofQdot = 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);
if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ+ num_dofs) &&
clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot+ num_dofs))
{
btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
//PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API
if (baseDofQ)
{
btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]);
btScalar yawZ, pitchY, rollX;
orn.getEulerZYX(yawZ, pitchY, rollX);
q[0] = rollX;
q[1] = pitchY;
q[2] = yawZ;
q[3] = pos[0];
q[4] = pos[1];
q[5] = pos[2];
}
else
{
for (int i = 0; i < num_dofs; i++)
{
q[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
qdot[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i + baseDofs] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
}
}
for (int i = 0; i < num_dofs + baseDofQdot; i++)
{
qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
@@ -8517,10 +8550,22 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
for (int i = 0; i < num_dofs; i++)
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs+ baseDofQdot;
//inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API.
if (baseDofQdot)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i + baseDofs];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2];
}
for (int i = baseDofQdot; i < num_dofs+ baseDofQdot; i++)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i];
}
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -658,7 +658,8 @@ enum EnumSdfRequestInfoFlags
struct CalculateInverseDynamicsArgs
{
int m_bodyUniqueId;
int m_dofCountQ;
int m_dofCountQdot;
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -7,7 +7,9 @@
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 201810250
#define SHARED_MEMORY_MAGIC_NUMBER 201811260
//#define SHARED_MEMORY_MAGIC_NUMBER 201810250
//#define SHARED_MEMORY_MAGIC_NUMBER 201809030
//#define SHARED_MEMORY_MAGIC_NUMBER 201809010
//#define SHARED_MEMORY_MAGIC_NUMBER 201807040

32
examples/pybullet/pybullet.c
View File

@@ -9590,23 +9590,25 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
int szObVel = PySequence_Size(objVelocitiesQdot);
int szObAcc = PySequence_Size(objAccelerations);
int dofCountOrg = b3ComputeDofCount(sm, bodyUniqueId);
if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) &&
(szObAcc == dofCountOrg))
if (szObVel == szObAcc)
{
int szInBytes = sizeof(double) * dofCountOrg;
int szInBytesQ = sizeof(double) * szObPos;
int szInBytesQdot = sizeof(double) * szObVel;
int i;
PyObject* pylist = 0;
double* jointPositionsQ = (double*)malloc(szInBytes);
double* jointVelocitiesQdot = (double*)malloc(szInBytes);
double* jointAccelerations = (double*)malloc(szInBytes);
double* jointForcesOutput = (double*)malloc(szInBytes);
double* jointPositionsQ = (double*)malloc(szInBytesQ);
double* jointVelocitiesQdot = (double*)malloc(szInBytesQdot);
double* jointAccelerations = (double*)malloc(szInBytesQdot);
double* jointForcesOutput = (double*)malloc(szInBytesQdot);
for (i = 0; i < dofCountOrg; i++)
for (i = 0; i < szObPos; i++)
{
jointPositionsQ[i] =
pybullet_internalGetFloatFromSequence(objPositionsQ, i);
}
for (i = 0; i < szObVel; i++)
{
jointVelocitiesQdot[i] =
pybullet_internalGetFloatFromSequence(objVelocitiesQdot, i);
jointAccelerations[i] =
@@ -9617,9 +9619,9 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateInverseDynamicsCommandInit(
sm, bodyUniqueId, jointPositionsQ, jointVelocitiesQdot,
jointAccelerations);
b3CalculateInverseDynamicsCommandInit2(
sm, bodyUniqueId, jointPositionsQ, szObPos, jointVelocitiesQdot,
jointAccelerations, szObVel);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
@@ -9650,7 +9652,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
else
{
PyErr_SetString(SpamError,
"Internal error in calculateInverseDynamics");
"Error in calculateInverseDynamics, please check arguments.");
}
}
free(jointPositionsQ);
@@ -9663,8 +9665,8 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
{
PyErr_SetString(SpamError,
"calculateInverseDynamics numDofs needs to be "
"positive and [joint positions], [joint velocities], "
"[joint accelerations] need to match the number of "
"positive and [joint velocities] and"
"[joint accelerations] need to be equal and match the number of "
"degrees of freedom.");
return NULL;
}

10
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -296,7 +296,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
capsuleB->getHalfHeight(), capsuleB->getRadius(), capsuleA->getUpAxis(), capsuleB->getUpAxis(),
@@ -318,7 +318,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
btCapsuleShape* capsuleA = (btCapsuleShape*)min0;
btSphereShape* capsuleB = (btSphereShape*)min1;
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, capsuleA->getHalfHeight(), capsuleA->getRadius(),
0., capsuleB->getRadius(), capsuleA->getUpAxis(), 1,
@@ -340,7 +340,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
btSphereShape* capsuleA = (btSphereShape*)min0;
btCapsuleShape* capsuleB = (btCapsuleShape*)min1;
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld, 0., capsuleA->getRadius(),
capsuleB->getHalfHeight(), capsuleB->getRadius(), 1, capsuleB->getUpAxis(),
@@ -480,7 +480,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*)min1;
if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
{
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
btScalar minDist = -1e30f;
btVector3 sepNormalWorldSpace;
@@ -547,7 +547,7 @@ void btConvexConvexAlgorithm ::processCollision(const btCollisionObjectWrapper*
//tri->initializePolyhedralFeatures();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
btVector3 sepNormalWorldSpace;
btScalar minDist = -1e30f;

2
src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp
View File

@@ -116,7 +116,7 @@ void btConvexPlaneCollisionAlgorithm::processCollision(const btCollisionObjectWr
btVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
btVector3 vtxInPlaneWorld = planeObjWrap->getWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold();
hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold;
resultOut->setPersistentManifold(m_manifoldPtr);
if (hasCollision)
{

8
src/BulletInverseDynamics/MultiBodyTree.hpp
View File

@@ -16,7 +16,9 @@ enum JointType
/// one translational degree of freedom relative to parent
PRISMATIC,
/// six degrees of freedom relative to parent
FLOATING
FLOATING,
/// three degrees of freedom, relative to parent
SPHERICAL
};
/// Interface class for calculating inverse dynamics for tree structured
@@ -31,12 +33,14 @@ enum JointType
/// - PRISMATIC: displacement [m]
/// - FLOATING: Euler x-y-z angles [rad] and displacement in body-fixed frame of parent [m]
/// (in that order)
/// - SPHERICAL: Euler x-y-z angles [rad]
/// The u vector contains the generalized speeds, which are
/// - FIXED: none
/// - REVOLUTE: time derivative of angle of rotation [rad/s]
/// - PRISMATIC: time derivative of displacement [m/s]
/// - FLOATING: angular velocity [rad/s] (*not* time derivative of rpy angles)
/// and time derivative of displacement in parent frame [m/s]
// - SPHERICAL: angular velocity [rad/s]
///
/// The q and u vectors are obtained by stacking contributions of all bodies in one
/// vector in the order of body indices.
@@ -47,7 +51,7 @@ enum JointType
/// - PRISMATIC: force [N], along joint axis
/// - FLOATING: moment vector [Nm] and force vector [N], both in body-fixed frame
/// (in that order)
///
/// - SPHERICAL: moment vector [Nm]
/// TODO - force element interface (friction, springs, dampers, etc)
/// - gears and motor inertia
class MultiBodyTree

112
src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
View File

@@ -35,6 +35,8 @@ const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &typ
return "prismatic";
case FLOATING:
return "floating";
case SPHERICAL:
return "spherical";
}
return "error: invalid";
}
@@ -88,6 +90,8 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
return 1;
case FLOATING:
return 6;
case SPHERICAL:
return 3;
}
bt_id_error_message("unknown joint type %d\n", type);
return 0;
@@ -150,6 +154,11 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets()
body.m_q_index = q_index;
q_index += 6;
break;
case SPHERICAL:
m_body_spherical_list.push_back(i);
body.m_q_index = q_index;
q_index += 3;
break;
default:
bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
return -1;
@@ -238,6 +247,16 @@ void MultiBodyTree::MultiBodyImpl::calculateStaticData()
case FLOATING:
// no static data
break;
case SPHERICAL:
//todo: review
body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
body.m_parent_vel_rel(0) = 0;
body.m_parent_vel_rel(1) = 0;
body.m_parent_vel_rel(2) = 0;
body.m_parent_acc_rel(0) = 0;
body.m_parent_acc_rel(1) = 0;
body.m_parent_acc_rel(2) = 0;
break;
}
// resize & initialize jacobians to zero.
@@ -352,6 +371,15 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const
(*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
}
// 4.4 spherical bodies (3-DoF joints)
for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
{
//todo: review
RigidBody &body = m_body_list[m_body_spherical_list[i]];
(*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
(*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
(*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
}
return 0;
}
@@ -413,7 +441,8 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
RigidBody &body = m_body_list[m_body_floating_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
transformY(q(body.m_q_index + 1)) * transformX(q(body.m_q_index));
transformY(q(body.m_q_index + 1)) *
transformX(q(body.m_q_index));
body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
@@ -445,6 +474,32 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
}
}
for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
{
//todo: review
RigidBody &body = m_body_list[m_body_spherical_list[i]];
body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
transformY(q(body.m_q_index + 1)) *
transformX(q(body.m_q_index));
body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
if (type >= POSITION_VELOCITY)
{
body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
}
if (type >= POSITION_VELOCITY_ACCELERATION)
{
body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
}
}
// 2. absolute kinematic quantities (vector valued)
// NOTE: this should be optimized by specializing for different body types
// (e.g., relative rotation is always zero for prismatic joints, etc.)
@@ -560,6 +615,12 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
break;
case SPHERICAL:
//todo: review
setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
break;
}
}
@@ -608,6 +669,32 @@ int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &
}
#endif
static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
{
switch (dof)
{
// rotational part
case 0:
Jac_JR(0) = 1;
Jac_JR(1) = 0;
Jac_JR(2) = 0;
setZero(Jac_JT);
break;
case 1:
Jac_JR(0) = 0;
Jac_JR(1) = 1;
Jac_JR(2) = 0;
setZero(Jac_JT);
break;
case 2:
Jac_JR(0) = 0;
Jac_JR(1) = 0;
Jac_JR(2) = 1;
setZero(Jac_JT);
break;
}
}
static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
{
switch (dof)
@@ -664,6 +751,8 @@ static inline int jointNumDoFs(const JointType &type)
return 1;
case FLOATING:
return 6;
case SPHERICAL:
return 3;
}
// this should never happen
bt_id_error_message("invalid joint type\n");
@@ -798,6 +887,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
{
setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
}
if (SPHERICAL == body.m_joint_type)
{
//todo: review
setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
}
vec3 body_eom_rot =
body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
@@ -810,15 +904,20 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
// 1. for multi-dof joints, rest of the dofs of this body
for (int row = col - 1; row >= q_index_min; row--)
{
if (FLOATING != body.m_joint_type)
if (SPHERICAL == body.m_joint_type)
{
bt_id_error_message("??\n");
return -1;
//todo: review
setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
setMatxxElem(col, row, Mrc, mass_matrix);
}
if (FLOATING == body.m_joint_type)
{
setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
setMatxxElem(col, row, Mrc, mass_matrix);
}
}
// 2. ancestor dofs
int child_idx = i;
int parent_idx = m_parent_index[i];
@@ -839,6 +938,11 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
vec3 Jac_JT = parent_body.m_Jac_JT;
for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
{
if (SPHERICAL == parent_body.m_joint_type)
{
//todo: review
setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
}
// set jacobians for 6-DoF joints
if (FLOATING == parent_body.m_joint_type)
{

2
src/BulletInverseDynamics/details/MultiBodyTreeImpl.hpp
View File

@@ -274,6 +274,8 @@ private:
idArray<int>::type m_body_prismatic_list;
// Indices of floating joints
idArray<int>::type m_body_floating_list;
// Indices of spherical joints
idArray<int>::type m_body_spherical_list;
// a user-provided integer
idArray<int>::type m_user_int;
// a user-provided pointer