ae8e83988b
Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py) Fix related to TinyRenderer object transforms not updating when using collision filtering
392 lines
12 KiB
C++
392 lines
12 KiB
C++
#include "btBulletDynamicsCommon.h"
|
|
|
|
//for inverse dynamics, DeepMimic implementation
|
|
#include "RBDModel.h"
|
|
#include "RBDUtil.h"
|
|
#include "KinTree.h"
|
|
|
|
//for BulletInverseDynamics
|
|
//#include "BulletInverseDynamics/IDConfig.hpp"
|
|
//#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
|
|
|
|
//#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
|
|
//#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
|
|
//#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
|
|
|
|
#include "BulletDynamics/Featherstone/btMultiBody.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
|
|
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
|
|
|
|
|
|
struct TempLink
|
|
{
|
|
int m_parentIndex;
|
|
const btCollisionObject* m_collider;
|
|
double m_mass;
|
|
int m_jointType;
|
|
int m_dofOffset;
|
|
int m_dofCount;
|
|
btVector3 m_dVector;
|
|
btVector3 m_eVector;
|
|
btQuaternion m_zeroRotParentToThis;
|
|
btQuaternion m_this_to_body1;
|
|
};
|
|
|
|
bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
|
|
{
|
|
|
|
bool result = true;
|
|
|
|
int num_joints = links.size();
|
|
|
|
btAlignedObjectArray<btVector3> bodyToLinkPositions;
|
|
btAlignedObjectArray<btQuaternion> bodyToLinkRotations;
|
|
btAlignedObjectArray<btQuaternion> dVectorRot;
|
|
bodyToLinkRotations.resize(num_joints);
|
|
bodyToLinkPositions.resize(num_joints);
|
|
dVectorRot.resize(num_joints);
|
|
|
|
jointMat.resize(num_joints, 19);
|
|
bodyDefs.resize(num_joints, 17);
|
|
for (int i = 0; i < num_joints * 19; i++)
|
|
{
|
|
jointMat(i) = SIMD_INFINITY;
|
|
}
|
|
for (int i = 0; i < num_joints * 17; i++)
|
|
{
|
|
bodyDefs(i) = SIMD_INFINITY;
|
|
}
|
|
|
|
for (int i = 0; i < num_joints * 17; i++)
|
|
{
|
|
bodyDefs(i) = SIMD_INFINITY;
|
|
}
|
|
|
|
btScalar unk = -12345;
|
|
|
|
int totalDofs = 0;
|
|
|
|
for (int j = 0; j < num_joints; ++j)
|
|
{
|
|
|
|
int i = j;
|
|
int parentIndex = links[j].m_parentIndex;
|
|
|
|
cShape::eShape shapeType = cShape::eShapeNull;
|
|
double param0 = 0, param1 = 0, param2 = 0;
|
|
if (links[j].m_collider)
|
|
{
|
|
const btCollisionShape* collisionShape = links[j].m_collider->getCollisionShape();
|
|
if (collisionShape->isCompound())
|
|
{
|
|
const btCompoundShape* compound = (const btCompoundShape*)collisionShape;
|
|
if (compound->getNumChildShapes() > 0)
|
|
{
|
|
collisionShape = compound->getChildShape(0);
|
|
}
|
|
}
|
|
switch (collisionShape->getShapeType())
|
|
{
|
|
case BOX_SHAPE_PROXYTYPE:
|
|
{
|
|
shapeType = cShape::eShapeBox;
|
|
btBoxShape* box = (btBoxShape*)collisionShape;
|
|
param0 = box->getHalfExtentsWithMargin()[0] * 2;
|
|
param1 = box->getHalfExtentsWithMargin()[1] * 2;
|
|
param2 = box->getHalfExtentsWithMargin()[2] * 2;
|
|
break;
|
|
}
|
|
case SPHERE_SHAPE_PROXYTYPE:
|
|
{
|
|
btSphereShape* sphere = (btSphereShape*)collisionShape;
|
|
param0 = sphere->getRadius() * 2;
|
|
param1 = sphere->getRadius() * 2;
|
|
param2 = sphere->getRadius() * 2;
|
|
shapeType = cShape::eShapeSphere;
|
|
break;
|
|
}
|
|
case CAPSULE_SHAPE_PROXYTYPE:
|
|
{
|
|
btCapsuleShape* caps = (btCapsuleShape*)collisionShape;
|
|
param0 = caps->getRadius() * 2;
|
|
param1 = caps->getHalfHeight() * 2;
|
|
param2 = caps->getRadius() * 2;
|
|
shapeType = cShape::eShapeCapsule;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
btAssert(0);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
btVector3 body_attach_pt1 = links[j].m_dVector;
|
|
|
|
//tQuaternion body_to_parent_body = parent_to_parent_body * this_to_parent * body_to_this;
|
|
//tQuaternion parent_to_parent_body = parent_body_to_parent.inverse();
|
|
|
|
|
|
bodyDefs(i, cKinTree::eBodyParam0) = param0;
|
|
bodyDefs(i, cKinTree::eBodyParam1) = param1;
|
|
bodyDefs(i, cKinTree::eBodyParam2) = param2;
|
|
|
|
bodyDefs(i, cKinTree::eBodyParamShape) = shapeType;
|
|
bodyDefs(i, cKinTree::eBodyParamMass) = links[j].m_mass;
|
|
|
|
|
|
bodyDefs(i, cKinTree::eBodyParamColGroup) = unk;
|
|
bodyDefs(i, cKinTree::eBodyParamEnableFallContact) = unk;
|
|
|
|
bodyDefs(i, cKinTree::eBodyColorR) = unk;
|
|
bodyDefs(i, cKinTree::eBodyColorG) = unk;
|
|
bodyDefs(i, cKinTree::eBodyColorB) = unk;
|
|
bodyDefs(i, cKinTree::eBodyColorA) = unk;
|
|
|
|
dVectorRot[j] = links[j].m_this_to_body1;
|
|
|
|
btVector3 body_attach_pt2 = quatRotate(links[j].m_this_to_body1.inverse(), body_attach_pt1);
|
|
bodyToLinkPositions[i] = body_attach_pt2;
|
|
bodyDefs(i, cKinTree::eBodyParamAttachX) = body_attach_pt2[0];
|
|
bodyDefs(i, cKinTree::eBodyParamAttachY) = body_attach_pt2[1];
|
|
bodyDefs(i, cKinTree::eBodyParamAttachZ) = body_attach_pt2[2];
|
|
btScalar bodyAttachThetaX = 0;
|
|
btScalar bodyAttachThetaY = 0;
|
|
btScalar bodyAttachThetaZ = 0;
|
|
btQuaternion body_to_this1 = links[j].m_this_to_body1.inverse();
|
|
|
|
|
|
|
|
body_to_this1.getEulerZYX(bodyAttachThetaZ, bodyAttachThetaY, bodyAttachThetaX);
|
|
bodyDefs(i, cKinTree::eBodyParamAttachThetaX) = bodyAttachThetaX;
|
|
bodyDefs(i, cKinTree::eBodyParamAttachThetaY) = bodyAttachThetaY;
|
|
bodyDefs(i, cKinTree::eBodyParamAttachThetaZ) = bodyAttachThetaZ;
|
|
|
|
jointMat(i, cKinTree::eJointDescType) = links[j].m_jointType;
|
|
jointMat(i, cKinTree::eJointDescParent) = parentIndex;
|
|
|
|
|
|
btVector3 jointAttachPointMy = links[j].m_eVector;
|
|
btVector3 jointAttachPointMyv0 = jointAttachPointMy;
|
|
btVector3 parentBodyAttachPtMy(0, 0, 0);
|
|
btQuaternion parentBodyToLink;
|
|
parentBodyToLink = btQuaternion::getIdentity();
|
|
btQuaternion linkToParentBody = btQuaternion::getIdentity();
|
|
int parent_joint = links[j].m_parentIndex;
|
|
|
|
if (parent_joint != gInvalidIdx)
|
|
{
|
|
parentBodyAttachPtMy = bodyToLinkPositions[parent_joint];
|
|
parentBodyToLink = bodyToLinkRotations[parent_joint];
|
|
linkToParentBody = parentBodyToLink.inverse();
|
|
}
|
|
parentBodyAttachPtMy = quatRotate(linkToParentBody, parentBodyAttachPtMy);
|
|
//bodyToLinkRotations
|
|
jointAttachPointMy += parentBodyAttachPtMy;
|
|
jointAttachPointMy = quatRotate(linkToParentBody.inverse(), jointAttachPointMy);
|
|
|
|
btVector3 parent_body_attach_pt1(0, 0, 0);
|
|
if (parentIndex >= 0)
|
|
{
|
|
parent_body_attach_pt1 = links[parentIndex].m_dVector;
|
|
}
|
|
btQuaternion myparent_body_to_body(0, 0, 0, 1);
|
|
btQuaternion mybody_to_parent_body(0, 0, 0, 1);
|
|
btQuaternion parent_body_to_body1 = links[i].m_zeroRotParentToThis;
|
|
btQuaternion body_to_parent_body1 = parent_body_to_body1.inverse();
|
|
|
|
bodyToLinkRotations[i] = body_to_this1;
|
|
|
|
jointMat(i, cKinTree::eJointDescAttachX) = jointAttachPointMy[0];
|
|
jointMat(i, cKinTree::eJointDescAttachY) = jointAttachPointMy[1];
|
|
jointMat(i, cKinTree::eJointDescAttachZ) = jointAttachPointMy[2];
|
|
|
|
|
|
btQuaternion parent2parent_body2(0, 0, 0, 1);
|
|
|
|
if (parent_joint >= 0)
|
|
{
|
|
//parent2parent_body2 = bulletMB->getLink(parent_joint).m_dVectorRot;
|
|
parent2parent_body2 = dVectorRot[parent_joint];
|
|
}
|
|
///btQuaternion this2bodyA = bulletMB->getLink(j).m_dVectorRot;
|
|
btQuaternion this2bodyA = dVectorRot[j];
|
|
|
|
btQuaternion parent_body_2_body = links[j].m_zeroRotParentToThis;
|
|
btQuaternion combined2 = parent_body_2_body.inverse();
|
|
btQuaternion recoverthis2parent = parent2parent_body2.inverse()*combined2*this2bodyA;// body2this.inverse();
|
|
btScalar eulZ, eulY, eulX;
|
|
recoverthis2parent.getEulerZYX(eulZ, eulY, eulX);
|
|
|
|
|
|
jointMat(i, cKinTree::eJointDescAttachThetaX) = eulX;
|
|
jointMat(i, cKinTree::eJointDescAttachThetaY) = eulY;
|
|
jointMat(i, cKinTree::eJointDescAttachThetaZ) = eulZ;
|
|
|
|
|
|
jointMat(i, cKinTree::eJointDescLimLow0) = unk;
|
|
jointMat(i, cKinTree::eJointDescLimLow1) = unk;
|
|
jointMat(i, cKinTree::eJointDescLimLow2) = unk;
|
|
jointMat(i, cKinTree::eJointDescLimHigh0) = unk;
|
|
jointMat(i, cKinTree::eJointDescLimHigh1) = unk;
|
|
jointMat(i, cKinTree::eJointDescLimHigh2) = unk;
|
|
jointMat(i, cKinTree::eJointDescTorqueLim) = unk;
|
|
|
|
jointMat(i, cKinTree::eJointDescForceLim) = unk;
|
|
jointMat(i, cKinTree::eJointDescIsEndEffector) = unk;
|
|
jointMat(i, cKinTree::eJointDescDiffWeight) = unk;
|
|
jointMat(i, cKinTree::eJointDescParamOffset) = totalDofs;
|
|
totalDofs += links[j].m_dofCount;
|
|
|
|
}
|
|
return result;
|
|
}
|
|
|
|
|
|
void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
|
|
{
|
|
btAlignedObjectArray<TempLink> links;
|
|
|
|
int numBaseShapes = 0;
|
|
if (bulletMB->getBaseCollider())
|
|
{
|
|
switch (bulletMB->getBaseCollider()->getCollisionShape()->getShapeType())
|
|
{
|
|
case CAPSULE_SHAPE_PROXYTYPE:
|
|
case SPHERE_SHAPE_PROXYTYPE:
|
|
case BOX_SHAPE_PROXYTYPE:
|
|
{
|
|
numBaseShapes++;
|
|
break;
|
|
}
|
|
case COMPOUND_SHAPE_PROXYTYPE:
|
|
{
|
|
btCompoundShape* compound = (btCompoundShape*)bulletMB->getBaseCollider()->getCollisionShape();
|
|
numBaseShapes += compound->getNumChildShapes();
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
//links include the 'base' and its childlinks
|
|
int baseLink = numBaseShapes? 1 : 0;
|
|
links.resize(bulletMB->getNumLinks() + baseLink);
|
|
for (int i = 0; i < links.size(); i++)
|
|
{
|
|
memset(&links[i], 0xffffffff, sizeof(TempLink));
|
|
}
|
|
|
|
int totalDofs = 0;
|
|
if (numBaseShapes)
|
|
{
|
|
//links[0] is the root/base
|
|
links[0].m_parentIndex = -1;
|
|
links[0].m_collider = bulletMB->getBaseCollider();
|
|
links[0].m_mass = bulletMB->getBaseMass();
|
|
links[0].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
|
|
links[0].m_dofOffset = 0;
|
|
links[0].m_dofCount = 7;
|
|
links[0].m_dVector.setValue(0, 0, 0);
|
|
links[0].m_eVector.setValue(0, 0, 0);
|
|
links[0].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
|
|
links[0].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
|
|
totalDofs = 7;
|
|
}
|
|
|
|
|
|
for (int j = 0; j < bulletMB->getNumLinks(); ++j)
|
|
{
|
|
int parentIndex = bulletMB->getLink(j).m_parent;
|
|
links[j + baseLink].m_parentIndex = parentIndex + baseLink;
|
|
links[j + baseLink].m_collider = bulletMB->getLinkCollider(j);
|
|
links[j + baseLink].m_mass = bulletMB->getLink(j).m_mass;
|
|
|
|
int jointType = 0;
|
|
btQuaternion this_to_body1(0, 0, 0, 1);
|
|
int dofCount = 0;
|
|
|
|
if ((baseLink)==0 &&j == 0)//for 'root' either use fixed or none
|
|
{
|
|
dofCount = 7;
|
|
links[j].m_parentIndex = -1;
|
|
links[j].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
|
|
links[j].m_dofOffset = 0;
|
|
links[j].m_dofCount = dofCount;
|
|
|
|
|
|
links[j].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
|
|
//links[j].m_dVector.setValue(0, 0, 0);
|
|
links[j].m_dVector = bulletMB->getLink(j).m_dVector;
|
|
links[j].m_eVector.setValue(0, 0, 0);
|
|
//links[j].m_eVector = bulletMB->getLink(j).m_eVector;
|
|
links[j].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
|
|
|
|
links[j].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
|
|
totalDofs = 7;
|
|
}
|
|
else
|
|
{
|
|
|
|
switch (bulletMB->getLink(j).m_jointType)
|
|
{
|
|
case btMultibodyLink::eFixed:
|
|
{
|
|
jointType = cKinTree::eJointTypeFixed;
|
|
break;
|
|
}
|
|
case btMultibodyLink::ePrismatic:
|
|
{
|
|
dofCount = 1;
|
|
btVector3 refAxis(1, 0, 0);
|
|
btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
|
|
this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
|
|
jointType = cKinTree::eJointTypePrismatic;
|
|
break;
|
|
}
|
|
case btMultibodyLink::eSpherical:
|
|
{
|
|
dofCount = 4;//??
|
|
jointType = cKinTree::eJointTypeSpherical;
|
|
break;
|
|
}
|
|
case btMultibodyLink::eRevolute:
|
|
{
|
|
dofCount = 1;
|
|
btVector3 refAxis(0, 0, 1);
|
|
btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
|
|
this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
|
|
jointType = cKinTree::eJointTypeRevolute;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
links[j + baseLink].m_jointType = jointType;
|
|
links[j + baseLink].m_dofOffset = totalDofs;
|
|
links[j + baseLink].m_dofCount = dofCount;
|
|
links[j + baseLink].m_dVector = bulletMB->getLink(j).m_dVector;
|
|
links[j + baseLink].m_eVector = bulletMB->getLink(j).m_eVector;
|
|
links[j + baseLink].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
|
|
links[j + baseLink].m_this_to_body1 = this_to_body1;
|
|
}
|
|
|
|
totalDofs += dofCount;
|
|
}
|
|
|
|
btExtractJointBodyFromTempLinks(links, bodyDefs, jointMat);
|
|
}
|