mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
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
909 lines
30 KiB
C++
909 lines
30 KiB
C++
|
|
#include "URDF2PhysX.h"
|
|
|
|
#include "PhysXUrdfImporter.h"
|
|
#include "Bullet3Common/b3Logging.h"
|
|
#include "PxArticulationReducedCoordinate.h"
|
|
#include "PxArticulationJointReducedCoordinate.h"
|
|
#include "PxRigidActorExt.h"
|
|
#include "PxArticulation.h"
|
|
#include "PxRigidBodyExt.h"
|
|
#include "PxRigidBody.h"
|
|
#include "LinearMath/btThreads.h"
|
|
#include "PxRigidActorExt.h"
|
|
#include "PxArticulationBase.h"
|
|
#include "PxArticulationLink.h"
|
|
#include "PxMaterial.h"
|
|
#include "PxCooking.h"
|
|
#include "PxScene.h"
|
|
#include "PxAggregate.h"
|
|
|
|
#include "PhysXUserData.h"
|
|
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
|
|
|
#include "Importers/ImportURDFDemo/UrdfParser.h"
|
|
|
|
|
|
|
|
|
|
struct URDF2PhysXCachedData
|
|
{
|
|
URDF2PhysXCachedData()
|
|
: m_currentMultiBodyLinkIndex(-1),
|
|
m_articulation(0),
|
|
m_totalNumJoints1(0)
|
|
{
|
|
}
|
|
//these arrays will be initialized in the 'InitURDF2BulletCache'
|
|
|
|
btAlignedObjectArray<int> m_urdfLinkParentIndices;
|
|
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
|
|
btAlignedObjectArray<class physx::PxArticulationLink*> m_urdfLink2physxLink;
|
|
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
|
|
|
|
int m_currentMultiBodyLinkIndex;
|
|
|
|
physx::PxArticulationReducedCoordinate* m_articulation;
|
|
|
|
btAlignedObjectArray<physx::PxTransform> m_linkTransWorldSpace;
|
|
btAlignedObjectArray<int> m_urdfLinkIndex;
|
|
btAlignedObjectArray<int> m_parentUrdfLinkIndex;
|
|
btAlignedObjectArray<physx::PxReal> m_linkMasses;
|
|
btAlignedObjectArray<physx::PxArticulationJointType::Enum> m_jointTypes;
|
|
|
|
btAlignedObjectArray<physx::PxTransform> m_parentLocalPoses;
|
|
btAlignedObjectArray<physx::PxTransform> m_childLocalPoses;
|
|
btAlignedObjectArray<UrdfGeomTypes> m_geomTypes;
|
|
btAlignedObjectArray<physx::PxVec3> m_geomDimensions;
|
|
btAlignedObjectArray<physx::PxVec3> m_linkMaterials;
|
|
btAlignedObjectArray<physx::PxTransform> m_geomLocalPoses;
|
|
|
|
|
|
|
|
//this will be initialized in the constructor
|
|
int m_totalNumJoints1;
|
|
int getParentUrdfIndex(int linkIndex) const
|
|
{
|
|
return m_urdfLinkParentIndices[linkIndex];
|
|
}
|
|
int getMbIndexFromUrdfIndex(int urdfIndex) const
|
|
{
|
|
if (urdfIndex == -2)
|
|
return -2;
|
|
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
|
|
}
|
|
|
|
void registerMultiBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& localInertialFrame)
|
|
{
|
|
m_urdfLink2physxLink[urdfLinkIndex] = body;
|
|
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
|
}
|
|
|
|
class physx::PxArticulationLink* getPhysxLinkFromLink(int urdfLinkIndex)
|
|
{
|
|
return m_urdfLink2physxLink[urdfLinkIndex];
|
|
}
|
|
|
|
void registerRigidBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
|
|
{
|
|
btAssert(m_urdfLink2physxLink[urdfLinkIndex] == 0);
|
|
|
|
m_urdfLink2physxLink[urdfLinkIndex] = body;
|
|
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
|
|
}
|
|
};
|
|
|
|
|
|
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
|
|
{
|
|
cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
|
|
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
|
|
|
|
btAlignedObjectArray<int> childIndices;
|
|
u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
|
|
for (int i = 0; i < childIndices.size(); i++)
|
|
{
|
|
ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
|
|
}
|
|
}
|
|
|
|
void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int linkIndex)
|
|
{
|
|
btAlignedObjectArray<int> childIndices;
|
|
u2b.getLinkChildIndices(linkIndex, childIndices);
|
|
cache.m_totalNumJoints1 += childIndices.size();
|
|
for (int i = 0; i < childIndices.size(); i++)
|
|
{
|
|
int childIndex = childIndices[i];
|
|
ComputeTotalNumberOfJoints(u2b, cache, childIndex);
|
|
}
|
|
}
|
|
|
|
|
|
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int flags)
|
|
{
|
|
//compute the number of links, and compute parent indices array (and possibly other cached data?)
|
|
cache.m_totalNumJoints1 = 0;
|
|
|
|
int rootLinkIndex = u2b.getRootLinkIndex();
|
|
if (rootLinkIndex >= 0)
|
|
{
|
|
ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
|
|
int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
|
|
|
|
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
|
|
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
|
|
cache.m_urdfLink2physxLink.resize(numTotalLinksIncludingBase);
|
|
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
|
|
|
|
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
|
|
|
|
bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER) != 0;
|
|
if (maintainLinkOrder)
|
|
{
|
|
URDF2PhysXCachedData cache2 = cache;
|
|
|
|
ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
|
|
|
|
for (int j = 0; j<numTotalLinksIncludingBase; j++)
|
|
{
|
|
cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
|
|
cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
struct ArticulationCreationInterface
|
|
{
|
|
physx::PxFoundation* m_foundation;
|
|
physx::PxPhysics* m_physics;
|
|
physx::PxCooking* m_cooking;
|
|
physx::PxScene* m_scene;
|
|
CommonFileIOInterface* m_fileIO;
|
|
|
|
b3AlignedObjectArray<int> m_mb2urdfLink;
|
|
void addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
|
|
{
|
|
if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
|
|
{
|
|
m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
|
|
}
|
|
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
|
|
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
|
|
}
|
|
};
|
|
|
|
static btVector4 colors4[4] =
|
|
{
|
|
btVector4(1, 0, 0, 1),
|
|
btVector4(0, 1, 0, 1),
|
|
btVector4(0, 1, 1, 1),
|
|
btVector4(1, 1, 0, 1),
|
|
};
|
|
|
|
static btVector4 selectColor4()
|
|
{
|
|
static btSpinMutex sMutex;
|
|
sMutex.lock();
|
|
static int curColor = 0;
|
|
btVector4 color = colors4[curColor];
|
|
curColor++;
|
|
curColor &= 3;
|
|
sMutex.unlock();
|
|
return color;
|
|
}
|
|
|
|
|
|
|
|
|
|
static physx::PxConvexMesh* createPhysXConvex(physx::PxU32 numVerts, const physx::PxVec3* verts, ArticulationCreationInterface& creation)
|
|
{
|
|
physx::PxCookingParams params = creation.m_cooking->getParams();
|
|
|
|
// Use the new (default) PxConvexMeshCookingType::eQUICKHULL
|
|
params.convexMeshCookingType = physx::PxConvexMeshCookingType::eQUICKHULL;
|
|
|
|
// If the gaussMapLimit is chosen higher than the number of output vertices, no gauss map is added to the convex mesh data (here 256).
|
|
// If the gaussMapLimit is chosen lower than the number of output vertices, a gauss map is added to the convex mesh data (here 16).
|
|
int gaussMapLimit = 256;
|
|
|
|
params.gaussMapLimit = gaussMapLimit;
|
|
creation.m_cooking->setParams(params);
|
|
|
|
// Setup the convex mesh descriptor
|
|
physx::PxConvexMeshDesc desc;
|
|
|
|
// We provide points only, therefore the PxConvexFlag::eCOMPUTE_CONVEX flag must be specified
|
|
desc.points.data = verts;
|
|
desc.points.count = numVerts;
|
|
desc.points.stride = sizeof(physx::PxVec3);
|
|
desc.flags = physx::PxConvexFlag::eCOMPUTE_CONVEX;
|
|
|
|
physx::PxU32 meshSize = 0;
|
|
|
|
// Directly insert mesh into PhysX
|
|
physx::PxConvexMesh* convex = creation.m_cooking->createConvexMesh(desc, creation.m_physics->getPhysicsInsertionCallback());
|
|
PX_ASSERT(convex);
|
|
|
|
return convex;
|
|
}
|
|
|
|
|
|
int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, ArticulationCreationInterface& creation, int urdfLinkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
|
physx::PxArticulationReducedCoordinate* articulation, int mbLinkIndex, physx::PxArticulationLink* linkPtr)
|
|
{
|
|
int numShapes = 0;
|
|
|
|
URDFLinkContactInfo contactInfo;
|
|
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
|
|
|
//static friction, dynamic frictoin, restitution
|
|
cache.m_linkMaterials.push_back(physx::PxVec3(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution));
|
|
physx::PxMaterial* material = creation.m_physics->createMaterial(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution);
|
|
|
|
const UrdfLink* link = u2b.getUrdfLink(urdfLinkIndex);//.convertLinkCollisionShapes m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
|
|
btAssert(linkPtr);
|
|
if (linkPtr)
|
|
{
|
|
|
|
for (int v = 0; v < link->m_collisionArray.size(); v++)
|
|
{
|
|
const UrdfCollision& col = link->m_collisionArray[v];
|
|
const UrdfCollision* collision = &col;
|
|
btTransform childTrans = col.m_linkLocalFrame;
|
|
btTransform localTrans = localInertiaFrame.inverse()*childTrans;
|
|
physx::PxTransform tr;
|
|
tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
|
|
tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
|
|
|
|
physx::PxShape* shape = 0;
|
|
cache.m_geomTypes.push_back(col.m_geometry.m_type);
|
|
|
|
|
|
switch (col.m_geometry.m_type)
|
|
{
|
|
case URDF_GEOM_PLANE:
|
|
{
|
|
btVector3 planeNormal = col.m_geometry.m_planeNormal;
|
|
btScalar planeConstant = 0; //not available?
|
|
//PxPlane(1, 0, 0, 0).
|
|
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material);
|
|
//todo: compensate for plane axis
|
|
//physx::PxTransform localPose = tr*physx::PxTransform
|
|
//shape->setLocalPose(localPose);
|
|
numShapes++;
|
|
break;
|
|
}
|
|
case URDF_GEOM_CAPSULE:
|
|
{
|
|
btScalar radius = collision->m_geometry.m_capsuleRadius;
|
|
btScalar height = collision->m_geometry.m_capsuleHeight;
|
|
|
|
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxCapsuleGeometry(radius, 0.5*height), *material);
|
|
|
|
btTransform childTrans = col.m_linkLocalFrame;
|
|
btTransform x2z;
|
|
x2z.setIdentity();
|
|
x2z.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
|
|
btTransform localTrans = localInertiaFrame.inverse()*childTrans*x2z;
|
|
physx::PxTransform tr;
|
|
tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
|
|
tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
|
|
|
|
shape->setLocalPose(tr);
|
|
cache.m_geomLocalPoses.push_back(tr);
|
|
numShapes++;
|
|
//btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height);
|
|
//shape = capsuleShape;
|
|
//shape->setMargin(gUrdfDefaultCollisionMargin);
|
|
break;
|
|
}
|
|
|
|
case URDF_GEOM_CYLINDER:
|
|
{
|
|
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
|
|
btScalar cylHalfLength = 0.5 * collision->m_geometry.m_capsuleHeight;
|
|
cache.m_geomDimensions.push_back(physx::PxVec3(cylRadius, cylHalfLength,0));
|
|
//if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER)
|
|
//{
|
|
// btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength);
|
|
// btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
|
|
// shape = cylZShape;
|
|
//}
|
|
//else
|
|
{
|
|
btAlignedObjectArray<physx::PxVec3> vertices;
|
|
|
|
int numSteps = 32;
|
|
for (int i = 0; i < numSteps; i++)
|
|
{
|
|
physx::PxVec3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylHalfLength);
|
|
vertices.push_back(vert);
|
|
vert[2] = -cylHalfLength;
|
|
vertices.push_back(vert);
|
|
}
|
|
|
|
physx::PxConvexMesh* convexMesh = createPhysXConvex(vertices.size(), &vertices[0], creation);
|
|
|
|
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr,
|
|
physx::PxConvexMeshGeometry(convexMesh), *material);
|
|
|
|
shape->setLocalPose(tr);
|
|
cache.m_geomLocalPoses.push_back(tr);
|
|
numShapes++;
|
|
|
|
}
|
|
break;
|
|
}
|
|
case URDF_GEOM_BOX:
|
|
{
|
|
btVector3 extents = collision->m_geometry.m_boxSize;
|
|
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxBoxGeometry(extents[0] * 0.5, extents[1] * 0.5, extents[2] * 0.5), *material);
|
|
cache.m_geomDimensions.push_back(physx::PxVec3(extents[0]*0.5, extents[1] * 0.5, extents[2] * 0.5));
|
|
shape->setLocalPose(tr);
|
|
cache.m_geomLocalPoses.push_back(tr);
|
|
numShapes++;
|
|
break;
|
|
}
|
|
case URDF_GEOM_SPHERE:
|
|
{
|
|
btScalar radius = collision->m_geometry.m_sphereRadius;
|
|
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxSphereGeometry(radius), *material);
|
|
cache.m_geomDimensions.push_back(physx::PxVec3(radius,0,0));
|
|
shape->setLocalPose(tr);
|
|
cache.m_geomLocalPoses.push_back(tr);
|
|
numShapes++;
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
|
|
if (shape)
|
|
{
|
|
//see https://github.com/NVIDIAGameWorks/PhysX/issues/21
|
|
physx::PxReal contactOffset = shape->getContactOffset();
|
|
physx::PxReal restOffset = shape->getContactOffset();
|
|
|
|
//shape->setContactOffset(physx::PxReal(.03));
|
|
//shape->setRestOffset(physx::PxReal(.01)); //
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
return numShapes;
|
|
}
|
|
|
|
btTransform ConvertURDF2PhysXInternal(
|
|
const PhysXURDFImporter& u2b,
|
|
ArticulationCreationInterface& creation,
|
|
URDF2PhysXCachedData& cache, int urdfLinkIndex,
|
|
const btTransform& parentTransformInWorldSpace,
|
|
bool createMultiBody, const char* pathPrefix,
|
|
int flags,
|
|
UrdfVisualShapeCache2* cachedLinkGraphicsShapesIn,
|
|
UrdfVisualShapeCache2* cachedLinkGraphicsShapesOut,
|
|
bool recursive)
|
|
{
|
|
B3_PROFILE("ConvertURDF2PhysXInternal");
|
|
//b3Printf("start converting/extracting data from URDF interface\n");
|
|
|
|
btTransform linkTransformInWorldSpace;
|
|
linkTransformInWorldSpace.setIdentity();
|
|
|
|
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
|
|
|
|
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
|
|
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
|
|
|
|
|
|
|
|
//b3Printf("mb link index = %d\n",mbLinkIndex);
|
|
|
|
btTransform parentLocalInertialFrame;
|
|
parentLocalInertialFrame.setIdentity();
|
|
btScalar parentMass(1);
|
|
btVector3 parentLocalInertiaDiagonal(1, 1, 1);
|
|
|
|
if (urdfParentIndex == -2)
|
|
{
|
|
//b3Printf("root link has no parent\n");
|
|
}
|
|
else
|
|
{
|
|
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
|
|
//b3Printf("mb parent index = %d\n",mbParentIndex);
|
|
//parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
|
|
u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
|
|
}
|
|
|
|
btScalar mass = 0;
|
|
btTransform localInertialFrame;
|
|
localInertialFrame.setIdentity();
|
|
btVector3 localInertiaDiagonal(0, 0, 0);
|
|
u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
|
|
|
|
btTransform parent2joint;
|
|
parent2joint.setIdentity();
|
|
|
|
int jointType;
|
|
btVector3 jointAxisInJointSpace;
|
|
btScalar jointLowerLimit;
|
|
btScalar jointUpperLimit;
|
|
btScalar jointDamping;
|
|
btScalar jointFriction;
|
|
btScalar jointMaxForce;
|
|
btScalar jointMaxVelocity;
|
|
|
|
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
|
|
|
|
btTransform axis2Reference;
|
|
axis2Reference.setIdentity();
|
|
|
|
switch (jointType)
|
|
{
|
|
case URDFContinuousJoint:
|
|
case URDFPrismaticJoint:
|
|
case URDFRevoluteJoint:
|
|
{
|
|
//rotate from revolute 'axis' to standard X axis
|
|
btVector3 refAxis(1, 0, 0);
|
|
btVector3 axis = jointAxisInJointSpace;
|
|
//btQuaternion axis2ReferenceRot(btVector3(0, 1, 0), SIMD_HALF_PI);// = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
|
|
btQuaternion axis2ReferenceRot = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
|
|
axis2Reference.setRotation(axis2ReferenceRot);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
};
|
|
|
|
parent2joint = parent2joint*axis2Reference;
|
|
//localInertialFrame = axis2Reference.inverse()*localInertialFrame;
|
|
|
|
std::string linkName = u2b.getLinkName(urdfLinkIndex);
|
|
|
|
if (flags & CUF_USE_SDF)
|
|
{
|
|
parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
|
|
}
|
|
else
|
|
{
|
|
if (flags & CUF_USE_MJCF)
|
|
{
|
|
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
|
|
}
|
|
else
|
|
{
|
|
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
|
|
}
|
|
}
|
|
|
|
|
|
int graphicsIndex;
|
|
{
|
|
B3_PROFILE("convertLinkVisualShapes");
|
|
|
|
|
|
|
|
graphicsIndex = u2b.convertLinkVisualShapes3(urdfLinkIndex, pathPrefix, localInertialFrame, u2b.getUrdfLink(urdfLinkIndex), u2b.getUrdfModel(), -1, u2b.getBodyUniqueId(), creation.m_fileIO);
|
|
|
|
|
|
#if 0
|
|
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
|
|
{
|
|
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
|
|
UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
|
|
u2b.setLinkColor2(urdfLinkIndex, matColor);
|
|
}
|
|
else
|
|
{
|
|
graphicsIndex =
|
|
if (cachedLinkGraphicsShapesOut)
|
|
{
|
|
cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex);
|
|
UrdfMaterialColor matColor;
|
|
u2b.getLinkColor2(urdfLinkIndex, matColor);
|
|
cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor);
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
if (1)
|
|
{
|
|
UrdfMaterialColor matColor;
|
|
btVector4 color2 = selectColor4();
|
|
btVector3 specular(0.5, 0.5, 0.5);
|
|
if (u2b.getLinkColor2(urdfLinkIndex, matColor))
|
|
{
|
|
color2 = matColor.m_rgbaColor;
|
|
specular = matColor.m_specularColor;
|
|
}
|
|
|
|
/*
|
|
if (visual->material.get())
|
|
{
|
|
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
|
|
}
|
|
*/
|
|
if (mass)
|
|
{
|
|
if (!(flags & CUF_USE_URDF_INERTIA))
|
|
{
|
|
//b3Assert(0);
|
|
//compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
|
|
btAssert(localInertiaDiagonal[0] < 1e10);
|
|
btAssert(localInertiaDiagonal[1] < 1e10);
|
|
btAssert(localInertiaDiagonal[2] < 1e10);
|
|
}
|
|
URDFLinkContactInfo contactInfo;
|
|
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
|
|
//temporary inertia scaling until we load inertia from URDF
|
|
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
|
|
{
|
|
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
|
|
}
|
|
}
|
|
|
|
|
|
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
|
|
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
|
|
|
|
physx::PxArticulationLink* linkPtr = 0;
|
|
|
|
if (!createMultiBody)
|
|
{
|
|
|
|
}
|
|
else
|
|
{
|
|
physx::PxTransform tr;
|
|
tr.p = physx::PxVec3(linkTransformInWorldSpace.getOrigin().x(), linkTransformInWorldSpace.getOrigin().y(), linkTransformInWorldSpace.getOrigin().z());
|
|
tr.q = physx::PxQuat(linkTransformInWorldSpace.getRotation().x(), linkTransformInWorldSpace.getRotation().y(), linkTransformInWorldSpace.getRotation().z(), linkTransformInWorldSpace.getRotation().w());
|
|
|
|
cache.m_linkTransWorldSpace.push_back(tr);
|
|
cache.m_urdfLinkIndex.push_back(urdfLinkIndex);
|
|
cache.m_parentUrdfLinkIndex.push_back(urdfParentIndex);
|
|
cache.m_linkMasses.push_back(mass);
|
|
|
|
if (cache.m_articulation == 0)
|
|
{
|
|
|
|
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
|
|
|
|
cache.m_articulation = creation.m_physics->createArticulationReducedCoordinate();
|
|
|
|
if (isFixedBase)
|
|
{
|
|
cache.m_articulation->setArticulationFlags(physx::PxArticulationFlag::eFIX_BASE);
|
|
}
|
|
|
|
|
|
|
|
physx::PxArticulationLink* base = cache.m_articulation->createLink(NULL,tr);
|
|
|
|
linkPtr = base;
|
|
|
|
|
|
//physx::PxRigidActorExt::createExclusiveShape(*base, PxBoxGeometry(0.5f, 0.25f, 1.5f), *gMaterial);
|
|
physx::PxRigidBody& body = *base;
|
|
|
|
//Now create the slider and fixed joints...
|
|
|
|
cache.m_articulation->setSolverIterationCounts(32);//todo: API?
|
|
|
|
cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED);
|
|
cache.m_parentLocalPoses.push_back(physx::PxTransform());
|
|
cache.m_childLocalPoses.push_back(physx::PxTransform());
|
|
|
|
// Stabilization can create artefacts on jointed objects so we just disable it
|
|
//cache.m_articulation->setStabilizationThreshold(0.0f);
|
|
//cache.m_articulation->setMaxProjectionIterations(16);
|
|
//cache.m_articulation->setSeparationTolerance(0.001f);
|
|
|
|
#if 0
|
|
int totalNumJoints = cache.m_totalNumJoints1;
|
|
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
|
|
if (flags & CUF_GLOBAL_VELOCITIES_MB)
|
|
{
|
|
cache.m_bulletMultiBody->useGlobalVelocities(true);
|
|
}
|
|
if (flags & CUF_USE_MJCF)
|
|
{
|
|
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
|
|
}
|
|
|
|
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
|
|
physx::PxArticulationLink* parentLink = cache.getPhysxLinkFromLink(urdfParentIndex);
|
|
linkPtr = cache.m_articulation->createLink(parentLink, tr);
|
|
|
|
physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(linkPtr->getInboundJoint());
|
|
|
|
switch (jointType)
|
|
{
|
|
case URDFFixedJoint:
|
|
{
|
|
joint->setJointType(physx::PxArticulationJointType::eFIX);
|
|
break;
|
|
}
|
|
case URDFSphericalJoint:
|
|
{
|
|
joint->setJointType(physx::PxArticulationJointType::eSPHERICAL);
|
|
joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
|
|
joint->setMotion(physx::PxArticulationAxis::eSWING2, physx::PxArticulationMotion::eFREE);
|
|
joint->setMotion(physx::PxArticulationAxis::eSWING1, physx::PxArticulationMotion::eFREE);
|
|
break;
|
|
}
|
|
case URDFContinuousJoint:
|
|
case URDFRevoluteJoint:
|
|
{
|
|
joint->setJointType(physx::PxArticulationJointType::eREVOLUTE);
|
|
joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
|
|
|
|
break;
|
|
}
|
|
case URDFPrismaticJoint:
|
|
{
|
|
joint->setJointType(physx::PxArticulationJointType::ePRISMATIC);
|
|
joint->setMotion(physx::PxArticulationAxis::eX, physx::PxArticulationMotion::eFREE);
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
joint->setJointType(physx::PxArticulationJointType::eFIX);
|
|
btAssert(0);
|
|
}
|
|
};
|
|
|
|
cache.m_jointTypes.push_back(joint->getJointType());
|
|
btTransform offsetInA, offsetInB;
|
|
|
|
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
|
|
offsetInB = (axis2Reference.inverse()*localInertialFrame).inverse();
|
|
|
|
physx::PxTransform parentPose(physx::PxVec3(offsetInA.getOrigin()[0], offsetInA.getOrigin()[1], offsetInA.getOrigin()[2]),
|
|
physx::PxQuat(offsetInA.getRotation()[0], offsetInA.getRotation()[1], offsetInA.getRotation()[2], offsetInA.getRotation()[3]));
|
|
|
|
physx::PxTransform childPose(physx::PxVec3(offsetInB.getOrigin()[0], offsetInB.getOrigin()[1], offsetInB.getOrigin()[2]),
|
|
physx::PxQuat(offsetInB.getRotation()[0], offsetInB.getRotation()[1], offsetInB.getRotation()[2], offsetInB.getRotation()[3]));
|
|
|
|
cache.m_parentLocalPoses.push_back(parentPose);
|
|
cache.m_childLocalPoses.push_back(childPose);
|
|
|
|
joint->setParentPose(parentPose);
|
|
joint->setChildPose(childPose);
|
|
|
|
}
|
|
cache.registerMultiBody(urdfLinkIndex, linkPtr, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame);
|
|
if (linkPtr)
|
|
{
|
|
//todo: mem leaks
|
|
MyPhysXUserData* userData = new MyPhysXUserData();
|
|
userData->m_graphicsUniqueId = graphicsIndex;
|
|
linkPtr->userData = userData;
|
|
}
|
|
}
|
|
|
|
|
|
//create collision shapes
|
|
|
|
//physx::PxRigidActorExt::createExclusiveShape
|
|
convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr);
|
|
|
|
|
|
physx::PxRigidBodyExt::updateMassAndInertia(*linkPtr, mass);
|
|
|
|
|
|
//base->setMass(massOut);
|
|
//base->setMassSpaceInertiaTensor(diagTensor);
|
|
//base->setCMassLocalPose(PxTransform(com, orient));
|
|
|
|
|
|
//create a joint if necessary
|
|
if (hasParentJoint)
|
|
{
|
|
btTransform offsetInA, offsetInB;
|
|
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
|
|
offsetInB = localInertialFrame.inverse();
|
|
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
|
|
|
|
bool disableParentCollision = true;
|
|
|
|
if (createMultiBody && cache.m_articulation)
|
|
{
|
|
#if 0
|
|
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
|
|
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
|
|
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
|
|
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
|
|
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce;
|
|
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
|
|
if (createMultiBody)
|
|
{
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
}
|
|
}//was if 'shape/compountShape'
|
|
|
|
btAlignedObjectArray<int> urdfChildIndices;
|
|
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
|
|
|
|
int numChildren = urdfChildIndices.size();
|
|
|
|
if (recursive)
|
|
{
|
|
for (int i = 0; i < numChildren; i++)
|
|
{
|
|
int urdfChildLinkIndex = urdfChildIndices[i];
|
|
|
|
ConvertURDF2PhysXInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
|
|
}
|
|
}
|
|
return linkTransformInWorldSpace;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const btTransform& rootTransformInWorldSpace, struct CommonFileIOInterface* fileIO)
|
|
{
|
|
URDF2PhysXCachedData cache;
|
|
InitURDF2BulletCache(u2p, cache, flags);
|
|
int urdfLinkIndex = u2p.getRootLinkIndex();
|
|
int rootIndex = u2p.getRootLinkIndex();
|
|
B3_PROFILE("ConvertURDF2Bullet");
|
|
|
|
UrdfVisualShapeCache2 cachedLinkGraphicsShapesOut;
|
|
UrdfVisualShapeCache2 cachedLinkGraphicsShapes;
|
|
|
|
ArticulationCreationInterface creation;
|
|
creation.m_foundation = foundation;
|
|
creation.m_physics = physics;
|
|
creation.m_cooking = cooking;
|
|
creation.m_scene = scene;
|
|
creation.m_fileIO = fileIO;
|
|
|
|
bool createMultiBody = true;
|
|
|
|
bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER) == 0;
|
|
|
|
if (recursive)
|
|
{
|
|
ConvertURDF2PhysXInternal(u2p, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, createMultiBody, pathPrefix, flags, &cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
|
|
|
}
|
|
else
|
|
{
|
|
#if 0
|
|
btAlignedObjectArray<btTransform> parentTransforms;
|
|
if (urdfLinkIndex >= parentTransforms.size())
|
|
{
|
|
parentTransforms.resize(urdfLinkIndex + 1);
|
|
}
|
|
parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
|
|
btAlignedObjectArray<childParentIndex> allIndices;
|
|
|
|
GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
|
|
allIndices.quickSort(MyIntCompareFunc);
|
|
|
|
for (int i = 0; i < allIndices.size(); i++)
|
|
{
|
|
int urdfLinkIndex = allIndices[i].m_index;
|
|
int parentIndex = allIndices[i].m_parentIndex;
|
|
btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
|
|
btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
|
|
if ((urdfLinkIndex + 1) >= parentTransforms.size())
|
|
{
|
|
parentTransforms.resize(urdfLinkIndex + 1);
|
|
}
|
|
parentTransforms[urdfLinkIndex] = tr;
|
|
}
|
|
#endif
|
|
|
|
}
|
|
#if 0
|
|
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
|
|
{
|
|
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
|
|
}
|
|
#endif
|
|
|
|
|
|
if (scene && cache.m_articulation)
|
|
{
|
|
#ifdef DEBUG_ARTICULATIONS
|
|
printf("\n-----------------\n");
|
|
printf("m_linkTransWorldSpace\n");
|
|
for (int i = 0; i < cache.m_linkTransWorldSpace.size(); i++)
|
|
{
|
|
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
|
cache.m_linkTransWorldSpace[i].p.x, cache.m_linkTransWorldSpace[i].p.y, cache.m_linkTransWorldSpace[i].p.z,
|
|
cache.m_linkTransWorldSpace[i].q.x, cache.m_linkTransWorldSpace[i].q.y, cache.m_linkTransWorldSpace[i].q.z, cache.m_linkTransWorldSpace[i].q.w);
|
|
}
|
|
printf("m_parentLocalPoses\n");
|
|
for (int i = 0; i < cache.m_parentLocalPoses.size(); i++)
|
|
{
|
|
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
|
cache.m_parentLocalPoses[i].p.x, cache.m_parentLocalPoses[i].p.y, cache.m_parentLocalPoses[i].p.z,
|
|
cache.m_parentLocalPoses[i].q.x, cache.m_parentLocalPoses[i].q.y, cache.m_parentLocalPoses[i].q.z, cache.m_parentLocalPoses[i].q.w);
|
|
}
|
|
|
|
printf("m_childLocalPoses\n");
|
|
for (int i = 0; i < cache.m_childLocalPoses.size(); i++)
|
|
{
|
|
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
|
cache.m_childLocalPoses[i].p.x, cache.m_childLocalPoses[i].p.y, cache.m_childLocalPoses[i].p.z,
|
|
cache.m_childLocalPoses[i].q.x, cache.m_childLocalPoses[i].q.y, cache.m_childLocalPoses[i].q.z, cache.m_childLocalPoses[i].q.w);
|
|
}
|
|
|
|
printf("m_geomDimensions\n");
|
|
for (int i = 0; i < cache.m_geomDimensions.size(); i++)
|
|
{
|
|
printf("PxVec3(%f,%f,%f),\n",
|
|
cache.m_geomDimensions[i].x, cache.m_geomDimensions[i].y, cache.m_geomDimensions[i].z);
|
|
}
|
|
|
|
printf("m_geomLocalPoses\n");
|
|
for (int i = 0; i < cache.m_geomLocalPoses.size(); i++)
|
|
{
|
|
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
|
|
cache.m_geomLocalPoses[i].p.x, cache.m_geomLocalPoses[i].p.y, cache.m_geomLocalPoses[i].p.z,
|
|
cache.m_geomLocalPoses[i].q.x, cache.m_geomLocalPoses[i].q.y, cache.m_geomLocalPoses[i].q.z, cache.m_geomLocalPoses[i].q.w);
|
|
}
|
|
|
|
printf("m_linkMaterials\n");
|
|
for (int i = 0; i < cache.m_linkMaterials.size(); i++)
|
|
{
|
|
printf("PxVec3(%f,%f,%f),\n",
|
|
cache.m_linkMaterials[i].x, cache.m_linkMaterials[i].y, cache.m_linkMaterials[i].z);
|
|
}
|
|
#endif //DEBUG_ARTICULATIONS
|
|
|
|
//see also https://github.com/NVIDIAGameWorks/PhysX/issues/43
|
|
if ((flags & CUF_USE_SELF_COLLISION) == 0)
|
|
{
|
|
physx::PxU32 nbActors = cache.m_articulation->getNbLinks();; // Max number of actors expected in the aggregate
|
|
bool selfCollisions = false;
|
|
physx::PxAggregate* aggregate = physics->createAggregate(nbActors, selfCollisions);
|
|
aggregate->addArticulation(*cache.m_articulation);
|
|
scene->addAggregate(*aggregate);
|
|
}
|
|
else
|
|
{
|
|
scene->addArticulation(*cache.m_articulation);
|
|
}
|
|
|
|
}
|
|
|
|
return cache.m_articulation;
|
|
}
|