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

MJCF: fix capsule length when given in size="", fix slider joint limits

Browse Source
This commit is contained in:
Oleg Klimov
2017-03-27 22:29:24 +03:00
parent 3048326add
commit 77608154a3
5 changed files with 74 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -357,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");
@@ -386,8 +387,8 @@ struct BulletMJCFImporterInternalData
jointTrans.setRotation(orn);
}
}
btVector3 jointAxis(1,0,0);
btVector3 jointAxis(1,0,0);
if (axisStr)
{
std::string ax = axisStr;
@@ -396,80 +397,15 @@ struct BulletMJCFImporterInternalData
{
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> 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()));
}
}
bool success = false;
if (limits.size()==2)
{
if (m_angleUnits=="degree")
{
range[0] = limits[0] * B3_PI / 180;
range[1] = limits[1] * B3_PI / 180;
success = true;
}
else if (m_angleUnits=="radian")
{
range[0] = limits[0];
range[1] = limits[1];
success = true;
}
}
if (!success)
{
logger->reportWarning( (sourceFileLocation(link_xml) + ": cannot parse 'range' attribute (units='" + m_angleUnits + "'')").c_str() );
}
}
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)
{
@@ -500,6 +436,64 @@ struct BulletMJCFImporterInternalData
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();
@@ -660,14 +654,14 @@ 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
{
@@ -1687,7 +1681,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;

6
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -564,7 +564,7 @@ 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 ->setMargin(gUrdfDefaultCollisionMargin);
@@ -574,7 +574,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));
@@ -797,7 +797,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);

4
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -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")
{

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

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

2
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -220,7 +220,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
rad = visual->m_geometry.m_capsuleRadius;
} else {
tr = visual->m_linkLocalFrame;
len = visual->m_geometry.m_capsuleHalfHeight;
len = visual->m_geometry.m_capsuleHeight;
rad = visual->m_geometry.m_capsuleRadius;
}
visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];