AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 13:20:07 +00:00
Code Issues Releases Wiki Activity

Further improvements in Bullet MuJoCo MJCF import:

Browse Source 
[MJCF import] improved humanoid.xml, inverted_pendulum.xml import (automatically add dummy-links to connect multiple joints between bodies)
[MJCF import] fix bug in quaternion conversion (w,x,y,z) -> (x,y,z,w)
This commit is contained in:
Erwin Coumans 2017-01-15 11:22:24 -08:00
parent 42ef8f538e
commit 310fabcd7e
9 changed files with 451 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
data/mjcf/inverted_double_pendulum.xml Normal file
View File

@ -0,0 +1,47 @@
<!-- Cartpole Model
The state space is populated with joints in the order that they are
defined in this file. The actuators also operate on joints.
State-Space (name/joint/parameter):
- cart slider position (m)
- pole hinge angle (rad)
- cart slider velocity (m/s)
- pole hinge angular velocity (rad/s)
Actuators (name/actuator/parameter):
- cart motor force x (N)
-->
<mujoco model="cartpole">
<compiler coordinate="local" inertiafromgeom="true"/>
<custom>
<numeric data="2" name="frame_skip"/>
</custom>
<default>
<joint damping="0.05"/>
<geom contype="0" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
</default>
<option gravity="1e-5 0 -9.81" integrator="RK4" timestep="0.01"/>
<size nstack="3000"/>
<worldbody>
<geom name="floor" pos="0 0 -3.0" rgba="0.8 0.9 0.8 1" size="40 40 40" type="plane"/>
<geom name="rail" pos="0 0 0" quat="0.707 0 0.707 0" rgba="0.3 0.3 0.7 1" size="0.02 1" type="capsule"/>
<body name="cart" pos="0 0 0">
<joint axis="1 0 0" limited="true" margin="0.01" name="slider" pos="0 0 0" range="-1 1" type="slide"/>
<geom name="cart" pos="0 0 0" quat="0.707 0 0.707 0" size="0.1 0.1" type="capsule"/>
<body name="pole" pos="0 0 0">
<joint axis="0 1 0" name="hinge" pos="0 0 0" type="hinge"/>
<geom fromto="0 0 0 0 0 0.6" name="cpole" rgba="0 0.7 0.7 1" size="0.045 0.3" type="capsule"/>
<body name="pole2" pos="0 0 0.6">
<joint axis="0 1 0" name="hinge2" pos="0 0 0" type="hinge"/>
<geom fromto="0 0 0 0 0 0.6" name="cpole2" rgba="0 0.7 0.7 1" size="0.045 0.3" type="capsule"/>
<site name="tip" pos="0 0 .6" size="0.01 0.01"/>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor ctrllimited="true" ctrlrange="-1 1" gear="500" joint="slider" name="slide"/>
</actuator>
</mujoco>

27
data/mjcf/inverted_pendulum.xml Normal file
View File

@ -0,0 +1,27 @@
<mujoco model="inverted pendulum">
<compiler inertiafromgeom="true"/>
<default>
<joint armature="0" damping="1" limited="true"/>
<geom contype="0" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
<tendon/>
<motor ctrlrange="-3 3"/>
</default>
<option gravity="0 0 -9.81" integrator="RK4" timestep="0.02"/>
<size nstack="3000"/>
<worldbody>
<!--geom name="ground" type="plane" pos="0 0 0" /-->
<!-- <geom name="rail" pos="0 0 0" quat="0.707 0 0.707 0" rgba="0.3 0.3 0.7 1" size="0.02 1" type="capsule"/> /-->
<body name="cart1" pos="0 0 0">
<joint axis="1 0 0" limited="true" name="slider" pos="0 0 0" range="-1 1" type="slide"/>
<geom name="cart" pos="0 0 0" quat="0.707 0 0.707 0" size="0.1 0.1" type="capsule"/>
<body name="pole" pos="0 0 0">
<joint axis="0 1 0" name="hinge" pos="0 0 0" range="-90 90" type="hinge"/>
<geom fromto="0 0 0 0.001 0 0.6" name="cpole" rgba="0 0.7 0.7 1" size="0.049 0.3" type="capsule"/>
<!-- <body name="pole2" pos="0.001 0 0.6"><joint name="hinge2" type="hinge" pos="0 0 0" axis="0 1 0"/><geom name="cpole2" type="capsule" fromto="0 0 0 0 0 0.6" size="0.05 0.3" rgba="0.7 0 0.7 1"/><site name="tip2" pos="0 0 .6"/></body>-->
</body>
</body>
</worldbody>
<actuator>
<motor gear="100" joint="slider" name="slide"/>
</actuator>
</mujoco>

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

@ -21,10 +21,12 @@
enum ePARENT_LINK_ENUMS enum ePARENT_LINK_ENUMS
{ {
BASE_LINK_INDEX=-1,
INVALID_LINK_INDEX=-2 INVALID_LINK_INDEX=-2
}; };
static int gUid=0;
static bool parseVector4(btVector4& vec4, const std::string& vector_str) static bool parseVector4(btVector4& vec4, const std::string& vector_str)
{ {
@ -120,12 +122,17 @@ struct BulletMJCFImporterInternalData
btAlignedObjectArray<UrdfModel*> m_models; btAlignedObjectArray<UrdfModel*> m_models;
int m_activeModel; int m_activeModel;
//todo: for full MJCF compatibility, we would need a stack of default values
int m_defaultCollisionGroup;
int m_defaultCollisionMask;
//those collision shapes are deleted by caller (todo: make sure this happens!) //those collision shapes are deleted by caller (todo: make sure this happens!)
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes; btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
BulletMJCFImporterInternalData() BulletMJCFImporterInternalData()
:m_activeModel(-1) :m_activeModel(-1),
m_defaultCollisionGroup(1),
m_defaultCollisionMask(1)
{ {
m_pathPrefix[0] = 0; m_pathPrefix[0] = 0;
} }
@ -144,18 +151,49 @@ struct BulletMJCFImporterInternalData
return 0; return 0;
} }
bool parseRootLevel(TiXmlElement* root_xml,MJCFErrorLogger* logger) bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger)
{ {
for (TiXmlElement* xml = root_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement()) bool handled= false;
//rudimentary 'default' support, would need more work for better feature coverage
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{ {
bool handled = false; bool handled = false;
std::string n = xml->Value(); std::string n = child_xml->Value();
if (n=="inertial")
{
}
if (n=="geom")
{
//contype, conaffinity
const char* conTypeStr = child_xml->Attribute("contype");
if (conTypeStr)
{
m_defaultCollisionGroup = urdfLexicalCast<int>(conTypeStr);
}
const char* conAffinityStr = child_xml->Attribute("conaffinity");
if (conAffinityStr)
{
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
}
}
}
handled=true;
return handled;
}
bool parseRootLevel(TiXmlElement* root_xml,MJCFErrorLogger* logger)
{
for (TiXmlElement* rootxml = root_xml->FirstChildElement() ; rootxml ; rootxml = rootxml->NextSiblingElement())
{
bool handled = false;
std::string n = rootxml->Value();
if (n=="body") if (n=="body")
{ {
int modelIndex = m_models.size(); int modelIndex = m_models.size();
UrdfModel* model = new UrdfModel(); UrdfModel* model = new UrdfModel();
m_models.push_back(model); m_models.push_back(model);
parseBody(xml,modelIndex, INVALID_LINK_INDEX,logger); parseBody(rootxml,modelIndex, INVALID_LINK_INDEX,logger);
initTreeAndRoot(*model,logger); initTreeAndRoot(*model,logger);
handled = true; handled = true;
} }
@ -168,7 +206,7 @@ struct BulletMJCFImporterInternalData
UrdfLink* linkPtr = new UrdfLink(); UrdfLink* linkPtr = new UrdfLink();
linkPtr->m_name = "anonymous"; linkPtr->m_name = "anonymous";
const char* namePtr = xml->Attribute("name"); const char* namePtr = rootxml->Attribute("name");
if (namePtr) if (namePtr)
{ {
linkPtr->m_name = namePtr; linkPtr->m_name = namePtr;
@ -177,12 +215,12 @@ struct BulletMJCFImporterInternalData
linkPtr->m_linkIndex = linkIndex; linkPtr->m_linkIndex = linkIndex;
modelPtr->m_links.insert(linkPtr->m_name.c_str(),linkPtr); modelPtr->m_links.insert(linkPtr->m_name.c_str(),linkPtr);
btTransform linkTransform = parseTransform(xml,logger); //don't parse geom transform here, it will be inside 'parseGeom'
linkPtr->m_linkTransformInWorld = linkTransform; linkPtr->m_linkTransformInWorld.setIdentity();
// modelPtr->m_rootLinks.push_back(linkPtr); // modelPtr->m_rootLinks.push_back(linkPtr);
parseGeom(xml,modelIndex, linkIndex,logger); parseGeom(rootxml,modelIndex, linkIndex,logger);
initTreeAndRoot(*modelPtr,logger); initTreeAndRoot(*modelPtr,logger);
handled = true; handled = true;
@ -202,7 +240,7 @@ struct BulletMJCFImporterInternalData
return true; return true;
} }
bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, btTransform& jointTransOut) bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
{ {
const char* jType = link_xml->Attribute("type"); const char* jType = link_xml->Attribute("type");
const char* limitedStr = link_xml->Attribute("limited"); const char* limitedStr = link_xml->Attribute("limited");
@ -210,6 +248,8 @@ struct BulletMJCFImporterInternalData
const char* posStr = link_xml->Attribute("pos"); const char* posStr = link_xml->Attribute("pos");
const char* ornStr = link_xml->Attribute("quat"); const char* ornStr = link_xml->Attribute("quat");
const char* nameStr = link_xml->Attribute("name"); const char* nameStr = link_xml->Attribute("name");
const char* rangeStr = link_xml->Attribute("range");
btTransform jointTrans; btTransform jointTrans;
jointTrans.setIdentity(); jointTrans.setIdentity();
if (posStr) if (posStr)
@ -242,12 +282,36 @@ struct BulletMJCFImporterInternalData
logger->reportWarning("joint without axis attribute"); logger->reportWarning("joint without axis attribute");
} }
bool isLimited = false; bool isLimited = false;
double range[2] = {1,0};
if (limitedStr) if (limitedStr)
{ {
std::string lim = limitedStr; std::string lim = limitedStr;
if (lim=="true") if (lim=="true")
{ {
isLimited = true; isLimited = true;
//parse the 'range' field
btArray<std::string> pieces;
btArray<float> sizes;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, rangeStr, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (sizes.size()==2)
{
range[0] = sizes[0];
range[1] = sizes[1];
} else
{
logger->reportWarning("Expected range[2] in joint with limits");
}
} }
} else } else
{ {
@ -259,7 +323,8 @@ struct BulletMJCFImporterInternalData
btTransform parentLinkToJointTransform; btTransform parentLinkToJointTransform;
parentLinkToJointTransform.setIdentity(); parentLinkToJointTransform.setIdentity();
parentLinkToJointTransform = jointTrans*linkPtr->m_linkTransformInWorld; parentLinkToJointTransform = parentToLinkTrans*jointTrans;
jointTransOut = jointTrans; jointTransOut = jointTrans;
UrdfJointTypes ejtype; UrdfJointTypes ejtype;
if (jType) if (jType)
@ -270,6 +335,11 @@ struct BulletMJCFImporterInternalData
ejtype = URDFFixedJoint; ejtype = URDFFixedJoint;
jointHandled = true; jointHandled = true;
} }
if (jointType == "slide")
{
ejtype = URDFPrismaticJoint;
jointHandled = true;
}
if (jointType == "hinge") if (jointType == "hinge")
{ {
if (isLimited) if (isLimited)
@ -288,7 +358,6 @@ struct BulletMJCFImporterInternalData
if (jointHandled) if (jointHandled)
{ {
//default to 'fixed' joint
UrdfJoint* jointPtr = new UrdfJoint(); UrdfJoint* jointPtr = new UrdfJoint();
jointPtr->m_childLinkName=linkPtr->m_name; jointPtr->m_childLinkName=linkPtr->m_name;
const UrdfLink* parentLink = getLink(modelIndex,parentLinkIndex); const UrdfLink* parentLink = getLink(modelIndex,parentLinkIndex);
@ -297,13 +366,18 @@ struct BulletMJCFImporterInternalData
jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform; jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform;
jointPtr->m_type = ejtype; jointPtr->m_type = ejtype;
int numJoints = m_models[modelIndex]->m_joints.size(); int numJoints = m_models[modelIndex]->m_joints.size();
//range
jointPtr->m_lowerLimit = range[0];
jointPtr->m_upperLimit = range[1];
if (nameStr) if (nameStr)
{ {
jointPtr->m_name =nameStr; jointPtr->m_name =nameStr;
} else } else
{ {
char jointName[1024]; char jointName[1024];
sprintf(jointName,"joint%d_%d",linkIndex,numJoints); sprintf(jointName,"joint%d_%d_%d",gUid++,linkIndex,numJoints);
jointPtr->m_name =jointName; jointPtr->m_name =jointName;
} }
m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr); m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr);
@ -335,6 +409,9 @@ struct BulletMJCFImporterInternalData
bool handledGeomType = false; bool handledGeomType = false;
UrdfGeometry geom; UrdfGeometry geom;
const char* rgba = link_xml->Attribute("rgba"); const char* rgba = link_xml->Attribute("rgba");
const char* gType = link_xml->Attribute("type"); const char* gType = link_xml->Attribute("type");
const char* sz = link_xml->Attribute("size"); const char* sz = link_xml->Attribute("size");
@ -356,7 +433,7 @@ struct BulletMJCFImporterInternalData
btVector4 o4; btVector4 o4;
if (parseVector4(o4,ornStr)) if (parseVector4(o4,ornStr))
{ {
orn.setValue(o4[3],o4[0],o4[1],o4[2]); orn.setValue(o4[1],o4[2],o4[3],o4[0]);
linkLocalFrame.setRotation(orn); linkLocalFrame.setRotation(orn);
} }
} }
@ -410,16 +487,52 @@ struct BulletMJCFImporterInternalData
if (geomType == "capsule") if (geomType == "capsule")
{ {
geom.m_type = URDF_GEOM_CAPSULE; geom.m_type = URDF_GEOM_CAPSULE;
geom.m_capsuleRadius = urdfLexicalCast<double>(sz);
btArray<std::string> pieces;
btArray<float> sizes;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, sz, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
geom.m_capsuleRadius = 0;
geom.m_capsuleHalfHeight = 0.f;
if (sizes.size()>0)
{
geom.m_capsuleRadius = sizes[0];
if (sizes.size()>1)
{
geom.m_capsuleHalfHeight = sizes[1];
}
} else
{
logger->reportWarning("couldn't convert 'size' attribute of capsule geom");
}
const char* fromtoStr = link_xml->Attribute("fromto"); const char* fromtoStr = link_xml->Attribute("fromto");
geom.m_hasFromTo = false;
if (fromtoStr) if (fromtoStr)
{ {
geom.m_hasFromTo = true;
std::string fromto = fromtoStr; std::string fromto = fromtoStr;
parseVector6(geom.m_capsuleFrom,geom.m_capsuleTo,fromto,logger); parseVector6(geom.m_capsuleFrom,geom.m_capsuleTo,fromto,logger);
handledGeomType = true; handledGeomType = true;
} else } else
{ {
logger->reportWarning("capsule without fromto attribute"); if (sizes.size()<2)
{
logger->reportWarning("capsule without fromto attribute requires 2 sizes (radius and halfheight)");
} else
{
handledGeomType = true;
}
} }
} }
#if 0 #if 0
@ -433,6 +546,26 @@ struct BulletMJCFImporterInternalData
{ {
UrdfCollision col; UrdfCollision col;
col.m_flags |= URDF_HAS_COLLISION_GROUP;
col.m_collisionGroup = m_defaultCollisionGroup;
col.m_flags |= URDF_HAS_COLLISION_MASK;
col.m_collisionMask = m_defaultCollisionMask;
//contype, conaffinity
const char* conTypeStr = link_xml->Attribute("contype");
if (conTypeStr)
{
col.m_flags |= URDF_HAS_COLLISION_GROUP;
col.m_collisionGroup = urdfLexicalCast<int>(conTypeStr);
}
const char* conAffinityStr = link_xml->Attribute("conaffinity");
if (conAffinityStr)
{
col.m_flags |= URDF_HAS_COLLISION_MASK;
col.m_collisionMask = urdfLexicalCast<int>(conAffinityStr);
}
col.m_geometry = geom; col.m_geometry = geom;
col.m_linkLocalFrame = linkLocalFrame; col.m_linkLocalFrame = linkLocalFrame;
linkPtr->m_collisionArray.push_back(col); linkPtr->m_collisionArray.push_back(col);
@ -478,7 +611,7 @@ struct BulletMJCFImporterInternalData
btQuaternion orn(0,0,0,1); btQuaternion orn(0,0,0,1);
if (parseVector4(o4,ornstr)) if (parseVector4(o4,ornstr))
{ {
orn.setValue(o4[3],o4[0],o4[1],o4[2]);//MuJoCo quats are [w,x,y,z], Bullet uses [x,y,z,w] orn.setValue(o4[1],o4[2],o4[3],o4[0]);//MuJoCo quats are [w,x,y,z], Bullet uses [x,y,z,w]
tr.setRotation(orn); tr.setRotation(orn);
} }
} else } else
@ -545,33 +678,61 @@ struct BulletMJCFImporterInternalData
return totalVolume; return totalVolume;
} }
UrdfLink* getLink(int modelIndex, int linkIndex)
{
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr && *linkPtrPtr)
{
return *linkPtrPtr;
}
btAssert(0);
return 0;
}
bool parseBody(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, MJCFErrorLogger* logger) int createBody(int modelIndex, const char* namePtr)
{ {
UrdfModel* modelPtr = m_models[modelIndex]; UrdfModel* modelPtr = m_models[modelIndex];
int linkIndex = modelPtr->m_links.size(); int orgChildLinkIndex = modelPtr->m_links.size();
UrdfLink* linkPtr = new UrdfLink(); UrdfLink* linkPtr = new UrdfLink();
char uniqueLinkName[1024]; char uniqueLinkName[1024];
sprintf(uniqueLinkName,"link%d",linkIndex); sprintf(uniqueLinkName,"link%d",orgChildLinkIndex );
linkPtr->m_name = uniqueLinkName; linkPtr->m_name = uniqueLinkName;
const char* namePtr = link_xml->Attribute("name");
if (namePtr) if (namePtr)
{ {
linkPtr->m_name = namePtr; linkPtr->m_name = namePtr;
} }
linkPtr->m_linkIndex = orgChildLinkIndex ;
linkPtr->m_linkIndex = linkIndex;
modelPtr->m_links.insert(linkPtr->m_name.c_str(),linkPtr); modelPtr->m_links.insert(linkPtr->m_name.c_str(),linkPtr);
return orgChildLinkIndex;
}
bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
{
int newParentLinkIndex = orgParentLinkIndex;
const char* bodyName = link_xml->Attribute("name");
int orgChildLinkIndex = createBody(modelIndex,bodyName);
int curChildLinkIndex = orgChildLinkIndex;
std::string bodyN;
if (bodyName)
{
bodyN = bodyName;
} else
{
char anon[1024];
sprintf(anon,"anon%d",gUid++);
bodyN = anon;
}
btTransform orgLinkTransform = parseTransform(link_xml,logger);
btTransform linkTransform = parseTransform(link_xml,logger); btTransform linkTransform = parseTransform(link_xml,logger);
UrdfLink* linkPtr = getLink(modelIndex,orgChildLinkIndex);
linkPtr->m_linkTransformInWorld = linkTransform;
//body/geom links with no parent are root links
if (parentLinkIndex==INVALID_LINK_INDEX)
{
// modelPtr->m_rootLinks.push_back(linkPtr);
}
bool massDefined = false; bool massDefined = false;
btVector3 inertialPos(0,0,0); btVector3 inertialPos(0,0,0);
@ -582,6 +743,7 @@ struct BulletMJCFImporterInternalData
bool hasJoint = false; bool hasJoint = false;
btTransform jointTrans; btTransform jointTrans;
jointTrans.setIdentity(); jointTrans.setIdentity();
bool skipFixedJoint = false;
for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement()) for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
{ {
@ -616,31 +778,56 @@ struct BulletMJCFImporterInternalData
if (n=="joint") if (n=="joint")
{ {
//skip joints at the root for now
//MuJoCo supports more than just 'free' or 'fixed',
//so we will need to emulate this with extra root links+joints
//for now, only convert 1st joint
if (!hasJoint) if (!hasJoint)
{ {
if (parentLinkIndex!=INVALID_LINK_INDEX) const char* jType = xml->Attribute("type");
std::string jointType = jType? jType:"";
if (newParentLinkIndex!=INVALID_LINK_INDEX || jointType!="free")
{ {
parseJoint(xml,modelIndex,parentLinkIndex, linkIndex,logger,jointTrans); if (newParentLinkIndex==INVALID_LINK_INDEX)
} {
int newRootLinkIndex = createBody(modelIndex,0);
UrdfLink* rootLink = getLink(modelIndex,newRootLinkIndex);
rootLink->m_inertia.m_mass = 0;
rootLink->m_linkTransformInWorld.setIdentity();
newParentLinkIndex = newRootLinkIndex;
} }
int newLinkIndex = createBody(modelIndex,0);
parseJoint(xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,linkTransform,jointTrans);
//getLink(modelIndex,newLinkIndex)->m_linkTransformInWorld = jointTrans*linkTransform;
linkTransform = jointTrans.inverse();
newParentLinkIndex = newLinkIndex;
//newParentLinkIndex, curChildLinkIndex
hasJoint = true; hasJoint = true;
handled = true; handled = true;
} }
} else
{
int newLinkIndex = createBody(modelIndex,0);
btTransform joint2nextjoint = jointTrans.inverse();
btTransform unused;
parseJoint(xml,modelIndex,newParentLinkIndex, newLinkIndex,logger,joint2nextjoint,unused);
newParentLinkIndex = newLinkIndex;
//todo: compute relative joint transforms (if any) and append to linkTransform
hasJoint = true;
handled = true;
}
}
if (n == "geom") if (n == "geom")
{ {
parseGeom(xml,modelIndex, linkIndex, logger); parseGeom(xml,modelIndex, orgChildLinkIndex , logger);
handled = true; handled = true;
} }
//recursive //recursive
if (n=="body") if (n=="body")
{ {
parseBody(xml,modelIndex,linkIndex,logger); parseBody(xml,modelIndex,orgChildLinkIndex,logger);
handled = true; handled = true;
} }
@ -657,18 +844,24 @@ struct BulletMJCFImporterInternalData
} }
} }
if ((!hasJoint) && (parentLinkIndex != INVALID_LINK_INDEX)) linkPtr->m_linkTransformInWorld = linkTransform;
if (bodyN == "cart1")//front_left_leg")
{ {
printf("found!\n");
}
if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
{
//linkPtr->m_linkTransformInWorld.setIdentity();
//default to 'fixed' joint //default to 'fixed' joint
UrdfJoint* jointPtr = new UrdfJoint(); UrdfJoint* jointPtr = new UrdfJoint();
jointPtr->m_childLinkName=linkPtr->m_name; jointPtr->m_childLinkName=linkPtr->m_name;
const UrdfLink* parentLink = getLink(modelIndex,parentLinkIndex); const UrdfLink* parentLink = getLink(modelIndex,newParentLinkIndex);
jointPtr->m_parentLinkName =parentLink->m_name; jointPtr->m_parentLinkName =parentLink->m_name;
jointPtr->m_localJointAxis.setValue(1,0,0); jointPtr->m_localJointAxis.setValue(1,0,0);
jointPtr->m_parentLinkToJointTransform = linkTransform; jointPtr->m_parentLinkToJointTransform = linkTransform;
jointPtr->m_type = URDFFixedJoint; jointPtr->m_type = URDFFixedJoint;
char jointName[1024]; char jointName[1024];
sprintf(jointName,"joint%d",linkIndex); sprintf(jointName,"jointfix_%d_%d",gUid++,newParentLinkIndex);
jointPtr->m_name =jointName; jointPtr->m_name =jointName;
m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr); m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr);
} }
@ -680,11 +873,26 @@ struct BulletMJCFImporterInternalData
double volume = computeVolume(linkPtr,logger); double volume = computeVolume(linkPtr,logger);
mass = density * volume; mass = density * volume;
} }
linkPtr->m_inertia.m_linkLocalFrame = jointTrans.inverse(); linkPtr->m_inertia.m_linkLocalFrame.setIdentity();// = jointTrans.inverse();
linkPtr->m_inertia.m_mass = mass; linkPtr->m_inertia.m_mass = mass;
return true; return true;
} }
void recurseAddChildLinks(UrdfModel* model, UrdfLink* link)
{
for (int i=0;i<link->m_childLinks.size();i++)
{
int linkIndex = model->m_links.size();
link->m_childLinks[i]->m_linkIndex = linkIndex;
const char* linkName = link->m_childLinks[i]->m_name.c_str();
model->m_links.insert(linkName,link->m_childLinks[i]);
}
for (int i=0;i<link->m_childLinks.size();i++)
{
recurseAddChildLinks(model,link->m_childLinks[i]);
}
}
bool initTreeAndRoot(UrdfModel& model, MJCFErrorLogger* logger) bool initTreeAndRoot(UrdfModel& model, MJCFErrorLogger* logger)
{ {
// every link has children links and joints, but no parents, so we create a // every link has children links and joints, but no parents, so we create a
@ -763,6 +971,23 @@ struct BulletMJCFImporterInternalData
logger->reportError("URDF without root link found"); logger->reportError("URDF without root link found");
return false; return false;
} }
//re-index the link indices so parent indices are always smaller than child indices
btAlignedObjectArray<UrdfLink*> links;
links.resize(model.m_links.size());
for (int i=0;i<model.m_links.size();i++)
{
links[i] = *model.m_links.getAtIndex(i);
}
model.m_links.clear();
for (int i=0;i<model.m_rootLinks.size();i++)
{
UrdfLink* rootLink = model.m_rootLinks[i];
int linkIndex = model.m_links.size();
rootLink->m_linkIndex = linkIndex;
model.m_links.insert(rootLink->m_name.c_str(),rootLink);
recurseAddChildLinks(&model, rootLink);
}
return true; return true;
} }
@ -850,6 +1075,13 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
m_data->m_fileModelName = modelName; m_data->m_fileModelName = modelName;
} }
//<compiler>,<option>,<size>,<default>,<body>,<keyframe>,<contactpair>,
//<light>, <camera>,<constraint>,<tendon>,<actuator>,<customfield>,<textfield>
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("default"); link_xml; link_xml = link_xml->NextSiblingElement("default"))
{
m_data->parseDefaults(link_xml,logger);
}
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body")) for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("body"); link_xml; link_xml = link_xml->NextSiblingElement("body"))
{ {
@ -861,8 +1093,7 @@ bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* l
m_data->parseRootLevel(link_xml,logger); m_data->parseRootLevel(link_xml,logger);
} }
//<compiler>,<option>,<size>,<default>,<body>,<keyframe>,<contactpair>,
//<light>, <camera>,<constraint>,<tendon>,<actuator>,<customfield>,<textfield>
return true; return true;
} }
@ -901,10 +1132,43 @@ bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
return false; return false;
} }
//todo: placeholder implementation
//MuJoCo type/affinity is different from Bullet group/mask, so we should implement a custom collision filter instead
//(contype1 & conaffinity2) || (contype2 & conaffinity1)
int BulletMJCFImporter::getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const
{
int flags = 0;
/*
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex);
if (link)
{
for (int i=0;i<link->m_collisionArray.size();i++)
{
const UrdfCollision& col = link->m_collisionArray[i];
colGroup = col.m_collisionGroup;
flags |= URDF_HAS_COLLISION_GROUP;
colMask = col.m_collisionMask;
flags |= URDF_HAS_COLLISION_MASK;
}
}
*/
return flags;
}
std::string BulletMJCFImporter::getJointName(int linkIndex) const std::string BulletMJCFImporter::getJointName(int linkIndex) const
{ {
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex); const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex);
if (link)
{
if (link->m_parentJoint)
{
return link->m_parentJoint->m_name;
}
return link->m_name; return link->m_name;
}
return "";
} }
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity. //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
@ -935,7 +1199,8 @@ void BulletMJCFImporter::getLinkChildIndices(int urdfLinkIndex, btAlignedObjectA
{ {
for (int i=0;i<link->m_childLinks.size();i++) for (int i=0;i<link->m_childLinks.size();i++)
{ {
childLinkIndices.push_back(link->m_childLinks[i]->m_linkIndex); int childIndex = link->m_childLinks[i]->m_linkIndex;
childLinkIndices.push_back(childIndex);
} }
} }
} }
@ -978,12 +1243,13 @@ bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
bool BulletMJCFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const bool BulletMJCFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
{ {
rootTransformInWorld.setIdentity(); rootTransformInWorld.setIdentity();
/*
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,0); const UrdfLink* link = m_data->getLink(m_data->m_activeModel,0);
if (link) if (link)
{ {
rootTransformInWorld = link->m_linkTransformInWorld; rootTransformInWorld = link->m_linkTransformInWorld;
} }
*/
return true; return true;
} }
@ -1060,7 +1326,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
case URDF_GEOM_CAPSULE: case URDF_GEOM_CAPSULE:
{ {
//todo: convert fromto to btCapsuleShape + local btTransform //todo: convert fromto to btCapsuleShape + local btTransform
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? //MuJoCo seems to take the average of the spheres as center?
@ -1072,6 +1339,12 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2); btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2);
childShape = ms; childShape = ms;
} else
{
btCapsuleShapeZ* cap = new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius,
2.*col->m_geometry.m_capsuleHalfHeight);
childShape = cap;
}
break; break;
} }
default: default:

3
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
View File

@ -46,6 +46,9 @@ public:
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
//optional method to get collision group (type) and mask (affinity)
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const ;
///this API will likely change, don't override it! ///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const; virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;

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

@ -77,7 +77,7 @@ struct ImportMJCFInternalData
ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName) ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper), :CommonMultiBodyBase(helper),
m_grav(0), m_grav(-10),
m_upAxis(2) m_upAxis(2)
{ {
m_data = new ImportMJCFInternalData; m_data = new ImportMJCFInternalData;
@ -118,9 +118,10 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
if (gMCFJFileNameArray.size()==0) if (gMCFJFileNameArray.size()==0)
{ {
gMCFJFileNameArray.push_back("mjcf/humanoid.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_mjcf2.xml"); gMCFJFileNameArray.push_back("mjcf/hello_mjcf2.xml");
gMCFJFileNameArray.push_back("mjcf/capsule.xml"); gMCFJFileNameArray.push_back("mjcf/capsule.xml");
gMCFJFileNameArray.push_back("mjcf/ant.xml");
// gMCFJFileNameArray.push_back("mjcf/hopper.xml"); // gMCFJFileNameArray.push_back("mjcf/hopper.xml");
// gMCFJFileNameArray.push_back("mjcf/swimmer.xml"); // gMCFJFileNameArray.push_back("mjcf/swimmer.xml");
// gMCFJFileNameArray.push_back("mjcf/reacher.xml"); // gMCFJFileNameArray.push_back("mjcf/reacher.xml");
@ -238,7 +239,7 @@ void ImportMJCFSetup::initPhysics()
ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF); ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
mb = creation.getBulletMultiBody(); mb = creation.getBulletMultiBody();
if (/* DISABLES CODE */ (0)) // mb) if (mb)
{ {
printf("first MJCF file converted!\n"); printf("first MJCF file converted!\n");
std::string* name = std::string* name =

21
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@ -231,6 +231,8 @@ void ConvertURDF2BulletInternal(
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
std::string linkName = u2b.getLinkName(urdfLinkIndex);
if (flags & CUF_USE_SDF) if (flags & CUF_USE_SDF)
{ {
parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace; parent2joint =parentTransformInWorldSpace.inverse()*linkTransformInWorldSpace;
@ -314,6 +316,10 @@ void ConvertURDF2BulletInternal(
bool isFixedBase = (mass==0);//todo: figure out when base is fixed bool isFixedBase = (mass==0);//todo: figure out when base is fixed
int totalNumJoints = cache.m_totalNumJoints1; int totalNumJoints = cache.m_totalNumJoints1;
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep); cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep);
if (flags & CUF_USE_MJCF)
{
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
}
cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame); cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
} }
@ -443,6 +449,16 @@ void ConvertURDF2BulletInternal(
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
int colGroup=0, colMask=0;
int flags = u2b.getCollisionGroupAndMask(urdfLinkIndex,colGroup, colMask);
if (flags & URDF_HAS_COLLISION_GROUP)
{
collisionFilterGroup = colGroup;
}
if (flags & URDF_HAS_COLLISION_MASK)
{
collisionFilterMask = colMask;
}
world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask); world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
btVector4 color = selectColor2();//(0.0,0.0,0.5); btVector4 color = selectColor2();//(0.0,0.0,0.5);
@ -505,7 +521,12 @@ void ConvertURDF2Bullet(
btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex]; btTransform localInertialFrameRoot = cache.m_urdfLinkLocalInertialFrames[urdfLinkIndex];
if (flags & CUF_USE_MJCF)
{
} else
{
mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot); mb->setBaseWorldTransform(rootTransformInWorldSpace*localInertialFrameRoot);
}
btAlignedObjectArray<btQuaternion> scratch_q; btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m; btAlignedObjectArray<btVector3> scratch_m;
mb->forwardKinematics(scratch_q,scratch_m); mb->forwardKinematics(scratch_q,scratch_m);

1
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@ -30,6 +30,7 @@ public:
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;} virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0;}
///this API will likely change, don't override it! ///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;} virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}

6
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@ -55,5 +55,11 @@ struct URDFLinkContactInfo
} }
}; };
enum UrdfCollisionFlags
{
URDF_FORCE_CONCAVE_TRIMESH=1,
URDF_HAS_COLLISION_GROUP=2,
URDF_HAS_COLLISION_MASK=4,
};
#endif //URDF_JOINT_TYPES_H #endif //URDF_JOINT_TYPES_H

9
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -58,6 +58,8 @@ struct UrdfGeometry
btVector3 m_boxSize; btVector3 m_boxSize;
double m_capsuleRadius; double m_capsuleRadius;
double m_capsuleHalfHeight;
int m_hasFromTo;
btVector3 m_capsuleFrom; btVector3 m_capsuleFrom;
btVector3 m_capsuleTo; btVector3 m_capsuleTo;
@ -80,10 +82,7 @@ struct UrdfVisual
UrdfMaterial m_localMaterial; UrdfMaterial m_localMaterial;
}; };
enum UrdfCollisionFlags
{
URDF_FORCE_CONCAVE_TRIMESH=1,
};
struct UrdfCollision struct UrdfCollision
@ -92,6 +91,8 @@ struct UrdfCollision
UrdfGeometry m_geometry; UrdfGeometry m_geometry;
std::string m_name; std::string m_name;
int m_flags; int m_flags;
int m_collisionGroup;
int m_collisionMask;
UrdfCollision() UrdfCollision()
:m_flags(0) :m_flags(0)
{ {