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

remove the contype=0 in inverted_pendulum.xml, so we can 'mouse-pick' the pendulum.

Browse Source 
[mjcf importer] add rudimentary support for inertial frame computation, for 'fromto' capsules.
This commit is contained in:
Erwin Coumans
2017-01-24 21:10:21 -08:00
parent a9be975601
commit 76dcf3a751
3 changed files with 38 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/mjcf/inverted_pendulum.xml
View File

@@ -2,7 +2,7 @@
<compiler inertiafromgeom="true"/> <compiler inertiafromgeom="true"/>
<default> <default>
<joint armature="0" damping="1" limited="true"/> <joint armature="0" damping="1" limited="true"/>
<geom contype="0" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/> <geom friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
<tendon/> <tendon/>
<motor ctrlrange="-3 3"/> <motor ctrlrange="-3 3"/>
</default> </default>

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

@@ -296,7 +296,8 @@ struct BulletMJCFImporterInternalData
// modelPtr->m_rootLinks.push_back(linkPtr); // modelPtr->m_rootLinks.push_back(linkPtr);
parseGeom(rootxml,modelIndex, linkIndex,logger); btVector3 inertialShift(0,0,0);
parseGeom(rootxml,modelIndex, linkIndex,logger,inertialShift);
initTreeAndRoot(*modelPtr,logger); initTreeAndRoot(*modelPtr,logger);
handled = true; handled = true;
@@ -469,7 +470,7 @@ struct BulletMJCFImporterInternalData
*/ */
return false; return false;
} }
bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger) bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
{ {
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex); UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr==0) if (linkPtrPtr==0)
@@ -599,6 +600,7 @@ struct BulletMJCFImporterInternalData
geom.m_hasFromTo = true; 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);
inertialShift=0.5*(geom.m_capsuleFrom+geom.m_capsuleTo);
handledGeomType = true; handledGeomType = true;
} else } else
{ {
@@ -807,7 +809,9 @@ struct BulletMJCFImporterInternalData
const char* bodyName = link_xml->Attribute("name"); const char* bodyName = link_xml->Attribute("name");
int orgChildLinkIndex = createBody(modelIndex,bodyName); int orgChildLinkIndex = createBody(modelIndex,bodyName);
btTransform localInertialFrame;
localInertialFrame.setIdentity();
// int curChildLinkIndex = orgChildLinkIndex; // int curChildLinkIndex = orgChildLinkIndex;
std::string bodyN; std::string bodyN;
@@ -830,8 +834,8 @@ struct BulletMJCFImporterInternalData
bool massDefined = false; bool massDefined = false;
btVector3 inertialPos(0,0,0);
btQuaternion inertialOrn(0,0,0,1);
btScalar mass = 0.f; btScalar mass = 0.f;
btVector3 localInertiaDiag(0,0,0); btVector3 localInertiaDiag(0,0,0);
// int thisLinkIndex = -2; // int thisLinkIndex = -2;
@@ -839,7 +843,7 @@ struct BulletMJCFImporterInternalData
btTransform jointTrans; btTransform jointTrans;
jointTrans.setIdentity(); jointTrans.setIdentity();
bool skipFixedJoint = false; bool skipFixedJoint = false;
for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement()) for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
{ {
bool handled = false; bool handled = false;
@@ -852,7 +856,22 @@ struct BulletMJCFImporterInternalData
if (p) if (p)
{ {
std::string posStr = p; std::string posStr = p;
parseVector3(inertialPos,posStr,logger); btVector3 inertialPos(0,0,0);
if (parseVector3(inertialPos,posStr,logger))
{
localInertialFrame.setOrigin(inertialPos);
}
}
const char* o = xml->Attribute("quat");
{
std::string ornStr = o;
btQuaternion orn(0,0,0,1);
btVector4 o4;
if (parseVector4(o4,ornStr))
{
orn.setValue(o4[1],o4[2],o4[3],o4[0]);
localInertialFrame.setRotation(orn);
}
} }
const char* m = xml->Attribute("mass"); const char* m = xml->Attribute("mass");
if (m) if (m)
@@ -915,7 +934,12 @@ struct BulletMJCFImporterInternalData
} }
if (n == "geom") if (n == "geom")
{ {
parseGeom(xml,modelIndex, orgChildLinkIndex , logger); btVector3 inertialShift(0,0,0);
parseGeom(xml,modelIndex, orgChildLinkIndex , logger,inertialShift);
if (!massDefined)
{
localInertialFrame.setOrigin(inertialShift);
}
handled = true; handled = true;
} }
@@ -968,7 +992,7 @@ struct BulletMJCFImporterInternalData
double volume = computeVolume(linkPtr,logger); double volume = computeVolume(linkPtr,logger);
mass = density * volume; mass = density * volume;
} }
linkPtr->m_inertia.m_linkLocalFrame.setIdentity();// = jointTrans.inverse(); linkPtr->m_inertia.m_linkLocalFrame = localInertialFrame;// = jointTrans.inverse();
linkPtr->m_inertia.m_mass = mass; linkPtr->m_inertia.m_mass = mass;
return true; return true;
} }
@@ -1631,7 +1655,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
if (childShape) if (childShape)
{ {
m_data->m_allocatedCollisionShapes.push_back(childShape); m_data->m_allocatedCollisionShapes.push_back(childShape);
compound->addChildShape(col->m_linkLocalFrame,childShape); compound->addChildShape(localInertiaFrame.inverse()*col->m_linkLocalFrame,childShape);
} }
} }
} }

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

@@ -26,11 +26,14 @@ struct UrdfMaterial
struct UrdfInertia struct UrdfInertia
{ {
btTransform m_linkLocalFrame; btTransform m_linkLocalFrame;
bool m_hasLinkLocalFrame;
double m_mass; double m_mass;
double m_ixx,m_ixy,m_ixz,m_iyy,m_iyz,m_izz; double m_ixx,m_ixy,m_ixz,m_iyy,m_iyz,m_izz;
UrdfInertia() UrdfInertia()
{ {
m_hasLinkLocalFrame = false;
m_linkLocalFrame.setIdentity(); m_linkLocalFrame.setIdentity();
m_mass = 0.f; m_mass = 0.f;
m_ixx=m_ixy=m_ixz=m_iyy=m_iyz=m_izz=0.f; m_ixx=m_ixy=m_ixz=m_iyy=m_iyz=m_izz=0.f;