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));
}