diff --git a/CHANGELOG.md b/CHANGELOG.md index c677ebc0d0..3ce04be50d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Default visualizer can be changed with `PINOCCHIO_VIEWER` environment variable ([#2419](https://github.com/stack-of-tasks/pinocchio/pull/2419)) - Add more Python and C++ examples related to inverse kinematics with 3d tasks ([#2428](https://github.com/stack-of-tasks/pinocchio/pull/2428)) +- Add parsing of equality/connect tag for closed-loop chains for MJCF format ([#2413](https://github.com/stack-of-tasks/pinocchio/pull/2413)) ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) diff --git a/include/pinocchio/parsers/mjcf.hpp b/include/pinocchio/parsers/mjcf.hpp index 303772561f..cb80988713 100644 --- a/include/pinocchio/parsers/mjcf.hpp +++ b/include/pinocchio/parsers/mjcf.hpp @@ -7,6 +7,9 @@ #include "pinocchio/multibody/model.hpp" #include "pinocchio/parsers/urdf.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/multibody/geometry.hpp" +#include "pinocchio/algorithm/contact-info.hpp" namespace pinocchio { @@ -75,6 +78,38 @@ namespace pinocchio ModelTpl & model, const bool verbose = false); + template class JointCollectionTpl> + ModelTpl & buildModelFromXML( + const std::string & xmlStream, + const typename ModelTpl::JointModel & rootJoint, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose = false); + + // TODO: update description, buildModel with contact model + template class JointCollectionTpl> + ModelTpl & buildModel( + const std::string & filename, + const typename ModelTpl::JointModel & rootJoint, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose = false); + + template class JointCollectionTpl> + ModelTpl & buildModelFromXML( + const std::string & xmlStream, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose = false); + + // TODO: update description, buildModel with contact model + template class JointCollectionTpl> + ModelTpl & buildModel( + const std::string & filename, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose = false); + /** * @brief Build The GeometryModel from a Mjcf file * diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index 8498d8c520..ca2667f06f 100644 --- a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp +++ b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp @@ -8,7 +8,7 @@ #include "pinocchio/parsers/urdf.hpp" #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/joint/joints.hpp" - +#include "pinocchio/algorithm/contact-info.hpp" #include #include #include @@ -21,6 +21,7 @@ #include #include #include +#include namespace pinocchio { @@ -325,6 +326,55 @@ namespace pinocchio void goThroughElement(const ptree & el, const MjcfGraph & currentGraph); }; + /* + typedef struct mjsEquality_ { // equality specification + mjsElement* element; // element type + mjString* name; // name + mjtEq type; // constraint type + double data[mjNEQDATA]; // type-dependent data + mjtByte active; // is equality initially active + mjString* name1; // name of object 1 + mjString* name2; // name of object 2 + mjtNum solref[mjNREF]; // solver reference + mjtNum solimp[mjNIMP]; // solver impedance + mjString* info; // message appended to errors + } mjsEquality; + */ + struct PINOCCHIO_PARSERS_DLLAPI MjcfEquality + { + typedef boost::property_tree::ptree ptree; + + // Optional name of the equality constraint + std::string name; + + // Type of the constraint: (connect for now) + std::string type; + + // // Optional class for setting unspecified attributes + // std::string class; + + // active: 'true' or 'false' (default: 'true') + // solref and solimp + + // Name of the first body participating in the constraint + std::string body1; + // Name of the second body participating in the constraint (optional, default: world) + std::string body2; + + // Coordinates of the 3D anchor point where the two bodies are connected. + // Specified relative to the local coordinate frame of the first body. + Eigen::Vector3d anchor = Eigen::Vector3d::Zero(); + + // TODO: implement when weld is introduced + // This attribute specifies the relative pose (3D position followed by 4D quaternion + // orientation) of body2 relative to body1. If the quaternion part (i.e., last 4 components + // of the vector) are all zeros, as in the default setting, this attribute is ignored and + // the relative pose is the one corresponding to the model reference pose in qpos0. The + // unusual default is because all equality constraint types share the same default for their + // numeric parameters. + // Eigen::VectorXd relativePose = Eigen::VectorXd::Zero(7); + }; + /// @brief The graph which contains all information taken from the mjcf file struct PINOCCHIO_PARSERS_DLLAPI MjcfGraph { @@ -337,6 +387,7 @@ namespace pinocchio typedef std::unordered_map MeshMap_t; typedef std::unordered_map TextureMap_t; typedef std::unordered_map ConfigMap_t; + typedef std::map EqualityMap_t; // Compiler Info needed to properly parse the rest of file MjcfCompiler compilerInfo; @@ -352,6 +403,8 @@ namespace pinocchio TextureMap_t mapOfTextures; // Map of model configurations ConfigMap_t mapOfConfigs; + // Map of equality constraints + EqualityMap_t mapOfEqualities; // reference configuration Eigen::VectorXd referenceConfig; @@ -428,6 +481,10 @@ namespace pinocchio /// @param el ptree keyframe node void parseKeyFrame(const ptree & el); + /// @brief Parse all the info from the equality tag + /// @param el ptree equality node + void parseEquality(const ptree & el); + /// @brief parse the mjcf file into a graph void parseGraph(); @@ -469,6 +526,13 @@ namespace pinocchio /// @param keyName Name of the keyframe void addKeyFrame(const Eigen::VectorXd & keyframe, const std::string & keyName); + /// @brief Parse the equality constraints and add them to the model + /// @param model Model to add the constraints to + /// @param contact_models Vector of contact models to add the constraints to + void parseContactInformation( + const Model & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models); + /// @brief Fill geometry model with all the info taken from the mjcf model file /// @param type Type of geometry to parse (COLLISION or VISUAL) /// @param geomModel geometry model to fill diff --git a/include/pinocchio/parsers/mjcf/model.hxx b/include/pinocchio/parsers/mjcf/model.hxx index 6339343e3d..d9c8739e27 100644 --- a/include/pinocchio/parsers/mjcf/model.hxx +++ b/include/pinocchio/parsers/mjcf/model.hxx @@ -7,6 +7,8 @@ #include "pinocchio/parsers/mjcf.hpp" #include "pinocchio/parsers/mjcf/mjcf-graph.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/algorithm/contact-info.hpp" namespace pinocchio { @@ -37,12 +39,46 @@ namespace pinocchio graph.parseGraphFromXML(xmlStr); - // // Use the Mjcf graph to create the model + // Use the Mjcf graph to create the model graph.parseRootTree(); return model; } + template class JointCollectionTpl> + ModelTpl & buildModel( + const std::string & filename, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose) + { + return buildModelFromXML(filename, model, contact_models, verbose); + } + + template class JointCollectionTpl> + ModelTpl & buildModelFromXML( + const std::string & xmlStr, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose) + { + ::pinocchio::urdf::details::UrdfVisitor visitor(model); + + typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph; + + MjcfGraph graph(visitor, xmlStr); + if (verbose) + visitor.log = &std::cout; + + graph.parseGraphFromXML(xmlStr); + + // Use the Mjcf graph to create the model + graph.parseRootTree(); + graph.parseContactInformation(model, contact_models); + + return model; + } + template class JointCollectionTpl> ModelTpl & buildModel( const std::string & filename, @@ -71,8 +107,45 @@ namespace pinocchio graph.parseGraphFromXML(xmlStr); - // // Use the Mjcf graph to create the model + // Use the Mjcf graph to create the model + graph.parseRootTree(); + + return model; + } + + template class JointCollectionTpl> + ModelTpl & buildModel( + const std::string & filename, + const typename ModelTpl::JointModel & rootJoint, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose) + { + return buildModelFromXML(filename, rootJoint, model, contact_models, verbose); + } + + template class JointCollectionTpl> + ModelTpl & buildModelFromXML( + const std::string & xmlStr, + const typename ModelTpl::JointModel & rootJoint, + ModelTpl & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models, + const bool verbose) + { + ::pinocchio::urdf::details::UrdfVisitorWithRootJoint + visitor(model, rootJoint); + + typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph; + + MjcfGraph graph(visitor, xmlStr); + if (verbose) + visitor.log = &std::cout; + + graph.parseGraphFromXML(xmlStr); + + // Use the Mjcf graph to create the model graph.parseRootTree(); + graph.parseContactInformation(model, contact_models); return model; } diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index ae60fabae9..cb40a0b4ef 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -3,6 +3,8 @@ // #include "pinocchio/parsers/mjcf/mjcf-graph.hpp" +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/algorithm/contact-info.hpp" namespace pinocchio { @@ -735,6 +737,53 @@ namespace pinocchio } } + void MjcfGraph::parseEquality(const ptree & el) + { + for (const ptree::value_type & v : el) + { + std::string type = v.first; + // List of supported constraints from mjcf description + // equality -> connect + + // The constraints below are not supported and will be ignored with the following + // warning: joint, flex, distance, weld + if (type != "connect") + { + // TODO(jcarpent): support extra constraint types such as joint, flex, distance, weld. + continue; + } + + MjcfEquality eq; + eq.type = type; + + // get the name of first body + auto body1 = v.second.get_optional(".body1"); + if (body1) + eq.body1 = *body1; + else + throw std::invalid_argument("Equality constraint needs a first body"); + + // get the name of second body + auto body2 = v.second.get_optional(".body2"); + if (body2) + eq.body2 = *body2; + + // get the name of the constraint (if it exists) + auto name = v.second.get_optional(".name"); + if (name) + eq.name = *name; + else + eq.name = eq.body1 + "_" + eq.body2 + "_constraint"; + + // get the anchor position + auto anchor = v.second.get_optional(".anchor"); + if (anchor) + eq.anchor = internal::getVectorFromStream<3>(*anchor); + + mapOfEqualities.insert(std::make_pair(eq.name, eq)); + } + } + void MjcfGraph::parseGraph() { boost::property_tree::ptree el; @@ -772,6 +821,11 @@ namespace pinocchio boost::optional childClass; parseJointAndBody(el.get_child("worldbody").get_child("body"), childClass); } + + if (v.first == "equality") + { + parseEquality(el.get_child("equality")); + } } } @@ -1040,6 +1094,37 @@ namespace pinocchio throw std::invalid_argument("Keyframe size does not match model size"); } + void MjcfGraph::parseContactInformation( + const Model & model, + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models) + { + for (const auto & entry : mapOfEqualities) + { + const MjcfEquality & eq = entry.second; + + SE3 jointPlacement; + jointPlacement.setIdentity(); + jointPlacement.translation() = eq.anchor; + + // Get Joint Indices from the model + const JointIndex body1 = urdfVisitor.getParentId(eq.body1); + + // when body2 is not specified, we link to the world + if (eq.body2 == "") + { + RigidConstraintModel rcm(CONTACT_3D, model, body1, jointPlacement, LOCAL); + contact_models.push_back(rcm); + } + else + { + const JointIndex body2 = urdfVisitor.getParentId(eq.body2); + RigidConstraintModel rcm( + CONTACT_3D, model, body1, jointPlacement, body2, jointPlacement.inverse(), LOCAL); + contact_models.push_back(rcm); + } + } + } + void MjcfGraph::parseRootTree() { urdfVisitor.setName(modelName); diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 7ef52dbd8b..9e666f1e6d 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -452,10 +452,11 @@ BOOST_AUTO_TEST_CASE(parse_default_class) typedef pinocchio::SE3::Vector3 Vector3; typedef pinocchio::SE3::Matrix3 Matrix3; + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models; std::string filename = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/test_mjcf.xml"); pinocchio::Model model_m; - pinocchio::mjcf::buildModel(filename, model_m); + pinocchio::mjcf::buildModel(filename, model_m, contact_models); std::string file_u = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/test_mjcf.urdf"); pinocchio::Model model_u; @@ -468,7 +469,7 @@ BOOST_AUTO_TEST_CASE(parse_default_class) "joint3"); model_u.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], model_u.joints[i]); } #endif // PINOCCHIO_WITH_URDFDOM @@ -581,7 +582,7 @@ BOOST_AUTO_TEST_CASE(parse_RX) idx = modelRX.addJoint(0, pinocchio::JointModelRX(), pinocchio::SE3::Identity(), "rx"); modelRX.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelRX.joints[i]); } @@ -618,7 +619,7 @@ BOOST_AUTO_TEST_CASE(parse_PX) idx = modelPX.addJoint(0, pinocchio::JointModelPX(), pinocchio::SE3::Identity(), "px"); modelPX.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelPX.joints[i]); } @@ -655,7 +656,7 @@ BOOST_AUTO_TEST_CASE(parse_Sphere) idx = modelS.addJoint(0, pinocchio::JointModelSpherical(), pinocchio::SE3::Identity(), "s"); modelS.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelS.joints[i]); } @@ -692,7 +693,7 @@ BOOST_AUTO_TEST_CASE(parse_Free) idx = modelF.addJoint(0, pinocchio::JointModelFreeFlyer(), pinocchio::SE3::Identity(), "f"); modelF.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelF.joints[i]); } @@ -734,7 +735,7 @@ BOOST_AUTO_TEST_CASE(parse_composite_RXRY) idx = modelRXRY.addJoint(0, joint_model_RXRY, pinocchio::SE3::Identity(), "rxry"); modelRXRY.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelRXRY.joints[i]); } @@ -776,7 +777,7 @@ BOOST_AUTO_TEST_CASE(parse_composite_PXPY) idx = modelPXPY.addJoint(0, joint_model_PXPY, pinocchio::SE3::Identity(), "pxpy"); modelPXPY.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelPXPY.joints[i]); } @@ -818,7 +819,7 @@ BOOST_AUTO_TEST_CASE(parse_composite_PXRY) idx = modelPXRY.addJoint(0, joint_model_PXRY, pinocchio::SE3::Identity(), "pxry"); modelPXRY.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelPXRY.joints[i]); } @@ -860,7 +861,7 @@ BOOST_AUTO_TEST_CASE(parse_composite_PXSphere) idx = modelPXSphere.addJoint(0, joint_model_PXSphere, pinocchio::SE3::Identity(), "pxsphere"); modelPXSphere.appendBodyToJoint(idx, inertia); - for (int i = 0; i < model_m.njoints; i++) + for (size_t i = 0; i < size_t(model_m.njoints); i++) BOOST_CHECK_EQUAL(model_m.joints[i], modelPXSphere.joints[i]); } @@ -1248,4 +1249,27 @@ BOOST_AUTO_TEST_CASE(test_geometry_parsing) } #endif // if defined(PINOCCHIO_WITH_HPP_FCL) +BOOST_AUTO_TEST_CASE(test_contact_parsing) +{ + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) contact_models; + std::string filename = PINOCCHIO_MODEL_DIR + std::string("/../unittest/models/closed_chain.xml"); + + pinocchio::Model model; + pinocchio::mjcf::buildModel(filename, model, contact_models); + + BOOST_CHECK_EQUAL(contact_models.size(), 4); + BOOST_CHECK_EQUAL( + contact_models[0].joint1_placement.translation(), pinocchio::SE3::Vector3(0.50120, 0, 0)); + BOOST_CHECK_EQUAL( + contact_models[1].joint1_placement.translation(), pinocchio::SE3::Vector3(0.35012, 0, 0)); + BOOST_CHECK_EQUAL( + contact_models[2].joint1_placement.translation(), pinocchio::SE3::Vector3(0.50120, 0, 0)); + BOOST_CHECK_EQUAL( + contact_models[3].joint1_placement.translation(), pinocchio::SE3::Vector3(0.35012, 0, 0)); + for (const auto & cm : contact_models) + { + BOOST_CHECK(cm.joint2_placement.isApprox(cm.joint1_placement.inverse())); + } +} + BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/models/closed_chain.xml b/unittest/models/closed_chain.xml new file mode 100644 index 0000000000..ab8bcabb0a --- /dev/null +++ b/unittest/models/closed_chain.xml @@ -0,0 +1,176 @@ + + + + diff --git a/unittest/models/test_mjcf.xml b/unittest/models/test_mjcf.xml index 734f1d65aa..e3db873cd7 100644 --- a/unittest/models/test_mjcf.xml +++ b/unittest/models/test_mjcf.xml @@ -28,4 +28,9 @@ + + + + +