From 634f4cfdbcac871e06e6c3da520ca6fd0af445a8 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Wed, 22 Mar 2017 21:05:00 +0300 Subject: [PATCH 1/4] MJCF: support for angle units in , kill two stdout messages --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 30 ++++++++++++++----- .../PhysicsServerCommandProcessor.cpp | 4 +-- .../TinyRendererVisualShapeConverter.cpp | 5 ++++ 3 files changed, 29 insertions(+), 10 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index 78c2745da..f4d6d231d 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -143,6 +143,7 @@ struct BulletMJCFImporterInternalData // std::string m_meshDir; std::string m_textureDir; + std::string m_angleUnits; int m_activeModel; @@ -213,6 +214,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()) { @@ -406,7 +409,7 @@ struct BulletMJCFImporterInternalData isLimited = true; //parse the 'range' field btArray pieces; - btArray sizes; + btArray limits; btAlignedObjectArray strArray; urdfIsAnyOf(" ", strArray); urdfStringSplit(pieces, rangeStr, strArray); @@ -414,17 +417,28 @@ struct BulletMJCFImporterInternalData { if (!pieces[i].empty()) { - sizes.push_back(urdfLexicalCast(pieces[i].c_str())); + limits.push_back(urdfLexicalCast(pieces[i].c_str())); } } - if (sizes.size()==2) + bool success = false; + if (limits.size()==2) { - // TODO angle units are in " - range[0] = sizes[0] * B3_PI / 180; - range[1] = sizes[1] * B3_PI / 180; - } else + 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("Expected range[2] in joint with limits"); + logger->reportWarning( (sourceFileLocation(link_xml) + ": cannot parse 'range' attribute (units='" + m_angleUnits + "'')").c_str() ); } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 614dc8da5..33cc39096 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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); diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 520ca6710..0ecacd81e 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -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); From dcb7e25f341dbf7230f98e91134036300780fd9c Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Fri, 24 Mar 2017 03:43:56 +0300 Subject: [PATCH 2/4] MJCF: improve error messages --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 30 ++++++++----------- 1 file changed, 13 insertions(+), 17 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index f4d6d231d..a933aeade 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -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]); @@ -348,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; @@ -394,7 +394,7 @@ struct BulletMJCFImporterInternalData 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}; @@ -497,7 +497,7 @@ struct BulletMJCFImporterInternalData } } else { - logger->reportWarning("Expected 'type' attribute for joint"); + logger->reportWarning( (sourceFileLocation(link_xml) + ": expected 'type' attribute for joint").c_str() ); } if (jointHandled) @@ -542,6 +542,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; } @@ -635,7 +636,7 @@ struct BulletMJCFImporterInternalData geom.m_sphereRadius = urdfLexicalCast(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; } @@ -670,7 +671,7 @@ struct BulletMJCFImporterInternalData } } 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; @@ -686,7 +687,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; @@ -747,13 +748,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; @@ -1035,10 +1034,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() ); } } From 3048326addc9a068a2a2264941bcfe1b04a9b44e Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Mon, 27 Mar 2017 22:30:43 +0300 Subject: [PATCH 3/4] C API: fix b3SetContactFilterLink() when using b3RequestContactPoint() --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 33cc39096..40dc3c5e1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3808,6 +3808,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; @@ -3820,8 +3826,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); From 77608154a3c40fa5b74a7abe9d70bc3fdf476c89 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Mon, 27 Mar 2017 22:29:24 +0300 Subject: [PATCH 4/4] MJCF: fix capsule length when given in size="", fix slider joint limits --- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 138 +++++++++--------- .../ImportURDFDemo/BulletUrdfImporter.cpp | 8 +- .../Importers/ImportURDFDemo/UrdfParser.cpp | 4 +- .../Importers/ImportURDFDemo/UrdfParser.h | 2 +- .../TinyRendererVisualShapeConverter.cpp | 2 +- 5 files changed, 74 insertions(+), 80 deletions(-) diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index a933aeade..123ceea68 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -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,84 +397,19 @@ 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 pieces; - btArray limits; - btAlignedObjectArray strArray; - urdfIsAnyOf(" ", strArray); - urdfStringSplit(pieces, rangeStr, strArray); - for (int i = 0; i < pieces.size(); ++i) - { - if (!pieces[i].empty()) - { - limits.push_back(urdfLexicalCast(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) { - std::string jointType = jType; + std::string jointType = jType; if (jointType == "fixed") { ejtype = URDFFixedJoint; @@ -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 pieces; + btArray limits; + btAlignedObjectArray strArray; + urdfIsAnyOf(" ", strArray); + urdfStringSplit(pieces, rangeStr, strArray); + for (int i = 0; i < pieces.size(); ++i) + { + if (!pieces[i].empty()) + { + limits.push_back(urdfLexicalCast(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; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 8c242fe3e..8407c6191 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -564,9 +564,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; } @@ -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 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); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 53857c6c2..2ec443db2 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -403,7 +403,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* } geom.m_hasFromTo = false; geom.m_capsuleRadius = urdfLexicalCast(shape->Attribute("radius")); - geom.m_capsuleHalfHeight = urdfLexicalCast(shape->Attribute("length")); + geom.m_capsuleHeight = urdfLexicalCast(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(shape->Attribute("radius")); - geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast(shape->Attribute("length")); + geom.m_capsuleHeight = urdfLexicalCast(shape->Attribute("length")); } else if (type_name == "mesh") { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 015342a0a..680fbc1ca 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -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; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 0ecacd81e..f0eaa8798 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -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];