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

improvements in MJCF importer: fix capsule shape inertia (for fromto definition)

Browse Source 
add cylinder support
don't crash when no "quat" is provided
inertia fix in btMinkowskiSumShape (based on AABB)
This commit is contained in:
Erwin Coumans
2017-04-05 12:22:38 -07:00
parent b095e1d917
commit e32debdca4
11 changed files with 180 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/mjcf/capsule.xml
View File

@@ -7,7 +7,7 @@
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/> <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1"> <body pos="0 0 1">
<joint type="free"/> <joint type="free"/>
<geom fromto="0.0 0.0 0.0 0. 0.2 0.0" name="aux_1_geom" size="0.05" type="capsule"/> <geom name="aux_1_geom" size="0.05 0.1" type="capsule"/>
</body> </body>
</worldbody> </worldbody>
</mujoco> </mujoco>

13
data/mjcf/capsule_fromtoX.xml Normal file
View File

@@ -0,0 +1,13 @@
<!--
MuJoCo MJCF test file. See http://mujoco.org/book/index.html
-->
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<joint type="free"/>
<geom fromto="-0.1 0.0 0.0 0.1 0.0 0.0" name="aux_1_geom" size="0.05" type="capsule"/>
</body>
</worldbody>
</mujoco>

13
data/mjcf/capsule_fromtoY.xml Normal file
View File

@@ -0,0 +1,13 @@
<!--
MuJoCo MJCF test file. See http://mujoco.org/book/index.html
-->
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<joint type="free"/>
<geom fromto="0.0 -0.1 0.0 0.0 0.1 0.0" name="aux_1_geom" size="0.05" type="capsule"/>
</body>
</worldbody>
</mujoco>

13
data/mjcf/capsule_fromtoZ.xml Normal file
View File

@@ -0,0 +1,13 @@
<!--
MuJoCo MJCF test file. See http://mujoco.org/book/index.html
-->
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<joint type="free"/>
<geom fromto="0.0 0.0 -0.1 0.0 0.0 0.1" name="aux_1_geom" size="0.05" type="capsule"/>
</body>
</worldbody>
</mujoco>

13
data/mjcf/cylinder.xml Normal file
View File

@@ -0,0 +1,13 @@
<!--
MuJoCo MJCF test file. See http://mujoco.org/book/index.html
-->
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<joint type="free"/>
<geom name="aux_1_geom" size="0.05 0.1" type="cylinder"/>
</body>
</worldbody>
</mujoco>

13
data/mjcf/cylinder_fromtoX.xml Normal file
View File

@@ -0,0 +1,13 @@
<!--
MuJoCo MJCF test file. See http://mujoco.org/book/index.html
-->
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<joint type="free"/>
<geom fromto="-0.1 0.0 0.0 0.1 0.0 0.0" name="aux_1_geom" size="0.05" type="cylinder"/>
</body>
</worldbody>
</mujoco>

13
data/mjcf/cylinder_fromtoY.xml Normal file
View File

@@ -0,0 +1,13 @@
<!--
MuJoCo MJCF test file. See http://mujoco.org/book/index.html
-->
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<joint type="free"/>
<geom fromto="0.0 -0.1 0.0 0.0 0.1 0.0" name="aux_1_geom" size="0.05" type="cylinder"/>
</body>
</worldbody>
</mujoco>

13
data/mjcf/cylinder_fromtoZ.xml Normal file
View File

@@ -0,0 +1,13 @@
<!--
MuJoCo MJCF test file. See http://mujoco.org/book/index.html
-->
<mujoco>
<worldbody>
<light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
<geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
<body pos="0 0 1">
<joint type="free"/>
<geom fromto="0.0 0.0 -0.1 0.0 0.0 0.1" name="aux_1_geom" size="0.05" type="cylinder"/>
</body>
</worldbody>
</mujoco>

65
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -814,7 +814,17 @@ struct BulletMJCFImporterInternalData
case URDF_GEOM_CYLINDER: 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; break;
} }
case URDF_GEOM_MESH: case URDF_GEOM_MESH:
@@ -832,10 +842,16 @@ struct BulletMJCFImporterInternalData
//one sphere //one sphere
double r = col->m_geometry.m_capsuleRadius; double r = col->m_geometry.m_capsuleRadius;
totalVolume += 4./3.*SIMD_PI*r*r*r; totalVolume += 4./3.*SIMD_PI*r*r*r;
//and one cylinder of 'height' btScalar h(0);
btScalar h = (col->m_geometry.m_capsuleFrom-col->m_geometry.m_capsuleTo).length(); 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; totalVolume += SIMD_PI*r*r*h;
break; break;
} }
default: default:
@@ -935,6 +951,7 @@ struct BulletMJCFImporterInternalData
} }
} }
const char* o = xml->Attribute("quat"); const char* o = xml->Attribute("quat");
if (o)
{ {
std::string ornStr = o; std::string ornStr = o;
btQuaternion orn(0,0,0,1); btQuaternion orn(0,0,0,1);
@@ -1585,7 +1602,40 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} }
case URDF_GEOM_CYLINDER: 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; break;
} }
case URDF_GEOM_MESH: case URDF_GEOM_MESH:
@@ -1673,15 +1723,10 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} }
case URDF_GEOM_CAPSULE: case URDF_GEOM_CAPSULE:
{ {
//todo: convert fromto to btCapsuleShape + local btTransform
if (col->m_geometry.m_hasFromTo) if (col->m_geometry.m_hasFromTo)
{ {
btVector3 f = col->m_geometry.m_capsuleFrom; btVector3 f = col->m_geometry.m_capsuleFrom;
btVector3 t = col->m_geometry.m_capsuleTo; 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}; btVector3 fromto[2] = {f,t};
btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius) btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius)
,btScalar(col->m_geometry.m_capsuleRadius)}; ,btScalar(col->m_geometry.m_capsuleRadius)};

17
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@@ -120,14 +120,25 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
if (gMCFJFileNameArray.size()==0) if (gMCFJFileNameArray.size()==0)
{ {
gMCFJFileNameArray.push_back("MPL/MPL.xml"); gMCFJFileNameArray.push_back("MPL/MPL.xml");
gMCFJFileNameArray.push_back("mjcf/humanoid.xml"); gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml"); gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
gMCFJFileNameArray.push_back("mjcf/ant.xml"); gMCFJFileNameArray.push_back("mjcf/ant.xml");
gMCFJFileNameArray.push_back("mjcf/hello_mjcf.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/capsule.xml");
// gMCFJFileNameArray.push_back("mjcf/hopper.xml"); gMCFJFileNameArray.push_back("mjcf/capsule_fromtoX.xml");
// gMCFJFileNameArray.push_back("mjcf/swimmer.xml"); gMCFJFileNameArray.push_back("mjcf/capsule_fromtoY.xml");
// gMCFJFileNameArray.push_back("mjcf/reacher.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(); int numFileNames = gMCFJFileNameArray.size();

21
src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp
View File

@@ -55,6 +55,23 @@ btScalar btMinkowskiSumShape::getMargin() const
void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const void btMinkowskiSumShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{ {
(void)mass; (void)mass;
btAssert(0); //inertia of the AABB of the Minkowski sum
inertia.setValue(0,0,0); 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));
} }