AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-19 05:20:06 +00:00
Code Issues 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
Show Stats 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"/>
<body pos="0 0 1">
<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>
</worldbody>
</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:
{
//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)};

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

@ -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();

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