Merge pull request #1041 from olegklimov/master

Random improvements (MJCF, error messages, gravity)
This commit is contained in:
erwincoumans 2017-03-29 09:12:48 -07:00 committed by GitHub
commit 0750d502a8
6 changed files with 109 additions and 89 deletions

View File

@ -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;
@ -213,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())
{
@ -345,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;
@ -354,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");
@ -383,79 +387,25 @@ 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)
{
@ -483,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();
@ -528,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;
}
@ -621,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;
}
@ -645,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;
@ -672,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;
@ -733,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;
@ -1021,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() );
}
}
@ -1677,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;

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

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

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;

View File

@ -1398,8 +1398,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex);
//int rootLinkIndex = u2b.getRootLinkIndex();
//b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_data->m_guiHelper);
u2b.getRootTransformInWorld(rootTrans);
@ -3812,6 +3812,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2();
if (
(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
{
continue;
}
}
int objectIndexA = -1;
@ -3824,8 +3830,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (mblA && mblA->m_multiBody)
{
linkIndexA = mblA->m_link;
objectIndexA = mblA->m_multiBody->getUserIndex2();
if (
(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
{
continue;
}
}
btAssert(bodyA || mblA);

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];
@ -414,6 +414,11 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
break;
} // case mesh
case URDF_GEOM_PLANE:
// TODO: plane in tiny renderer
// TODO: export visualShapeOut for external render
break;
default:
{
b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type);