From e32debdca4a9bf6bf6a7dd1eb149a4fdd9254c41 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 5 Apr 2017 12:22:38 -0700 Subject: [PATCH] improvements in MJCF importer: fix capsule shape inertia (for fromto definition) add cylinder support don't crash when no "quat" is provided inertia fix in btMinkowskiSumShape (based on AABB) --- data/mjcf/capsule.xml | 2 +- data/mjcf/capsule_fromtoX.xml | 13 ++++ data/mjcf/capsule_fromtoY.xml | 13 ++++ data/mjcf/capsule_fromtoZ.xml | 13 ++++ data/mjcf/cylinder.xml | 13 ++++ data/mjcf/cylinder_fromtoX.xml | 13 ++++ data/mjcf/cylinder_fromtoY.xml | 13 ++++ data/mjcf/cylinder_fromtoZ.xml | 13 ++++ .../ImportMJCFDemo/BulletMJCFImporter.cpp | 65 ++++++++++++++++--- .../ImportMJCFDemo/ImportMJCFSetup.cpp | 17 ++++- .../CollisionShapes/btMinkowskiSumShape.cpp | 21 +++++- 11 files changed, 180 insertions(+), 16 deletions(-) create mode 100644 data/mjcf/capsule_fromtoX.xml create mode 100644 data/mjcf/capsule_fromtoY.xml create mode 100644 data/mjcf/capsule_fromtoZ.xml create mode 100644 data/mjcf/cylinder.xml create mode 100644 data/mjcf/cylinder_fromtoX.xml create mode 100644 data/mjcf/cylinder_fromtoY.xml create mode 100644 data/mjcf/cylinder_fromtoZ.xml diff --git a/data/mjcf/capsule.xml b/data/mjcf/capsule.xml index f9fb5483a..7e58ce12c 100644 --- a/data/mjcf/capsule.xml +++ b/data/mjcf/capsule.xml @@ -7,7 +7,7 @@ - + \ No newline at end of file diff --git a/data/mjcf/capsule_fromtoX.xml b/data/mjcf/capsule_fromtoX.xml new file mode 100644 index 000000000..1ae780cec --- /dev/null +++ b/data/mjcf/capsule_fromtoX.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/data/mjcf/capsule_fromtoY.xml b/data/mjcf/capsule_fromtoY.xml new file mode 100644 index 000000000..60bc24564 --- /dev/null +++ b/data/mjcf/capsule_fromtoY.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/data/mjcf/capsule_fromtoZ.xml b/data/mjcf/capsule_fromtoZ.xml new file mode 100644 index 000000000..34c37fdbe --- /dev/null +++ b/data/mjcf/capsule_fromtoZ.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/data/mjcf/cylinder.xml b/data/mjcf/cylinder.xml new file mode 100644 index 000000000..a447ab03b --- /dev/null +++ b/data/mjcf/cylinder.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/data/mjcf/cylinder_fromtoX.xml b/data/mjcf/cylinder_fromtoX.xml new file mode 100644 index 000000000..1746fc0e1 --- /dev/null +++ b/data/mjcf/cylinder_fromtoX.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/data/mjcf/cylinder_fromtoY.xml b/data/mjcf/cylinder_fromtoY.xml new file mode 100644 index 000000000..8eea2fc53 --- /dev/null +++ b/data/mjcf/cylinder_fromtoY.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/data/mjcf/cylinder_fromtoZ.xml b/data/mjcf/cylinder_fromtoZ.xml new file mode 100644 index 000000000..84fe829cd --- /dev/null +++ b/data/mjcf/cylinder_fromtoZ.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index c181a379d..891c3c291 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -814,7 +814,17 @@ struct BulletMJCFImporterInternalData case URDF_GEOM_CYLINDER: { - //todo + double r = col->m_geometry.m_capsuleRadius; + btScalar h(0); + //and one cylinder of 'height' + if (col->m_geometry.m_hasFromTo) + { + h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length(); + } else + { + h = col->m_geometry.m_capsuleHeight; + } + totalVolume += SIMD_PI*r*r*h; break; } case URDF_GEOM_MESH: @@ -832,10 +842,16 @@ struct BulletMJCFImporterInternalData //one sphere double r = col->m_geometry.m_capsuleRadius; totalVolume += 4./3.*SIMD_PI*r*r*r; - //and one cylinder of 'height' - btScalar h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length(); + btScalar h(0); + if (col->m_geometry.m_hasFromTo) + { + //and one cylinder of 'height' + h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length(); + } else + { + h = col->m_geometry.m_capsuleHeight; + } totalVolume += SIMD_PI*r*r*h; - break; } default: @@ -935,6 +951,7 @@ struct BulletMJCFImporterInternalData } } const char* o = xml->Attribute("quat"); + if (o) { std::string ornStr = o; btQuaternion orn(0,0,0,1); @@ -1585,7 +1602,40 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } case URDF_GEOM_CYLINDER: { -// childShape = new btCylinderShape(col->m_geometry...); + if (col->m_geometry.m_hasFromTo) + { + btVector3 f = col->m_geometry.m_capsuleFrom; + btVector3 t = col->m_geometry.m_capsuleTo; + + //compute the local 'fromto' transform + btVector3 localPosition = btScalar(0.5)*(t+f); + btQuaternion localOrn; + localOrn = btQuaternion::getIdentity(); + + btVector3 diff = t-f; + btScalar lenSqr = diff.length2(); + btScalar height = 0.f; + + if (lenSqr > SIMD_EPSILON) + { + height = btSqrt(lenSqr); + btVector3 ax = diff / height; + + btVector3 zAxis(0,0,1); + localOrn = shortestArcQuat(zAxis,ax); + } + btCylinderShapeZ* cyl = new btCylinderShapeZ(btVector3(col->m_geometry.m_capsuleRadius,col->m_geometry.m_capsuleRadius,btScalar(0.5)*height)); + + btCompoundShape* compound = new btCompoundShape(); + btTransform localTransform(localOrn,localPosition); + compound->addChildShape(localTransform,cyl); + childShape = compound; + } else + { + btCylinderShapeZ* cap = new btCylinderShapeZ(btVector3(col->m_geometry.m_capsuleRadius, + col->m_geometry.m_capsuleRadius,btScalar(0.5)*col->m_geometry.m_capsuleHeight)); + childShape = cap; + } break; } case URDF_GEOM_MESH: @@ -1673,15 +1723,10 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } case URDF_GEOM_CAPSULE: { - //todo: convert fromto to btCapsuleShape + local btTransform if (col->m_geometry.m_hasFromTo) { btVector3 f = col->m_geometry.m_capsuleFrom; btVector3 t = col->m_geometry.m_capsuleTo; - //MuJoCo seems to take the average of the spheres as center? - //btVector3 c = (f+t)*0.5; - //f-=c; - //t-=c; btVector3 fromto[2] = {f,t}; btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius) ,btScalar(col->m_geometry.m_capsuleRadius)}; diff --git a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp index b5e8cda6c..c533d8062 100644 --- a/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp +++ b/examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp @@ -120,14 +120,25 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, if (gMCFJFileNameArray.size()==0) { gMCFJFileNameArray.push_back("MPL/MPL.xml"); + gMCFJFileNameArray.push_back("mjcf/humanoid.xml"); gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml"); gMCFJFileNameArray.push_back("mjcf/ant.xml"); gMCFJFileNameArray.push_back("mjcf/hello_mjcf.xml"); + + gMCFJFileNameArray.push_back("mjcf/cylinder.xml"); + gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoX.xml"); + gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoY.xml"); + gMCFJFileNameArray.push_back("mjcf/cylinder_fromtoZ.xml"); + gMCFJFileNameArray.push_back("mjcf/capsule.xml"); -// gMCFJFileNameArray.push_back("mjcf/hopper.xml"); -// gMCFJFileNameArray.push_back("mjcf/swimmer.xml"); -// gMCFJFileNameArray.push_back("mjcf/reacher.xml"); + gMCFJFileNameArray.push_back("mjcf/capsule_fromtoX.xml"); + gMCFJFileNameArray.push_back("mjcf/capsule_fromtoY.xml"); + gMCFJFileNameArray.push_back("mjcf/capsule_fromtoZ.xml"); + + gMCFJFileNameArray.push_back("mjcf/hopper.xml"); + gMCFJFileNameArray.push_back("mjcf/swimmer.xml"); + gMCFJFileNameArray.push_back("mjcf/reacher.xml"); } int numFileNames = gMCFJFileNameArray.size(); diff --git a/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp b/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp index 06707e24e..899ef5005 100644 --- a/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp +++ b/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp @@ -55,6 +55,23 @@ btScalar btMinkowskiSumShape::getMargin() const void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const { (void)mass; - btAssert(0); - inertia.setValue(0,0,0); + //inertia of the AABB of the Minkowski sum + btTransform identity; + identity.setIdentity(); + btVector3 aabbMin,aabbMax; + getAabb(identity,aabbMin,aabbMax); + + btVector3 halfExtents = (aabbMax-aabbMin)*btScalar(0.5); + + btScalar margin = getMargin(); + + btScalar lx=btScalar(2.)*(halfExtents.x()+margin); + btScalar ly=btScalar(2.)*(halfExtents.y()+margin); + btScalar lz=btScalar(2.)*(halfExtents.z()+margin); + const btScalar x2 = lx*lx; + const btScalar y2 = ly*ly; + const btScalar z2 = lz*lz; + const btScalar scaledmass = mass * btScalar(0.08333333); + + inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2)); }