93471a1c31
fixed some more warnings added alignment macros to some classes btPersistentManifold from 128 to 16 bytes aligned prepare command to select collision filter mode (SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
1387 lines
34 KiB
C++
1387 lines
34 KiB
C++
#include "BulletMJCFImporter.h"
|
|
#include "../../ThirdPartyLibs/tinyxml/tinyxml.h"
|
|
#include "Bullet3Common/b3FileUtils.h"
|
|
#include <string>
|
|
#include "../../Utils/b3ResourcePath.h"
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include "../ImportURDFDemo/UrdfParser.h"
|
|
#include "../ImportURDFDemo/urdfStringSplit.h"
|
|
#include "../ImportURDFDemo/urdfLexicalCast.h"
|
|
|
|
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
|
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
|
|
#include "BulletCollision/CollisionShapes/btBoxShape.h"
|
|
#include "BulletCollision/CollisionShapes/btSphereShape.h"
|
|
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
|
|
#include "BulletCollision/CollisionShapes/btCylinderShape.h"
|
|
#include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
|
|
|
|
|
|
|
|
enum ePARENT_LINK_ENUMS
|
|
{
|
|
BASE_LINK_INDEX=-1,
|
|
|
|
INVALID_LINK_INDEX=-2
|
|
};
|
|
|
|
static int gUid=0;
|
|
|
|
static bool parseVector4(btVector4& vec4, const std::string& vector_str)
|
|
{
|
|
vec4.setZero();
|
|
btArray<std::string> pieces;
|
|
btArray<float> rgba;
|
|
btAlignedObjectArray<std::string> strArray;
|
|
urdfIsAnyOf(" ", strArray);
|
|
urdfStringSplit(pieces, vector_str, strArray);
|
|
for (int i = 0; i < pieces.size(); ++i)
|
|
{
|
|
if (!pieces[i].empty())
|
|
{
|
|
rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
|
}
|
|
}
|
|
if (rgba.size() != 4)
|
|
{
|
|
return false;
|
|
}
|
|
vec4.setValue(rgba[0],rgba[1],rgba[2],rgba[3]);
|
|
return true;
|
|
}
|
|
|
|
static bool parseVector3(btVector3& vec3, const std::string& vector_str, MJCFErrorLogger* logger, bool lastThree = false)
|
|
{
|
|
vec3.setZero();
|
|
btArray<std::string> pieces;
|
|
btArray<float> rgba;
|
|
btAlignedObjectArray<std::string> strArray;
|
|
urdfIsAnyOf(" ", strArray);
|
|
urdfStringSplit(pieces, vector_str, strArray);
|
|
for (int i = 0; i < pieces.size(); ++i)
|
|
{
|
|
if (!pieces[i].empty())
|
|
{
|
|
rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
|
}
|
|
}
|
|
if (rgba.size() < 3)
|
|
{
|
|
logger->reportWarning("Couldn't parse vector3");
|
|
return false;
|
|
}
|
|
if (lastThree) {
|
|
vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
|
|
}
|
|
else
|
|
{
|
|
vec3.setValue(rgba[0],rgba[1],rgba[2]);
|
|
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
static bool parseVector6(btVector3& v0, btVector3& v1, const std::string& vector_str, MJCFErrorLogger* logger)
|
|
{
|
|
v0.setZero();
|
|
v1.setZero();
|
|
|
|
btArray<std::string> pieces;
|
|
btArray<float> values;
|
|
btAlignedObjectArray<std::string> strArray;
|
|
urdfIsAnyOf(" ", strArray);
|
|
urdfStringSplit(pieces, vector_str, strArray);
|
|
for (int i = 0; i < pieces.size(); ++i)
|
|
{
|
|
if (!pieces[i].empty())
|
|
{
|
|
values.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
|
}
|
|
}
|
|
if (values.size() < 6)
|
|
{
|
|
logger->reportWarning("Couldn't parse 6 floats");
|
|
return false;
|
|
}
|
|
v0.setValue(values[0],values[1],values[2]);
|
|
v1.setValue(values[3],values[4],values[5]);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
struct BulletMJCFImporterInternalData
|
|
{
|
|
GUIHelperInterface* m_guiHelper;
|
|
char m_pathPrefix[1024];
|
|
|
|
std::string m_fileModelName;
|
|
|
|
btAlignedObjectArray<UrdfModel*> m_models;
|
|
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!)
|
|
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
|
|
|
BulletMJCFImporterInternalData()
|
|
:m_activeModel(-1),
|
|
m_defaultCollisionGroup(1),
|
|
m_defaultCollisionMask(1)
|
|
{
|
|
m_pathPrefix[0] = 0;
|
|
}
|
|
|
|
const UrdfLink* getLink(int modelIndex, int linkIndex) const
|
|
{
|
|
if (modelIndex>=0 && modelIndex<m_models.size())
|
|
{
|
|
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
|
|
if (linkPtrPtr && *linkPtrPtr)
|
|
{
|
|
UrdfLink* linkPtr = *linkPtrPtr;
|
|
return linkPtr;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
bool parseDefaults(TiXmlElement* root_xml, MJCFErrorLogger* logger)
|
|
{
|
|
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())
|
|
{
|
|
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")
|
|
{
|
|
int modelIndex = m_models.size();
|
|
UrdfModel* model = new UrdfModel();
|
|
m_models.push_back(model);
|
|
parseBody(rootxml,modelIndex, INVALID_LINK_INDEX,logger);
|
|
initTreeAndRoot(*model,logger);
|
|
handled = true;
|
|
}
|
|
|
|
if (n=="geom")
|
|
{
|
|
int modelIndex = m_models.size();
|
|
UrdfModel* modelPtr = new UrdfModel();
|
|
m_models.push_back(modelPtr);
|
|
|
|
UrdfLink* linkPtr = new UrdfLink();
|
|
linkPtr->m_name = "anonymous";
|
|
const char* namePtr = rootxml->Attribute("name");
|
|
if (namePtr)
|
|
{
|
|
linkPtr->m_name = namePtr;
|
|
}
|
|
int linkIndex = modelPtr->m_links.size();
|
|
linkPtr->m_linkIndex = linkIndex;
|
|
modelPtr->m_links.insert(linkPtr->m_name.c_str(),linkPtr);
|
|
|
|
//don't parse geom transform here, it will be inside 'parseGeom'
|
|
linkPtr->m_linkTransformInWorld.setIdentity();
|
|
|
|
// modelPtr->m_rootLinks.push_back(linkPtr);
|
|
|
|
parseGeom(rootxml,modelIndex, linkIndex,logger);
|
|
initTreeAndRoot(*modelPtr,logger);
|
|
|
|
handled = true;
|
|
}
|
|
|
|
|
|
if (n=="site")
|
|
{
|
|
handled = true;
|
|
}
|
|
if (!handled)
|
|
{
|
|
logger->reportWarning("Unhandled root element");
|
|
logger->reportWarning(n.c_str());
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
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* limitedStr = link_xml->Attribute("limited");
|
|
const char* axisStr = link_xml->Attribute("axis");
|
|
const char* posStr = link_xml->Attribute("pos");
|
|
const char* ornStr = link_xml->Attribute("quat");
|
|
const char* nameStr = link_xml->Attribute("name");
|
|
const char* rangeStr = link_xml->Attribute("range");
|
|
|
|
btTransform jointTrans;
|
|
jointTrans.setIdentity();
|
|
if (posStr)
|
|
{
|
|
btVector3 pos;
|
|
std::string p=posStr;
|
|
if (parseVector3(pos,p,logger))
|
|
{
|
|
jointTrans.setOrigin(pos);
|
|
}
|
|
}
|
|
if (ornStr)
|
|
{
|
|
std::string o = ornStr;
|
|
btVector4 o4;
|
|
if (parseVector4(o4,o))
|
|
{
|
|
btQuaternion orn(o4[3],o4[0],o4[1],o4[2]);
|
|
jointTrans.setRotation(orn);
|
|
}
|
|
}
|
|
btVector3 jointAxis(1,0,0);
|
|
|
|
if (axisStr)
|
|
{
|
|
std::string ax = axisStr;
|
|
parseVector3(jointAxis,ax,logger);
|
|
} else
|
|
{
|
|
logger->reportWarning("joint without axis attribute");
|
|
}
|
|
bool isLimited = false;
|
|
double range[2] = {1,0};
|
|
|
|
if (limitedStr)
|
|
{
|
|
std::string lim = limitedStr;
|
|
if (lim=="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
|
|
{
|
|
// logger->reportWarning("joint without limited field");
|
|
}
|
|
|
|
bool jointHandled = false;
|
|
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
|
|
|
|
btTransform parentLinkToJointTransform;
|
|
parentLinkToJointTransform.setIdentity();
|
|
parentLinkToJointTransform = parentToLinkTrans*jointTrans;
|
|
|
|
jointTransOut = jointTrans;
|
|
UrdfJointTypes ejtype;
|
|
if (jType)
|
|
{
|
|
std::string jointType = jType;
|
|
if (jointType == "fixed")
|
|
{
|
|
ejtype = URDFFixedJoint;
|
|
jointHandled = true;
|
|
}
|
|
if (jointType == "slide")
|
|
{
|
|
ejtype = URDFPrismaticJoint;
|
|
jointHandled = true;
|
|
}
|
|
if (jointType == "hinge")
|
|
{
|
|
if (isLimited)
|
|
{
|
|
ejtype = URDFRevoluteJoint;
|
|
} else
|
|
{
|
|
ejtype = URDFContinuousJoint;
|
|
}
|
|
jointHandled = true;
|
|
}
|
|
} else
|
|
{
|
|
logger->reportWarning("Expected 'type' attribute for joint");
|
|
}
|
|
|
|
if (jointHandled)
|
|
{
|
|
UrdfJoint* jointPtr = new UrdfJoint();
|
|
jointPtr->m_childLinkName=linkPtr->m_name;
|
|
const UrdfLink* parentLink = getLink(modelIndex,parentLinkIndex);
|
|
jointPtr->m_parentLinkName =parentLink->m_name;
|
|
jointPtr->m_localJointAxis=jointAxis;
|
|
jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform;
|
|
jointPtr->m_type = ejtype;
|
|
int numJoints = m_models[modelIndex]->m_joints.size();
|
|
|
|
//range
|
|
jointPtr->m_lowerLimit = range[0];
|
|
jointPtr->m_upperLimit = range[1];
|
|
|
|
if (nameStr)
|
|
{
|
|
jointPtr->m_name =nameStr;
|
|
} else
|
|
{
|
|
char jointName[1024];
|
|
sprintf(jointName,"joint%d_%d_%d",gUid++,linkIndex,numJoints);
|
|
jointPtr->m_name =jointName;
|
|
}
|
|
m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr);
|
|
return true;
|
|
}
|
|
/*
|
|
URDFRevoluteJoint=1,
|
|
URDFPrismaticJoint,
|
|
URDFContinuousJoint,
|
|
URDFFloatingJoint,
|
|
URDFPlanarJoint,
|
|
URDFFixedJoint,
|
|
*/
|
|
return false;
|
|
}
|
|
bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger)
|
|
{
|
|
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
|
|
if (linkPtrPtr==0)
|
|
{
|
|
logger->reportWarning("Invalide linkindex");
|
|
return false;
|
|
}
|
|
UrdfLink* linkPtr = *linkPtrPtr;
|
|
|
|
btTransform linkLocalFrame;
|
|
linkLocalFrame.setIdentity();
|
|
|
|
|
|
bool handledGeomType = false;
|
|
UrdfGeometry geom;
|
|
|
|
|
|
|
|
// const char* rgba = link_xml->Attribute("rgba");
|
|
const char* gType = link_xml->Attribute("type");
|
|
const char* sz = link_xml->Attribute("size");
|
|
const char* posS = link_xml->Attribute("pos");
|
|
if (posS)
|
|
{
|
|
btVector3 pos(0,0,0);
|
|
std::string p = posS;
|
|
if (parseVector3(pos,p,logger))
|
|
{
|
|
linkLocalFrame.setOrigin(pos);
|
|
}
|
|
}
|
|
const char* ornS = link_xml->Attribute("quat");
|
|
if (ornS)
|
|
{
|
|
std::string ornStr = ornS;
|
|
btQuaternion orn(0,0,0,1);
|
|
btVector4 o4;
|
|
if (parseVector4(o4,ornStr))
|
|
{
|
|
orn.setValue(o4[1],o4[2],o4[3],o4[0]);
|
|
linkLocalFrame.setRotation(orn);
|
|
}
|
|
}
|
|
if (gType)
|
|
{
|
|
std::string geomType = gType;
|
|
|
|
|
|
if (geomType == "plane")
|
|
{
|
|
geom.m_type = URDF_GEOM_PLANE;
|
|
geom.m_planeNormal.setValue(0,0,1);
|
|
btVector3 size(1,1,1);
|
|
if (sz)
|
|
{
|
|
std::string sizeStr = sz;
|
|
bool lastThree = false;
|
|
parseVector3(size,sizeStr,logger,lastThree);
|
|
}
|
|
geom.m_boxSize = size;
|
|
handledGeomType = true;
|
|
}
|
|
if (geomType == "box")
|
|
{
|
|
btVector3 size(1,1,1);
|
|
if (sz)
|
|
{
|
|
std::string sizeStr = sz;
|
|
bool lastThree = false;
|
|
parseVector3(size,sizeStr,logger,lastThree);
|
|
}
|
|
geom.m_type = URDF_GEOM_BOX;
|
|
geom.m_boxSize = size;
|
|
handledGeomType = true;
|
|
}
|
|
|
|
if (geomType == "sphere")
|
|
{
|
|
geom.m_type = URDF_GEOM_SPHERE;
|
|
if (sz)
|
|
{
|
|
geom.m_sphereRadius = urdfLexicalCast<double>(sz);
|
|
} else
|
|
{
|
|
logger->reportWarning("Expected size field (scalar) in sphere geom");
|
|
}
|
|
handledGeomType = true;
|
|
}
|
|
|
|
//todo: capsule, cylinder, meshes or heightfields etc
|
|
if (geomType == "capsule")
|
|
{
|
|
geom.m_type = URDF_GEOM_CAPSULE;
|
|
|
|
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");
|
|
geom.m_hasFromTo = false;
|
|
|
|
if (fromtoStr)
|
|
{
|
|
geom.m_hasFromTo = true;
|
|
std::string fromto = fromtoStr;
|
|
parseVector6(geom.m_capsuleFrom,geom.m_capsuleTo,fromto,logger);
|
|
handledGeomType = true;
|
|
} else
|
|
{
|
|
if (sizes.size()<2)
|
|
{
|
|
logger->reportWarning("capsule without fromto attribute requires 2 sizes (radius and halfheight)");
|
|
} else
|
|
{
|
|
handledGeomType = true;
|
|
}
|
|
}
|
|
}
|
|
#if 0
|
|
if (geomType == "cylinder")
|
|
{
|
|
geom.m_type = URDF_GEOM_CYLINDER;
|
|
handledGeomType = true;
|
|
}
|
|
#endif
|
|
if (handledGeomType)
|
|
{
|
|
|
|
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_linkLocalFrame = linkLocalFrame;
|
|
linkPtr->m_collisionArray.push_back(col);
|
|
|
|
} else
|
|
{
|
|
char warn[1024];
|
|
sprintf(warn,"Unknown/unhandled geom type: %s", geomType.c_str());
|
|
logger->reportWarning(warn);
|
|
}
|
|
} else
|
|
{
|
|
logger->reportWarning("geom requires type");
|
|
}
|
|
|
|
return handledGeomType;
|
|
}
|
|
|
|
btTransform parseTransform(TiXmlElement* link_xml, MJCFErrorLogger* logger)
|
|
{
|
|
btTransform tr;
|
|
tr.setIdentity();
|
|
|
|
const char* p = link_xml->Attribute("pos");
|
|
if (p)
|
|
{
|
|
btVector3 pos(0,0,0);
|
|
std::string pstr = p;
|
|
if (parseVector3(pos,pstr,logger))
|
|
{
|
|
tr.setOrigin(pos);
|
|
}
|
|
|
|
} else
|
|
{
|
|
// logger->reportWarning("body should have pos attribute");
|
|
}
|
|
const char* o = link_xml->Attribute("quat");
|
|
if (o)
|
|
{
|
|
std::string ornstr = o;
|
|
btVector4 o4;
|
|
btQuaternion orn(0,0,0,1);
|
|
if (parseVector4(o4,ornstr))
|
|
{
|
|
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);
|
|
}
|
|
} else
|
|
{
|
|
// logger->reportWarning("body doesn't have quat (orientation) attribute");
|
|
}
|
|
return tr;
|
|
}
|
|
|
|
double computeVolume(const UrdfLink* linkPtr,MJCFErrorLogger* logger) const
|
|
{
|
|
double totalVolume = 0;
|
|
|
|
for (int i=0;i<linkPtr->m_collisionArray.size();i++)
|
|
{
|
|
const UrdfCollision* col = &linkPtr->m_collisionArray[i];
|
|
switch (col->m_geometry.m_type)
|
|
{
|
|
case URDF_GEOM_SPHERE:
|
|
{
|
|
double r = col->m_geometry.m_sphereRadius;
|
|
totalVolume += 4./3.*SIMD_PI*r*r*r;
|
|
break;
|
|
}
|
|
case URDF_GEOM_BOX:
|
|
{
|
|
totalVolume += 8. * col->m_geometry.m_boxSize[0]*
|
|
col->m_geometry.m_boxSize[1]*
|
|
col->m_geometry.m_boxSize[2];
|
|
break;
|
|
}
|
|
|
|
case URDF_GEOM_CYLINDER:
|
|
{
|
|
//todo
|
|
break;
|
|
}
|
|
case URDF_GEOM_MESH:
|
|
{
|
|
//todo (based on mesh bounding box?)
|
|
break;
|
|
}
|
|
case URDF_GEOM_PLANE:
|
|
{
|
|
//todo
|
|
break;
|
|
}
|
|
case URDF_GEOM_CAPSULE:
|
|
{
|
|
//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();
|
|
totalVolume += SIMD_PI*r*r*h;
|
|
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
int createBody(int modelIndex, const char* namePtr)
|
|
{
|
|
UrdfModel* modelPtr = m_models[modelIndex];
|
|
int orgChildLinkIndex = modelPtr->m_links.size();
|
|
UrdfLink* linkPtr = new UrdfLink();
|
|
char uniqueLinkName[1024];
|
|
sprintf(uniqueLinkName,"link%d",orgChildLinkIndex );
|
|
linkPtr->m_name = uniqueLinkName;
|
|
if (namePtr)
|
|
{
|
|
linkPtr->m_name = namePtr;
|
|
}
|
|
linkPtr->m_linkIndex = orgChildLinkIndex ;
|
|
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);
|
|
UrdfLink* linkPtr = getLink(modelIndex,orgChildLinkIndex);
|
|
|
|
|
|
|
|
bool massDefined = false;
|
|
btVector3 inertialPos(0,0,0);
|
|
btQuaternion inertialOrn(0,0,0,1);
|
|
btScalar mass = 0.f;
|
|
btVector3 localInertiaDiag(0,0,0);
|
|
// int thisLinkIndex = -2;
|
|
bool hasJoint = false;
|
|
btTransform jointTrans;
|
|
jointTrans.setIdentity();
|
|
bool skipFixedJoint = false;
|
|
|
|
for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
|
|
{
|
|
bool handled = false;
|
|
std::string n = xml->Value();
|
|
if (n=="inertial")
|
|
{
|
|
// <inertial pos="0 0 0" quat="0.5 0.5 -0.5 0.5" mass="297.821" diaginertia="109.36 69.9714 69.9714" />
|
|
|
|
const char* p = xml->Attribute("pos");
|
|
if (p)
|
|
{
|
|
std::string posStr = p;
|
|
parseVector3(inertialPos,posStr,logger);
|
|
}
|
|
const char* m = xml->Attribute("mass");
|
|
if (m)
|
|
{
|
|
mass = urdfLexicalCast<double>(m);
|
|
}
|
|
const char* i = xml->Attribute("diaginertia");
|
|
if (i)
|
|
{
|
|
std::string istr = i;
|
|
parseVector3(localInertiaDiag,istr,logger);
|
|
}
|
|
|
|
massDefined = true;
|
|
|
|
handled = true;
|
|
}
|
|
|
|
if (n=="joint")
|
|
{
|
|
if (!hasJoint)
|
|
{
|
|
const char* jType = xml->Attribute("type");
|
|
std::string jointType = jType? jType:"";
|
|
|
|
if (newParentLinkIndex!=INVALID_LINK_INDEX || jointType!="free")
|
|
{
|
|
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;
|
|
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")
|
|
{
|
|
parseGeom(xml,modelIndex, orgChildLinkIndex , logger);
|
|
handled = true;
|
|
}
|
|
|
|
//recursive
|
|
if (n=="body")
|
|
{
|
|
parseBody(xml,modelIndex,orgChildLinkIndex,logger);
|
|
handled = true;
|
|
}
|
|
|
|
if (n=="light")
|
|
{
|
|
handled = true;
|
|
}
|
|
if (!handled)
|
|
{
|
|
char warn[1024];
|
|
std::string n = xml->Value();
|
|
sprintf(warn,"Unknown/unhandled field: %s", n.c_str());
|
|
logger->reportWarning(warn);
|
|
}
|
|
}
|
|
|
|
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
|
|
UrdfJoint* jointPtr = new UrdfJoint();
|
|
jointPtr->m_childLinkName=linkPtr->m_name;
|
|
const UrdfLink* parentLink = getLink(modelIndex,newParentLinkIndex);
|
|
jointPtr->m_parentLinkName =parentLink->m_name;
|
|
jointPtr->m_localJointAxis.setValue(1,0,0);
|
|
jointPtr->m_parentLinkToJointTransform = linkTransform;
|
|
jointPtr->m_type = URDFFixedJoint;
|
|
char jointName[1024];
|
|
sprintf(jointName,"jointfix_%d_%d",gUid++,newParentLinkIndex);
|
|
jointPtr->m_name =jointName;
|
|
m_models[modelIndex]->m_joints.insert(jointPtr->m_name.c_str(),jointPtr);
|
|
}
|
|
|
|
//check mass/inertia
|
|
if (!massDefined)
|
|
{
|
|
double density = 1000;
|
|
double volume = computeVolume(linkPtr,logger);
|
|
mass = density * volume;
|
|
}
|
|
linkPtr->m_inertia.m_linkLocalFrame.setIdentity();// = jointTrans.inverse();
|
|
linkPtr->m_inertia.m_mass = mass;
|
|
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)
|
|
{
|
|
// every link has children links and joints, but no parents, so we create a
|
|
// local convenience data structure for keeping child->parent relations
|
|
btHashMap<btHashString,btHashString> parentLinkTree;
|
|
|
|
// loop through all joints, for every link, assign children links and children joints
|
|
for (int i=0;i<model.m_joints.size();i++)
|
|
{
|
|
UrdfJoint** jointPtr = model.m_joints.getAtIndex(i);
|
|
if (jointPtr)
|
|
{
|
|
UrdfJoint* joint = *jointPtr;
|
|
std::string parent_link_name = joint->m_parentLinkName;
|
|
std::string child_link_name = joint->m_childLinkName;
|
|
if (parent_link_name.empty() || child_link_name.empty())
|
|
{
|
|
logger->reportError("parent link or child link is empty for joint");
|
|
logger->reportError(joint->m_name.c_str());
|
|
return false;
|
|
}
|
|
|
|
UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
|
|
if (!childLinkPtr)
|
|
{
|
|
logger->reportError("Cannot find child link for joint ");
|
|
logger->reportError(joint->m_name.c_str());
|
|
|
|
return false;
|
|
}
|
|
UrdfLink* childLink = *childLinkPtr;
|
|
|
|
UrdfLink** parentLinkPtr = model.m_links.find(joint->m_parentLinkName.c_str());
|
|
if (!parentLinkPtr)
|
|
{
|
|
logger->reportError("Cannot find parent link for a joint");
|
|
logger->reportError(joint->m_name.c_str());
|
|
return false;
|
|
}
|
|
UrdfLink* parentLink = *parentLinkPtr;
|
|
|
|
childLink->m_parentLink = parentLink;
|
|
|
|
childLink->m_parentJoint = joint;
|
|
parentLink->m_childJoints.push_back(joint);
|
|
parentLink->m_childLinks.push_back(childLink);
|
|
parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str());
|
|
}
|
|
}
|
|
|
|
//search for children that have no parent, those are 'root'
|
|
for (int i=0;i<model.m_links.size();i++)
|
|
{
|
|
UrdfLink** linkPtr = model.m_links.getAtIndex(i);
|
|
btAssert(linkPtr);
|
|
if (linkPtr)
|
|
{
|
|
UrdfLink* link = *linkPtr;
|
|
link->m_linkIndex = i;
|
|
|
|
if (!link->m_parentLink)
|
|
{
|
|
model.m_rootLinks.push_back(link);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
if (model.m_rootLinks.size()>1)
|
|
{
|
|
logger->reportWarning("URDF file with multiple root links found");
|
|
}
|
|
|
|
if (model.m_rootLinks.size()==0)
|
|
{
|
|
logger->reportError("URDF without root link found");
|
|
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;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper)
|
|
{
|
|
m_data = new BulletMJCFImporterInternalData();
|
|
m_data->m_guiHelper = helper;
|
|
}
|
|
|
|
BulletMJCFImporter::~BulletMJCFImporter()
|
|
{
|
|
delete m_data;
|
|
}
|
|
|
|
bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase)
|
|
{
|
|
if (strlen(fileName)==0)
|
|
return false;
|
|
|
|
//int argc=0;
|
|
char relativeFileName[1024];
|
|
|
|
b3FileUtils fu;
|
|
|
|
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
|
|
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
|
|
|
|
std::string xml_string;
|
|
m_data->m_pathPrefix[0] = 0;
|
|
|
|
if (!fileFound){
|
|
std::cerr << "MJCF file not found" << std::endl;
|
|
return false;
|
|
} else
|
|
{
|
|
|
|
int maxPathLen = 1024;
|
|
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
|
|
|
|
|
|
std::fstream xml_file(relativeFileName, std::fstream::in);
|
|
while ( xml_file.good())
|
|
{
|
|
std::string line;
|
|
std::getline( xml_file, line);
|
|
xml_string += (line + "\n");
|
|
}
|
|
xml_file.close();
|
|
|
|
if (parseMJCFString(xml_string.c_str(), logger))
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
return false;
|
|
}
|
|
|
|
bool BulletMJCFImporter::parseMJCFString(const char* xmlText, MJCFErrorLogger* logger)
|
|
{
|
|
TiXmlDocument xml_doc;
|
|
xml_doc.Parse(xmlText);
|
|
if (xml_doc.Error())
|
|
{
|
|
logger->reportError(xml_doc.ErrorDesc());
|
|
xml_doc.ClearError();
|
|
return false;
|
|
}
|
|
|
|
TiXmlElement *mujoco_xml = xml_doc.FirstChildElement("mujoco");
|
|
if (!mujoco_xml)
|
|
{
|
|
logger->reportWarning("Cannot find <mujoco> root element");
|
|
return false;
|
|
}
|
|
|
|
const char* modelName = mujoco_xml->Attribute("model");
|
|
if (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"))
|
|
{
|
|
m_data->parseRootLevel(link_xml,logger);
|
|
}
|
|
|
|
for (TiXmlElement* link_xml = mujoco_xml->FirstChildElement("worldbody"); link_xml; link_xml = link_xml->NextSiblingElement("worldbody"))
|
|
{
|
|
m_data->parseRootLevel(link_xml,logger);
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
const char* BulletMJCFImporter::getPathPrefix()
|
|
{
|
|
return m_data->m_pathPrefix;
|
|
}
|
|
|
|
int BulletMJCFImporter::getRootLinkIndex() const
|
|
{
|
|
if (m_data->m_activeModel>=0 && m_data->m_activeModel<m_data->m_models.size())
|
|
{
|
|
if (m_data->m_models[m_data->m_activeModel]->m_rootLinks.size())
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
|
|
std::string BulletMJCFImporter::getLinkName(int linkIndex) const
|
|
{
|
|
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex);
|
|
if (link)
|
|
{
|
|
return link->m_name;
|
|
}
|
|
return "";
|
|
}
|
|
|
|
bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
|
|
{
|
|
// UrdfLink* link = m_data->getLink(linkIndex);
|
|
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
|
|
{
|
|
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 "";
|
|
}
|
|
|
|
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
|
|
void BulletMJCFImporter::getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
|
|
{
|
|
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,urdfLinkIndex);
|
|
if (link)
|
|
{
|
|
mass = link->m_inertia.m_mass;
|
|
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,
|
|
link->m_inertia.m_iyy,
|
|
link->m_inertia.m_izz);
|
|
inertialFrame.setIdentity();
|
|
inertialFrame = link->m_inertia.m_linkLocalFrame;
|
|
} else
|
|
{
|
|
mass = 0;
|
|
localInertiaDiagonal.setZero();
|
|
inertialFrame.setIdentity();
|
|
}
|
|
}
|
|
|
|
///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
|
|
void BulletMJCFImporter::getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const
|
|
{
|
|
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,urdfLinkIndex);
|
|
if (link)
|
|
{
|
|
for (int i=0;i<link->m_childLinks.size();i++)
|
|
{
|
|
int childIndex = link->m_childLinks[i]->m_linkIndex;
|
|
childLinkIndices.push_back(childIndex);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
|
|
{
|
|
jointLowerLimit = 0.f;
|
|
jointUpperLimit = 0.f;
|
|
jointDamping = 0.f;
|
|
jointFriction = 0.f;
|
|
|
|
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,urdfLinkIndex);
|
|
if (link)
|
|
{
|
|
|
|
linkTransformInWorld = link->m_linkTransformInWorld;
|
|
|
|
if (link->m_parentJoint)
|
|
{
|
|
UrdfJoint* pj = link->m_parentJoint;
|
|
parent2joint = pj->m_parentLinkToJointTransform;
|
|
jointType = pj->m_type;
|
|
jointAxisInJointSpace = pj->m_localJointAxis;
|
|
jointLowerLimit = pj->m_lowerLimit;
|
|
jointUpperLimit = pj->m_upperLimit;
|
|
jointDamping = pj->m_jointDamping;
|
|
jointFriction = pj->m_jointFriction;
|
|
|
|
return true;
|
|
} else
|
|
{
|
|
parent2joint.setIdentity();
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
bool BulletMJCFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
|
|
{
|
|
rootTransformInWorld.setIdentity();
|
|
/*
|
|
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,0);
|
|
if (link)
|
|
{
|
|
rootTransformInWorld = link->m_linkTransformInWorld;
|
|
}
|
|
*/
|
|
return true;
|
|
}
|
|
|
|
int BulletMJCFImporter::convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const
|
|
{
|
|
return -1;
|
|
}
|
|
|
|
bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
|
|
{
|
|
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,linkIndex);
|
|
if (link)
|
|
{
|
|
contactInfo = link->m_contactInfo;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
|
|
{
|
|
}
|
|
void BulletMJCFImporter::setBodyUniqueId(int bodyId)
|
|
{
|
|
|
|
}
|
|
|
|
int BulletMJCFImporter::getBodyUniqueId() const
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
|
|
{
|
|
btCompoundShape* compound = new btCompoundShape();
|
|
m_data->m_allocatedCollisionShapes.push_back(compound);
|
|
|
|
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];
|
|
btCollisionShape* childShape = 0;
|
|
|
|
switch (col->m_geometry.m_type)
|
|
{
|
|
case URDF_GEOM_PLANE:
|
|
{
|
|
childShape = new btStaticPlaneShape(col->m_geometry.m_planeNormal,0);
|
|
break;
|
|
}
|
|
case URDF_GEOM_SPHERE:
|
|
{
|
|
childShape = new btSphereShape(col->m_geometry.m_sphereRadius);
|
|
break;
|
|
}
|
|
case URDF_GEOM_BOX:
|
|
{
|
|
childShape = new btBoxShape(col->m_geometry.m_boxSize);
|
|
break;
|
|
}
|
|
case URDF_GEOM_CYLINDER:
|
|
{
|
|
// childShape = new btCylinderShape(col->m_geometry...);
|
|
break;
|
|
}
|
|
case URDF_GEOM_MESH:
|
|
{
|
|
break;
|
|
}
|
|
|
|
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)};
|
|
|
|
btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2);
|
|
childShape = ms;
|
|
} else
|
|
{
|
|
btCapsuleShapeZ* cap = new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius,
|
|
2.*col->m_geometry.m_capsuleHalfHeight);
|
|
childShape = cap;
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
}
|
|
}
|
|
if (childShape)
|
|
{
|
|
m_data->m_allocatedCollisionShapes.push_back(childShape);
|
|
compound->addChildShape(col->m_linkLocalFrame,childShape);
|
|
}
|
|
}
|
|
}
|
|
return compound;
|
|
}
|
|
|
|
int BulletMJCFImporter::getNumAllocatedCollisionShapes() const
|
|
{
|
|
return m_data->m_allocatedCollisionShapes.size();
|
|
}
|
|
class btCollisionShape* BulletMJCFImporter::getAllocatedCollisionShape(int index)
|
|
{
|
|
return m_data->m_allocatedCollisionShapes[index];
|
|
}
|
|
|
|
|
|
int BulletMJCFImporter::getNumModels() const
|
|
{
|
|
return m_data->m_models.size();
|
|
}
|
|
|
|
void BulletMJCFImporter::activateModel(int modelIndex)
|
|
{
|
|
if ((modelIndex>=0) && (modelIndex<m_data->m_models.size()))
|
|
{
|
|
m_data->m_activeModel = modelIndex;
|
|
}
|
|
}
|
|
|