diff --git a/data/kuka_iiwa/kuka_world.sdf b/data/kuka_iiwa/kuka_world.sdf new file mode 100644 index 000000000..d48d51382 --- /dev/null +++ b/data/kuka_iiwa/kuka_world.sdf @@ -0,0 +1,414 @@ + + + + + world + lbr_iiwa_link_0 + + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0.01 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.36 1.5708 -0 -3.14159 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.5645 0 0 0 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.78 1.5708 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 -0 0.9645 0 -0 -3.14159 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.18 1.5708 -0 -3.14159 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 1.261 0 0 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + \ No newline at end of file diff --git a/data/kuka_iiwa/model.sdf b/data/kuka_iiwa/model.sdf new file mode 100644 index 000000000..900626708 --- /dev/null +++ b/data/kuka_iiwa/model.sdf @@ -0,0 +1,410 @@ + + + + + 0 0 0 0 -0 0 + + -0.1 0 0.07 0 -0 0 + 0 + + 0.05 + 0 + 0 + 0.06 + 0 + 0.03 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_0.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_0.stl + + + + + + 0 0 0.1575 0 -0 0 + + 0 -0.03 0.12 0 -0 0 + 4 + + 0.1 + 0 + 0 + 0.09 + 0 + 0.02 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_1.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_1.stl + + + + + + lbr_iiwa_link_1 + lbr_iiwa_link_0 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.2025 1.57079632679 0 3.14159265359 + + 0.0003 0.059 0.042 0 -0 0 + 4 + + 0.05 + 0 + 0 + 0.018 + 0 + 0.044 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_2.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_2.stl + + + + + + lbr_iiwa_link_2 + lbr_iiwa_link_1 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0.2045 0 1.57079632679 0 3.14159265359 + + 0 0.03 0.13 0 -0 0 + 3 + + 0.08 + 0 + 0 + 0.075 + 0 + 0.01 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_3.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_3.stl + + + + + + lbr_iiwa_link_3 + lbr_iiwa_link_2 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.2155 1.57079632679 0 0 + + 0 0.067 0.034 0 -0 0 + 2.7 + + 0.03 + 0 + 0 + 0.01 + 0 + 0.029 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_4.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_4.stl + + + + + + lbr_iiwa_link_4 + lbr_iiwa_link_3 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0.1845 0 -1.57079632679 3.14159265359 0 + + 0.0001 0.021 0.076 0 -0 0 + 1.7 + + 0.02 + 0 + 0 + 0.018 + 0 + 0.005 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_5.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_5.stl + + + + + + lbr_iiwa_link_5 + lbr_iiwa_link_4 + + 0 0 1 + + -2.96706 + 2.96706 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0 0.2155 1.57079632679 0 0 + + 0 0.0006 0.0004 0 -0 0 + 1.8 + + 0.005 + 0 + 0 + 0.0036 + 0 + 0.0047 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_6.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_6.stl + + + + + + lbr_iiwa_link_6 + lbr_iiwa_link_5 + + 0 0 1 + + -2.0944 + 2.0944 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + 0 0.081 0 -1.57079632679 3.14159265359 0 + + 0 0 0.02 0 -0 0 + 0.3 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/coarse/link_7.stl + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/link_7.stl + + + + + + lbr_iiwa_link_7 + lbr_iiwa_link_6 + + 0 0 1 + + -3.05433 + 3.05433 + 300 + 10 + + + 0.5 + 0 + 0 + 0 + + 0 + + + + + \ No newline at end of file diff --git a/data/kuka_iiwa/model_for_sdf.urdf b/data/kuka_iiwa/model_for_sdf.urdf new file mode 100644 index 000000000..12a165423 --- /dev/null +++ b/data/kuka_iiwa/model_for_sdf.urdf @@ -0,0 +1,285 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index b95f528a9..fa54b0d29 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -131,7 +131,7 @@ ImportSDFSetup::ImportSDFSetup(struct GUIHelperInterface* helper, int option, co if (gFileNameArray.size()==0) { - gFileNameArray.push_back("two_cubes.sdf"); + gFileNameArray.push_back("kuka_iiwa/model.sdf"); } @@ -291,7 +291,7 @@ void ImportSDFSetup::stepSimulation(float deltaTime) } //the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge - m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); + //m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.); } } diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index d2be60000..a7db650ef 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -200,10 +200,7 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction); - - - linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint; - + int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 25ca7461e..074266fc6 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -77,7 +77,7 @@ static bool parseVector4(btVector4& vec4, const std::string& vector_str) return true; } -static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger) +static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false) { vec3.setZero(); btArray pieces; @@ -90,16 +90,22 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLo rgba.push_back(urdfLexicalCast(pieces[i].c_str())); } } - if (rgba.size() != 3) + if (rgba.size() < 3) { logger->reportWarning("Couldn't parse vector3"); return false; } - vec3.setValue(rgba[0],rgba[1],rgba[2]); + if (lastThree) { + vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]); + } + else + { + vec3.setValue(rgba[0],rgba[1],rgba[2]); + + } return true; } - bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, ErrorLogger* logger) { @@ -143,47 +149,78 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err } -bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger) +bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false) { - tr.setIdentity(); - - { - const char* xyz_str = xml->Attribute("xyz"); - if (xyz_str) - { - parseVector3(tr.getOrigin(),std::string(xyz_str),logger); - } - } - - { - const char* rpy_str = xml->Attribute("rpy"); - if (rpy_str != NULL) - { - btVector3 rpy; - if (parseVector3(rpy,std::string(rpy_str),logger)) - { - double phi, the, psi; - double roll = rpy[0]; - double pitch = rpy[1]; - double yaw = rpy[2]; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - btQuaternion orn( - sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), - cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), - cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), - cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)); - - orn.normalize(); - tr.setRotation(orn); - } - } - } - return true; + tr.setIdentity(); + + if (parseSDF) + { + parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger); + } + else + { + const char* xyz_str = xml->Attribute("xyz"); + if (xyz_str) + { + parseVector3(tr.getOrigin(),std::string(xyz_str),logger); + } + } + + if (parseSDF) + { + btVector3 rpy; + if (parseVector3(rpy,std::string(xml->GetText()),logger,true)) + { + double phi, the, psi; + double roll = rpy[0]; + double pitch = rpy[1]; + double yaw = rpy[2]; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + btQuaternion orn( + sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), + cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), + cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), + cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)); + + orn.normalize(); + tr.setRotation(orn); + } + } + else + { + const char* rpy_str = xml->Attribute("rpy"); + if (rpy_str != NULL) + { + btVector3 rpy; + if (parseVector3(rpy,std::string(rpy_str),logger)) + { + double phi, the, psi; + double roll = rpy[0]; + double pitch = rpy[1]; + double yaw = rpy[2]; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + btQuaternion orn( + sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), + cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), + cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), + cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)); + + orn.normalize(); + tr.setRotation(orn); + } + } + } + return true; } + bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorLogger* logger) { inertia.m_linkLocalFrame.setIdentity(); @@ -333,21 +370,38 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* else if (type_name == "mesh") { geom.m_type = URDF_GEOM_MESH; - if (!shape->Attribute("filename")) { - logger->reportError("Mesh must contain a filename attribute"); - return false; - } - - geom.m_meshFileName = shape->Attribute("filename"); - - if (shape->Attribute("scale")) - { - parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger); - } else - { - geom.m_meshScale.setValue(1,1,1); - } - + if (m_parseSDF) + { + TiXmlElement* scale = shape->FirstChildElement("scale"); + if (0==scale) + { + geom.m_meshScale.setValue(1,1,1); + } + else + { + parseVector3(geom.m_meshScale,scale->GetText(),logger); + } + + TiXmlElement* filename = shape->FirstChildElement("uri"); + geom.m_meshFileName = filename->GetText(); + } + else + { + if (!shape->Attribute("filename")) { + logger->reportError("Mesh must contain a filename attribute"); + return false; + } + + geom.m_meshFileName = shape->Attribute("filename"); + + if (shape->Attribute("scale")) + { + parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger); + } else + { + geom.m_meshScale.setValue(1,1,1); + } + } } else { @@ -473,7 +527,18 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi return false; } link.m_name = linkName; - + + if (m_parseSDF) { + TiXmlElement* pose = config->FirstChildElement("pose"); + if (0==pose) + { + link.m_parentLinktoLinkTransform.setIdentity(); + } + else + { + parseTransform(link.m_parentLinktoLinkTransform, pose,logger,m_parseSDF); + } + } // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); @@ -552,35 +617,109 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL joint.m_effortLimit = 0.f; joint.m_velocityLimit = 0.f; - const char* lower_str = config->Attribute("lower"); - if (lower_str) - { - joint.m_lowerLimit = urdfLexicalCast(lower_str); - } - - const char* upper_str = config->Attribute("upper"); - if (upper_str) - { - joint.m_upperLimit = urdfLexicalCast(upper_str); - } - - - // Get joint effort limit - const char* effort_str = config->Attribute("effort"); - if (effort_str) - { - joint.m_effortLimit = urdfLexicalCast(effort_str); - } - - // Get joint velocity limit - const char* velocity_str = config->Attribute("velocity"); - if (velocity_str) - { - joint.m_velocityLimit = urdfLexicalCast(velocity_str); - } + if (m_parseSDF) + { + TiXmlElement *lower_xml = config->FirstChildElement("lower"); + if (lower_xml) { + joint.m_lowerLimit = urdfLexicalCast(lower_xml->GetText()); + } + + TiXmlElement *upper_xml = config->FirstChildElement("upper"); + if (upper_xml) { + joint.m_upperLimit = urdfLexicalCast(upper_xml->GetText()); + } + + TiXmlElement *effort_xml = config->FirstChildElement("effort"); + if (effort_xml) { + joint.m_effortLimit = urdfLexicalCast(effort_xml->GetText()); + } + + TiXmlElement *velocity_xml = config->FirstChildElement("velocity"); + if (velocity_xml) { + joint.m_velocityLimit = urdfLexicalCast(velocity_xml->GetText()); + } + } + else + { + const char* lower_str = config->Attribute("lower"); + if (lower_str) + { + joint.m_lowerLimit = urdfLexicalCast(lower_str); + } + + const char* upper_str = config->Attribute("upper"); + if (upper_str) + { + joint.m_upperLimit = urdfLexicalCast(upper_str); + } + + + // Get joint effort limit + const char* effort_str = config->Attribute("effort"); + if (effort_str) + { + joint.m_effortLimit = urdfLexicalCast(effort_str); + } + + // Get joint velocity limit + const char* velocity_str = config->Attribute("velocity"); + if (velocity_str) + { + joint.m_velocityLimit = urdfLexicalCast(velocity_str); + } + } return true; } + +bool UrdfParser::parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger) +{ + joint.m_jointDamping = 0; + joint.m_jointFriction = 0; + + if (m_parseSDF) { + TiXmlElement *damping_xml = config->FirstChildElement("damping"); + if (damping_xml) { + joint.m_jointDamping = urdfLexicalCast(damping_xml->GetText()); + } + + TiXmlElement *friction_xml = config->FirstChildElement("friction"); + if (friction_xml) { + joint.m_jointFriction = urdfLexicalCast(friction_xml->GetText()); + } + + if (damping_xml == NULL && friction_xml == NULL) + { + logger->reportError("joint dynamics element specified with no damping and no friction"); + return false; + } + } + else + { + // Get joint damping + const char* damping_str = config->Attribute("damping"); + if (damping_str) + { + joint.m_jointDamping = urdfLexicalCast(damping_str); + } + + // Get joint friction + const char* friction_str = config->Attribute("friction"); + if (friction_str) + { + joint.m_jointFriction = urdfLexicalCast(friction_str); + } + + if (damping_str == NULL && friction_str == NULL) + { + logger->reportError("joint dynamics element specified with no damping and no friction"); + return false; + } + } + + return true; +} + bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* logger) { @@ -611,34 +750,48 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* TiXmlElement *parent_xml = config->FirstChildElement("parent"); if (parent_xml) { - const char *pname = parent_xml->Attribute("link"); - if (!pname) - { - logger->reportError("no parent link name specified for Joint link. this might be the root?"); - logger->reportError(joint.m_name.c_str()); - return false; - } - else - { - joint.m_parentLinkName = std::string(pname); - } + if (m_parseSDF) + { + joint.m_parentLinkName = std::string(parent_xml->GetText()); + } + else + { + const char *pname = parent_xml->Attribute("link"); + if (!pname) + { + logger->reportError("no parent link name specified for Joint link. this might be the root?"); + logger->reportError(joint.m_name.c_str()); + return false; + } + else + { + joint.m_parentLinkName = std::string(pname); + } + } } - + // Get Child Link TiXmlElement *child_xml = config->FirstChildElement("child"); if (child_xml) { - const char *pname = child_xml->Attribute("link"); - if (!pname) - { - logger->reportError("no child link name specified for Joint link [%s]."); - logger->reportError(joint.m_name.c_str()); - return false; - } - else - { - joint.m_childLinkName = std::string(pname); - } + if (m_parseSDF) + { + joint.m_childLinkName = std::string(child_xml->GetText()); + } + else + { + const char *pname = child_xml->Attribute("link"); + if (!pname) + { + logger->reportError("no child link name specified for Joint link [%s]."); + logger->reportError(joint.m_name.c_str()); + return false; + } + else + { + joint.m_childLinkName = std::string(pname); + } + } } // Get Joint type @@ -672,83 +825,146 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, TiXmlElement *config, ErrorLogger* return false; } - // Get Joint Axis - if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) - { - // axis - TiXmlElement *axis_xml = config->FirstChildElement("axis"); - if (!axis_xml){ - logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); - logger->reportWarning(joint.m_name.c_str()); - joint.m_localJointAxis.setValue(1,0,0); - } - else{ - if (axis_xml->Attribute("xyz")) - { - if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger)) - { - logger->reportError("Malformed axis element:"); - logger->reportError(joint.m_name.c_str()); - logger->reportError(" for joint:"); - logger->reportError(axis_xml->Attribute("xyz")); - return false; - } - } - } - } - - // Get limit - TiXmlElement *limit_xml = config->FirstChildElement("limit"); - if (limit_xml) - { - if (!parseJointLimits(joint, limit_xml,logger)) - { - logger->reportError("Could not parse limit element for joint:"); - logger->reportError(joint.m_name.c_str()); - return false; - } - } - else if (joint.m_type == URDFRevoluteJoint) - { - logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); - logger->reportError(joint.m_name.c_str()); - return false; - } - else if (joint.m_type == URDFPrismaticJoint) - { - logger->reportError("Joint is of type PRISMATIC without limits"); - logger->reportError( joint.m_name.c_str()); - return false; - } - - joint.m_jointDamping = 0; - joint.m_jointFriction = 0; - - // Get Dynamics - TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); - if (prop_xml) - { - - // Get joint damping - const char* damping_str = prop_xml->Attribute("damping"); - if (damping_str) - { - joint.m_jointDamping = urdfLexicalCast(damping_str); - } - - // Get joint friction - const char* friction_str = prop_xml->Attribute("friction"); - if (friction_str) - { - joint.m_jointFriction = urdfLexicalCast(friction_str); - } - - if (damping_str == NULL && friction_str == NULL) - { - logger->reportError("joint dynamics element specified with no damping and no friction"); - return false; - } - } + if (m_parseSDF) + { + if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) + { + // axis + TiXmlElement *axis_xml = config->FirstChildElement("axis"); + if (!axis_xml){ + logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); + logger->reportWarning(joint.m_name.c_str()); + joint.m_localJointAxis.setValue(1,0,0); + } + else{ + TiXmlElement *xyz_xml = axis_xml->FirstChildElement("xyz"); + if (xyz_xml) { + if (!parseVector3(joint.m_localJointAxis,std::string(xyz_xml->GetText()),logger)) + { + logger->reportError("Malformed axis element:"); + logger->reportError(joint.m_name.c_str()); + logger->reportError(" for joint:"); + logger->reportError(xyz_xml->GetText()); + return false; + } + } + + TiXmlElement *limit_xml = axis_xml->FirstChildElement("limit"); + if (limit_xml) + { + if (!parseJointLimits(joint, limit_xml,logger)) + { + logger->reportError("Could not parse limit element for joint:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } + else if (joint.m_type == URDFRevoluteJoint) + { + logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); + logger->reportError(joint.m_name.c_str()); + return false; + } + else if (joint.m_type == URDFPrismaticJoint) + { + logger->reportError("Joint is of type PRISMATIC without limits"); + logger->reportError( joint.m_name.c_str()); + return false; + } + + TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + if (prop_xml) + { + if (!parseJointDynamics(joint, prop_xml,logger)) + { + logger->reportError("Could not parse dynamics element for joint:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } + } + } + } + else + { + // Get Joint Axis + if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint) + { + // axis + TiXmlElement *axis_xml = config->FirstChildElement("axis"); + if (!axis_xml){ + logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis"); + logger->reportWarning(joint.m_name.c_str()); + joint.m_localJointAxis.setValue(1,0,0); + } + else{ + if (axis_xml->Attribute("xyz")) + { + if (!parseVector3(joint.m_localJointAxis,axis_xml->Attribute("xyz"),logger)) + { + logger->reportError("Malformed axis element:"); + logger->reportError(joint.m_name.c_str()); + logger->reportError(" for joint:"); + logger->reportError(axis_xml->Attribute("xyz")); + return false; + } + } + } + } + + // Get limit + TiXmlElement *limit_xml = config->FirstChildElement("limit"); + if (limit_xml) + { + if (!parseJointLimits(joint, limit_xml,logger)) + { + logger->reportError("Could not parse limit element for joint:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } + else if (joint.m_type == URDFRevoluteJoint) + { + logger->reportError("Joint is of type REVOLUTE but it does not specify limits"); + logger->reportError(joint.m_name.c_str()); + return false; + } + else if (joint.m_type == URDFPrismaticJoint) + { + logger->reportError("Joint is of type PRISMATIC without limits"); + logger->reportError( joint.m_name.c_str()); + return false; + } + + joint.m_jointDamping = 0; + joint.m_jointFriction = 0; + + // Get Dynamics + TiXmlElement *prop_xml = config->FirstChildElement("dynamics"); + if (prop_xml) + { + + // Get joint damping + const char* damping_str = prop_xml->Attribute("damping"); + if (damping_str) + { + joint.m_jointDamping = urdfLexicalCast(damping_str); + } + + // Get joint friction + const char* friction_str = prop_xml->Attribute("friction"); + if (friction_str) + { + joint.m_jointFriction = urdfLexicalCast(friction_str); + } + + if (damping_str == NULL && friction_str == NULL) + { + logger->reportError("joint dynamics element specified with no damping and no friction"); + return false; + } + } + } return true; } @@ -801,6 +1017,10 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger) parentLink->m_childJoints.push_back(joint); parentLink->m_childLinks.push_back(childLink); parentLinkTree.insert(childLink->m_name.c_str(),parentLink->m_name.c_str()); + + if (m_parseSDF) { + joint->m_parentLinkToJointTransform = childLink->m_parentLinktoLinkTransform; + } } } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index b966c2761..62897257e 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -88,6 +88,7 @@ struct UrdfLink { std::string m_name; UrdfInertia m_inertia; + btTransform m_parentLinktoLinkTransform; btArray m_visualArray; btArray m_collisionArray; UrdfLink* m_parentLink; @@ -155,6 +156,7 @@ protected: bool initTreeAndRoot(UrdfModel& model, ErrorLogger* logger); bool parseMaterial(UrdfMaterial& material, class TiXmlElement *config, ErrorLogger* logger); bool parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger); + bool parseJointDynamics(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger); bool parseJoint(UrdfJoint& link, TiXmlElement *config, ErrorLogger* logger); bool parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger);