From bca0782a5dbdb79ba867b24c460dc5502d7ee98f Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Tue, 10 Sep 2024 19:10:42 +0900 Subject: [PATCH 01/10] feat: create constraint struct and parse --- include/pinocchio/parsers/mjcf/mjcf-graph.hpp | 55 ++++++++++++++ include/pinocchio/parsers/mjcf/model.hxx | 2 + src/parsers/mjcf/mjcf-graph.cpp | 72 +++++++++++++++++++ unittest/models/test_mjcf.xml | 5 ++ 4 files changed, 134 insertions(+) diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index 8498d8c520..0eab872ea9 100644 --- a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp +++ b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp @@ -325,6 +325,54 @@ 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: (weld or 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(); + + // 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 +385,7 @@ namespace pinocchio typedef std::unordered_map MeshMap_t; typedef std::unordered_map TextureMap_t; typedef std::unordered_map ConfigMap_t; + typedef std::unordered_map EqualityMap_t; // Compiler Info needed to properly parse the rest of file MjcfCompiler compilerInfo; @@ -352,6 +401,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 +479,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(); diff --git a/include/pinocchio/parsers/mjcf/model.hxx b/include/pinocchio/parsers/mjcf/model.hxx index 6339343e3d..c88f625379 100644 --- a/include/pinocchio/parsers/mjcf/model.hxx +++ b/include/pinocchio/parsers/mjcf/model.hxx @@ -39,6 +39,7 @@ namespace pinocchio // // Use the Mjcf graph to create the model graph.parseRootTree(); + // TODO: insert the parse contact information here return model; } @@ -73,6 +74,7 @@ namespace pinocchio // // Use the Mjcf graph to create the model graph.parseRootTree(); + // TODO: insert the parse contact information here return model; } diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index ae60fabae9..f14722112a 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -735,6 +735,73 @@ namespace pinocchio } } + void MjcfGraph::parseEquality(const ptree & el) + { + for (const ptree::value_type & v : el) + { + // std::cout << v.first << " " << v.second << std::endl; + + std::string type = v.first; + // List of supported constraints from mjcf description + // equality -> connect + // equality -> weld + + // The constraints below are not supported and will be ignored with the following + // warning equality -> joint equality -> flex equality -> distance + if (type != "connect" && type != "weld") + { + std::cout << "Warning - Constraint " << type << " is not supported" << std::endl; + 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; + else + eq.body2 = "world"; // TODO: find out what is the right name for the world in pinocchio + + // 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); + + // get the relative position + auto relpose = v.second.get_optional(".relpose"); + if (relpose) + eq.relativePose = internal::getVectorFromStream<3>(*relpose); + + // print what constraint is being added + std::cout << "MjcfEquality: {" << std::endl; + std::cout << " Name: " << eq.name << std::endl; + std::cout << " Type: " << eq.type << std::endl; + std::cout << " Body1: " << eq.body1 << std::endl; + std::cout << " Body2: " << eq.body2 << std::endl; + std::cout << " Anchor: [" << eq.anchor.transpose() << "]" << std::endl; + std::cout << " Relative Pose: [" << eq.relativePose.transpose() << "]" << std::endl; + std::cout << "}" << std::endl; + + mapOfEqualities.insert(std::make_pair("equality", eq)); + } + } + void MjcfGraph::parseGraph() { boost::property_tree::ptree el; @@ -772,6 +839,11 @@ namespace pinocchio boost::optional childClass; parseJointAndBody(el.get_child("worldbody").get_child("body"), childClass); } + + if (v.first == "equality") + { + parseEquality(el.get_child("equality")); + } } } 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 @@ + + + + + From be847828a7d5404a213f4abf5c80871f80989cde Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Tue, 10 Sep 2024 20:11:38 +0900 Subject: [PATCH 02/10] feat: generate a bunch of functions for contact model creation with mjcf --- include/pinocchio/parsers/mjcf.hpp | 35 ++++++++++++ include/pinocchio/parsers/mjcf/model.hxx | 72 ++++++++++++++++++++++++ unittest/mjcf.cpp | 3 +- 3 files changed, 109 insertions(+), 1 deletion(-) 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/model.hxx b/include/pinocchio/parsers/mjcf/model.hxx index c88f625379..f3e2242bf7 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 { @@ -44,6 +46,40 @@ namespace pinocchio 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(); + // TODO: insert the parse contact information here + + return model; + } + template class JointCollectionTpl> ModelTpl & buildModel( const std::string & filename, @@ -72,6 +108,42 @@ namespace pinocchio graph.parseGraphFromXML(xmlStr); + // // 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(); // TODO: insert the parse contact information here diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 7ef52dbd8b..94be645d2c 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; From 43e38456f98778e72d5004d3f643a1d1c6e7759c Mon Sep 17 00:00:00 2001 From: Lev Kozlov Date: Wed, 11 Sep 2024 12:16:41 +0900 Subject: [PATCH 03/10] feat: add a simple test using reduced cassie model --- include/pinocchio/parsers/mjcf/mjcf-graph.hpp | 14 +- include/pinocchio/parsers/mjcf/model.hxx | 13 +- src/parsers/mjcf/mjcf-graph.cpp | 66 ++++--- unittest/mjcf.cpp | 11 ++ unittest/models/closed_chain.xml | 176 ++++++++++++++++++ 5 files changed, 249 insertions(+), 31 deletions(-) create mode 100644 unittest/models/closed_chain.xml diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index 0eab872ea9..8b6b76a0f1 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 @@ -346,7 +346,7 @@ namespace pinocchio // Optional name of the equality constraint std::string name; - // Type of the constraint: (weld or connect for now) + // Type of the constraint: (connect for now) std::string type; // // Optional class for setting unspecified attributes @@ -364,13 +364,14 @@ namespace pinocchio // 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); + // Eigen::VectorXd relativePose = Eigen::VectorXd::Zero(7); }; /// @brief The graph which contains all information taken from the mjcf file @@ -524,6 +525,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 + PINOCCHIO_PARSERS_DLLAPI 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 f3e2242bf7..d9c8739e27 100644 --- a/include/pinocchio/parsers/mjcf/model.hxx +++ b/include/pinocchio/parsers/mjcf/model.hxx @@ -39,9 +39,8 @@ namespace pinocchio graph.parseGraphFromXML(xmlStr); - // // Use the Mjcf graph to create the model + // Use the Mjcf graph to create the model graph.parseRootTree(); - // TODO: insert the parse contact information here return model; } @@ -73,9 +72,9 @@ namespace pinocchio graph.parseGraphFromXML(xmlStr); - // // Use the Mjcf graph to create the model + // Use the Mjcf graph to create the model graph.parseRootTree(); - // TODO: insert the parse contact information here + graph.parseContactInformation(model, contact_models); return model; } @@ -108,7 +107,7 @@ 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; @@ -144,9 +143,9 @@ namespace pinocchio graph.parseGraphFromXML(xmlStr); - // // Use the Mjcf graph to create the model + // Use the Mjcf graph to create the model graph.parseRootTree(); - // TODO: insert the parse contact information here + graph.parseContactInformation(model, contact_models); return model; } diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index f14722112a..74945be80b 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 { @@ -744,11 +746,10 @@ namespace pinocchio std::string type = v.first; // List of supported constraints from mjcf description // equality -> connect - // equality -> weld // The constraints below are not supported and will be ignored with the following - // warning equality -> joint equality -> flex equality -> distance - if (type != "connect" && type != "weld") + // warning: joint, flex, distance, weld + if (type != "connect") { std::cout << "Warning - Constraint " << type << " is not supported" << std::endl; continue; @@ -768,8 +769,6 @@ namespace pinocchio auto body2 = v.second.get_optional(".body2"); if (body2) eq.body2 = *body2; - else - eq.body2 = "world"; // TODO: find out what is the right name for the world in pinocchio // get the name of the constraint (if it exists) auto name = v.second.get_optional(".name"); @@ -783,22 +782,16 @@ namespace pinocchio if (anchor) eq.anchor = internal::getVectorFromStream<3>(*anchor); - // get the relative position - auto relpose = v.second.get_optional(".relpose"); - if (relpose) - eq.relativePose = internal::getVectorFromStream<3>(*relpose); - - // print what constraint is being added - std::cout << "MjcfEquality: {" << std::endl; - std::cout << " Name: " << eq.name << std::endl; - std::cout << " Type: " << eq.type << std::endl; - std::cout << " Body1: " << eq.body1 << std::endl; - std::cout << " Body2: " << eq.body2 << std::endl; - std::cout << " Anchor: [" << eq.anchor.transpose() << "]" << std::endl; - std::cout << " Relative Pose: [" << eq.relativePose.transpose() << "]" << std::endl; - std::cout << "}" << std::endl; - - mapOfEqualities.insert(std::make_pair("equality", eq)); + // // print what constraint is being added + // std::cout << "MjcfEquality: {" << std::endl; + // std::cout << " Name: " << eq.name << std::endl; + // std::cout << " Type: " << eq.type << std::endl; + // std::cout << " Body1: " << eq.body1 << std::endl; + // std::cout << " Body2: " << eq.body2 << std::endl; + // std::cout << " Anchor: [" << eq.anchor.transpose() << "]" << std::endl; + // std::cout << "}" << std::endl; + + mapOfEqualities.insert(std::make_pair(eq.name, eq)); } } @@ -1112,6 +1105,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 94be645d2c..06c3409de8 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -1249,4 +1249,15 @@ 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_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 @@ + + + + From 54419ed6e2287144f1b3f12071544b1abca8f0e3 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 17:15:06 +0200 Subject: [PATCH 04/10] parsers/mjcf: remove redundant PINOCCHIO_PARSERS_DLLAPI See https://learn.microsoft.com/en-us/cpp/error-messages/compiler-errors-1/compiler-error-c2487?view=msvc-170 for further explanations --- include/pinocchio/parsers/mjcf/mjcf-graph.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index 8b6b76a0f1..17db59428d 100644 --- a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp +++ b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp @@ -528,7 +528,7 @@ namespace pinocchio /// @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 - PINOCCHIO_PARSERS_DLLAPI void parseContactInformation( + void parseContactInformation( const Model & model, PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) & contact_models); From 36b6d1ffed371a642da3ce593876178d188256e2 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 17:21:48 +0200 Subject: [PATCH 05/10] changelog: sync --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) 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)) From 479d0f5be3489869f34726c15fbe2291c5cc0836 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 17:39:16 +0200 Subject: [PATCH 06/10] parsers/mjcf: clean --- src/parsers/mjcf/mjcf-graph.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index 74945be80b..cb40a0b4ef 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -741,8 +741,6 @@ namespace pinocchio { for (const ptree::value_type & v : el) { - // std::cout << v.first << " " << v.second << std::endl; - std::string type = v.first; // List of supported constraints from mjcf description // equality -> connect @@ -751,7 +749,7 @@ namespace pinocchio // warning: joint, flex, distance, weld if (type != "connect") { - std::cout << "Warning - Constraint " << type << " is not supported" << std::endl; + // TODO(jcarpent): support extra constraint types such as joint, flex, distance, weld. continue; } @@ -782,15 +780,6 @@ namespace pinocchio if (anchor) eq.anchor = internal::getVectorFromStream<3>(*anchor); - // // print what constraint is being added - // std::cout << "MjcfEquality: {" << std::endl; - // std::cout << " Name: " << eq.name << std::endl; - // std::cout << " Type: " << eq.type << std::endl; - // std::cout << " Body1: " << eq.body1 << std::endl; - // std::cout << " Body2: " << eq.body2 << std::endl; - // std::cout << " Anchor: [" << eq.anchor.transpose() << "]" << std::endl; - // std::cout << "}" << std::endl; - mapOfEqualities.insert(std::make_pair(eq.name, eq)); } } From 78e97631402612127fe03350bfc14569cc0fe898 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 17:56:41 +0200 Subject: [PATCH 07/10] test/mjcf: enhance testing --- unittest/mjcf.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 06c3409de8..5b59337f6b 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -1258,6 +1258,18 @@ BOOST_AUTO_TEST_CASE(test_contact_parsing) 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.35012, 0, 0)); + BOOST_CHECK_EQUAL( + contact_models[1].joint1_placement.translation(), pinocchio::SE3::Vector3(0.50120, 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() From ad278dcb639d95481a80db3ee1c224281bf087a5 Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 19:04:53 +0200 Subject: [PATCH 08/10] test/mjcf: fix compilation warnings --- unittest/mjcf.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 5b59337f6b..39f90909d4 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -469,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 @@ -582,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]); } @@ -619,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]); } @@ -656,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]); } @@ -693,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]); } @@ -735,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]); } @@ -777,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]); } @@ -819,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]); } @@ -861,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]); } From 9091a41683af6b8d85655a36640e4cfef6b4befb Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 19:25:57 +0200 Subject: [PATCH 09/10] parsers/mjcf: use map for Equality constraints --- include/pinocchio/parsers/mjcf/mjcf-graph.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index 17db59428d..ca2667f06f 100644 --- a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp +++ b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp @@ -21,6 +21,7 @@ #include #include #include +#include namespace pinocchio { @@ -386,7 +387,7 @@ namespace pinocchio typedef std::unordered_map MeshMap_t; typedef std::unordered_map TextureMap_t; typedef std::unordered_map ConfigMap_t; - typedef std::unordered_map EqualityMap_t; + typedef std::map EqualityMap_t; // Compiler Info needed to properly parse the rest of file MjcfCompiler compilerInfo; From 4775bfa39910937c04b103ad1df9ce647813f6ec Mon Sep 17 00:00:00 2001 From: Justin Carpentier Date: Sun, 22 Sep 2024 19:26:15 +0200 Subject: [PATCH 10/10] test/mjcf: fix test of equality constraints --- unittest/mjcf.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 39f90909d4..9e666f1e6d 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -1259,9 +1259,9 @@ BOOST_AUTO_TEST_CASE(test_contact_parsing) BOOST_CHECK_EQUAL(contact_models.size(), 4); BOOST_CHECK_EQUAL( - contact_models[0].joint1_placement.translation(), pinocchio::SE3::Vector3(0.35012, 0, 0)); + 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.50120, 0, 0)); + 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(