From d7ef9d50df9e1b7abd009d700496e16dd6554936 Mon Sep 17 00:00:00 2001 From: Benjamin Navarro Date: Sat, 2 May 2020 11:13:41 +0200 Subject: [PATCH] [parsers] URDF and YAML generation from ParserResult The generated URDF/YAML models must be read with transformInertia = false becausethere is no way to recover the original rotation from the available data --- src/parsers/CMakeLists.txt | 14 +- src/parsers/RBDyn/parsers/urdf.h | 2 + src/parsers/RBDyn/parsers/yaml.h | 2 + src/parsers/to_urdf.cpp | 364 +++++++++++++++++++++++ src/parsers/to_yaml.cpp | 361 +++++++++++++++++++++++ src/parsers/urdf_yaml_conv.cpp | 141 +++++++++ src/parsers/yaml.cpp | 4 +- tests/CMakeLists.txt | 2 + tests/ParsersTestUtils.h | 487 +++++++++++++++++++++++++++++++ tests/URDFOutputTest.cpp | 90 ++++++ tests/URDFParserTest.cpp | 224 +------------- tests/YAMLOutputTest.cpp | 90 ++++++ tests/YAMLParserTest.cpp | 273 +---------------- 13 files changed, 1560 insertions(+), 494 deletions(-) create mode 100644 src/parsers/to_urdf.cpp create mode 100644 src/parsers/to_yaml.cpp create mode 100644 src/parsers/urdf_yaml_conv.cpp create mode 100644 tests/ParsersTestUtils.h create mode 100644 tests/URDFOutputTest.cpp create mode 100644 tests/YAMLOutputTest.cpp diff --git a/src/parsers/CMakeLists.txt b/src/parsers/CMakeLists.txt index 569102ec..9444cc03 100644 --- a/src/parsers/CMakeLists.txt +++ b/src/parsers/CMakeLists.txt @@ -19,7 +19,7 @@ endif() # CMake <= 3.5.0 needs at least one component to define Boost::boost add_project_dependency(Boost REQUIRED COMPONENTS system) -set(SOURCES common.cpp urdf.cpp yaml.cpp) +set(SOURCES common.cpp urdf.cpp to_urdf.cpp yaml.cpp to_yaml.cpp) set(HEADERS RBDyn/parsers/api.h RBDyn/parsers/common.h RBDyn/parsers/urdf.h RBDyn/parsers/yaml.h) add_library(RBDynParsers SHARED ${SOURCES} ${HEADERS}) @@ -39,3 +39,15 @@ install( RUNTIME DESTINATION "bin" ) install(FILES ${HEADERS} DESTINATION ${INCLUDE_INSTALL_DESTINATION}/parsers) + + +add_executable(urdf_yaml_converter urdf_yaml_conv.cpp) +target_link_libraries(urdf_yaml_converter PUBLIC RBDyn::Parsers) +set_target_properties(urdf_yaml_converter PROPERTIES COMPILE_FLAGS "-RBDYN_PARSERS_DLLIMPORT") + +install( + TARGETS urdf_yaml_converter + LIBRARY DESTINATION "lib" + ARCHIVE DESTINATION "lib" + RUNTIME DESTINATION "bin" +) \ No newline at end of file diff --git a/src/parsers/RBDyn/parsers/urdf.h b/src/parsers/RBDyn/parsers/urdf.h index 265a6879..faa8bcd3 100644 --- a/src/parsers/RBDyn/parsers/urdf.h +++ b/src/parsers/RBDyn/parsers/urdf.h @@ -67,6 +67,8 @@ RBDYN_PARSERS_DLLAPI ParserResult from_urdf_file(const std::string & file_path, bool withVirtualLinks = true, const std::string & sphericalSuffix = "_spherical"); +RBDYN_PARSERS_DLLAPI std::string to_urdf(const ParserResult & res); + } // namespace parsers } // namespace rbd diff --git a/src/parsers/RBDyn/parsers/yaml.h b/src/parsers/RBDyn/parsers/yaml.h index 9f4518bb..2ad35491 100644 --- a/src/parsers/RBDyn/parsers/yaml.h +++ b/src/parsers/RBDyn/parsers/yaml.h @@ -109,6 +109,8 @@ RBDYN_PARSERS_DLLAPI ParserResult from_yaml_file(const std::string & file_path, bool withVirtualLinks = true, const std::string & sphericalSuffix = "_spherical"); +RBDYN_PARSERS_DLLAPI std::string to_yaml(const ParserResult & res); + } // namespace parsers } // namespace rbd diff --git a/src/parsers/to_urdf.cpp b/src/parsers/to_urdf.cpp new file mode 100644 index 00000000..136d39d6 --- /dev/null +++ b/src/parsers/to_urdf.cpp @@ -0,0 +1,364 @@ +#include + +#include + +namespace rbd +{ +namespace parsers +{ + +std::string to_urdf(const ParserResult & res) +{ + using namespace tinyxml2; + + XMLDocument doc; + auto robot = doc.NewElement("robot"); + robot->SetAttribute("name", res.name.c_str()); + + auto set_vec3d = [](XMLElement * e, const char * name, Eigen::Ref xyz) { + std::stringstream ss; + Eigen::IOFormat f(Eigen::StreamPrecision, Eigen::DontAlignCols); + ss << xyz.transpose().format(f); + e->SetAttribute(name, ss.str().c_str()); + }; + + auto set_origin_from_ptransform = [&](XMLElement * node, const sva::PTransformd & X) { + const auto & xyz = X.translation(); + const auto rpy = X.rotation().transpose().eulerAngles(0, 1, 2); + if(!xyz.isZero() || !rpy.isZero()) + { + auto origin_node = doc.NewElement("origin"); + if(!xyz.isZero()) + { + set_vec3d(origin_node, "xyz", xyz); + } + if(!rpy.isZero()) + { + set_vec3d(origin_node, "rpy", rpy); + } + node->InsertEndChild(origin_node); + } + }; + + // Links + for(const auto & link : res.mb.bodies()) + { + auto node = doc.NewElement("link"); + node->SetAttribute("name", link.name().c_str()); + + // Inertial + const auto has_mass = link.inertia().mass() > 0.; + const auto has_momentum = !link.inertia().momentum().isZero(); + const auto has_inertia = !link.inertia().inertia().isZero(); + if(has_mass || has_momentum || has_inertia) + { + + auto inertial_node = doc.NewElement("inertial"); + + const auto com = [&]() -> Eigen::Vector3d { + if(link.inertia().mass() > 0.) + { + return link.inertia().momentum() / link.inertia().mass(); + } + else + { + return Eigen::Vector3d::Zero(); + } + }(); + if(!com.isZero()) + { + auto origin_node = doc.NewElement("origin"); + set_vec3d(origin_node, "xyz", com); + inertial_node->InsertEndChild(origin_node); + } + + if(link.inertia().mass() > 0.) + { + auto mass_node = doc.NewElement("mass"); + mass_node->SetAttribute("value", link.inertia().mass()); + inertial_node->InsertEndChild(mass_node); + } + + if(!link.inertia().inertia().isZero()) + { + //! Transform the inertia before writing it so that it can be later read using the 'transformInertia' parameter + //! to false (default value) + const auto inertia = sva::inertiaToOrigin(link.inertia().inertia(), -link.inertia().mass(), com, + Eigen::Matrix3d::Identity().eval()); + + auto inertia_node = doc.NewElement("inertia"); + inertia_node->SetAttribute("ixx", inertia(0, 0)); + inertia_node->SetAttribute("ixy", inertia(0, 1)); + inertia_node->SetAttribute("ixz", inertia(0, 2)); + inertia_node->SetAttribute("iyy", inertia(1, 1)); + inertia_node->SetAttribute("iyz", inertia(1, 2)); + inertia_node->SetAttribute("izz", inertia(2, 2)); + inertial_node->InsertEndChild(inertia_node); + } + + node->InsertEndChild(inertial_node); + } + + auto generate_visual = [&](const char * type, const std::map> & visuals) { + auto visuals_it = visuals.find(link.name()); + if(visuals_it != visuals.end()) + { + for(const auto & visual : visuals_it->second) + { + if(visual.geometry.type == Geometry::Type::UNKNOWN) + { + continue; + } + + auto visual_node = doc.NewElement(type); + + set_origin_from_ptransform(visual_node, visual.origin); + + auto geometry_node = doc.NewElement("geometry"); + switch(visual.geometry.type) + { + case Geometry::Type::BOX: + { + auto node = doc.NewElement("box"); + const auto box = boost::get(visual.geometry.data); + set_vec3d(node, "size", box.size); + geometry_node->InsertEndChild(node); + } + break; + case Geometry::Type::CYLINDER: + { + auto node = doc.NewElement("cylinder"); + const auto cylinder = boost::get(visual.geometry.data); + node->SetAttribute("radius", std::to_string(cylinder.radius).c_str()); + node->SetAttribute("length", std::to_string(cylinder.length).c_str()); + geometry_node->InsertEndChild(node); + } + break; + case Geometry::Type::MESH: + { + auto node = doc.NewElement("mesh"); + const auto mesh = boost::get(visual.geometry.data); + node->SetAttribute("filename", mesh.filename.c_str()); + node->SetAttribute("scale", std::to_string(mesh.scale).c_str()); + geometry_node->InsertEndChild(node); + } + break; + case Geometry::Type::SPHERE: + { + auto node = doc.NewElement("sphere"); + const auto sphere = boost::get(visual.geometry.data); + node->SetAttribute("radius", std::to_string(sphere.radius).c_str()); + geometry_node->InsertEndChild(node); + } + break; + case Geometry::Type::SUPERELLIPSOID: + { + auto node = doc.NewElement("superellipsoid"); + const auto superellipsoid = boost::get(visual.geometry.data); + set_vec3d(node, "size", superellipsoid.size); + node->SetAttribute("epsilon1", std::to_string(superellipsoid.epsilon1).c_str()); + node->SetAttribute("epsilon2", std::to_string(superellipsoid.epsilon2).c_str()); + geometry_node->InsertEndChild(node); + } + break; + case Geometry::Type::UNKNOWN: + break; + } + visual_node->InsertEndChild(geometry_node); + + node->InsertEndChild(visual_node); + } + } + }; + + generate_visual("visual", res.visual); + generate_visual("collision", res.collision); + + robot->InsertEndChild(node); + } + + auto is_continuous = [&](const rbd::Joint & joint) -> bool { + const bool has_upper_limit = res.limits.upper.count(joint.name()) > 0; + const bool has_lower_limit = res.limits.lower.count(joint.name()) > 0; + if(!has_upper_limit && !has_lower_limit) + { + return true; + } + else if(has_upper_limit && res.limits.upper.at(joint.name())[0] != std::numeric_limits::infinity()) + { + return false; + } + else if(has_lower_limit && res.limits.lower.at(joint.name())[0] != -std::numeric_limits::infinity()) + { + return false; + } + else + { + return true; + } + }; + + auto set_vector = [](XMLElement * e, const char * name, const std::vector & v) { + std::stringstream ss; + for(size_t i = 0; i < v.size(); i++) + { + ss << v[i]; + if(i != v.size() - 1) + { + ss << ' '; + } + } + e->SetAttribute(name, ss.str().c_str()); + }; + + auto has_limits = [&](const Joint & joint) { + auto check = [](const Joint & joint, const std::map> & limits) { + auto it = limits.find(joint.name()); + if(it != limits.end()) + { + return !it->second.empty(); + } + else + { + return false; + } + }; + return check(joint, res.limits.lower) && check(joint, res.limits.upper) && check(joint, res.limits.velocity) + && check(joint, res.limits.torque); + }; + + auto set_limit = [&](XMLElement * e, const Joint & joint, const char * name, + const std::map> & limits) { + auto it = limits.find(joint.name()); + if(it != limits.end()) + { + set_vector(e, name, it->second); + } + }; + + // Joints + for(const auto & joint : res.mb.joints()) + { + // Skip the root joint + if(res.mb.jointIndexByName(joint.name()) == 0) + { + continue; + } + auto node = doc.NewElement("joint"); + node->SetAttribute("name", joint.name().c_str()); + switch(joint.type()) + { + case Joint::Type::Rev: + if(is_continuous(joint)) + { + node->SetAttribute("type", "continuous"); + } + else + { + node->SetAttribute("type", "revolute"); + } + break; + case Joint::Type::Prism: + node->SetAttribute("type", "prismatic"); + break; + case Joint::Type::Spherical: + throw std::invalid_argument("URDF: Spherical is an unsupported joint type"); + break; + case Joint::Type::Planar: + node->SetAttribute("type", "planar"); + break; + case Joint::Type::Cylindrical: + throw std::invalid_argument("URDF: Cylindrical is an unsupported joint type"); + break; + case Joint::Type::Free: + node->SetAttribute("type", "floating"); + break; + case Joint::Type::Fixed: + node->SetAttribute("type", "fixed"); + break; + } + + auto index = res.mb.jointIndexByName(joint.name()); + const auto pred = res.mb.predecessor(index); + auto parent_node = doc.NewElement("parent"); + const auto & parent = [&]() { + if(pred != -1) + { + return res.mb.body(pred); + } + else + { + return res.mb.body(0); + } + }(); + parent_node->SetAttribute("link", parent.name().c_str()); + + auto succ = res.mb.successor(index); + const auto & child = res.mb.body(succ); + auto child_node = doc.NewElement("child"); + child_node->SetAttribute("link", child.name().c_str()); + + node->InsertEndChild(parent_node); + node->InsertEndChild(child_node); + + for(const auto & arc : res.mbg.nodeByName(parent.name())->arcs) + { + if(arc.joint.name() == joint.name()) + { + set_origin_from_ptransform(node, arc.X); + } + } + + auto axis_node = doc.NewElement("axis"); + Eigen::Vector3d axis = Eigen::Vector3d::Zero(); + switch(joint.type()) + { + case Joint::Type::Rev: + axis = joint.motionSubspace().col(0).head<3>(); + break; + case Joint::Type::Prism: + axis = joint.motionSubspace().col(0).tail<3>(); + break; + case Joint::Type::Spherical: + throw std::invalid_argument("URDF: Spherical is an unsupported joint type"); + break; + case Joint::Type::Planar: + axis = Eigen::Vector3d::UnitZ(); // Axis is not handled is RBDyn and defaults to Z + break; + case Joint::Type::Cylindrical: + throw std::invalid_argument("URDF: Cylindrical is an unsupported joint type"); + break; + case Joint::Type::Free: + break; + case Joint::Type::Fixed: + break; + } + if(!axis.isZero()) + { + set_vec3d(axis_node, "xyz", axis); + node->InsertEndChild(axis_node); + } + + if(has_limits(joint) && !(joint.type() == Joint::Type::Rev && is_continuous(joint))) + { + auto limit_node = doc.NewElement("limit"); + set_limit(limit_node, joint, "lower", res.limits.lower); + set_limit(limit_node, joint, "upper", res.limits.upper); + set_limit(limit_node, joint, "velocity", res.limits.velocity); + set_limit(limit_node, joint, "effort", res.limits.torque); + node->InsertEndChild(limit_node); + } + + robot->InsertEndChild(node); + } + + doc.InsertEndChild(robot); + + XMLPrinter printer; + doc.Print(&printer); + + return std::string(printer.CStr()); +} + +} // namespace parsers +} // namespace rbd diff --git a/src/parsers/to_yaml.cpp b/src/parsers/to_yaml.cpp new file mode 100644 index 00000000..68dd65f9 --- /dev/null +++ b/src/parsers/to_yaml.cpp @@ -0,0 +1,361 @@ +#include + +#include + +namespace rbd +{ +namespace parsers +{ + +std::string to_yaml(const ParserResult & res) +{ + using namespace YAML; + + Emitter doc; + + doc << BeginMap; + doc << Key << "robot" << Value << BeginMap; + doc << Key << "name" << Value << res.name; + doc << Key << "anglesInDegrees" << Value << false; + + auto set_vec3d = [&](const char * name, Eigen::Ref xyz) { + doc << Value << name << Key << Flow << BeginSeq; + doc << xyz.x() << xyz.y() << xyz.z(); + doc << EndSeq; + }; + + auto set_origin_from_ptransform = [&](const sva::PTransformd & X) { + const auto & xyz = X.translation(); + const auto rpy = X.rotation().transpose().eulerAngles(0, 1, 2); + if(!xyz.isZero() || !rpy.isZero()) + { + doc << Key << "frame" << BeginMap; + if(!xyz.isZero()) + { + set_vec3d("xyz", xyz); + } + if(!rpy.isZero()) + { + set_vec3d("rpy", rpy); + } + doc << EndMap; + } + }; + + // Links + doc << Key << "links" << Value << BeginSeq; + for(const auto & link : res.mb.bodies()) + { + doc << BeginMap; + doc << Key << "name" << Value << link.name(); + + // Inertial + const auto has_mass = link.inertia().mass() > 0.; + const auto has_momentum = !link.inertia().momentum().isZero(); + const auto has_inertia = !link.inertia().inertia().isZero(); + if(has_mass || has_momentum || has_inertia) + { + doc << Key << "inertial" << BeginMap; + + if(link.inertia().mass() > 0.) + { + doc << Key << "mass" << Value << link.inertia().mass(); + } + + const auto com = [&]() -> Eigen::Vector3d { + if(link.inertia().mass() > 0.) + { + return link.inertia().momentum() / link.inertia().mass(); + } + else + { + return Eigen::Vector3d::Zero(); + } + }(); + if(!com.isZero()) + { + doc << Key << "frame" << BeginMap; + set_vec3d("xyz", com); + doc << EndMap; + } + + if(!link.inertia().inertia().isZero()) + { + //! Transform the inertia before writing it so that it can be later read using the 'transformInertia' parameter + //! to false (default value) + const auto inertia = sva::inertiaToOrigin(link.inertia().inertia(), -link.inertia().mass(), com, + Eigen::Matrix3d::Identity().eval()); + + doc << Key << "inertia" << BeginMap; + doc << Key << "Ixx" << Value << inertia(0, 0); + doc << Key << "Iyy" << Value << inertia(1, 1); + doc << Key << "Izz" << Value << inertia(2, 2); + doc << Key << "Iyz" << Value << inertia(1, 2); + doc << Key << "Ixz" << Value << inertia(0, 2); + doc << Key << "Ixy" << Value << inertia(0, 1); + doc << EndMap; + } + + doc << EndMap; + } + + auto generate_visual = [&](const char * type, const std::map> & visuals) { + auto visuals_it = visuals.find(link.name()); + if(visuals_it != visuals.end()) + { + doc << Key << type << Value << BeginSeq; + for(const auto & visual : visuals_it->second) + { + if(visual.geometry.type == Geometry::Type::UNKNOWN) + { + continue; + } + + doc << BeginMap; + + set_origin_from_ptransform(visual.origin); + + doc << Key << "geometry" << Value << BeginMap; + + switch(visual.geometry.type) + { + case Geometry::Type::BOX: + { + doc << Key << "box" << BeginMap; + const auto box = boost::get(visual.geometry.data); + set_vec3d("size", box.size); + doc << EndMap; + } + break; + case Geometry::Type::CYLINDER: + { + doc << Key << "cylinder" << BeginMap; + const auto cylinder = boost::get(visual.geometry.data); + doc << Key << "radius" << Value << cylinder.radius; + doc << Key << "length" << Value << cylinder.length; + doc << EndMap; + } + break; + case Geometry::Type::MESH: + { + doc << Key << "mesh" << BeginMap; + const auto mesh = boost::get(visual.geometry.data); + doc << Key << "filename" << Value << mesh.filename; + doc << Key << "scale" << Value << mesh.scale; + doc << EndMap; + } + break; + case Geometry::Type::SPHERE: + { + doc << Key << "sphere" << BeginMap; + const auto sphere = boost::get(visual.geometry.data); + doc << Key << "radius" << Value << sphere.radius; + doc << EndMap; + } + break; + case Geometry::Type::SUPERELLIPSOID: + { + doc << Key << "superellipsoid" << BeginMap; + const auto superellipsoid = boost::get(visual.geometry.data); + set_vec3d("size", superellipsoid.size); + doc << Key << "epsilon1" << Value << superellipsoid.epsilon1; + doc << Key << "epsilon2" << Value << superellipsoid.epsilon2; + doc << EndMap; + } + break; + case Geometry::Type::UNKNOWN: + break; + } + + doc << EndMap << EndMap; + } + + doc << EndSeq; + } + }; + + generate_visual("visual", res.visual); + generate_visual("collision", res.collision); + + doc << EndMap; + } + doc << EndSeq; + + auto is_continuous = [&](const rbd::Joint & joint) -> bool { + const bool has_upper_limit = res.limits.upper.count(joint.name()) > 0; + const bool has_lower_limit = res.limits.lower.count(joint.name()) > 0; + if(!has_upper_limit && !has_lower_limit) + { + return true; + } + else if(has_upper_limit && res.limits.upper.at(joint.name())[0] != std::numeric_limits::infinity()) + { + return false; + } + else if(has_lower_limit && res.limits.lower.at(joint.name())[0] != -std::numeric_limits::infinity()) + { + return false; + } + else + { + return true; + } + }; + + auto set_vector = [&](const char * name, const std::vector & v) { + doc << Key << name << Value << Flow << BeginSeq << v << EndSeq; + }; + + auto has_limits = [&](const Joint & joint) { + auto check = [](const Joint & joint, const std::map> & limits) { + auto it = limits.find(joint.name()); + if(it != limits.end()) + { + return !it->second.empty(); + } + else + { + return false; + } + }; + return check(joint, res.limits.lower) && check(joint, res.limits.upper) && check(joint, res.limits.velocity) + && check(joint, res.limits.torque); + }; + + auto set_limit = [&](const Joint & joint, const char * name, + const std::map> & limits) { + auto it = limits.find(joint.name()); + if(it != limits.end()) + { + if(it->second.size() == 1) + { + doc << Key << name << Value << it->second[0]; + } + else + { + set_vector(name, it->second); + } + } + }; + + // Joints + doc << Key << "joints" << Value << BeginSeq; + for(const auto & joint : res.mb.joints()) + { + // Skip the root joint + if(res.mb.jointIndexByName(joint.name()) == 0) + { + continue; + } + + doc << BeginMap; + doc << Key << "name" << Value << joint.name(); + + switch(joint.type()) + { + case Joint::Type::Rev: + if(is_continuous(joint)) + { + doc << Key << "type" << Value << "continuous"; + } + else + { + doc << Key << "type" << Value << "revolute"; + } + break; + case Joint::Type::Prism: + doc << Key << "type" << Value << "prismatic"; + break; + case Joint::Type::Spherical: + throw std::invalid_argument("URDF: Spherical is an unsupported joint type"); + break; + case Joint::Type::Planar: + doc << Key << "type" << Value << "planar"; + break; + case Joint::Type::Cylindrical: + throw std::invalid_argument("URDF: Cylindrical is an unsupported joint type"); + break; + case Joint::Type::Free: + doc << Key << "type" << Value << "floating"; + break; + case Joint::Type::Fixed: + doc << Key << "type" << Value << "fixed"; + break; + } + + auto index = res.mb.jointIndexByName(joint.name()); + const auto pred = res.mb.predecessor(index); + const auto & parent = [&]() { + if(pred != -1) + { + return res.mb.body(pred); + } + else + { + return res.mb.body(0); + } + }(); + doc << Key << "parent" << Value << parent.name(); + + auto succ = res.mb.successor(index); + const auto & child = res.mb.body(succ); + doc << Key << "child" << Value << child.name(); + + for(const auto & arc : res.mbg.nodeByName(parent.name())->arcs) + { + if(arc.joint.name() == joint.name()) + { + set_origin_from_ptransform(arc.X); + } + } + + Eigen::Vector3d axis = Eigen::Vector3d::Zero(); + switch(joint.type()) + { + case Joint::Type::Rev: + axis = joint.motionSubspace().col(0).head<3>(); + break; + case Joint::Type::Prism: + axis = joint.motionSubspace().col(0).tail<3>(); + break; + case Joint::Type::Spherical: + throw std::invalid_argument("URDF: Spherical is an unsupported joint type"); + break; + case Joint::Type::Planar: + axis = Eigen::Vector3d::UnitZ(); // Axis is not handled is RBDyn and defaults to Z + break; + case Joint::Type::Cylindrical: + throw std::invalid_argument("URDF: Cylindrical is an unsupported joint type"); + break; + case Joint::Type::Free: + break; + case Joint::Type::Fixed: + break; + } + if(!axis.isZero()) + { + set_vec3d("axis", axis); + } + + if(has_limits(joint) && !(joint.type() == Joint::Type::Rev && is_continuous(joint))) + { + doc << Key << "limits" << Value << BeginMap; + set_limit(joint, "lower", res.limits.lower); + set_limit(joint, "upper", res.limits.upper); + set_limit(joint, "velocity", res.limits.velocity); + set_limit(joint, "effort", res.limits.torque); + doc << EndMap; + } + + doc << EndMap; + } + doc << EndSeq; // Joints + + doc << EndMap; // Robot + doc << EndMap; // Doc + + return std::string(doc.c_str()); +} + +} // namespace parsers +} // namespace rbd diff --git a/src/parsers/urdf_yaml_conv.cpp b/src/parsers/urdf_yaml_conv.cpp new file mode 100644 index 00000000..2f8c82a5 --- /dev/null +++ b/src/parsers/urdf_yaml_conv.cpp @@ -0,0 +1,141 @@ +/* + * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include +#include + +#include +#include + +std::string getContent(std::istream & in) +{ + // Without sync_with_stdio = false, in_avail always returns zero even if data is available + std::ios::sync_with_stdio(false); + const auto available_char = in.rdbuf()->in_avail(); + std::ios::sync_with_stdio(true); + if(available_char == 0) + { + return {}; + } + + std::string content; + std::string line; + while(std::getline(in, line)) + { + content += line + '\n'; + } + return content; +} + +int main(int argc, const char * argv[]) +{ + auto print_usage = [&]() { + std::cout << "Converts between URDF and YAML robot model formats. The input format is detected based on the given " + "content.\n"; + std::cout << "Usage:\n"; + std::cout << "\t" << argv[0] << ": reads from the standard input and prints to the standard output\n"; + std::cout << "\t" << argv[0] << " input_file: reads input_file and prints to the standard output\n"; + std::cout << "\t" << argv[0] << " input_file output_file: reads input_file and writes to output_file\n"; + }; + if(argc == 2 && (std::strcmp(argv[1], "-h") == 0 || std::strcmp(argv[1], "--help") == 0)) + { + print_usage(); + std::exit(0); + } + + if(argc > 3) + { + print_usage(); + std::exit(-1); + } + + const auto stdin_content = getContent(std::cin); + const auto file_input = stdin_content.size() == 0; + const auto file_output = (file_input && argc == 3) || (!file_input && argc == 2); + + if(file_input && argc == 1) + { + std::cerr << "No file given and nothing to read from the standard input" << std::endl; + print_usage(); + std::exit(-1); + } + + const auto content = [&]() -> std::string { + if(file_input) + { + std::ifstream file(argv[1]); + if(file.is_open()) + { + return getContent(file); + } + else + { + std::cerr << "Failed to read the content of " << argv[1] << std::endl; + std::exit(-1); + } + } + else + { + return stdin_content; + } + }(); + + const auto robot_pos = content.find("robot"); + if(robot_pos == std::string::npos) + { + std::cerr << "The description doesn't contain a robot node" << std::endl; + std::exit(-1); + } + + bool is_urdf{true}; + const auto parser_result = [&]() { + if((robot_pos > 0) && (content[robot_pos - 1] == '<')) + { + return rbd::parsers::from_urdf(content); + } + else if((content.size() > robot_pos + 5) && (content[robot_pos + 5] == ':')) + { + is_urdf = false; + return rbd::parsers::from_yaml(content); + } + else + { + std::cerr << "The description is neither a valid URDF or YAML one, no '<' before or ':' after 'robot'" + << std::endl; + std::exit(-1); + } + }(); + + const auto conversion_result = [&]() { + if(is_urdf) + { + return rbd::parsers::to_yaml(parser_result); + } + else + { + return rbd::parsers::to_urdf(parser_result); + } + }(); + + if(file_output) + { + const auto file_path = argv[argc - 1]; + std::ofstream file(file_path, std::ios::out); + if(file.is_open()) + { + file << conversion_result; + } + else + { + std::cerr << "Failed to open " << file_path << " for writing" << std::endl; + std::exit(-1); + } + } + else + { + std::cout << conversion_result << std::endl; + } + + return 0; +} \ No newline at end of file diff --git a/src/parsers/yaml.cpp b/src/parsers/yaml.cpp index d1ef8288..eab0fdb6 100644 --- a/src/parsers/yaml.cpp +++ b/src/parsers/yaml.cpp @@ -205,10 +205,10 @@ void RBDynFromYAML::parseInertial(const YAML::Node & inertial, Eigen::Vector3d & rpy, Eigen::Matrix3d & inertia) { - mass = 1.; + mass = 0.; xyz.setZero(); rpy.setZero(); - inertia.setIdentity(); + inertia.setZero(); if(inertial) { mass = inertial["mass"].as(mass); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 37e0900f..0cc0f75a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -51,7 +51,9 @@ addUnitTest("IntegrationTest") addUnitTest("ExpandTest") addUnitTest("CoriolisTest") addParserUnitTest("URDFParserTest") +addParserUnitTest("URDFOutputTest") addParserUnitTest("YAMLParserTest") +addParserUnitTest("YAMLOutputTest") if(${BENCHMARKS}) option(BENCHMARK_ENABLE_TESTING "Enable testing of the benchmark library." OFF) diff --git a/tests/ParsersTestUtils.h b/tests/ParsersTestUtils.h new file mode 100644 index 00000000..2e759f38 --- /dev/null +++ b/tests/ParsersTestUtils.h @@ -0,0 +1,487 @@ +/* + * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include + +constexpr double TOL = 1e-6; + +rbd::parsers::ParserResult createRobot() +{ + auto create_ptransform = [](double x, double y, double z, double rx, double ry, double rz) { + Eigen::Quaterniond q = Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) + * Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()); + + return sva::PTransformd(q.inverse(), Eigen::Vector3d(x, y, z)); + }; + + rbd::parsers::ParserResult res; + + Eigen::Matrix3d I0, I1, I2, I3, I4; + + I0 << 0.1, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.001; + + I1 << 1.35, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 1.251; + + I2 << 0.6, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.501; + + I3 << 0.475, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.376; + + I4 << 0.1, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.251; + Eigen::Affine3d T0, T1; + T0.matrix() << 1, 0, 0, .1, 0, 1, 0, .2, 0, 0, 1, .3, 0, 0, 0, 1; + T1 = Eigen::Affine3d::Identity(); + + rbd::Body b0(1., Eigen::Vector3d::Zero(), I0, "b0"); + rbd::Body b1(5., Eigen::Vector3d(0., 0.5, 0.), I1, "b1"); + rbd::Body b2(2., Eigen::Vector3d(0., 0.5, 0.), I2, "b2"); + rbd::Body b3(1.5, Eigen::Vector3d(0., 0.5, 0.), I3, "b3"); + rbd::Body b4(1., Eigen::Vector3d(0.5, 0., 0.), I4, "b4"); + + res.mbg.addBody(b0); + res.mbg.addBody(b1); + res.mbg.addBody(b2); + res.mbg.addBody(b3); + res.mbg.addBody(b4); + + rbd::Joint j0(rbd::Joint::RevX, true, "j0"); + rbd::Joint j1(rbd::Joint::RevY, true, "j1"); + rbd::Joint j2(rbd::Joint::RevZ, true, "j2"); + rbd::Joint j3(rbd::Joint::RevX, true, "j3"); + + res.mbg.addJoint(j0); + res.mbg.addJoint(j1); + res.mbg.addJoint(j2); + res.mbg.addJoint(j3); + + sva::PTransformd to(Eigen::Vector3d(0., 1., 0.)); + sva::PTransformd from(sva::PTransformd::Identity()); + + res.mbg.linkBodies("b0", to, "b1", from, "j0"); + res.mbg.linkBodies("b1", to, "b2", from, "j1"); + res.mbg.linkBodies("b2", to, "b3", from, "j2"); + res.mbg.linkBodies("b1", sva::PTransformd(sva::RotX(1.), Eigen::Vector3d(1., 0., 0.)), "b4", from, "j3"); + + res.limits.lower = {{"j0", {-1.}}, {"j1", {-1.}}, {"j2", {-1.}}}; + res.limits.upper = {{"j0", {1.}}, {"j1", {1.}}, {"j2", {1.}}}; + res.limits.velocity = {{"j0", {10.}}, {"j1", {10.}}, {"j2", {10.}}}; + res.limits.torque = {{"j0", {50.}}, {"j1", {50.}}, {"j2", {50.}}}; + + // b0 visuals + { + rbd::parsers::Visual v1, v2; + rbd::parsers::Geometry::Mesh mesh; + v1.origin = create_ptransform(0.1, 0.2, 0.3, 0, 0, 0); + mesh.filename = "test_mesh1.dae"; + v1.geometry.type = rbd::parsers::Geometry::Type::MESH; + v1.geometry.data = mesh; + + v2.origin = create_ptransform(0, 0, 0, 0, 0, 0); + mesh.filename = "test_mesh2.dae"; + v2.geometry.type = rbd::parsers::Geometry::Type::MESH; + v2.geometry.data = mesh; + + res.visual["b0"] = {v1, v2}; + } + + // b1 visual and collision + { + rbd::parsers::Visual v1, v2; + rbd::parsers::Geometry::Box box; + v1.origin = create_ptransform(0.4, 0.5, 0.6, 1, 0, 0); + box.size << 1, 2, 3; + v1.geometry.type = rbd::parsers::Geometry::Type::BOX; + v1.geometry.data = box; + + v2.origin = create_ptransform(0.4, 0.5, 0.6, 0, 1, 0); + box.size << 1, 2, 3; + v2.geometry.type = rbd::parsers::Geometry::Type::BOX; + v2.geometry.data = box; + + res.visual["b1"] = {v1}; + res.collision["b1"] = {v2}; + } + + // b2 visual + { + rbd::parsers::Visual v1; + rbd::parsers::Geometry::Cylinder cylinder; + v1.origin = create_ptransform(0.4, 0.5, 0.6, 0, 0, 1); + cylinder.radius = 1; + cylinder.length = 2; + v1.geometry.type = rbd::parsers::Geometry::Type::CYLINDER; + v1.geometry.data = cylinder; + + res.visual["b2"] = {v1}; + } + + // b3 visual + { + rbd::parsers::Visual v1; + rbd::parsers::Geometry::Sphere sphere; + v1.origin = create_ptransform(0.4, 0.5, 0.6, 1, 0, 0); + sphere.radius = 2; + v1.geometry.type = rbd::parsers::Geometry::Type::SPHERE; + v1.geometry.data = sphere; + + res.visual["b3"] = {v1}; + } + + // b4 visual + { + rbd::parsers::Visual v1; + rbd::parsers::Geometry::Superellipsoid superellipsoid; + v1.origin = create_ptransform(0.4, 0.5, 0.6, 0, 1, 0); + superellipsoid.size << 0.1, 0.2, 0.3; + superellipsoid.epsilon1 = 0.5; + superellipsoid.epsilon2 = 1; + v1.geometry.type = rbd::parsers::Geometry::Type::SUPERELLIPSOID; + v1.geometry.data = superellipsoid; + + res.visual["b4"] = {v1}; + } + + res.mb = res.mbg.makeMultiBody("b0", true); + res.mbc = rbd::MultiBodyConfig(res.mb); + res.mbc.zero(res.mb); + + return res; +} + +namespace rbd +{ + +namespace parsers +{ + +bool operator==(const Geometry::Mesh & m1, const Geometry::Mesh & m2) +{ + return m1.scale == m2.scale && m1.filename == m2.filename; +} + +bool operator==(const Geometry::Box & b1, const Geometry::Box & b2) +{ + return b1.size == b2.size; +} + +bool operator==(const Geometry::Sphere & b1, const Geometry::Sphere & b2) +{ + return b1.radius == b2.radius; +} + +bool operator==(const Geometry::Cylinder & b1, const Geometry::Cylinder & b2) +{ + return b1.radius == b2.radius && b1.length == b2.length; +} + +bool operator==(const Geometry & g1, const Geometry & g2) +{ + return g1.type == g2.type && g1.data == g2.data; +} +bool operator==(const Visual & v1, const Visual & v2) +{ + return v1.name == v2.name && v1.origin == v2.origin && v1.geometry == v2.geometry; +} + +bool operator==(const Geometry::Superellipsoid & se1, const Geometry::Superellipsoid & se2) +{ + return (se1.size == se2.size && se1.epsilon1 == se2.epsilon1 && se1.epsilon2 == se2.epsilon2); +} + +} // namespace parsers + +} // namespace rbd + +const std::string XYZSarmUrdf( + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"); + +const std::string XYZSarmYaml( + R"(robot: + name: XYZSarm + anglesInDegrees: false + links: + - name: b0 + inertial: + mass: 1 + frame: + xyz: [0, 0, 0] + rpy: [0, 0, 0] + inertia: + Ixx: 0.1 + Iyy: 0.05 + Izz: 0.001 + Iyz: 0 + Ixz: 0 + Ixy: 0 + visual: + - frame: + xyz: [0.1, 0.2, 0.3] + rpy: [0, 0, 0] + geometry: + mesh: + filename: test_mesh1.dae + - frame: + xyz: [0, 0, 0] + rpy: [0, 0, 0] + geometry: + mesh: + filename: test_mesh2.dae + - frame: + xyz: [0, 0, 0] + rpy: [0, 0, 0] + - name: b1 + inertial: + mass: 5 + frame: + xyz: [0, 0.5, 0] + rpy: [0, 0, 0] + inertia: + Ixx: 0.1 + Iyy: 0.05 + Izz: 0.001 + Iyz: 0 + Ixz: 0 + Ixy: 0 + visual: + - frame: + xyz: [0.4, 0.5, 0.6] + rpy: [1, 0, 0] + geometry: + box: + size: [1, 2, 3] + collision: + - frame: + xyz: [0.4, 0.5, 0.6] + rpy: [0, 1, 0] + geometry: + box: + size: [1, 2, 3] + - name: b2 + inertial: + mass: 2 + frame: + xyz: [0, 0.5, 0] + rpy: [0, 0, 0] + inertia: + Ixx: 0.1 + Iyy: 0.05 + Izz: 0.001 + Iyz: 0 + Ixz: 0 + Ixy: 0 + visual: + - frame: + xyz: [0.4, 0.5, 0.6] + rpy: [0, 0, 1] + geometry: + cylinder: + radius: 1 + length: 2 + - name: b3 + inertial: + mass: 1.5 + frame: + xyz: [0, 0.5, 0] + rpy: [0, 0, 0] + inertia: + Ixx: 0.1 + Iyy: 0.05 + Izz: 0.001 + Iyz: 0 + Ixz: 0 + Ixy: 0 + visual: + - frame: + xyz: [0.4, 0.5, 0.6] + rpy: [1, 0, 0] + geometry: + sphere: + radius: 2 + - name: b4 + inertial: + mass: 1 + frame: + xyz: [0.5, 0, 0] + rpy: [0, 0, 0] + inertia: + Ixx: 0.1 + Iyy: 0.05 + Izz: 0.001 + Iyz: 0 + Ixz: 0 + Ixy: 0 + visual: + - frame: + xyz: [0.4, 0.5, 0.6] + rpy: [0, 1, 0] + geometry: + superellipsoid: + size: [0.1, 0.2, 0.3] + epsilon1: 0.5 + epsilon2: 1 + joints: + - name: j0 + parent: b0 + child: b1 + type: revolute + axis: [1, 0, 0] + frame: + xyz: [0, 1, 0] + rpy: [0, 0, 0] + limits: + upper: 1 + lower: -1 + velocity: 10 + effort: 50 + - name: j1 + parent: b1 + child: b2 + type: revolute + axis: [0, 1, 0] + frame: + xyz: [0, 1, 0] + rpy: [0, 0, 0] + limits: + upper: 1 + lower: -1 + velocity: 10 + effort: 50 + - name: j2 + parent: b2 + child: b3 + type: revolute + axis: [0, 0, 1] + frame: + xyz: [0, 1, 0] + rpy: [0, 0, 0] + limits: + upper: 1 + lower: -1 + velocity: 10 + effort: 50 + - name: j3 + parent: b1 + child: b4 + type: continuous + axis: [1, 0, 0] + frame: + xyz: [1, 0, 0] + rpy: [1, 0, 0] + anglesInDegrees: false)"); diff --git a/tests/URDFOutputTest.cpp b/tests/URDFOutputTest.cpp new file mode 100644 index 00000000..da11d0cf --- /dev/null +++ b/tests/URDFOutputTest.cpp @@ -0,0 +1,90 @@ +/* + * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +// boost +#define BOOST_TEST_MODULE URDFOutputTest +#include + +// RBDyn URDF parser +#include + +#include + +// Test utilties +#include "ParsersTestUtils.h" + +BOOST_AUTO_TEST_CASE(outputTest) +{ + auto strRobot = rbd::parsers::from_urdf(XYZSarmUrdf); + auto urdf = rbd::parsers::to_urdf(strRobot); + std::cout << urdf << std::endl; + auto strRobot2 = rbd::parsers::from_urdf(urdf); + + BOOST_CHECK_EQUAL(strRobot.mb.nrBodies(), strRobot2.mb.nrBodies()); + BOOST_CHECK_EQUAL(strRobot.mb.nrJoints(), strRobot2.mb.nrJoints()); + BOOST_CHECK_EQUAL(strRobot.mb.nrParams(), strRobot2.mb.nrParams()); + BOOST_CHECK_EQUAL(strRobot.mb.nrDof(), strRobot2.mb.nrDof()); + + BOOST_CHECK(std::equal(strRobot.mb.predecessors().begin(), strRobot.mb.predecessors().end(), + strRobot2.mb.predecessors().begin())); + BOOST_CHECK( + std::equal(strRobot.mb.successors().begin(), strRobot.mb.successors().end(), strRobot2.mb.successors().begin())); + BOOST_CHECK(std::equal(strRobot.mb.parents().begin(), strRobot.mb.parents().end(), strRobot2.mb.parents().begin())); + BOOST_CHECK( + std::equal(strRobot.mb.transforms().begin(), strRobot.mb.transforms().end(), strRobot2.mb.transforms().begin())); + + BOOST_CHECK(std::equal(strRobot.limits.lower.begin(), strRobot.limits.lower.end(), strRobot2.limits.lower.begin())); + BOOST_CHECK(std::equal(strRobot.limits.upper.begin(), strRobot.limits.upper.end(), strRobot2.limits.upper.begin())); + BOOST_CHECK( + std::equal(strRobot.limits.velocity.begin(), strRobot.limits.velocity.end(), strRobot2.limits.velocity.begin())); + BOOST_CHECK( + std::equal(strRobot.limits.torque.begin(), strRobot.limits.torque.end(), strRobot2.limits.torque.begin())); + + const auto & str_visuals = strRobot.visual; + const auto & str2_visuals = strRobot2.visual; + BOOST_CHECK_EQUAL(str2_visuals.size(), str_visuals.size()); + for(const auto & g : str2_visuals) + { + BOOST_CHECK_EQUAL(g.second.size(), str_visuals.at(g.first).size()); + } + + const auto & str_collision = strRobot.collision; + const auto & str2_collision = strRobot2.collision; + BOOST_CHECK_EQUAL(str2_collision.size(), str_collision.size()); + for(const auto & g : str2_collision) + { + BOOST_CHECK_EQUAL(g.second.size(), str_collision.at(g.first).size()); + } + + for(const auto & body : strRobot.mb.bodies()) + { + BOOST_CHECK(std::equal(strRobot.visual[body.name()].begin(), strRobot.visual[body.name()].end(), + strRobot2.visual[body.name()].begin())); + BOOST_CHECK(std::equal(strRobot.collision[body.name()].begin(), strRobot.collision[body.name()].end(), + strRobot2.collision[body.name()].begin())); + } + + for(int i = 0; i < strRobot.mb.nrBodies(); ++i) + { + const auto & b1 = strRobot.mb.body(i); + const auto & b2 = strRobot2.mb.body(i); + + BOOST_CHECK_EQUAL(b1.name(), b2.name()); + + BOOST_CHECK_EQUAL(b1.inertia().mass(), b2.inertia().mass()); + BOOST_CHECK_EQUAL(b1.inertia().momentum(), b2.inertia().momentum()); + BOOST_CHECK_SMALL((b1.inertia().inertia() - b2.inertia().inertia()).norm(), TOL); + } + + for(int i = 0; i < strRobot.mb.nrJoints(); ++i) + { + const auto & j1 = strRobot.mb.joint(i); + const auto & j2 = strRobot2.mb.joint(i); + + BOOST_CHECK_EQUAL(j1.name(), j2.name()); + BOOST_CHECK_EQUAL(j1.type(), j2.type()); + BOOST_CHECK_EQUAL(j1.direction(), j2.direction()); + BOOST_CHECK_EQUAL(j1.motionSubspace(), j2.motionSubspace()); + } +} diff --git a/tests/URDFParserTest.cpp b/tests/URDFParserTest.cpp index c4c2a727..dce39188 100644 --- a/tests/URDFParserTest.cpp +++ b/tests/URDFParserTest.cpp @@ -1,5 +1,5 @@ /* - * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL + * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL */ // boost @@ -9,226 +9,8 @@ // RBDyn URDF parser #include -// XYZSarm Robot - -// b4 -// j3 | RevX -// Root j0 | j1 j2 -// ---- b0 ---- b1 ---- b2 ----b3 -// Fixed RevX RevY RevZ - -// X -// ^ -// | -// -- > Y - -std::string XYZSarmUrdf( - R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -)"); - -namespace rbd -{ - -namespace parsers -{ - -bool operator==(const Geometry::Mesh & m1, const Geometry::Mesh & m2) -{ - return m1.scale == m2.scale && m1.filename == m2.filename; -} - -bool operator==(const Geometry::Box & b1, const Geometry::Box & b2) -{ - return b1.size == b2.size; -} - -bool operator==(const Geometry::Sphere & b1, const Geometry::Sphere & b2) -{ - return b1.radius == b2.radius; -} - -bool operator==(const Geometry::Cylinder & b1, const Geometry::Cylinder & b2) -{ - return b1.radius == b2.radius && b1.length == b2.length; -} - -bool operator==(const Geometry::Superellipsoid & b1, const Geometry::Superellipsoid & b2) -{ - return b1.size == b2.size && b1.epsilon1 == b2.epsilon1 && b1.epsilon2 == b2.epsilon2; -} - -bool operator==(const Geometry & g1, const Geometry & g2) -{ - return g1.type == g2.type && g1.data == g2.data; -} -bool operator==(const Visual & v1, const Visual & v2) -{ - return v1.name == v2.name && v1.origin == v2.origin && v1.geometry == v2.geometry; -} - -} // namespace parsers - -} // namespace rbd - -rbd::parsers::ParserResult createRobot() -{ - rbd::parsers::ParserResult res; - - Eigen::Matrix3d I0, I1, I2, I3, I4; - - I0 << 0.1, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.001; - - I1 << 1.35, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 1.251; - - I2 << 0.6, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.501; - - I3 << 0.475, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.376; - - I4 << 0.1, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.251; - Eigen::Affine3d T0, T1; - T0.matrix() << 1, 0, 0, .1, 0, 1, 0, .2, 0, 0, 1, .3, 0, 0, 0, 1; - T1 = Eigen::Affine3d::Identity(); - - rbd::Body b0(1., Eigen::Vector3d::Zero(), I0, "b0"); - rbd::Body b1(5., Eigen::Vector3d(0., 0.5, 0.), I1, "b1"); - rbd::Body b2(2., Eigen::Vector3d(0., 0.5, 0.), I2, "b2"); - rbd::Body b3(1.5, Eigen::Vector3d(0., 0.5, 0.), I3, "b3"); - rbd::Body b4(1., Eigen::Vector3d(0.5, 0., 0.), I4, "b4"); - - res.mbg.addBody(b0); - res.mbg.addBody(b1); - res.mbg.addBody(b2); - res.mbg.addBody(b3); - res.mbg.addBody(b4); - - rbd::Joint j0(rbd::Joint::RevX, true, "j0"); - rbd::Joint j1(rbd::Joint::RevY, true, "j1"); - rbd::Joint j2(rbd::Joint::RevZ, true, "j2"); - rbd::Joint j3(rbd::Joint::RevX, true, "j3"); - - res.mbg.addJoint(j0); - res.mbg.addJoint(j1); - res.mbg.addJoint(j2); - res.mbg.addJoint(j3); - - sva::PTransformd to(Eigen::Vector3d(0., 1., 0.)); - sva::PTransformd from(sva::PTransformd::Identity()); - - res.mbg.linkBodies("b0", to, "b1", from, "j0"); - res.mbg.linkBodies("b1", to, "b2", from, "j1"); - res.mbg.linkBodies("b2", to, "b3", from, "j2"); - res.mbg.linkBodies("b1", sva::PTransformd(sva::RotX(1.), Eigen::Vector3d(1., 0., 0.)), "b4", from, "j3"); - - res.limits.lower = {{"j0", {-1.}}, {"j1", {-1.}}, {"j2", {-1.}}}; - res.limits.upper = {{"j0", {1.}}, {"j1", {1.}}, {"j2", {1.}}}; - res.limits.velocity = {{"j0", {10.}}, {"j1", {10.}}, {"j2", {10.}}}; - res.limits.torque = {{"j0", {50.}}, {"j1", {50.}}, {"j2", {50.}}}; - - rbd::parsers::Visual v1, v2; - rbd::parsers::Geometry::Mesh mesh; - v1.origin = sva::PTransformd(T0.rotation(), T0.translation()); - mesh.filename = "test_mesh1.dae"; - v1.geometry.type = rbd::parsers::Geometry::Type::MESH; - v1.geometry.data = mesh; - - v2.origin = sva::PTransformd(T1.rotation(), T1.translation()); - mesh.filename = "test_mesh2.dae"; - v2.geometry.type = rbd::parsers::Geometry::Type::MESH; - v2.geometry.data = mesh; - - res.visual = {{"b0", {v1, v2}}}; - - res.mb = res.mbg.makeMultiBody("b0", true); - res.mbc = rbd::MultiBodyConfig(res.mb); - res.mbc.zero(res.mb); - - return res; -} - -const double TOL = 1e-6; +// Test utilties +#include "ParsersTestUtils.h" BOOST_AUTO_TEST_CASE(loadTest) { diff --git a/tests/YAMLOutputTest.cpp b/tests/YAMLOutputTest.cpp new file mode 100644 index 00000000..592d36c2 --- /dev/null +++ b/tests/YAMLOutputTest.cpp @@ -0,0 +1,90 @@ +/* + * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +// boost +#define BOOST_TEST_MODULE YAMLOutputTest +#include + +// RBDyn YAML parser +#include + +#include + +// Test utilties +#include "ParsersTestUtils.h" + +BOOST_AUTO_TEST_CASE(outputTest) +{ + auto strRobot = rbd::parsers::from_yaml(XYZSarmYaml); + auto yaml = rbd::parsers::to_yaml(strRobot); + std::cout << yaml << std::endl; + auto strRobot2 = rbd::parsers::from_yaml(yaml); + + BOOST_CHECK_EQUAL(strRobot.mb.nrBodies(), strRobot2.mb.nrBodies()); + BOOST_CHECK_EQUAL(strRobot.mb.nrJoints(), strRobot2.mb.nrJoints()); + BOOST_CHECK_EQUAL(strRobot.mb.nrParams(), strRobot2.mb.nrParams()); + BOOST_CHECK_EQUAL(strRobot.mb.nrDof(), strRobot2.mb.nrDof()); + + BOOST_CHECK(std::equal(strRobot.mb.predecessors().begin(), strRobot.mb.predecessors().end(), + strRobot2.mb.predecessors().begin())); + BOOST_CHECK( + std::equal(strRobot.mb.successors().begin(), strRobot.mb.successors().end(), strRobot2.mb.successors().begin())); + BOOST_CHECK(std::equal(strRobot.mb.parents().begin(), strRobot.mb.parents().end(), strRobot2.mb.parents().begin())); + BOOST_CHECK( + std::equal(strRobot.mb.transforms().begin(), strRobot.mb.transforms().end(), strRobot2.mb.transforms().begin())); + + BOOST_CHECK(std::equal(strRobot.limits.lower.begin(), strRobot.limits.lower.end(), strRobot2.limits.lower.begin())); + BOOST_CHECK(std::equal(strRobot.limits.upper.begin(), strRobot.limits.upper.end(), strRobot2.limits.upper.begin())); + BOOST_CHECK( + std::equal(strRobot.limits.velocity.begin(), strRobot.limits.velocity.end(), strRobot2.limits.velocity.begin())); + BOOST_CHECK( + std::equal(strRobot.limits.torque.begin(), strRobot.limits.torque.end(), strRobot2.limits.torque.begin())); + + const auto & str_visuals = strRobot.visual; + const auto & str2_visuals = strRobot2.visual; + BOOST_CHECK_EQUAL(str2_visuals.size(), str_visuals.size()); + for(const auto & g : str2_visuals) + { + BOOST_CHECK_EQUAL(g.second.size(), str_visuals.at(g.first).size()); + } + + const auto & str_collision = strRobot.collision; + const auto & str2_collision = strRobot2.collision; + BOOST_CHECK_EQUAL(str2_collision.size(), str_collision.size()); + for(const auto & g : str2_collision) + { + BOOST_CHECK_EQUAL(g.second.size(), str_collision.at(g.first).size()); + } + + for(const auto & body : strRobot.mb.bodies()) + { + BOOST_CHECK(std::equal(strRobot.visual[body.name()].begin(), strRobot.visual[body.name()].end(), + strRobot2.visual[body.name()].begin())); + BOOST_CHECK(std::equal(strRobot.collision[body.name()].begin(), strRobot.collision[body.name()].end(), + strRobot2.collision[body.name()].begin())); + } + + for(int i = 0; i < strRobot.mb.nrBodies(); ++i) + { + const auto & b1 = strRobot.mb.body(i); + const auto & b2 = strRobot2.mb.body(i); + + BOOST_CHECK_EQUAL(b1.name(), b2.name()); + + BOOST_CHECK_EQUAL(b1.inertia().mass(), b2.inertia().mass()); + BOOST_CHECK_EQUAL(b1.inertia().momentum(), b2.inertia().momentum()); + BOOST_CHECK_SMALL((b1.inertia().inertia() - b2.inertia().inertia()).norm(), TOL); + } + + for(int i = 0; i < strRobot.mb.nrJoints(); ++i) + { + const auto & j1 = strRobot.mb.joint(i); + const auto & j2 = strRobot2.mb.joint(i); + + BOOST_CHECK_EQUAL(j1.name(), j2.name()); + BOOST_CHECK_EQUAL(j1.type(), j2.type()); + BOOST_CHECK_EQUAL(j1.direction(), j2.direction()); + BOOST_CHECK_EQUAL(j1.motionSubspace(), j2.motionSubspace()); + } +} diff --git a/tests/YAMLParserTest.cpp b/tests/YAMLParserTest.cpp index 6148449c..0c0d3105 100644 --- a/tests/YAMLParserTest.cpp +++ b/tests/YAMLParserTest.cpp @@ -1,5 +1,5 @@ /* - * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL + * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL */ #define BOOST_TEST_MODULE YAMLParserTest @@ -8,275 +8,8 @@ #include -// XYZSarm Robot - -// b4 -// j3 | RevX -// Root j0 | j1 j2 -// ---- b0 ---- b1 ---- b2 ----b3 -// Fixed RevX RevY RevZ - -// X -// ^ -// | -// -- > Y - -std::string XYZSarmYaml( - R"(robot: - name: XYZSarm - anglesInDegrees: false - links: - - name: b0 - inertial: - mass: 1 - frame: - xyz: [0, 0, 0] - rpy: [0, 0, 0] - inertia: - Ixx: 0.1 - Iyy: 0.05 - Izz: 0.001 - Iyz: 0 - Ixz: 0 - Ixy: 0 - visual: - - frame: - xyz: [0.1, 0.2, 0.3] - rpy: [0, 0, 0] - geometry: - mesh: - filename: test_mesh1.dae - - frame: - xyz: [0, 0, 0] - rpy: [0, 0, 0] - geometry: - mesh: - filename: test_mesh2.dae - - frame: - xyz: [0, 0, 0] - rpy: [0, 0, 0] - - name: b1 - inertial: - mass: 5 - frame: - xyz: [0, 0.5, 0] - rpy: [0, 0, 0] - inertia: - Ixx: 0.1 - Iyy: 0.05 - Izz: 0.001 - Iyz: 0 - Ixz: 0 - Ixy: 0 - - name: b2 - inertial: - mass: 2 - frame: - xyz: [0, 0.5, 0] - rpy: [0, 0, 0] - inertia: - Ixx: 0.1 - Iyy: 0.05 - Izz: 0.001 - Iyz: 0 - Ixz: 0 - Ixy: 0 - - name: b3 - inertial: - mass: 1.5 - frame: - xyz: [0, 0.5, 0] - rpy: [0, 0, 0] - inertia: - Ixx: 0.1 - Iyy: 0.05 - Izz: 0.001 - Iyz: 0 - Ixz: 0 - Ixy: 0 - - name: b4 - inertial: - mass: 1 - frame: - xyz: [0.5, 0, 0] - rpy: [0, 0, 0] - inertia: - Ixx: 0.1 - Iyy: 0.05 - Izz: 0.001 - Iyz: 0 - Ixz: 0 - Ixy: 0 - joints: - - name: j0 - parent: b0 - child: b1 - type: revolute - axis: [1, 0, 0] - frame: - xyz: [0, 1, 0] - rpy: [0, 0, 0] - limits: - upper: 1 - lower: -1 - velocity: 10 - effort: 50 - - name: j1 - parent: b1 - child: b2 - type: revolute - axis: [0, 1, 0] - frame: - xyz: [0, 1, 0] - rpy: [0, 0, 0] - limits: - upper: 1 - lower: -1 - velocity: 10 - effort: 50 - - name: j2 - parent: b2 - child: b3 - type: revolute - axis: [0, 0, 1] - frame: - xyz: [0, 1, 0] - rpy: [0, 0, 0] - limits: - upper: 1 - lower: -1 - velocity: 10 - effort: 50 - - name: j3 - parent: b1 - child: b4 - type: continuous - axis: [1, 0, 0] - frame: - xyz: [1, 0, 0] - rpy: [1, 0, 0] - anglesInDegrees: false)"); - -namespace rbd -{ - -namespace parsers -{ - -bool operator==(const Geometry::Mesh & m1, const Geometry::Mesh & m2) -{ - return m1.scale == m2.scale && m1.filename == m2.filename; -} - -bool operator==(const Geometry::Box & b1, const Geometry::Box & b2) -{ - return b1.size == b2.size; -} - -bool operator==(const Geometry::Sphere & b1, const Geometry::Sphere & b2) -{ - return b1.radius == b2.radius; -} - -bool operator==(const Geometry::Cylinder & b1, const Geometry::Cylinder & b2) -{ - return b1.radius == b2.radius && b1.length == b2.length; -} - -bool operator==(const Geometry & g1, const Geometry & g2) -{ - return g1.type == g2.type && g1.data == g2.data; -} -bool operator==(const Visual & v1, const Visual & v2) -{ - return v1.name == v2.name && v1.origin == v2.origin && v1.geometry == v2.geometry; -} - -bool operator==(const Geometry::Superellipsoid & se1, const Geometry::Superellipsoid & se2) -{ - return (se1.size == se2.size && se1.epsilon1 == se2.epsilon1 && se1.epsilon2 == se2.epsilon2); -} - -} // namespace parsers - -} // namespace rbd - -rbd::parsers::ParserResult createRobot() -{ - rbd::parsers::ParserResult res; - - Eigen::Matrix3d I0, I1, I2, I3, I4; - - I0 << 0.1, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.001; - - I1 << 1.35, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 1.251; - - I2 << 0.6, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.501; - - I3 << 0.475, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.376; - - I4 << 0.1, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.251; - Eigen::Affine3d T0, T1; - T0.matrix() << 1, 0, 0, .1, 0, 1, 0, .2, 0, 0, 1, .3, 0, 0, 0, 1; - T1 = Eigen::Affine3d::Identity(); - - rbd::Body b0(1., Eigen::Vector3d::Zero(), I0, "b0"); - rbd::Body b1(5., Eigen::Vector3d(0., 0.5, 0.), I1, "b1"); - rbd::Body b2(2., Eigen::Vector3d(0., 0.5, 0.), I2, "b2"); - rbd::Body b3(1.5, Eigen::Vector3d(0., 0.5, 0.), I3, "b3"); - rbd::Body b4(1., Eigen::Vector3d(0.5, 0., 0.), I4, "b4"); - - res.mbg.addBody(b0); - res.mbg.addBody(b1); - res.mbg.addBody(b2); - res.mbg.addBody(b3); - res.mbg.addBody(b4); - - rbd::Joint j0(rbd::Joint::RevX, true, "j0"); - rbd::Joint j1(rbd::Joint::RevY, true, "j1"); - rbd::Joint j2(rbd::Joint::RevZ, true, "j2"); - rbd::Joint j3(rbd::Joint::RevX, true, "j3"); - - res.mbg.addJoint(j0); - res.mbg.addJoint(j1); - res.mbg.addJoint(j2); - res.mbg.addJoint(j3); - - sva::PTransformd to(Eigen::Vector3d(0., 1., 0.)); - sva::PTransformd from(sva::PTransformd::Identity()); - - res.mbg.linkBodies("b0", to, "b1", from, "j0"); - res.mbg.linkBodies("b1", to, "b2", from, "j1"); - res.mbg.linkBodies("b2", to, "b3", from, "j2"); - res.mbg.linkBodies("b1", sva::PTransformd(sva::RotX(1.), Eigen::Vector3d(1., 0., 0.)), "b4", from, "j3"); - - res.limits.lower = {{"j0", {-1.}}, {"j1", {-1.}}, {"j2", {-1.}}}; - res.limits.upper = {{"j0", {1.}}, {"j1", {1.}}, {"j2", {1.}}}; - res.limits.velocity = {{"j0", {10.}}, {"j1", {10.}}, {"j2", {10.}}}; - res.limits.torque = {{"j0", {50.}}, {"j1", {50.}}, {"j2", {50.}}}; - - rbd::parsers::Visual v1, v2; - rbd::parsers::Geometry::Mesh mesh; - v1.origin = sva::PTransformd(T0.rotation(), T0.translation()); - mesh.filename = "test_mesh1.dae"; - v1.geometry.type = rbd::parsers::Geometry::Type::MESH; - v1.geometry.data = mesh; - - v2.origin = sva::PTransformd(T1.rotation(), T1.translation()); - mesh.filename = "test_mesh2.dae"; - v2.geometry.type = rbd::parsers::Geometry::Type::MESH; - v2.geometry.data = mesh; - - res.visual = {{"b0", {v1, v2}}}; - - res.mb = res.mbg.makeMultiBody("b0", true); - res.mbc = rbd::MultiBodyConfig(res.mb); - res.mbc.zero(res.mb); - - return res; -} - -const double TOL = 1e-6; +// Test utilties +#include "ParsersTestUtils.h" BOOST_AUTO_TEST_CASE(loadTest) {