Merge remote-tracking branch 'upstream/master'
This commit is contained in:
yunfeibai
@@ -79,7 +79,7 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, MJCFErr
|
||||
}
|
||||
if (rgba.size() < 3)
|
||||
{
|
||||
logger->reportWarning("Couldn't parse vector3");
|
||||
logger->reportWarning( ("Couldn't parse vector3 '" + vector_str + "'").c_str() );
|
||||
return false;
|
||||
}
|
||||
if (lastThree) {
|
||||
@@ -113,7 +113,8 @@ static bool parseVector6(btVector3& v0, btVector3& v1, const std::string& vector
|
||||
}
|
||||
if (values.size() < 6)
|
||||
{
|
||||
logger->reportWarning("Couldn't parse 6 floats");
|
||||
logger->reportWarning( ("Couldn't parse 6 floats '" + vector_str + "'").c_str() );
|
||||
|
||||
return false;
|
||||
}
|
||||
v0.setValue(values[0],values[1],values[2]);
|
||||
@@ -143,6 +144,7 @@ struct BulletMJCFImporterInternalData
|
||||
//<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/>
|
||||
std::string m_meshDir;
|
||||
std::string m_textureDir;
|
||||
std::string m_angleUnits;
|
||||
|
||||
|
||||
int m_activeModel;
|
||||
@@ -173,9 +175,17 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
std::string sourceFileLocation(TiXmlElement* e)
|
||||
{
|
||||
#if 0
|
||||
//no C++11 snprintf etc
|
||||
char buf[1024];
|
||||
snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row());
|
||||
return buf;
|
||||
#else
|
||||
char row[1024];
|
||||
sprintf(row,"%d",e->Row());
|
||||
std::string str = m_sourceFileName.c_str() + std::string(":") + std::string(row);
|
||||
return str;
|
||||
#endif
|
||||
}
|
||||
|
||||
const UrdfLink* getLink(int modelIndex, int linkIndex) const
|
||||
@@ -205,6 +215,8 @@ struct BulletMJCFImporterInternalData
|
||||
{
|
||||
m_textureDir = textureDirStr;
|
||||
}
|
||||
const char* angle = root_xml->Attribute("angle");
|
||||
m_angleUnits = angle ? angle : "degree"; // degrees by default, http://www.mujoco.org/book/modeling.html#compiler
|
||||
#if 0
|
||||
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
|
||||
{
|
||||
@@ -337,8 +349,7 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
if (!handled)
|
||||
{
|
||||
logger->reportWarning("Unhandled root element");
|
||||
logger->reportWarning(n.c_str());
|
||||
logger->reportWarning( (sourceFileLocation(rootxml) + ": unhandled root element '" + n + "'").c_str() );
|
||||
}
|
||||
}
|
||||
return true;
|
||||
@@ -346,6 +357,7 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
|
||||
{
|
||||
bool jointHandled = false;
|
||||
const char* jType = link_xml->Attribute("type");
|
||||
const char* limitedStr = link_xml->Attribute("limited");
|
||||
const char* axisStr = link_xml->Attribute("axis");
|
||||
@@ -375,83 +387,29 @@ struct BulletMJCFImporterInternalData
|
||||
jointTrans.setRotation(orn);
|
||||
}
|
||||
}
|
||||
btVector3 jointAxis(1,0,0);
|
||||
|
||||
btVector3 jointAxis(1,0,0);
|
||||
if (axisStr)
|
||||
{
|
||||
std::string ax = axisStr;
|
||||
parseVector3(jointAxis,ax,logger);
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("joint without axis attribute");
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": joint without axis attribute").c_str() );
|
||||
}
|
||||
bool isLimited = false;
|
||||
double range[2] = {1,0};
|
||||
|
||||
double range[2] = {1,0};
|
||||
std::string lim = m_defaultJointLimited;
|
||||
if (limitedStr)
|
||||
{
|
||||
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)
|
||||
{
|
||||
// TODO angle units are in "<compiler angle="degree" inertiafromgeom="true"/>
|
||||
range[0] = sizes[0] * B3_PI / 180;
|
||||
range[1] = sizes[1] * B3_PI / 180;
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("Expected range[2] in joint with limits");
|
||||
}
|
||||
}
|
||||
bool isLimited = lim=="true";
|
||||
|
||||
// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
|
||||
// degrees of freedom created by this joint. These are constants added to the
|
||||
// diagonal of the inertia matrix in generalized coordinates. They make the
|
||||
// simulation more stable, and often increase physical realism. This is because
|
||||
// when a motor is attached to the system with a transmission that amplifies
|
||||
// the motor force by c, the inertia of the rotor (i.e. the moving part of the
|
||||
// motor) is amplified by c*c. The same holds for gears in the early stages of
|
||||
// planetary gear boxes. These extra inertias often dominate the inertias of
|
||||
// the robot parts that are represented explicitly in the model, and the
|
||||
// armature attribute is the way to model them.
|
||||
|
||||
// TODO damping : real, "0" Damping applied to all degrees of
|
||||
// freedom created by this joint. Unlike friction loss
|
||||
// which is computed by the constraint solver, damping is
|
||||
// simply a force linear in velocity. It is included in
|
||||
// the passive forces. Despite this simplicity, larger
|
||||
// damping values can make numerical integrators unstable,
|
||||
// which is why our Euler integrator handles damping
|
||||
// implicitly. See Integration in the Computation chapter.
|
||||
|
||||
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;
|
||||
std::string jointType = jType;
|
||||
if (jointType == "fixed")
|
||||
{
|
||||
ejtype = URDFFixedJoint;
|
||||
@@ -475,9 +433,67 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("Expected 'type' attribute for joint");
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": expected 'type' attribute for joint").c_str() );
|
||||
}
|
||||
|
||||
if (isLimited)
|
||||
{
|
||||
//parse the 'range' field
|
||||
btArray<std::string> pieces;
|
||||
btArray<float> limits;
|
||||
btAlignedObjectArray<std::string> strArray;
|
||||
urdfIsAnyOf(" ", strArray);
|
||||
urdfStringSplit(pieces, rangeStr, strArray);
|
||||
for (int i = 0; i < pieces.size(); ++i)
|
||||
{
|
||||
if (!pieces[i].empty())
|
||||
{
|
||||
limits.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
||||
}
|
||||
}
|
||||
if (limits.size()==2)
|
||||
{
|
||||
range[0] = limits[0];
|
||||
range[1] = limits[1];
|
||||
if (m_angleUnits=="degree" && ejtype==URDFRevoluteJoint)
|
||||
{
|
||||
range[0] = limits[0] * B3_PI / 180;
|
||||
range[1] = limits[1] * B3_PI / 180;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": cannot parse 'range' attribute (units='" + m_angleUnits + "'')").c_str() );
|
||||
}
|
||||
}
|
||||
|
||||
// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
|
||||
// degrees of freedom created by this joint. These are constants added to the
|
||||
// diagonal of the inertia matrix in generalized coordinates. They make the
|
||||
// simulation more stable, and often increase physical realism. This is because
|
||||
// when a motor is attached to the system with a transmission that amplifies
|
||||
// the motor force by c, the inertia of the rotor (i.e. the moving part of the
|
||||
// motor) is amplified by c*c. The same holds for gears in the early stages of
|
||||
// planetary gear boxes. These extra inertias often dominate the inertias of
|
||||
// the robot parts that are represented explicitly in the model, and the
|
||||
// armature attribute is the way to model them.
|
||||
|
||||
// TODO damping : real, "0" Damping applied to all degrees of
|
||||
// freedom created by this joint. Unlike friction loss
|
||||
// which is computed by the constraint solver, damping is
|
||||
// simply a force linear in velocity. It is included in
|
||||
// the passive forces. Despite this simplicity, larger
|
||||
// damping values can make numerical integrators unstable,
|
||||
// which is why our Euler integrator handles damping
|
||||
// implicitly. See Integration in the Computation chapter.
|
||||
|
||||
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
|
||||
|
||||
btTransform parentLinkToJointTransform;
|
||||
parentLinkToJointTransform.setIdentity();
|
||||
parentLinkToJointTransform = parentToLinkTrans*jointTrans;
|
||||
jointTransOut = jointTrans;
|
||||
|
||||
if (jointHandled)
|
||||
{
|
||||
UrdfJoint* jointPtr = new UrdfJoint();
|
||||
@@ -520,6 +536,7 @@ struct BulletMJCFImporterInternalData
|
||||
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
|
||||
if (linkPtrPtr==0)
|
||||
{
|
||||
// XXX: should it be assert?
|
||||
logger->reportWarning("Invalide linkindex");
|
||||
return false;
|
||||
}
|
||||
@@ -613,7 +630,7 @@ struct BulletMJCFImporterInternalData
|
||||
geom.m_sphereRadius = urdfLexicalCast<double>(sz);
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("Expected size field (scalar) in sphere geom");
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": no size field (scalar) in sphere geom").c_str() );
|
||||
}
|
||||
handledGeomType = true;
|
||||
}
|
||||
@@ -637,18 +654,18 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
|
||||
geom.m_capsuleRadius = 0;
|
||||
geom.m_capsuleHalfHeight = 0.f;
|
||||
geom.m_capsuleHeight = 0.f;
|
||||
|
||||
if (sizes.size()>0)
|
||||
{
|
||||
geom.m_capsuleRadius = sizes[0];
|
||||
if (sizes.size()>1)
|
||||
{
|
||||
geom.m_capsuleHalfHeight = sizes[1];
|
||||
geom.m_capsuleHeight = 2*sizes[1];
|
||||
}
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("couldn't convert 'size' attribute of capsule geom");
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": couldn't convert 'size' attribute of capsule geom").c_str() );
|
||||
}
|
||||
const char* fromtoStr = link_xml->Attribute("fromto");
|
||||
geom.m_hasFromTo = false;
|
||||
@@ -664,7 +681,7 @@ struct BulletMJCFImporterInternalData
|
||||
{
|
||||
if (sizes.size()<2)
|
||||
{
|
||||
logger->reportWarning("capsule without fromto attribute requires 2 sizes (radius and halfheight)");
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": capsule without fromto attribute requires 2 sizes (radius and halfheight)").c_str() );
|
||||
} else
|
||||
{
|
||||
handledGeomType = true;
|
||||
@@ -725,13 +742,11 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
} else
|
||||
{
|
||||
char warn[1024];
|
||||
sprintf(warn,"Unknown/unhandled geom type: %s", geomType.c_str());
|
||||
logger->reportWarning(warn);
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": unhandled geom type '" + geomType + "'").c_str() );
|
||||
}
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("geom requires type");
|
||||
logger->reportWarning( (sourceFileLocation(link_xml) + ": geom requires type").c_str() );
|
||||
}
|
||||
|
||||
return handledGeomType;
|
||||
@@ -1013,10 +1028,7 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
if (!handled)
|
||||
{
|
||||
char warn[1024];
|
||||
std::string n = xml->Value();
|
||||
sprintf(warn,"Unknown/unhandled field: %s", n.c_str());
|
||||
logger->reportWarning(warn);
|
||||
logger->reportWarning( (sourceFileLocation(xml) + ": unknown field '" + n + "'").c_str() );
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1395,12 +1407,14 @@ void BulletMJCFImporter::getLinkChildIndices(int urdfLinkIndex, btAlignedObjectA
|
||||
}
|
||||
}
|
||||
|
||||
bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
|
||||
bool BulletMJCFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
|
||||
{
|
||||
jointLowerLimit = 0.f;
|
||||
jointLowerLimit = 0.f;
|
||||
jointUpperLimit = 0.f;
|
||||
jointDamping = 0.f;
|
||||
jointFriction = 0.f;
|
||||
jointMaxForce = 0;
|
||||
jointMaxVelocity = 0;
|
||||
|
||||
const UrdfLink* link = m_data->getLink(m_data->m_activeModel,urdfLinkIndex);
|
||||
if (link)
|
||||
@@ -1418,7 +1432,9 @@ bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
|
||||
jointUpperLimit = pj->m_upperLimit;
|
||||
jointDamping = pj->m_jointDamping;
|
||||
jointFriction = pj->m_jointFriction;
|
||||
|
||||
jointMaxForce = pj->m_effortLimit;
|
||||
jointMaxVelocity = pj->m_velocityLimit;
|
||||
|
||||
return true;
|
||||
} else
|
||||
{
|
||||
@@ -1429,6 +1445,14 @@ bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
|
||||
{
|
||||
//backwards compatibility for custom file importers
|
||||
btScalar jointMaxForce = 0;
|
||||
btScalar jointMaxVelocity = 0;
|
||||
return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction,jointMaxForce, jointMaxVelocity);
|
||||
}
|
||||
|
||||
bool BulletMJCFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
|
||||
{
|
||||
@@ -1662,7 +1686,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
} else
|
||||
{
|
||||
btCapsuleShapeZ* cap = new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius,
|
||||
2.*col->m_geometry.m_capsuleHalfHeight);
|
||||
col->m_geometry.m_capsuleHeight);
|
||||
childShape = cap;
|
||||
}
|
||||
break;
|
||||
|
||||
@@ -62,7 +62,8 @@ public:
|
||||
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
|
||||
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
|
||||
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
@@ -365,12 +365,14 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
|
||||
}
|
||||
}
|
||||
|
||||
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
|
||||
bool BulletURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
|
||||
{
|
||||
jointLowerLimit = 0.f;
|
||||
jointUpperLimit = 0.f;
|
||||
jointDamping = 0.f;
|
||||
jointFriction = 0.f;
|
||||
jointMaxForce = 0.f;
|
||||
jointMaxVelocity = 0.f;
|
||||
|
||||
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
|
||||
btAssert(linkPtr);
|
||||
@@ -389,7 +391,8 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
|
||||
jointUpperLimit = pj->m_upperLimit;
|
||||
jointDamping = pj->m_jointDamping;
|
||||
jointFriction = pj->m_jointFriction;
|
||||
|
||||
jointMaxForce = pj->m_effortLimit;
|
||||
jointMaxVelocity = pj->m_velocityLimit;
|
||||
return true;
|
||||
} else
|
||||
{
|
||||
@@ -399,6 +402,14 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
|
||||
}
|
||||
|
||||
return false;
|
||||
|
||||
};
|
||||
|
||||
bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
|
||||
{
|
||||
btScalar jointMaxForce;
|
||||
btScalar jointMaxVelocity;
|
||||
return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
|
||||
|
||||
}
|
||||
|
||||
@@ -558,9 +569,9 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
case URDF_GEOM_CAPSULE:
|
||||
{
|
||||
btScalar radius = collision->m_geometry.m_capsuleRadius;
|
||||
btScalar height = btScalar(2.f)*collision->m_geometry.m_capsuleHalfHeight;
|
||||
btScalar height = collision->m_geometry.m_capsuleHeight;
|
||||
btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius,height);
|
||||
shape = capsuleShape;
|
||||
shape = capsuleShape;
|
||||
shape ->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
}
|
||||
@@ -568,7 +579,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
case URDF_GEOM_CYLINDER:
|
||||
{
|
||||
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
|
||||
btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
|
||||
btScalar cylLength = collision->m_geometry.m_capsuleHeight;
|
||||
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||
@@ -791,7 +802,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
{
|
||||
|
||||
btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
|
||||
btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight;
|
||||
btScalar cylLength = visual->m_geometry.m_capsuleHeight;
|
||||
|
||||
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
|
||||
vertices.push_back(vert);
|
||||
|
||||
@@ -48,7 +48,8 @@ public:
|
||||
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
|
||||
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
|
||||
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
@@ -232,9 +232,11 @@ void ConvertURDF2BulletInternal(
|
||||
btScalar jointUpperLimit;
|
||||
btScalar jointDamping;
|
||||
btScalar jointFriction;
|
||||
btScalar jointMaxForce;
|
||||
btScalar jointMaxVelocity;
|
||||
|
||||
|
||||
bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
|
||||
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
|
||||
std::string linkName = u2b.getLinkName(urdfLinkIndex);
|
||||
|
||||
if (flags & CUF_USE_SDF)
|
||||
@@ -344,6 +346,17 @@ void ConvertURDF2BulletInternal(
|
||||
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
|
||||
|
||||
bool disableParentCollision = true;
|
||||
|
||||
if (createMultiBody && cache.m_bulletMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
|
||||
}
|
||||
|
||||
switch (jointType)
|
||||
{
|
||||
case URDFFixedJoint:
|
||||
@@ -374,8 +387,6 @@ void ConvertURDF2BulletInternal(
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
|
||||
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
|
||||
@@ -46,6 +46,14 @@ public:
|
||||
virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
|
||||
|
||||
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
|
||||
|
||||
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
|
||||
{
|
||||
//backwards compatibility for custom file importers
|
||||
jointMaxForce = 0;
|
||||
jointMaxVelocity = 0;
|
||||
return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
|
||||
};
|
||||
|
||||
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
|
||||
|
||||
|
||||
@@ -403,7 +403,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
||||
}
|
||||
geom.m_hasFromTo = false;
|
||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||
geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
|
||||
}
|
||||
else if (type_name == "capsule")
|
||||
@@ -417,7 +417,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
||||
}
|
||||
geom.m_hasFromTo = false;
|
||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||
geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
}
|
||||
else if (type_name == "mesh")
|
||||
{
|
||||
@@ -1679,7 +1679,17 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
|
||||
|
||||
std::string UrdfParser::sourceFileLocation(TiXmlElement* e)
|
||||
{
|
||||
#if 0
|
||||
//no C++11 etc, no snprintf
|
||||
|
||||
char buf[1024];
|
||||
snprintf(buf, sizeof(buf), "%s:%i", m_urdf2Model.m_sourceFile.c_str(), e->Row());
|
||||
return buf;
|
||||
#else
|
||||
char row[1024];
|
||||
sprintf(row,"%d",e->Row());
|
||||
std::string str = m_urdf2Model.m_sourceFile.c_str() + std::string(":") + std::string(row);
|
||||
return str;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
@@ -65,7 +65,7 @@ struct UrdfGeometry
|
||||
btVector3 m_boxSize;
|
||||
|
||||
double m_capsuleRadius;
|
||||
double m_capsuleHalfHeight;
|
||||
double m_capsuleHeight;
|
||||
int m_hasFromTo;
|
||||
btVector3 m_capsuleFrom;
|
||||
btVector3 m_capsuleTo;
|
||||
|
||||
Reference in New Issue
Block a user