From 5434dea41a6f96522c235687fa914fac679f4669 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 001/166] Add Urdf support for mimic joint (revolute and prismatic) --- include/pinocchio/parsers/urdf.hpp | 6 ++ include/pinocchio/parsers/urdf/model.hxx | 132 ++++++++++++++++++++--- src/parsers/urdf/model.cpp | 73 ++++++++++--- 3 files changed, 180 insertions(+), 31 deletions(-) diff --git a/include/pinocchio/parsers/urdf.hpp b/include/pinocchio/parsers/urdf.hpp index 14e3af6f9e..942fa3085f 100644 --- a/include/pinocchio/parsers/urdf.hpp +++ b/include/pinocchio/parsers/urdf.hpp @@ -59,6 +59,7 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -73,6 +74,7 @@ namespace pinocchio ModelTpl & buildModel( const std::string & filename, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -92,6 +94,7 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -129,6 +132,7 @@ namespace pinocchio ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -169,6 +173,7 @@ namespace pinocchio const std::string & xml_stream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /// @@ -185,6 +190,7 @@ namespace pinocchio ModelTpl & buildModelFromXML( const std::string & xml_stream, ModelTpl & model, + const bool mimic = true, const bool verbose = false); /** diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 824a33eaf5..bb9c7fda88 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -35,7 +35,9 @@ namespace pinocchio PRISMATIC, FLOATING, PLANAR, - SPHERICAL + SPHERICAL, + MIMIC_REVOLUTE, + MIMIC_PRISMATIC }; typedef enum ::pinocchio::FrameType FrameType; @@ -65,7 +67,10 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) = 0; + const VectorConstRef & damping, + const Scalar multiplier = 0, + const Scalar offset = 0, + const std::string mimic_name = "") = 0; virtual void addJointAndBody( JointType type, @@ -81,7 +86,10 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) = 0; + const VectorConstRef & damping, + const Scalar multiplier = 0, + const Scalar offset = 0, + const std::string mimic_name = "") = 0; virtual void addFixedJointAndBody( const FrameIndex & parentFrameId, @@ -180,11 +188,15 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) + const VectorConstRef & damping, + const Scalar multiplier = 0, + const Scalar offset = 0, + const std::string mimic_name = "") { addJointAndBody( type, axis, parentFrameId, placement, joint_name, Y, SE3::Identity(), body_name, - max_effort, max_velocity, min_config, max_config, friction, damping); + max_effort, max_velocity, min_config, max_config, friction, damping, + multiplier, offset, mimic_name); } void addJointAndBody( @@ -201,7 +213,10 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) + const VectorConstRef & damping, + const Scalar multiplier = 0, + const Scalar offset = 0, + const std::string mimic_name = "") { JointIndex joint_id; const Frame & frame = model.frames[parentFrameId]; @@ -250,6 +265,28 @@ namespace pinocchio frame.placement * placement, joint_name, max_effort, max_velocity, min_config, max_config, friction, damping); break; + case Base::MIMIC_REVOLUTE: + joint_id = addMimicJoint< + typename JointCollection::JointModelRX, + typename JointCollection::JointModelRY, + typename JointCollection::JointModelRZ, + typename JointCollection::JointModelRevoluteUnaligned> ( + axis, frame, placement, joint_name, + max_effort, max_velocity, min_config, max_config, + friction, damping, + mimic_name, multiplier, offset); + break; + case Base::MIMIC_PRISMATIC: + joint_id = addMimicJoint< + typename JointCollection::JointModelPX, + typename JointCollection::JointModelPY, + typename JointCollection::JointModelPZ, + typename JointCollection::JointModelPrismaticUnaligned> ( + axis, frame, placement, joint_name, + max_effort, max_velocity, min_config, max_config, + friction, damping, + mimic_name, multiplier, offset); + break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); }; @@ -406,6 +443,65 @@ namespace pinocchio } } + template + JointIndex addMimicJoint( + const Vector3& axis, + const Frame & frame, + const SE3 & placement, + const std::string & joint_name, + const VectorConstRef& max_effort, + const VectorConstRef& max_velocity, + const VectorConstRef& min_config, + const VectorConstRef& max_config, + const VectorConstRef& friction, + const VectorConstRef& damping, + const std::string &mimic_name, + const Scalar multiplier, + const Scalar offset) + { + CartesianAxis axisType = extractCartesianAxis(axis); + auto joint_mimic = model.joints[model.getJointId(mimic_name)]; + switch (axisType) + { + case AXIS_X: + return model.addJoint(frame.parent, + typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + frame.placement * placement, joint_name, + max_effort,max_velocity,min_config,max_config, + friction, damping); + break; + + case AXIS_Y: + return model.addJoint(frame.parent, + typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + frame.placement * placement, joint_name, + max_effort,max_velocity,min_config,max_config, + friction, damping); + break; + + case AXIS_Z: + return model.addJoint(frame.parent, + typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + frame.placement * placement, joint_name, + max_effort,max_velocity,min_config,max_config, + friction, damping); + break; + + case AXIS_UNALIGNED: + return model.addJoint(frame.parent, + typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + frame.placement * placement, joint_name, + max_effort,max_velocity,min_config,max_config, + friction, damping); + break; + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); + break; + } + } + + private: /// /// \brief The four possible cartesian types of an 3D axis. @@ -483,13 +579,13 @@ namespace pinocchio typedef UrdfVisitorBaseTpl UrdfVisitorBase; PINOCCHIO_PARSERS_DLLAPI void - parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model); + parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = true); PINOCCHIO_PARSERS_DLLAPI void - parseRootTree(const std::string & filename, UrdfVisitorBase & model); + parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic = true); PINOCCHIO_PARSERS_DLLAPI void - parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model); + parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic = true); } // namespace details template class JointCollectionTpl> @@ -498,6 +594,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic, const bool verbose) { if (rootJointName.empty()) @@ -508,7 +605,7 @@ namespace pinocchio model, rootJoint, rootJointName); if (verbose) visitor.log = &std::cout; - details::parseRootTree(filename, visitor); + details::parseRootTree(filename, visitor, mimic); return model; } @@ -526,12 +623,13 @@ namespace pinocchio ModelTpl & buildModel( const std::string & filename, ModelTpl & model, + const bool mimic, const bool verbose) { details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTree(filename, visitor); + details::parseRootTree(filename, visitor, mimic); return model; } @@ -541,6 +639,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic, const bool verbose) { if (rootJointName.empty()) @@ -551,7 +650,7 @@ namespace pinocchio model, rootJoint, rootJointName); if (verbose) visitor.log = &std::cout; - details::parseRootTreeFromXML(xmlStream, visitor); + details::parseRootTreeFromXML(xmlStream, visitor, mimic); return model; } @@ -569,12 +668,13 @@ namespace pinocchio ModelTpl & buildModelFromXML( const std::string & xmlStream, ModelTpl & model, + const bool mimic, const bool verbose) { details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTreeFromXML(xmlStream, visitor); + details::parseRootTreeFromXML(xmlStream, visitor, mimic); return model; } @@ -584,6 +684,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic, const bool verbose) { if (rootJointName.empty()) @@ -595,7 +696,7 @@ namespace pinocchio model, rootJoint, rootJointName); if (verbose) visitor.log = &std::cout; - details::parseRootTree(urdfTree.get(), visitor); + details::parseRootTree(urdfTree.get(), visitor, mimic); return model; } @@ -613,13 +714,14 @@ namespace pinocchio ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, + const bool mimic, const bool verbose) { PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); details::UrdfVisitor visitor(model); if (verbose) visitor.log = &std::cout; - details::parseRootTree(urdfTree.get(), visitor); + details::parseRootTree(urdfTree.get(), visitor, mimic); return model; } diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index fb530c6468..d119489f14 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -64,7 +64,7 @@ namespace pinocchio /// \param[in] link The current URDF link. /// \param[in] model The model where the link must be added. /// - void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model) + void parseTree(::urdf::LinkConstSharedPtr link, UrdfVisitorBase & model, const bool mimic) { typedef UrdfVisitorBase::Scalar Scalar; typedef UrdfVisitorBase::SE3 SE3; @@ -137,10 +137,31 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } - - model.addJointAndBody( - UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + if(joint->mimic && mimic) + { + max_effort = Vector::Constant(0, infty); + max_velocity = Vector::Constant(0, infty); + min_config = Vector::Constant(0,-infty); + max_config = Vector::Constant(0, infty); + + friction = Vector::Constant(0, 0.); + damping = Vector::Constant(0, 0.); + + std::string mimic_name = joint->mimic->joint_name; + Scalar multiplier = joint->mimic->multiplier; + Scalar offset = joint->mimic->offset; + + model.addJointAndBody(UrdfVisitorBase::MIMIC_REVOLUTE, axis, + parentFrameId,jointPlacement,joint->name, + Y,link->name, + max_effort,max_velocity,min_config,max_config, + friction,damping, + multiplier, offset, mimic_name); + } + else + model.addJointAndBody( + UrdfVisitorBase::REVOLUTE, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); break; case ::urdf::Joint::CONTINUOUS: // Revolute joint with no joint limits @@ -191,10 +212,30 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } - - model.addJointAndBody( - UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + if(joint->mimic && mimic) + { + max_effort = Vector::Constant(0, infty); + max_velocity = Vector::Constant(0, infty); + min_config = Vector::Constant(0,-infty); + max_config = Vector::Constant(0, infty); + + friction = Vector::Constant(0, 0.); + damping = Vector::Constant(0, 0.); + std::string mimic_name = joint->mimic->joint_name; + Scalar multiplier = joint->mimic->multiplier; + Scalar offset = joint->mimic->offset; + + model.addJointAndBody(UrdfVisitorBase::MIMIC_PRISMATIC, axis, + parentFrameId,jointPlacement,joint->name, + Y,link->name, + max_effort,max_velocity,min_config,max_config, + friction,damping, + multiplier, offset, mimic_name); + } + else + model.addJointAndBody( + UrdfVisitorBase::PRISMATIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); break; case ::urdf::Joint::PLANAR: @@ -251,7 +292,7 @@ namespace pinocchio BOOST_FOREACH (::urdf::LinkConstSharedPtr child, link->child_links) { - parseTree(child, model); + parseTree(child, model, mimic); } } @@ -263,7 +304,7 @@ namespace pinocchio /// \param[in] link The current URDF link. /// \param[in] model The model where the link must be added. /// - void parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model) + void parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic) { model.setName(urdfTree->getName()); @@ -272,15 +313,15 @@ namespace pinocchio BOOST_FOREACH (::urdf::LinkConstSharedPtr child, root_link->child_links) { - parseTree(child, model); + parseTree(child, model, mimic); } } - void parseRootTree(const std::string & filename, UrdfVisitorBase & model) + void parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic) { ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename); if (urdfTree) - return parseRootTree(urdfTree.get(), model); + return parseRootTree(urdfTree.get(), model, mimic); else throw std::invalid_argument( "The file " + filename @@ -288,11 +329,11 @@ namespace pinocchio "contain a valid URDF model."); } - void parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model) + void parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic) { ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString); if (urdfTree) - return parseRootTree(urdfTree.get(), model); + return parseRootTree(urdfTree.get(), model, mimic); else throw std::invalid_argument("The XML stream does not contain a valid " "URDF model."); From 1f3d67851097fb2b027b0a714da44bb79974ac96 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 002/166] Sample Model : Now support a mimic joint at the end if wanted --- include/pinocchio/multibody/sample-models.hpp | 2 +- include/pinocchio/multibody/sample-models.hxx | 29 ++++++++++++++----- include/pinocchio/multibody/sample-models.txx | 2 +- src/multibody/sample-models.cpp | 2 +- 4 files changed, 24 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/multibody/sample-models.hpp b/include/pinocchio/multibody/sample-models.hpp index 58b088372e..37e1c0a8ba 100644 --- a/include/pinocchio/multibody/sample-models.hpp +++ b/include/pinocchio/multibody/sample-models.hpp @@ -18,7 +18,7 @@ namespace pinocchio * \param model: model, typically given empty, where the kinematic chain is added. */ template class JointCollectionTpl> - void manipulator(ModelTpl & model); + void manipulator(ModelTpl & model, const bool mimic = false); #ifdef PINOCCHIO_WITH_HPP_FCL /** \brief Create the geometries on top of the kinematic model created by manipulator function. diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 3b0ac01ff1..86a8d0c791 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -57,6 +57,7 @@ namespace pinocchio template class JointCollectionTpl> static void addManipulator( ModelTpl & model, + const bool mimic = false, typename ModelTpl::JointIndex root_joint_idx = 0, const typename ModelTpl::SE3 & Mroot = ModelTpl::SE3::Identity(), @@ -104,8 +105,20 @@ namespace pinocchio model, typename JC::JointModelRX(), model.names[joint_id], pre + "wrist1", Marm); model.inertias[joint_id] = Ijoint; - joint_id = addJointAndBody( - model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4); + if(mimic) + { + Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); + Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); + + joint_id = addJointAndBody(model, typename JC::JointModelMimic(boost::get(model.joints[joint_id]), multiplier, offset), + model.names[joint_id], pre+"wrist1_joint_mimic", Id4); + } + else + { + joint_id = addJointAndBody( + model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4); + } + model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "effector_body", joint_id); @@ -199,9 +212,9 @@ namespace pinocchio } // namespace details template class JointCollectionTpl> - void manipulator(ModelTpl & model) + void manipulator(ModelTpl & model, const bool mimic) { - details::addManipulator(model); + details::addManipulator(model, mimic); } #ifdef PINOCCHIO_WITH_HPP_FCL @@ -315,11 +328,11 @@ namespace pinocchio /* --- Lower limbs --- */ details::addManipulator( - model, ffidx, + model, false, ffidx, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, -0.2, -.1)), "rleg_"); details::addManipulator( - model, ffidx, + model, false, ffidx, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, 0.2, -.1)), "lleg_"); @@ -359,11 +372,11 @@ namespace pinocchio /* --- Upper Limbs --- */ details::addManipulator( - model, chest, + model, false, chest, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, -0.3, 1.)), "rarm_"); details::addManipulator( - model, chest, + model, false, chest, SE3(details::rotate(pi, SE3::Vector3::UnitX()), typename SE3::Vector3(0, 0.3, 1.)), "larm_"); } diff --git a/include/pinocchio/multibody/sample-models.txx b/include/pinocchio/multibody/sample-models.txx index 38a3d31940..64ff36ac76 100644 --- a/include/pinocchio/multibody/sample-models.txx +++ b/include/pinocchio/multibody/sample-models.txx @@ -12,7 +12,7 @@ namespace pinocchio namespace buildModels { extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void - manipulator(context::Model &); + manipulator(context::Model &, bool); extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void humanoid(context::Model &, bool); diff --git a/src/multibody/sample-models.cpp b/src/multibody/sample-models.cpp index af2d62565d..3db04c7acb 100644 --- a/src/multibody/sample-models.cpp +++ b/src/multibody/sample-models.cpp @@ -13,7 +13,7 @@ namespace pinocchio namespace buildModels { template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void - manipulator(context::Model &); + manipulator(context::Model &, bool); template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void humanoid(context::Model &, bool); From c75857e119bd3f77e9d98b1725f751d72376a2c6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 003/166] [Joints] Added support for nj and idx_j --- include/pinocchio/multibody/joint/fwd.hpp | 14 +++++ .../multibody/joint/joint-basic-visitors.hpp | 29 ++++++++- .../multibody/joint/joint-basic-visitors.hxx | 63 +++++++++++++++++-- .../multibody/joint/joint-collection.hpp | 29 ++++----- .../multibody/joint/joint-composite.hpp | 58 ++++++++++++++--- .../multibody/joint/joint-free-flyer.hpp | 6 +- .../multibody/joint/joint-generic.hpp | 15 ++++- .../joint/joint-helical-unaligned.hpp | 6 +- .../multibody/joint/joint-helical.hpp | 6 +- .../multibody/joint/joint-model-base.hpp | 51 +++++++++++---- .../multibody/joint/joint-planar.hpp | 6 +- .../joint/joint-prismatic-unaligned.hpp | 6 +- .../multibody/joint/joint-prismatic.hpp | 6 +- .../joint/joint-revolute-unaligned.hpp | 6 +- .../joint-revolute-unbounded-unaligned.hpp | 6 +- .../joint/joint-revolute-unbounded.hpp | 6 +- .../multibody/joint/joint-revolute.hpp | 6 +- .../multibody/joint/joint-spherical-ZYX.hpp | 6 +- .../multibody/joint/joint-spherical.hpp | 6 +- .../multibody/joint/joint-translation.hpp | 6 +- .../multibody/joint/joint-universal.hpp | 6 +- 21 files changed, 266 insertions(+), 77 deletions(-) diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index 05f83e5185..774111bb1e 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -148,6 +148,20 @@ namespace pinocchio struct JointDataCompositeTpl; typedef JointDataCompositeTpl JointDataComposite; + template< + typename Scalar, + int Options = context::Options, + template class JointCollectionTpl = JointCollectionDefaultTpl> + struct JointModelMimicTpl; + typedef JointModelMimicTpl JointModelMimic; + + template< + typename Scalar, + int Options = context::Options, + template class JointCollectionTpl = JointCollectionDefaultTpl> + struct JointDataMimicTpl; + typedef JointDataMimicTpl JointDataMimic; + template< typename Scalar, int Options = context::Options, diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index 4aa7a32358..445ebf6585 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -144,6 +144,18 @@ namespace pinocchio template class JointCollectionTpl> inline int nq(const JointModelTpl & jmodel); + /** + * @brief Visit a JointModelTpl through JointNjVisitor to get the dimension of + * the joint configuration space + * + * @param[in] jmodel The JointModelVariant + * + * @return The dimension of joint jacobian space + */ + template class JointCollectionTpl> + inline int nj(const JointModelTpl & jmodel); + + /** * @brief Visit a JointModelTpl through JointConfigurationLimitVisitor * to get the configurations limits @@ -192,6 +204,19 @@ namespace pinocchio template class JointCollectionTpl> inline int idx_v(const JointModelTpl & jmodel); + /** + * @brief Visit a JointModelTpl through JointIdjVVisitor to get the index in the full model + * tangent space corresponding to the first joint jacobian space degree + * + * @param[in] jmodel The JointModelVariant + * + * @return The index in the full model tangent space corresponding to the first + * joint jacobian space degree + */ + template class JointCollectionTpl> + inline int idx_j(const JointModelTpl & jmodel); + + /** * @brief Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the * kinematic chain @@ -213,12 +238,14 @@ namespace pinocchio * degree of freedom * @param[in] v The index in the full model tangent space corresponding to the first joint * tangent space degree + * @param[in] j The index in the full model tangent space corresponding to the first joint + * jacobian space degree * * @return The index of the joint in the kinematic chain */ template class JointCollectionTpl> inline void setIndexes( - JointModelTpl & jmodel, JointIndex id, int q, int v); + JointModelTpl & jmodel, JointIndex id, int q, int v, int j); /** * @brief Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 548b1926c0..b9b550cbaf 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -241,6 +241,31 @@ namespace pinocchio return JointNqVisitor::run(jmodel); } + /** + * @brief JointNjVisitor visitor + */ + struct JointNjVisitor : boost::static_visitor + { + template + int operator()(const JointModelBase & jmodel) const + { + return jmodel.nj(); + } + + template class JointCollectionTpl> + static int run(const JointModelTpl & jmodel) + { + return boost::apply_visitor(JointNjVisitor(), jmodel); + } + }; + + template class JointCollectionTpl> + inline int nj(const JointModelTpl & jmodel) + { + return JointNjVisitor::run(jmodel); + } + + /** * @brief JointConfigurationLimitVisitor visitor */ @@ -341,6 +366,30 @@ namespace pinocchio return JointIdxVVisitor::run(jmodel); } + /** + * @brief JointIdxjVisitor visitor + */ + struct JointIdxJVisitor : boost::static_visitor + { + template + int operator()(const JointModelBase & jmodel) const + { + return jmodel.idx_j(); + } + + template class JointCollectionTpl> + static int run(const JointModelTpl & jmodel) + { + return boost::apply_visitor(JointIdxJVisitor(), jmodel); + } + }; + + template class JointCollectionTpl> + inline int idx_j(const JointModelTpl & jmodel) + { + return JointIdxJVisitor::run(jmodel); + } + /** * @brief JointIdVisitor visitor */ @@ -373,33 +422,35 @@ namespace pinocchio JointIndex id; int q; int v; + int j; - JointSetIndexesVisitor(JointIndex id, int q, int v) + JointSetIndexesVisitor(JointIndex id, int q, int v, int j) : id(id) , q(q) , v(v) + , j(j) { } template void operator()(JointModelBase & jmodel) const { - jmodel.setIndexes(id, q, v); + jmodel.setIndexes(id, q, v, j); } template class JointCollectionTpl> static void - run(JointModelTpl & jmodel, JointIndex id, int q, int v) + run(JointModelTpl & jmodel, JointIndex id, int q, int v, int j) { - return boost::apply_visitor(JointSetIndexesVisitor(id, q, v), jmodel); + return boost::apply_visitor(JointSetIndexesVisitor(id, q, v, j), jmodel); } }; template class JointCollectionTpl> inline void setIndexes( - JointModelTpl & jmodel, JointIndex id, int q, int v) + JointModelTpl & jmodel, JointIndex id, int q, int v, int j) { - return JointSetIndexesVisitor::run(jmodel, id, q, v); + return JointSetIndexesVisitor::run(jmodel, id, q, v, j); } /** diff --git a/include/pinocchio/multibody/joint/joint-collection.hpp b/include/pinocchio/multibody/joint/joint-collection.hpp index f8158bb197..9f1fb36e57 100644 --- a/include/pinocchio/multibody/joint/joint-collection.hpp +++ b/include/pinocchio/multibody/joint/joint-collection.hpp @@ -28,10 +28,6 @@ namespace pinocchio typedef JointModelRevoluteTpl JointModelRY; typedef JointModelRevoluteTpl JointModelRZ; - typedef JointModelMimic JointModelMimicRX; - typedef JointModelMimic JointModelMimicRY; - typedef JointModelMimic JointModelMimicRZ; - // Joint Revolute Unaligned typedef JointModelRevoluteUnalignedTpl JointModelRevoluteUnaligned; @@ -71,6 +67,10 @@ namespace pinocchio typedef JointModelCompositeTpl JointModelComposite; + // Joint Mimic + typedef JointModelMimicTpl + JointModelMimic; + // Joint Helical typedef JointModelHelicalTpl JointModelHx; typedef JointModelHelicalTpl JointModelHy; @@ -87,9 +87,6 @@ namespace pinocchio JointModelRX, JointModelRY, JointModelRZ, - JointModelMimicRX, - JointModelMimicRY, - JointModelMimicRZ, JointModelFreeFlyer, JointModelPlanar, JointModelRevoluteUnaligned, @@ -109,7 +106,9 @@ namespace pinocchio JointModelHz, JointModelHelicalUnaligned, JointModelUniversal, - boost::recursive_wrapper> + boost::recursive_wrapper, + boost::recursive_wrapper + > JointModelVariant; // Joint Revolute @@ -117,10 +116,6 @@ namespace pinocchio typedef JointDataRevoluteTpl JointDataRY; typedef JointDataRevoluteTpl JointDataRZ; - typedef JointDataMimic JointDataMimicRX; - typedef JointDataMimic JointDataMimicRY; - typedef JointDataMimic JointDataMimicRZ; - // Joint Revolute Unaligned typedef JointDataRevoluteUnalignedTpl JointDataRevoluteUnaligned; @@ -160,6 +155,10 @@ namespace pinocchio typedef JointDataCompositeTpl JointDataComposite; + // Joint Mimic + typedef JointDataMimicTpl + JointDataMimic; + // Joint Helical typedef JointDataHelicalTpl JointDataHx; typedef JointDataHelicalTpl JointDataHy; @@ -176,9 +175,6 @@ namespace pinocchio JointDataRX, JointDataRY, JointDataRZ, - JointDataMimicRX, - JointDataMimicRY, - JointDataMimicRZ, JointDataFreeFlyer, JointDataPlanar, JointDataRevoluteUnaligned, @@ -198,7 +194,8 @@ namespace pinocchio JointDataHz, JointDataHelicalUnaligned, JointDataUniversal, - boost::recursive_wrapper> + boost::recursive_wrapper, + boost::recursive_wrapper> JointDataVariant; }; diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index 9216238396..b8a4ffe0f5 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -28,7 +28,8 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, - NV = Eigen::Dynamic + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic }; typedef JointCollectionTpl JointCollection; @@ -195,8 +196,10 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::nq; using Base::nv; + using Base::nj; using Base::setIndexes; /// \brief Default contructor @@ -205,6 +208,7 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) + , m_nj(0) , njoints(0) { } @@ -215,14 +219,17 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) + , m_nj(0) , njoints(0) { joints.reserve(size); jointPlacements.reserve(size); m_idx_q.reserve(size); m_idx_v.reserve(size); + m_idx_j.reserve(size); m_nqs.reserve(size); m_nvs.reserve(size); + m_njs.reserve(size); } /// @@ -238,10 +245,13 @@ namespace pinocchio , jointPlacements(1, placement) , m_nq(jmodel.nq()) , m_nv(jmodel.nv()) + , m_nj(jmodel.nj()) , m_idx_q(1, 0) , m_nqs(1, jmodel.nq()) , m_idx_v(1, 0) , m_nvs(1, jmodel.nv()) + , m_idx_j(1, 0) + , m_njs(1, jmodel.nj()) , njoints(1) { } @@ -257,10 +267,13 @@ namespace pinocchio , jointPlacements(other.jointPlacements) , m_nq(other.m_nq) , m_nv(other.m_nv) + , m_nj(other.m_nj) , m_idx_q(other.m_idx_q) , m_nqs(other.m_nqs) , m_idx_v(other.m_idx_v) , m_nvs(other.m_nvs) + , m_idx_j(other.m_idx_j) + , m_njs(other.m_njs) , njoints(other.njoints) { } @@ -282,6 +295,7 @@ namespace pinocchio m_nq += jmodel.nq(); m_nv += jmodel.nv(); + m_nj += jmodel.nj(); updateJointIndexes(); njoints++; @@ -367,13 +381,17 @@ namespace pinocchio { return m_nq; } + int nj_impl() const + { + return m_nj; + } /** * @brief Update the indexes of subjoints in the stack */ - void setIndexes_impl(JointIndex id, int q, int v) + void setIndexes_impl(JointIndex id, int q, int v, int j) { - Base::setIndexes_impl(id, q, v); + Base::setIndexes_impl(id, q, v, j); updateJointIndexes(); } @@ -391,10 +409,13 @@ namespace pinocchio Base::operator=(other); m_nq = other.m_nq; m_nv = other.m_nv; + m_nj = other.m_nj; m_idx_q = other.m_idx_q; m_idx_v = other.m_idx_v; + m_idx_j = other.m_idx_j; m_nqs = other.m_nqs; m_nvs = other.m_nvs; + m_njs = other.m_njs; joints = other.joints; jointPlacements = other.jointPlacements; njoints = other.njoints; @@ -407,10 +428,13 @@ namespace pinocchio { return Base::isEqual(other) && internal::comparison_eq(nq(), other.nq()) && internal::comparison_eq(nv(), other.nv()) + && internal::comparison_eq(nj(), other.nj()) && internal::comparison_eq(m_idx_q, other.m_idx_q) && internal::comparison_eq(m_idx_v, other.m_idx_v) + && internal::comparison_eq(m_idx_j, other.m_idx_j) && internal::comparison_eq(m_nqs, other.m_nqs) && internal::comparison_eq(m_nvs, other.m_nvs) + && internal::comparison_eq(m_njs, other.m_njs) && internal::comparison_eq(joints, other.joints) && internal::comparison_eq(jointPlacements, other.jointPlacements) && internal::comparison_eq(njoints, other.njoints); @@ -422,13 +446,16 @@ namespace pinocchio { typedef JointModelCompositeTpl ReturnType; ReturnType res((size_t)njoints); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); res.m_nq = m_nq; res.m_nv = m_nv; + res.m_nj = m_nj; res.m_idx_q = m_idx_q; res.m_idx_v = m_idx_v; + res.m_idx_j = m_idx_j; res.m_nqs = m_nqs; res.m_nvs = m_nvs; + res.m_njs = m_njs; res.njoints = njoints; res.joints.resize(joints.size()); @@ -478,12 +505,12 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointCols(const Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_v, nv()); + return A.middleCols(Base::i_j, nj()); } template typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_v, nv()); + return A.middleCols(Base::i_j, nj()); } template @@ -515,13 +542,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointCols_impl(const Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_v, nv()); + return A.middleCols(Base::i_j, nj()); } template typename SizeDepType::template ColsReturn::Type jointCols_impl(Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_v, nv()); + return A.middleCols(Base::i_j, nj()); } protected: @@ -536,11 +563,14 @@ namespace pinocchio { int idx_q = this->idx_q(); int idx_v = this->idx_v(); + int idx_j = this->idx_j(); m_idx_q.resize(joints.size()); m_idx_v.resize(joints.size()); + m_idx_j.resize(joints.size()); m_nqs.resize(joints.size()); m_nvs.resize(joints.size()); + m_njs.resize(joints.size()); for (size_t i = 0; i < joints.size(); ++i) { @@ -548,16 +578,19 @@ namespace pinocchio m_idx_q[i] = idx_q; m_idx_v[i] = idx_v; - ::pinocchio::setIndexes(joint, i, idx_q, idx_v); + m_idx_j[i] = idx_j; + ::pinocchio::setIndexes(joint, i, idx_q, idx_v, idx_j); m_nqs[i] = ::pinocchio::nq(joint); m_nvs[i] = ::pinocchio::nv(joint); + m_njs[i] = ::pinocchio::nj(joint); idx_q += m_nqs[i]; idx_v += m_nvs[i]; + idx_j += m_njs[i]; } } /// \brief Dimensions of the config and tangent space of the composite joint. - int m_nq, m_nv; + int m_nq, m_nv, m_nj; /// Keep information of both the dimension and the position of the joints in the composition. @@ -569,6 +602,11 @@ namespace pinocchio std::vector m_idx_v; /// \brief Dimension of the segment in the tangent vector std::vector m_nvs; + /// \brief Index in the jacobian matrix + std::vector m_idx_j; + /// \brief Dimension of the segment in the jacobian matrix + std::vector m_njs; + public: /// \brief Number of joints contained in the JointModelComposite diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 7274494c3b..3ee4285971 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -168,7 +168,8 @@ namespace pinocchio enum { NQ = 7, - NV = 6 + NV = 6, + NJ = 6 }; typedef _Scalar Scalar; enum @@ -289,6 +290,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const @@ -402,7 +404,7 @@ namespace pinocchio { typedef JointModelFreeFlyerTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index 478c1278b8..d68461a910 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -29,7 +29,8 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, // Dynamic because unknown at compile time - NV = Eigen::Dynamic + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic }; typedef _Scalar Scalar; @@ -396,6 +397,10 @@ namespace pinocchio { return ::pinocchio::nv(*this); } + int nj_impl() const + { + return ::pinocchio::nj(*this); + } int idx_q_impl() const { @@ -405,15 +410,19 @@ namespace pinocchio { return ::pinocchio::idx_v(*this); } + int idx_j_impl() const + { + return ::pinocchio::idx_j(*this); + } JointIndex id_impl() const { return ::pinocchio::id(*this); } - void setIndexes(JointIndex id, int nq, int nv) + void setIndexes(JointIndex id, int nq, int nv, int nj) { - ::pinocchio::setIndexes(*this, id, nq, nv); + ::pinocchio::setIndexes(*this, id, nq, nv, nj); } /// \returns An expression of *this with the Scalar type casted to NewScalar. diff --git a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp index 790f9ec01b..a2b70d4054 100644 --- a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp @@ -524,7 +524,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -638,6 +639,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointModelHelicalUnalignedTpl() @@ -758,7 +760,7 @@ namespace pinocchio { typedef JointModelHelicalUnalignedTpl ReturnType; ReturnType res(axis.template cast(), ScalarCast::cast(m_pitch)); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index dc92b2fb44..4b308188b7 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -735,7 +735,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -837,6 +838,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -947,7 +949,7 @@ namespace pinocchio { typedef JointModelHelicalTpl ReturnType; ReturnType res(ScalarCast::cast(m_pitch)); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index da7164dc96..fc29a2a345 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -29,7 +29,8 @@ { \ Options = traits::Options, \ NQ = traits::NQ, \ - NV = traits::NV \ + NV = traits::NV, \ + NJ = traits::NJ \ }; \ typedef TYPENAME traits::ConfigVector_t ConfigVector_t; \ typedef TYPENAME traits::TangentVector_t TangentVector_t @@ -59,7 +60,8 @@ #define PINOCCHIO_JOINT_USE_INDEXES(Joint) \ typedef JointModelBase Base; \ using Base::idx_q; \ - using Base::idx_v + using Base::idx_v; \ + using Base::idx_j #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ template \ @@ -146,6 +148,10 @@ namespace pinocchio { return derived().nq_impl(); } + int nj() const + { + return derived().nj_impl(); + } // Default _impl methods are reimplemented by dynamic-size joints. int nv_impl() const @@ -156,6 +162,10 @@ namespace pinocchio { return NQ; } + int nj_impl() const + { + return NJ; + } int idx_q() const { @@ -165,6 +175,10 @@ namespace pinocchio { return derived().idx_v_impl(); } + int idx_j() const + { + return derived().idx_j_impl(); + } JointIndex id() const { return derived().id_impl(); @@ -178,20 +192,25 @@ namespace pinocchio { return i_v; } + int idx_j_impl() const + { + return i_j; + } JointIndex id_impl() const { return i_id; } - void setIndexes(JointIndex id, int q, int v) + void setIndexes(JointIndex id, int q, int v, int j) { - derived().setIndexes_impl(id, q, v); + derived().setIndexes_impl(id, q, v, j); } - void setIndexes_impl(JointIndex id, int q, int v) + void setIndexes_impl(JointIndex id, int q, int v, int j) { i_id = id, i_q = q; i_v = v; + i_j = j; } void disp(std::ostream & os) const @@ -201,8 +220,10 @@ namespace pinocchio << " index: " << id() << endl << " index q: " << idx_q() << endl << " index v: " << idx_v() << endl + << " index j: " << idx_j() << endl << " nq: " << nq() << endl - << " nv: " << nv() << endl; + << " nv: " << nv() << endl + << " nj: " << nj() << endl; } friend std::ostream & operator<<(std::ostream & os, const JointModelBase & joint) @@ -254,7 +275,8 @@ namespace pinocchio { return internal::comparison_eq(other.id(), id()) && internal::comparison_eq(other.idx_q(), idx_q()) - && internal::comparison_eq(other.idx_v(), idx_v()); + && internal::comparison_eq(other.idx_v(), idx_v()) + && internal::comparison_eq(other.idx_j(), idx_j()); } /* Acces to dedicated segment in robot config space. */ @@ -332,7 +354,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointCols_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_v(), nv()); + return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } // Non-const access @@ -346,7 +368,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type jointCols_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_v(), nv()); + return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } /* Acces to dedicated rows in a matrix.*/ @@ -362,7 +384,7 @@ namespace pinocchio typename SizeDepType::template RowsReturn::ConstType jointRows_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_v(), nv()); + return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } // Non-const access @@ -376,7 +398,7 @@ namespace pinocchio typename SizeDepType::template RowsReturn::Type jointRows_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_v(), nv()); + return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the @@ -393,7 +415,7 @@ namespace pinocchio typename SizeDepType::template BlockReturn::ConstType jointBlock_impl(const Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); + return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } // Non-const access @@ -408,7 +430,7 @@ namespace pinocchio typename SizeDepType::template BlockReturn::Type jointBlock_impl(Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); + return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } protected: @@ -419,6 +441,7 @@ namespace pinocchio : i_id(std::numeric_limits::max()) , i_q(-1) , i_v(-1) + , i_j(-1) { } @@ -440,6 +463,7 @@ namespace pinocchio i_id = clone.i_id; i_q = clone.i_q; i_v = clone.i_v; + i_j = clone.i_j; return *this; } @@ -447,6 +471,7 @@ namespace pinocchio JointIndex i_id; // ID of the joint in the multibody list. int i_q; // Index of the joint configuration in the joint configuration vector. int i_v; // Index of the joint velocity in the joint velocity vector. + int i_j; // Index of the joint jacobian in the joint jacobian matrix. }; // struct JointModelBase diff --git a/include/pinocchio/multibody/joint/joint-planar.hpp b/include/pinocchio/multibody/joint/joint-planar.hpp index 57e529454f..a15fcc44e8 100644 --- a/include/pinocchio/multibody/joint/joint-planar.hpp +++ b/include/pinocchio/multibody/joint/joint-planar.hpp @@ -471,7 +471,8 @@ namespace pinocchio enum { NQ = 4, - NV = 3 + NV = 3, + NJ = 3 }; enum { @@ -566,6 +567,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const @@ -672,7 +674,7 @@ namespace pinocchio { typedef JointModelPlanarTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp index dd3f45e13e..2a932d63ef 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp @@ -478,7 +478,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -591,6 +592,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -694,7 +696,7 @@ namespace pinocchio { typedef JointModelPrismaticUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp index 2a8233cbfc..aa85e4ffc5 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -570,7 +570,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -673,6 +674,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -766,7 +768,7 @@ namespace pinocchio { typedef JointModelPrismaticTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp index a5401bd4e1..2b8c3e9198 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp @@ -503,7 +503,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -617,6 +618,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointModelRevoluteUnalignedTpl() @@ -718,7 +720,7 @@ namespace pinocchio { typedef JointModelRevoluteUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp index 31c5836d00..757bc70952 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -24,7 +24,8 @@ namespace pinocchio enum { NQ = 2, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -141,6 +142,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointModelRevoluteUnboundedUnalignedTpl() @@ -246,7 +248,7 @@ namespace pinocchio { typedef JointModelRevoluteUnboundedUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp index fb635e4319..26dc075b43 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -23,7 +23,8 @@ namespace pinocchio enum { NQ = 2, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -127,6 +128,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -224,7 +226,7 @@ namespace pinocchio { typedef JointModelRevoluteUnboundedTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp index 9eaeeebe62..5b1fcdf6f5 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -659,7 +659,8 @@ namespace pinocchio enum { NQ = 1, - NV = 1 + NV = 1, + NJ = 1 }; typedef _Scalar Scalar; enum @@ -761,6 +762,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; @@ -862,7 +864,7 @@ namespace pinocchio { typedef JointModelRevoluteTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp index 46378a8f62..93f54ffad8 100644 --- a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp @@ -279,7 +279,8 @@ namespace pinocchio enum { NQ = 3, - NV = 3 + NV = 3, + NJ = 3 }; typedef _Scalar Scalar; enum @@ -379,6 +380,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const @@ -500,7 +502,7 @@ namespace pinocchio { typedef JointModelSphericalZYXTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-spherical.hpp b/include/pinocchio/multibody/joint/joint-spherical.hpp index 800696fa97..abb5f06e34 100644 --- a/include/pinocchio/multibody/joint/joint-spherical.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical.hpp @@ -382,7 +382,8 @@ namespace pinocchio enum { NQ = 4, - NV = 3 + NV = 3, + NJ = 3 }; typedef _Scalar Scalar; enum @@ -480,6 +481,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const @@ -600,7 +602,7 @@ namespace pinocchio { typedef JointModelSphericalTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index bf2b113f66..bdfea32718 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -479,7 +479,8 @@ namespace pinocchio enum { NQ = 3, - NV = 3 + NV = 3, + NJ = 3 }; typedef _Scalar Scalar; enum @@ -577,6 +578,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const @@ -657,7 +659,7 @@ namespace pinocchio { typedef JointModelTranslationTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-universal.hpp b/include/pinocchio/multibody/joint/joint-universal.hpp index d22e435e74..6ec5e12adb 100644 --- a/include/pinocchio/multibody/joint/joint-universal.hpp +++ b/include/pinocchio/multibody/joint/joint-universal.hpp @@ -313,7 +313,8 @@ namespace pinocchio enum { NQ = 2, - NV = 2 + NV = 2, + NJ = 2 }; typedef _Scalar Scalar; enum @@ -413,6 +414,7 @@ namespace pinocchio using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::setIndexes; JointModelUniversalTpl() @@ -600,7 +602,7 @@ namespace pinocchio { typedef JointModelUniversalTpl ReturnType; ReturnType res(axis1.template cast(), axis2.template cast()); - res.setIndexes(id(), idx_q(), idx_v()); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } From add81cee55b878171e8849850821f7a01e54c04e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 004/166] [Joint] Mimic joint is now based on the variant and not templated --- .../pinocchio/multibody/joint/joint-mimic.hpp | 1225 ++++++++--------- 1 file changed, 611 insertions(+), 614 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 4725bc968e..c08b39ae7a 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -5,123 +5,105 @@ #ifndef __pinocchio_multibody_joint_mimic_hpp__ #define __pinocchio_multibody_joint_mimic_hpp__ +#include "pinocchio/multibody/joint/fwd.hpp" +#include "pinocchio/multibody/joint/joint-collection.hpp" #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" +#include +#include + namespace pinocchio { - - template - struct ScaledJointMotionSubspace; - - template - struct traits> + template struct ScaledJointMotionSubspaceTpl; + + template + struct traits< ScaledJointMotionSubspaceTpl<_Scalar, _Options> > { - typedef typename traits::Scalar Scalar; - enum - { - Options = traits::Options + typedef JointMotionSubspaceTpl RefJointMotionSubspace; + typedef typename traits::Scalar Scalar; + enum { Options = traits::Options }; + enum { + LINEAR = traits::LINEAR, + ANGULAR = traits::ANGULAR }; - enum - { - LINEAR = traits::LINEAR, - ANGULAR = traits::ANGULAR - }; - - typedef typename traits::JointMotion JointMotion; - typedef typename traits::JointForce JointForce; - typedef typename traits::DenseBase DenseBase; - typedef typename traits::ReducedSquaredMatrix ReducedSquaredMatrix; - - typedef typename traits::MatrixReturnType MatrixReturnType; - typedef typename traits::ConstMatrixReturnType ConstMatrixReturnType; - typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; - }; // traits ScaledJointMotionSubspace - - template - struct SE3GroupAction> - { - typedef typename SE3GroupAction::ReturnType ReturnType; - }; - - template - struct MotionAlgebraAction, MotionDerived> + typedef typename traits::JointMotion JointMotion; + typedef typename traits::JointForce JointForce; + typedef typename traits::DenseBase DenseBase; + typedef typename traits::MatrixReturnType MatrixReturnType; + typedef typename traits::ConstMatrixReturnType ConstMatrixReturnType; + }; // traits ScaledJointMotionSubspaceTpl + + template + struct SE3GroupAction< ScaledJointMotionSubspaceTpl<_Scalar, _Options> > + { typedef typename SE3GroupAction>::RefJointMotionSubspace>::ReturnType ReturnType; }; + + template + struct MotionAlgebraAction< ScaledJointMotionSubspaceTpl<_Scalar, _Options>, MotionDerived > + { typedef typename MotionAlgebraAction>::RefJointMotionSubspace, MotionDerived>::ReturnType ReturnType; }; + + template + struct ConstraintForceOp< ScaledJointMotionSubspaceTpl<_Scalar, _Options>, ForceDerived> { - typedef typename MotionAlgebraAction::ReturnType ReturnType; - }; - - template - struct ConstraintForceOp, ForceDerived> - { - typedef typename Constraint::Scalar Scalar; - typedef typename ConstraintForceOp::ReturnType OriginalReturnType; - - typedef typename ScalarMatrixProduct::type IdealReturnType; - typedef Eigen::Matrix< - Scalar, - IdealReturnType::RowsAtCompileTime, - IdealReturnType::ColsAtCompileTime, - Constraint::Options> - ReturnType; + typedef typename traits>::RefJointMotionSubspace::Scalar Scalar; + // typedef typename ConstraintForceOp>::RefJointMotionSubspace,ForceDerived>::ReturnType OriginalReturnType; + typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options> OriginalReturnType; + + typedef typename ScalarMatrixProduct::type IdealReturnType; + typedef Eigen::Matrix>::RefJointMotionSubspace::Options> ReturnType; }; - - template - struct ConstraintForceSetOp, ForceSet> + + template + struct ConstraintForceSetOp< ScaledJointMotionSubspaceTpl<_Scalar, _Options>, ForceSet> { - typedef typename Constraint::Scalar Scalar; - typedef typename ConstraintForceSetOp::ReturnType OriginalReturnType; - typedef typename ScalarMatrixProduct::type IdealReturnType; - typedef Eigen::Matrix< - Scalar, - Constraint::NV, - ForceSet::ColsAtCompileTime, - Constraint::Options | Eigen::RowMajor> - ReturnType; + typedef typename traits>::RefJointMotionSubspace::Scalar Scalar; + // typedef typename ConstraintForceSetOp>::RefJointMotionSubspace, ForceSet>::ReturnType OriginalReturnType; + typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options> OriginalReturnType; + typedef typename ScalarMatrixProduct::type IdealReturnType; + typedef Eigen::Matrix>::RefJointMotionSubspace::NV,ForceSet::ColsAtCompileTime, traits>::RefJointMotionSubspace::Options | Eigen::RowMajor> ReturnType; }; - template - struct ScaledJointMotionSubspace : JointMotionSubspaceBase> + template + struct ScaledJointMotionSubspaceTpl + : JointMotionSubspaceBase< ScaledJointMotionSubspaceTpl<_Scalar, _Options> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspace) - enum - { - NV = Constraint::NV - }; - typedef JointMotionSubspaceBase Base; + + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspaceTpl) + enum { NV = Eigen::Dynamic}; + typedef JointMotionSubspaceBase Base; using Base::nv; - - typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; - - ScaledJointMotionSubspace() - { - } - - explicit ScaledJointMotionSubspace(const Scalar & scaling_factor) + + typedef typename traits>::RefJointMotionSubspace RefJointMotionSubspace; + typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; + + ScaledJointMotionSubspaceTpl() + : ScaledJointMotionSubspaceTpl(1.0) + {} + + explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor) : m_scaling_factor(scaling_factor) - { - } - - ScaledJointMotionSubspace(const Constraint & constraint, const Scalar & scaling_factor) + , m_constraint(0) + {} + + ScaledJointMotionSubspaceTpl(const RefJointMotionSubspace & constraint, + const Scalar & scaling_factor) : m_constraint(constraint) , m_scaling_factor(scaling_factor) - { - } + {} - ScaledJointMotionSubspace(const ScaledJointMotionSubspace & other) + ScaledJointMotionSubspaceTpl(const ScaledJointMotionSubspaceTpl & other) : m_constraint(other.m_constraint) , m_scaling_factor(other.m_scaling_factor) - { - } + {} - ScaledJointMotionSubspace & operator=(const ScaledJointMotionSubspace & other) + ScaledJointMotionSubspaceTpl & operator=(const ScaledJointMotionSubspaceTpl & other) { m_constraint = other.m_constraint; m_scaling_factor = other.m_scaling_factor; return *this; } - + template JointMotion __mult__(const Eigen::MatrixBase & v) const { @@ -129,514 +111,518 @@ namespace pinocchio JointMotion jm = m_constraint * v; return jm * m_scaling_factor; } - + template - SE3ActionReturnType se3Action(const SE3Tpl & m) const + SE3ActionReturnType + se3Action(const SE3Tpl & m) const { SE3ActionReturnType res = m_constraint.se3Action(m); return m_scaling_factor * res; } - + template - SE3ActionReturnType se3ActionInverse(const SE3Tpl & m) const + SE3ActionReturnType + se3ActionInverse(const SE3Tpl & m) const { SE3ActionReturnType res = m_constraint.se3ActionInverse(m); return m_scaling_factor * res; } - - int nv_impl() const + + int nv_impl() const { return m_constraint.nv(); } + + struct TransposeConst { - return m_constraint.nv(); - } - - struct TransposeConst : JointMotionSubspaceTransposeBase - { - const ScaledJointMotionSubspace & ref; - TransposeConst(const ScaledJointMotionSubspace & ref) - : ref(ref) - { - } - + const ScaledJointMotionSubspaceTpl & ref; + TransposeConst(const ScaledJointMotionSubspaceTpl & ref) : ref(ref) {} + template - typename ConstraintForceOp::ReturnType + typename ConstraintForceOp::ReturnType operator*(const ForceDense & f) const { - // TODO: I don't know why, but we should a dense a return type, otherwise it fails at the - // evaluation level; - typedef - typename ConstraintForceOp::ReturnType ReturnType; + // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level; + typedef typename ConstraintForceOp::ReturnType ReturnType; return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); } - - /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) + + /// [CRBA] MatrixBase operator* (RefConstraint::Transpose S, ForceSet::Block) template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) const { - typedef - typename ConstraintForceSetOp::ReturnType ReturnType; + typedef typename ConstraintForceSetOp::ReturnType ReturnType; return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); } - + }; // struct TransposeConst - - TransposeConst transpose() const + + TransposeConst transpose() const { return TransposeConst(*this); } + + const DenseBase & matrix_impl() const { - return TransposeConst(*this); + S = m_scaling_factor * m_constraint.matrix_impl(); + return S; } - - DenseBase matrix_impl() const + + DenseBase & matrix_impl() { - DenseBase S = m_scaling_factor * m_constraint.matrix(); + S = m_scaling_factor * m_constraint.matrix_impl(); return S; } template - typename MotionAlgebraAction::ReturnType + typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { - typedef typename MotionAlgebraAction::ReturnType - ReturnType; + typedef typename MotionAlgebraAction::ReturnType ReturnType; ReturnType res = m_scaling_factor * m_constraint.motionAction(m); return res; } - - inline const Scalar & scaling() const - { - return m_scaling_factor; - } - inline Scalar & scaling() + + inline const Scalar & scaling() const { return m_scaling_factor; } + inline Scalar & scaling() { return m_scaling_factor; } + + inline const RefJointMotionSubspace & constraint() const { return m_constraint.derived(); } + inline RefJointMotionSubspace & constraint() { return m_constraint.derived(); } + + bool isEqual(const ScaledJointMotionSubspaceTpl & other) const { - return m_scaling_factor; + return m_constraint == other.m_constraint + && m_scaling_factor == other.m_scaling_factor; } - - inline const Constraint & constraint() const - { - return m_constraint; - } - inline Constraint & constraint() - { - return m_constraint; - } - - bool isEqual(const ScaledJointMotionSubspace & other) const - { - return internal::comparison_eq(m_constraint, other.m_constraint) - && internal::comparison_eq(m_scaling_factor, other.m_scaling_factor); - } - + protected: - Constraint m_constraint; + + RefJointMotionSubspace m_constraint; Scalar m_scaling_factor; - }; // struct ScaledJointMotionSubspace - - namespace details - { - template - struct StDiagonalMatrixSOperation> - { - typedef ScaledJointMotionSubspace Constraint; - typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; - - static ReturnType run(const JointMotionSubspaceBase & constraint) - { - const Constraint & constraint_ = constraint.derived(); - return (constraint_.constraint().transpose() * constraint_.constraint()) - * (constraint_.scaling() * constraint_.scaling()); - } - }; - } // namespace details - - template - struct MultiplicationOp, ScaledJointMotionSubspace<_Constraint>> + mutable DenseBase S; + }; // struct ScaledJointMotionSubspaceTpl + + template + struct MultiplicationOp, ScaledJointMotionSubspaceTpl > { - typedef InertiaTpl Inertia; - typedef ScaledJointMotionSubspace<_Constraint> Constraint; + typedef InertiaTpl Inertia; + typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename Constraint::Scalar Scalar; - - typedef typename MultiplicationOp::ReturnType OriginalReturnType; - // typedef typename ScalarMatrixProduct::type ReturnType; - typedef OriginalReturnType ReturnType; + + // typedef typename MultiplicationOp::ReturnType OriginalReturnType; + typedef Eigen::Matrix ReturnType; + // typedef typename ScalarMatrixProduct::type ReturnType; + // typedef OriginalReturnType ReturnType; }; - + /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ namespace impl { - template - struct LhsMultiplicationOp, ScaledJointMotionSubspace<_Constraint>> - { - typedef InertiaTpl Inertia; - typedef ScaledJointMotionSubspace<_Constraint> Constraint; - typedef typename MultiplicationOp::ReturnType ReturnType; - - static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) + template + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl > + { + typedef InertiaTpl Inertia; + typedef ScaledJointMotionSubspaceTpl Constraint; + typedef typename MultiplicationOp::ReturnType ReturnType; + + static inline ReturnType run(const Inertia & Y, + const Constraint & scaled_constraint) { return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); } }; } // namespace impl - - template - struct MultiplicationOp, ScaledJointMotionSubspace<_Constraint>> + + template + struct MultiplicationOp, ScaledJointMotionSubspaceTpl > { - typedef typename MultiplicationOp::ReturnType OriginalReturnType; + typedef ScaledJointMotionSubspaceTpl Constraint; + typedef typename MultiplicationOp::ReturnType OriginalReturnType; typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; }; - + /* [ABA] operator* (Inertia Y,Constraint S) */ namespace impl { - template - struct LhsMultiplicationOp, ScaledJointMotionSubspace<_Constraint>> - { - typedef ScaledJointMotionSubspace<_Constraint> Constraint; - typedef - typename MultiplicationOp, Constraint>::ReturnType ReturnType; - - static inline ReturnType - run(const Eigen::MatrixBase & Y, const Constraint & scaled_constraint) + template + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl > + { + typedef ScaledJointMotionSubspaceTpl Constraint; + typedef typename MultiplicationOp,Constraint>::ReturnType ReturnType; + + static inline ReturnType run(const Eigen::MatrixBase & Y, + const Constraint & scaled_constraint) { return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint()); } }; } // namespace impl - - template - struct JointMimic; - template - struct JointModelMimic; - template - struct JointDataMimic; - - template - struct traits> + +template +struct JointCollectionMimicableTpl +{ + typedef _Scalar Scalar; + enum { Options = _Options }; + + // Joint Revolute + typedef JointModelRevoluteTpl JointModelRX; + typedef JointModelRevoluteTpl JointModelRY; + typedef JointModelRevoluteTpl JointModelRZ; + + // Joint Revolute Unaligned + typedef JointModelRevoluteUnalignedTpl JointModelRevoluteUnaligned; + + // Joint Revolute UBounded + typedef JointModelRevoluteUnboundedTpl JointModelRUBX; + typedef JointModelRevoluteUnboundedTpl JointModelRUBY; + typedef JointModelRevoluteUnboundedTpl JointModelRUBZ; + + // Joint Revolute Unbounded Unaligned + typedef JointModelRevoluteUnboundedUnalignedTpl JointModelRevoluteUnboundedUnaligned; + + // Joint Prismatic + typedef JointModelPrismaticTpl JointModelPX; + typedef JointModelPrismaticTpl JointModelPY; + typedef JointModelPrismaticTpl JointModelPZ; + + // Joint Prismatic Unaligned + typedef JointModelPrismaticUnalignedTpl JointModelPrismaticUnaligned; + + // Joint Translation + typedef JointModelTranslationTpl JointModelTranslation; + + typedef boost::variant< + JointModelRX, JointModelRY, JointModelRZ + , JointModelRevoluteUnaligned + , JointModelPX, JointModelPY, JointModelPZ + , JointModelPrismaticUnaligned + , JointModelTranslation + , JointModelRUBX, JointModelRUBY, JointModelRUBZ + , JointModelRevoluteUnboundedUnaligned + > JointModelVariant; + + // Joint Revolute + typedef JointDataRevoluteTpl JointDataRX; + typedef JointDataRevoluteTpl JointDataRY; + typedef JointDataRevoluteTpl JointDataRZ; + + // Joint Revolute Unaligned + typedef JointDataRevoluteUnalignedTpl JointDataRevoluteUnaligned; + + // Joint Revolute Unaligned + typedef JointDataRevoluteUnboundedUnalignedTpl JointDataRevoluteUnboundedUnaligned; + + // Joint Revolute UBounded + typedef JointDataRevoluteUnboundedTpl JointDataRUBX; + typedef JointDataRevoluteUnboundedTpl JointDataRUBY; + typedef JointDataRevoluteUnboundedTpl JointDataRUBZ; + + // Joint Prismatic + typedef JointDataPrismaticTpl JointDataPX; + typedef JointDataPrismaticTpl JointDataPY; + typedef JointDataPrismaticTpl JointDataPZ; + + // Joint Prismatic Unaligned + typedef JointDataPrismaticUnalignedTpl JointDataPrismaticUnaligned; + + // Joint Translation + typedef JointDataTranslationTpl JointDataTranslation; + + typedef boost::variant< + JointDataRX, JointDataRY, JointDataRZ + , JointDataRevoluteUnaligned + , JointDataPX, JointDataPY, JointDataPZ + , JointDataPrismaticUnaligned + , JointDataTranslation + , JointDataRUBX, JointDataRUBY, JointDataRUBZ + , JointDataRevoluteUnboundedUnaligned + > JointDataVariant; +}; + + + template class JointCollectionTpl> struct JointMimicTpl; + template class JointCollectionTpl> struct JointModelMimicTpl; + template class JointCollectionTpl> struct JointDataMimicTpl; + + template class JointCollectionTpl> + struct traits< JointMimicTpl<_Scalar, _Options, JointCollectionTpl> > { + typedef _Scalar Scalar; + enum { - NQ = traits::NV, - NV = traits::NQ - }; - typedef typename traits::Scalar Scalar; - enum - { - Options = traits::Options + Options = _Options, + NQ = Eigen::Dynamic, + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic }; - - typedef typename traits::JointDataDerived JointDataBase; - typedef typename traits::JointModelDerived JointModelBase; - - typedef JointDataMimic JointDataDerived; - typedef JointModelMimic JointModelDerived; - - typedef ScaledJointMotionSubspace::Constraint_t> Constraint_t; - typedef typename traits::Transformation_t Transformation_t; - typedef typename traits::Motion_t Motion_t; - typedef typename traits::Bias_t Bias_t; - + + typedef JointCollectionTpl JointCollection; + typedef JointDataMimicTpl JointDataDerived; + typedef JointModelMimicTpl JointModelDerived; + typedef ScaledJointMotionSubspaceTpl Constraint_t; + typedef SE3Tpl Transformation_t; + + typedef MotionTpl Motion_t; + typedef MotionTpl Bias_t; + // [ABA] - typedef typename traits::U_t U_t; - typedef typename traits::D_t D_t; - typedef typename traits::UD_t UD_t; - - typedef typename traits::ConfigVector_t ConfigVector_t; - typedef typename traits::TangentVector_t TangentVector_t; + typedef Eigen::Matrix U_t; + typedef Eigen::Matrix D_t; + typedef Eigen::Matrix UD_t; + + typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix TangentVector_t; PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - - template - struct traits> - { - typedef JointMimic::JointDerived> JointDerived; - typedef typename traits::Scalar Scalar; + + template class JointCollectionTpl> + struct traits< JointDataMimicTpl<_Scalar, Options, JointCollectionTpl> > + { + typedef JointMimicTpl<_Scalar,Options,JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; }; - - template - struct traits> - { - typedef JointMimic::JointDerived> JointDerived; - typedef typename traits::Scalar Scalar; + + template class JointCollectionTpl> + struct traits< JointModelMimicTpl<_Scalar, Options, JointCollectionTpl> > + { + typedef JointMimicTpl<_Scalar,Options,JointCollectionTpl> JointDerived; + typedef _Scalar Scalar; }; - template - struct JointDataMimic : public JointDataBase> + template class JointCollectionTpl> + struct JointDataMimicTpl + : public JointDataBase< JointDataMimicTpl<_Scalar, _Options, JointCollectionTpl> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef typename traits::JointDerived JointDerived; - typedef JointDataBase> Base; - + typedef JointDataBase Base; + typedef JointMimicTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); + + // typedef typename boost::make_variant_over>::type>::type MimicableJointDataVariant; + typedef JointDataTpl<_Scalar, _Options, JointCollectionMimicableTpl> RefJointData; - JointDataMimic() + JointDataMimicTpl() : m_scaling((Scalar)0) - , joint_q(ConfigVector_t::Zero()) - , joint_v(TangentVector_t::Zero()) , S((Scalar)0) { - } - - JointDataMimic(const JointDataMimic & other) - { - *this = other; - } - - JointDataMimic(const JointDataBase & jdata, const Scalar & scaling) + m_q_transform.resize(0); + m_v_transform.resize(0); + } + + // JointDataMimicTpl(const JointDataMimicTpl & other) + // { *this = other; } + + JointDataMimicTpl(const RefJointData & jdata, + const Scalar & scaling, + const Scalar & nq, + const Scalar & nv) : m_jdata_ref(jdata.derived()) , m_scaling(scaling) - , S(m_jdata_ref.S, scaling) + , S(m_jdata_ref.S(),scaling) { + m_q_transform.resize(nq, 1); + m_v_transform.resize(nv, 1); } - - JointDataMimic & operator=(const JointDataMimic & other) + + JointDataMimicTpl & operator=(const JointDataMimicTpl & other) { m_jdata_ref = other.m_jdata_ref; m_scaling = other.m_scaling; - joint_q = other.joint_q; - joint_v = other.joint_v; - S = Constraint_t(m_jdata_ref.S, other.m_scaling); + m_q_transform = other.m_q_transform; + m_v_transform = other.m_v_transform; + S = Constraint_t(m_jdata_ref.S(),other.m_scaling); return *this; } - + using Base::isEqual; - bool isEqual(const JointDataMimic & other) const - { - return Base::isEqual(other) && internal::comparison_eq(m_jdata_ref, other.m_jdata_ref) - && internal::comparison_eq(m_scaling, other.m_scaling) - && internal::comparison_eq(joint_q, other.joint_q) - && internal::comparison_eq(joint_v, other.joint_v); - } - - static std::string classname() - { - return std::string("JointDataMimic<") + JointData::classname() + std::string(">"); - } - - std::string shortname() const - { - return std::string("JointDataMimic<") + m_jdata_ref.shortname() + std::string(">"); - } - - // Accessors - ConfigVectorTypeConstRef joint_q_accessor() const - { - return joint_q; - } - ConfigVectorTypeRef joint_q_accessor() - { - return joint_q; - } - - TangentVectorTypeConstRef joint_v_accessor() const - { - return joint_v; - } - TangentVectorTypeRef joint_v_accessor() - { - return joint_v; - } - - ConstraintTypeConstRef S_accessor() const - { - return S; - } - ConstraintTypeRef S_accessor() - { - return S; - } - - TansformTypeConstRef M_accessor() const - { - return m_jdata_ref.M; - } - TansformTypeRef M_accessor() - { - return m_jdata_ref.M; - } - - MotionTypeConstRef v_accessor() const - { - return m_jdata_ref.v; - } - MotionTypeRef v_accessor() - { - return m_jdata_ref.v; - } - - BiasTypeConstRef c_accessor() const - { - return m_jdata_ref.c; - } - BiasTypeRef c_accessor() - { - return m_jdata_ref.c; - } - - UTypeConstRef U_accessor() const - { - return m_jdata_ref.U; - } - UTypeRef U_accessor() - { - return m_jdata_ref.U; - } - - DTypeConstRef Dinv_accessor() const - { - return m_jdata_ref.Dinv; - } - DTypeRef Dinv_accessor() + bool isEqual(const JointDataMimicTpl & other) const { - return m_jdata_ref.Dinv; - } - - UDTypeConstRef UDinv_accessor() const - { - return m_jdata_ref.UDinv; - } - UDTypeRef UDinv_accessor() - { - return m_jdata_ref.UDinv; + return Base::isEqual(other) + && m_jdata_ref == other.m_jdata_ref + && m_scaling == other.m_scaling + && m_q_transform == other.m_q_transform + && m_v_transform == other.m_v_transform + ; } - - DTypeConstRef StU_accessor() const - { - return m_jdata_ref.StU; + + static std::string classname() { return std::string("JointDataMimic"); } + + std::string shortname() const { return classname(); } + + // // Accessors + ConstraintTypeConstRef S_accessor() const { return S; } + ConstraintTypeRef S_accessor() { return S; } + + TansformTypeConstRef M_accessor() const { + M_ = m_jdata_ref.M(); + return M_; } + TansformTypeRef M_accessor() { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + M_ = m_jdata_ref.M(); + return M_; } + + MotionTypeConstRef v_accessor() const { + v_ = m_jdata_ref.v(); + return v_; } + MotionTypeRef v_accessor() { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + v_ = m_jdata_ref.v(); + return v_; } + + BiasTypeConstRef c_accessor() const { + c_ = m_jdata_ref.c(); + return c_; } + BiasTypeRef c_accessor() { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + c_ = m_jdata_ref.c(); + return c_; } + + UTypeConstRef U_accessor() const { + U_ = m_jdata_ref.U(); + return U_; } + UTypeRef U_accessor() { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + U_ = m_jdata_ref.U(); + return U_; } + + DTypeConstRef Dinv_accessor() const { + Dinv_ = m_jdata_ref.Dinv(); + return Dinv_; } + DTypeRef Dinv_accessor() { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + Dinv_ = m_jdata_ref.Dinv(); + return Dinv_; } + + UDTypeConstRef UDinv_accessor() const { + UDinv_ = m_jdata_ref.UDinv(); + return UDinv_; } + UDTypeRef UDinv_accessor() { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + UDinv_ = m_jdata_ref.UDinv(); + return UDinv_; } + + DTypeConstRef StU_accessor() const + { + StU_ = m_jdata_ref.StU(); + return StU_; } DTypeRef StU_accessor() { - return m_jdata_ref.StU; - } - - template - friend struct JointModelMimic; - - const JointData & jdata() const - { - return m_jdata_ref; - } - JointData & jdata() - { - return m_jdata_ref; - } - - const Scalar & scaling() const - { - return m_scaling; - } - Scalar & scaling() - { - return m_scaling; - } - - ConfigVector_t & jointConfiguration() - { - return joint_q; - } - const ConfigVector_t & jointConfiguration() const - { - return joint_q; - } - - TangentVector_t & jointVelocity() - { - return joint_v; - } - const TangentVector_t & jointVelocity() const - { - return joint_v; - } - + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + StU_ = m_jdata_ref.StU(); + return StU_; + } + + friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>; + + const RefJointData & jdata() const { return m_jdata_ref; } + RefJointData & jdata() { return m_jdata_ref; } + + const Scalar & scaling() const { return m_scaling; } + Scalar & scaling() { return m_scaling; } + + ConfigVector_t & joint_q_accessor() { return m_q_transform; } + const ConfigVector_t & joint_q_accessor() const { return m_q_transform; } + + TangentVector_t & joint_v_accessor() { return m_v_transform; } + const TangentVector_t & joint_v_accessor() const { return m_v_transform; } + protected: - JointData m_jdata_ref; + + RefJointData m_jdata_ref; Scalar m_scaling; /// \brief Transform configuration vector - ConfigVector_t joint_q; + ConfigVector_t m_q_transform; /// \brief Transform velocity vector. - TangentVector_t joint_v; + TangentVector_t m_v_transform; public: // data - Constraint_t S; - - }; // struct JointDataMimic + Constraint_t S; - template - struct CastType> + protected: + /// \brief Buffer variable for accessors to return references + mutable Transformation_t M_; + mutable Motion_t v_; + mutable Bias_t c_; + mutable U_t U_; + mutable D_t Dinv_; + mutable UD_t UDinv_; + mutable D_t StU_; + + }; // struct JointDataMimicTpl + + template class JointCollectionTpl> + struct CastType< NewScalar, JointModelMimicTpl > { - typedef typename CastType::type JointModelNewType; - typedef JointModelMimic type; + typedef JointModelMimicTpl type; }; - - template - struct JointModelMimic : public JointModelBase> + + template class JointCollectionTpl> + struct JointModelMimicTpl + : public JointModelBase< JointModelMimicTpl<_Scalar,_Options,JointCollectionTpl> > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef JointModelBase Base; + typedef JointMimicTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); + + typedef JointCollectionTpl JointCollection; + typedef JointModelTpl JointModel; + typedef JointModel JointModelVariant; + // typedef typename boost::make_variant_over>::type>::type MimicableJointModelVariant; + // typedef JointModelTpl<_Scalar, _Options, JointCollectionMimicableTpl> MimicableJointModel; - typedef typename traits::JointDerived JointDerived; - PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); + typedef SE3Tpl SE3; + typedef MotionTpl Motion; + typedef InertiaTpl Inertia; - typedef JointModelBase Base; using Base::id; using Base::idx_q; using Base::idx_v; + using Base::idx_j; using Base::nq; using Base::nv; + using Base::nj; using Base::setIndexes; - - JointModelMimic() - { - } - - JointModelMimic( - const JointModelBase & jmodel, const Scalar & scaling, const Scalar & offset) + + JointModelMimicTpl() + {} + + template + JointModelMimicTpl(const JointModelBase & jmodel, + const Scalar & scaling, + const Scalar & offset) : m_jmodel_ref(jmodel.derived()) , m_scaling(scaling) , m_offset(offset) - { - } - - Base & base() - { - return *static_cast(this); - } - const Base & base() const - { - return *static_cast(this); - } - - inline int nq_impl() const - { - return 0; - } - inline int nv_impl() const - { - return 0; - } - - inline int idx_q_impl() const - { - return m_jmodel_ref.idx_q(); - } - inline int idx_v_impl() const - { - return m_jmodel_ref.idx_v(); - } - - void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/) + { } + + Base & base() { return *static_cast(this); } + const Base & base() const { return *static_cast(this); } + + inline int nq_impl() const { return 0; } + inline int nv_impl() const { return 0; } + inline int nj_impl() const { return m_jmodel_ref.nj(); } + + inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); } + inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); } + + void setIndexes_impl(JointIndex id, int q, int v, int j) { Base::i_id = id; // Only the id of the joint in the model is different. Base::i_q = m_jmodel_ref.idx_q(); Base::i_v = m_jmodel_ref.idx_v(); + Base::i_j = j; } - + JointDataDerived createData() const { - return JointDataDerived(m_jmodel_ref.createData(), scaling()); - } + return JointDataDerived(m_jmodel_ref.createData(),scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); + } + const std::vector hasConfigurationLimit() const { return m_jmodel_ref.hasConfigurationLimit(); @@ -648,38 +634,33 @@ namespace pinocchio } template - EIGEN_DONT_INLINE void - calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const + EIGEN_DONT_INLINE + void calc(JointDataDerived & jdata, + const typename Eigen::MatrixBase & qs) const { typedef typename ConfigVectorAffineTransform::Type AffineTransform; - - AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); - m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q); + + AffineTransform::run(qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), + m_scaling,m_offset,jdata.m_q_transform); + m_jmodel_ref.calc(jdata.m_jdata_ref,qs); } - - template - EIGEN_DONT_INLINE void calc( - JointDataDerived & jdata, - const Blank blank, - const typename Eigen::MatrixBase & vs) const - { - jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, blank, jdata.joint_v); - } - + template - EIGEN_DONT_INLINE void calc( - JointDataDerived & jdata, - const typename Eigen::MatrixBase & qs, - const typename Eigen::MatrixBase & vs) const + EIGEN_DONT_INLINE + void calc(JointDataDerived & jdata, + const typename Eigen::MatrixBase & qs, + const typename Eigen::MatrixBase & vs) const { typedef typename ConfigVectorAffineTransform::Type AffineTransform; - - AffineTransform::run(qs.head(m_jmodel_ref.nq()), m_scaling, m_offset, jdata.joint_q); - jdata.joint_v = m_scaling * vs.head(m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q, jdata.joint_v); - } - + + AffineTransform::run(qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), + m_scaling,m_offset,jdata.m_q_transform); + jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + m_jmodel_ref.calc(jdata.m_jdata_ref, + qs, + vs); + } + template void calc_aba( JointDataDerived & data, @@ -688,159 +669,175 @@ namespace pinocchio const bool update_I) const { // TODO: fixme - m_jmodel_ref.calc_aba( - data.m_jdata_ref, armature, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); - } - - static std::string classname() - { - return std::string("JointModelMimic<") + JointModel::classname() + std::string(">"); - ; - } - - std::string shortname() const - { - return std::string("JointModelMimic<") + m_jmodel_ref.shortname() + std::string(">"); - } - + assert(false && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use this function"); + m_jmodel_ref.calc_aba(data.m_jdata_ref, + armature, + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I), + update_I); + } + + static std::string classname() { return std::string("JointModelMimic"); } + + std::string shortname() const { return classname(); } + /// \returns An expression of *this with the Scalar type casted to NewScalar. template - typename CastType::type cast() const - { - typedef typename CastType::type ReturnType; - - ReturnType res( - m_jmodel_ref.template cast(), pinocchio::cast(m_scaling), - pinocchio::cast(m_offset)); - res.setIndexes(id(), idx_q(), idx_v()); + typename CastType::type cast() const + { + typedef typename CastType::type ReturnType; + + ReturnType res(m_jmodel_ref.template cast(), + (NewScalar)m_scaling, + (NewScalar)m_offset); + res.setIndexes(id(),idx_q(),idx_v(), idx_j()); return res; } - - const JointModel & jmodel() const - { - return m_jmodel_ref; - } - JointModel & jmodel() - { - return m_jmodel_ref; - } - - const Scalar & scaling() const - { - return m_scaling; - } - Scalar & scaling() - { - return m_scaling; - } - - const Scalar & offset() const - { - return m_offset; - } - Scalar & offset() - { - return m_offset; - } - + + const JointModel & jmodel() const { return m_jmodel_ref; } + JointModel & jmodel() { return m_jmodel_ref; } + + const Scalar & scaling() const { return m_scaling; } + Scalar & scaling() { return m_scaling; } + + const Scalar & offset() const { return m_offset; } + Scalar & offset() { return m_offset; } + protected: + // data JointModel m_jmodel_ref; Scalar m_scaling, m_offset; - + public: + /* Acces to dedicated segment in robot config space. */ // Const access template typename SizeDepType::template SegmentReturn::ConstType jointConfigSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), + m_jmodel_ref.idx_q(), + m_jmodel_ref.nq()); } - + // Non-const access template typename SizeDepType::template SegmentReturn::Type jointConfigSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), + m_jmodel_ref.idx_q(), + m_jmodel_ref.nq()); } - + /* Acces to dedicated segment in robot config velocity space. */ // Const access template typename SizeDepType::template SegmentReturn::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), + m_jmodel_ref.idx_v(), + m_jmodel_ref.nv()); } - + // Non-const access template typename SizeDepType::template SegmentReturn::Type jointVelocitySelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), + m_jmodel_ref.idx_v(), + m_jmodel_ref.nv()); } - + /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ // Const access - template - typename SizeDepType::template ColsReturn::ConstType - jointCols_impl(const Eigen::MatrixBase & A) const - { - return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - } - - // Non-const access - template - typename SizeDepType::template ColsReturn::Type - jointCols_impl(Eigen::MatrixBase & A) const - { - return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - } - - /* Acces to dedicated rows in a matrix.*/ - // Const access - template - typename SizeDepType::template RowsReturn::ConstType - jointRows_impl(const Eigen::MatrixBase & A) const - { - return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - } - - // Non-const access - template - typename SizeDepType::template RowsReturn::Type - jointRows_impl(Eigen::MatrixBase & A) const - { - return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - } - - /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the - /// matrix Mat - // Const access - template - typename SizeDepType::template BlockReturn::ConstType - jointBlock_impl(const Eigen::MatrixBase & Mat) const - { - return SizeDepType::block( - Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), - m_jmodel_ref.nv()); - } - - // Non-const access - template - typename SizeDepType::template BlockReturn::Type - jointBlock_impl(Eigen::MatrixBase & Mat) const - { - return SizeDepType::block( - Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), - m_jmodel_ref.nv()); - } - - }; // struct JointModelMimic - + // template + // typename SizeDepType::template ColsReturn::ConstType + // jointCols_impl(const Eigen::MatrixBase & A) const + // { + // return SizeDepType::middleCols(A.derived(), + // m_jmodel_ref.idx_v(), + // m_jmodel_ref.nv()); + // } + + // // Non-const access + // template + // typename SizeDepType::template ColsReturn::Type + // jointCols_impl(Eigen::MatrixBase & A) const + // { + // return SizeDepType::middleCols(A.derived(), + // m_jmodel_ref.idx_v(), + // m_jmodel_ref.nv()); + // } + + // /* Acces to dedicated rows in a matrix.*/ + // // Const access + // template + // typename SizeDepType::template RowsReturn::ConstType + // jointRows_impl(const Eigen::MatrixBase & A) const + // { + // return SizeDepType::middleRows(A.derived(), + // m_jmodel_ref.idx_v(), + // m_jmodel_ref.nv()); + // } + + // // Non-const access + // template + // typename SizeDepType::template RowsReturn::Type + // jointRows_impl(Eigen::MatrixBase & A) const + // { + // return SizeDepType::middleRows(A.derived(), + // m_jmodel_ref.idx_v(), + // m_jmodel_ref.nv()); + // } + + // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat + // // Const access + // template + // typename SizeDepType::template BlockReturn::ConstType + // jointBlock_impl(const Eigen::MatrixBase & Mat) const + // { + // return SizeDepType::block(Mat.derived(), + // m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(), + // m_jmodel_ref.nv(),m_jmodel_ref.nv()); + // } + + // // Non-const access + // template + // typename SizeDepType::template BlockReturn::Type + // jointBlock_impl(Eigen::MatrixBase & Mat) const + // { + // return SizeDepType::block(Mat.derived(), + // m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(), + // m_jmodel_ref.nv(),m_jmodel_ref.nv()); + // } + + }; // struct JointModelMimicTpl + } // namespace pinocchio -#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ +#include + +namespace boost +{ + template class JointCollectionTpl> + struct has_nothrow_constructor< ::pinocchio::JointModelMimicTpl > + : public integral_constant {}; + + template class JointCollectionTpl> + struct has_nothrow_copy< ::pinocchio::JointModelMimicTpl > + : public integral_constant {}; + + template class JointCollectionTpl> + struct has_nothrow_constructor< ::pinocchio::JointDataMimicTpl > + : public integral_constant {}; + + template class JointCollectionTpl> + struct has_nothrow_copy< ::pinocchio::JointDataMimicTpl > + : public integral_constant {}; +} + +#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ \ No newline at end of file From c4b6962d7ec2daa2b18a2d5bb0a6d1cde4138739 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 005/166] [Model] Support for nj and idx_j --- include/pinocchio/multibody/model.hpp | 13 +++++++++++++ include/pinocchio/multibody/model.hxx | 20 +++++++++++++++----- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index 37a12fc81e..0126004e1f 100644 --- a/include/pinocchio/multibody/model.hpp +++ b/include/pinocchio/multibody/model.hpp @@ -100,6 +100,9 @@ namespace pinocchio /// \brief Dimension of the velocity vector space. int nv; + /// \brief Dimension of the jacobian space. + int nj; + /// \brief Number of joints. int njoints; @@ -130,6 +133,13 @@ namespace pinocchio /// \brief Dimension of the *i*th joint tangent subspace. std::vector nvs; + /// \brief Starting index of the *i*th joint in the jacobian space. + std::vector idx_js; + + /// \brief Dimension of the *i*th joint jacobian subspace. + std::vector njs; + + /// \brief Vector of parent joint indexes. The parent of joint *i*, denoted *li*, corresponds to /// li==parents[i]. std::vector parents; @@ -200,6 +210,7 @@ namespace pinocchio ModelTpl() : nq(0) , nv(0) + , nj(0) , njoints(1) , nbodies(1) , nframes(0) @@ -210,6 +221,8 @@ namespace pinocchio , nqs(1, 0) , idx_vs(1, 0) , nvs(1, 0) + , idx_js(1, 0) + , njs(1, 0) , parents(1, 0) , children(1) , names(1) diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index f8bd3fcbe2..25e869cb48 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -72,7 +72,7 @@ namespace pinocchio assert( (njoints == (int)joints.size()) && (njoints == (int)inertias.size()) && (njoints == (int)parents.size()) && (njoints == (int)jointPlacements.size())); - assert((joint_model.nq() >= 0) && (joint_model.nv() >= 0)); + assert((joint_model.nq() >= 0) && (joint_model.nv() >= 0) && (joint_model.nj() >= 0)); assert(joint_model.nq() >= joint_model.nv()); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -97,15 +97,18 @@ namespace pinocchio joints.push_back(JointModel(joint_model.derived())); JointModel & jmodel = joints.back(); - jmodel.setIndexes(joint_id, nq, nv); + jmodel.setIndexes(joint_id, nq, nv, nj); const int joint_nq = jmodel.nq(); const int joint_idx_q = jmodel.idx_q(); const int joint_nv = jmodel.nv(); const int joint_idx_v = jmodel.idx_v(); + const int joint_nj = jmodel.nj(); + const int joint_idx_j = jmodel.idx_j(); assert(joint_idx_q >= 0); assert(joint_idx_v >= 0); + assert(joint_idx_j >= 0); inertias.push_back(Inertia::Zero()); parents.push_back(parent); @@ -120,6 +123,11 @@ namespace pinocchio nv += joint_nv; nvs.push_back(joint_nv); idx_vs.push_back(joint_idx_v); + nj += joint_nj; + njs.push_back(joint_nj); + idx_js.push_back(joint_idx_j); + + if (joint_nq > 0 && joint_nv > 0) { @@ -230,6 +238,7 @@ namespace pinocchio ReturnType res; res.nq = nq; res.nv = nv; + res.nj = nj; res.njoints = njoints; res.nbodies = nbodies; res.nframes = nframes; @@ -245,7 +254,8 @@ namespace pinocchio res.nqs = nqs; res.idx_vs = idx_vs; res.nvs = nvs; - + res.idx_js = idx_js; + res.njs = njs; // Eigen Vectors res.armature = armature.template cast(); res.friction = friction.template cast(); @@ -289,12 +299,12 @@ namespace pinocchio template class JointCollectionTpl> bool ModelTpl::operator==(const ModelTpl & other) const { - bool res = other.nq == nq && other.nv == nv && other.njoints == njoints + bool res = other.nq == nq && other.nv == nv && other.nj == nj && other.njoints == njoints && other.nbodies == nbodies && other.nframes == nframes && other.parents == parents && other.children == children && other.names == names && other.subtrees == subtrees && other.gravity == gravity && other.name == name; - res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs; + res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs && other.idx_js == idx_js && other.njs == njs; if (other.referenceConfigurations.size() != referenceConfigurations.size()) return false; From 3e048872405864b6bd992dd3521bb026c067c420 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 006/166] [Data] Jacobian is now indexed on nj --- include/pinocchio/algorithm/check.hxx | 9 +++++++-- include/pinocchio/multibody/data.hxx | 19 +++++++++++++------ 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 8b0e9045e6..4c54254ba1 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -124,8 +124,8 @@ namespace pinocchio CHECK_DATA(data.U.rows() == model.nv); CHECK_DATA(data.D.size() == model.nv); CHECK_DATA(data.tmp.size() >= model.nv); - CHECK_DATA(data.J.cols() == model.nv); - CHECK_DATA(data.Jcom.cols() == model.nv); + CHECK_DATA(data.J.cols() == model.nj); + CHECK_DATA(data.Jcom.cols() == model.nj); CHECK_DATA(data.torque_residual.size() == model.nv); CHECK_DATA(data.dq_after.size() == model.nv); // CHECK_DATA( data.impulse_c.size()== model.nv ); @@ -154,6 +154,8 @@ namespace pinocchio CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q()); CHECK_DATA(model.nvs[joint_id] == jmodel.nv()); CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v()); + CHECK_DATA(model.njs[joint_id] == jmodel.nj()); + CHECK_DATA(model.idx_js[joint_id] == jmodel.idx_j()); } for (JointIndex j = 1; int(j) < model.njoints; ++j) @@ -172,6 +174,9 @@ namespace pinocchio CHECK_DATA((model.parents[d] < j) || (model.parents[d] > c)); int row = model.joints[j].idx_v(); + if(boost::get(&model.joints[j])) + continue ; + CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]); const JointModel & jparent = model.joints[model.parents[j]]; diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index c0aed6741c..8863485d77 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -79,9 +79,9 @@ namespace pinocchio , parents_fromRow((std::size_t)model.nv, -1) , supports_fromRow((std::size_t)model.nv) , nvSubtree_fromRow((std::size_t)model.nv, -1) - , J(Matrix6x::Zero(6, model.nv)) - , dJ(Matrix6x::Zero(6, model.nv)) - , ddJ(Matrix6x::Zero(6, model.nv)) + , J(Matrix6x::Zero(6, model.nj)) + , dJ(Matrix6x::Zero(6, model.nj)) + , ddJ(Matrix6x::Zero(6, model.nj)) , psid(Matrix6x::Zero(6, model.nv)) , psidd(Matrix6x::Zero(6, model.nv)) , dVdq(Matrix6x::Zero(6, model.nv)) @@ -97,7 +97,7 @@ namespace pinocchio , vcom((std::size_t)model.njoints, Vector3::Zero()) , acom((std::size_t)model.njoints, Vector3::Zero()) , mass((std::size_t)model.njoints, (Scalar)(-1)) - , Jcom(Matrix3x::Zero(3, model.nv)) + , Jcom(Matrix3x::Zero(3, model.nj)) , kinetic_energy(Scalar(0)) , potential_energy(Scalar(0)) , mechanical_energy(Scalar(0)) @@ -200,10 +200,17 @@ namespace pinocchio if (lastChild[(Index)i] == -1) lastChild[(Index)i] = i; const Index & parent = model.parents[(Index)i]; - lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); + + lastChild[parent] = std::max(lastChild[(Index)i],lastChild[parent]); + Scalar nv_; + + if(boost::get(&model.joints[(Index)lastChild[(Index)i]]) && lastChild[(Index)i] != i) + nv_ = boost::get(model.joints[(Index)lastChild[(Index)i]]).jmodel().nv(); + else + nv_ = nv(model.joints[(Index)lastChild[(Index)i]]); nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_v() - + model.joints[(Index)lastChild[(Index)i]].nv() + + nv_ - model.joints[(Index)i].idx_v(); } } From 30a7c17d5f3ca486565d187a5e5872a37fd63cac Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 007/166] [LieGroup] Neutral Specialization for mimic joints --- .../pinocchio/multibody/liegroup/liegroup-algo.hxx | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index 374d5aaea7..72dacdd5a0 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -162,6 +162,15 @@ namespace pinocchio using Algo::run; \ }; +#define PINOCCHIO_DETAILS_JOINT_MIMIC_1(Algo) \ + template \ + struct Algo> { \ + typedef typename Visitor::ArgsType ArgsType; \ + static void run (PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModelMimicTpl)) \ + {} \ + } + + } // namespace details template @@ -725,6 +734,8 @@ namespace pinocchio }; PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NeutralStepAlgo); + PINOCCHIO_DETAILS_JOINT_MIMIC_1(NeutralStepAlgo); + template struct IntegrateCoeffWiseJacobianStepAlgo; From e35415e86a154bdadfe577a0fa0984af7ef72b92 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 008/166] [Serialization] Fix serialization --- .../serialization/joints-constraint.hpp | 0 include/pinocchio/serialization/joints-data.hpp | 6 +++--- .../pinocchio/serialization/joints-model.hpp | 17 +++++++++++------ .../serialization/joints-motion-subspace.hpp | 4 ++-- 4 files changed, 16 insertions(+), 11 deletions(-) create mode 100644 include/pinocchio/serialization/joints-constraint.hpp diff --git a/include/pinocchio/serialization/joints-constraint.hpp b/include/pinocchio/serialization/joints-constraint.hpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 14f1e510a5..616a67c595 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -233,11 +233,11 @@ namespace boost ar & make_nvp("base_variant", base_object(joint)); } - template + template typename JointCollection> void serialize( - Archive & ar, pinocchio::JointDataMimic & joint, const unsigned int version) + Archive & ar, pinocchio::JointDataMimicTpl & joint, const unsigned int version) { - typedef pinocchio::JointDataMimic JointType; + typedef pinocchio::JointDataMimicTpl JointType; fix::serialize(ar, static_cast &>(joint), version); ar & make_nvp("jdata", joint.jdata()); diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 81d4167fd1..f565cf5bb1 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -18,10 +18,13 @@ namespace pinocchio ar & make_nvp("m_nq", joint.m_nq); ar & make_nvp("m_nv", joint.m_nv); + ar & make_nvp("m_nj", joint.m_nj); ar & make_nvp("m_idx_q", joint.m_idx_q); ar & make_nvp("m_nqs", joint.m_nqs); ar & make_nvp("m_idx_v", joint.m_idx_v); ar & make_nvp("m_nvs", joint.m_nvs); + ar & make_nvp("m_idx_j", joint.m_idx_j); + ar & make_nvp("m_njs", joint.m_njs); ar & make_nvp("njoints", joint.njoints); ar & make_nvp("joints", joint.joints); @@ -54,11 +57,12 @@ namespace boost const unsigned int /*version*/) { const pinocchio::JointIndex i_id = joint.id(); - const int i_q = joint.idx_q(), i_v = joint.idx_v(); + const int i_q = joint.idx_q(), i_v = joint.idx_v(), i_j = joint.idx_j(); ar & make_nvp("i_id", i_id); ar & make_nvp("i_q", i_q); ar & make_nvp("i_v", i_v); + ar & make_nvp("i_j", i_j); } template @@ -66,13 +70,14 @@ namespace boost load(Archive & ar, pinocchio::JointModelBase & joint, const unsigned int /*version*/) { pinocchio::JointIndex i_id; - int i_q, i_v; + int i_q, i_v, i_j; ar & make_nvp("i_id", i_id); ar & make_nvp("i_q", i_q); ar & make_nvp("i_v", i_v); + ar & make_nvp("i_j", i_j); - joint.setIndexes(i_id, i_q, i_v); + joint.setIndexes(i_id, i_q, i_v, i_j); } template @@ -284,11 +289,11 @@ namespace boost ar & make_nvp("base_variant", base_object(joint)); } - template + template void serialize( - Archive & ar, pinocchio::JointModelMimic & joint, const unsigned int version) + Archive & ar, pinocchio::JointModelMimicTpl & joint, const unsigned int version) { - typedef pinocchio::JointModelMimic JointType; + typedef pinocchio::JointModelMimicTpl JointType; // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); diff --git a/include/pinocchio/serialization/joints-motion-subspace.hpp b/include/pinocchio/serialization/joints-motion-subspace.hpp index 46a3571be1..154ce4110c 100644 --- a/include/pinocchio/serialization/joints-motion-subspace.hpp +++ b/include/pinocchio/serialization/joints-motion-subspace.hpp @@ -110,10 +110,10 @@ namespace boost ar & make_nvp("matrix", S.matrix()); } - template + template void serialize( Archive & ar, - pinocchio::ScaledJointMotionSubspace & S, + pinocchio::ScaledJointMotionSubspaceTpl & S, const unsigned int /*version*/) { ar & make_nvp("scaling", S.scaling()); From c30ec70757c5b5aa83fca5eb95fc998e21b1e1fe Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 009/166] [Unittest] Fix unittest --- unittest/aba.cpp | 8 +- unittest/all-joints.cpp | 37 +++--- unittest/finite-differences.cpp | 33 +++-- unittest/joint-composite.cpp | 14 +- unittest/joint-generic.cpp | 59 +++++---- unittest/joint-mimic.cpp | 200 +++++++++++++++-------------- unittest/joint-motion-subspace.cpp | 72 ++++++----- unittest/joint-prismatic.cpp | 2 +- unittest/liegroups.cpp | 6 +- unittest/serialization.cpp | 103 ++++++++------- unittest/visitor.cpp | 14 +- 11 files changed, 288 insertions(+), 260 deletions(-) diff --git a/unittest/aba.cpp b/unittest/aba.cpp index a8204b9336..b213083589 100644 --- a/unittest/aba.cpp +++ b/unittest/aba.cpp @@ -74,7 +74,7 @@ struct TestJointMethods void operator()(const pinocchio::JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -84,7 +84,7 @@ struct TestJointMethods pinocchio::JointModelComposite jmodel_composite; jmodel_composite.addJoint(pinocchio::JointModelRX()); jmodel_composite.addJoint(pinocchio::JointModelRY()); - jmodel_composite.setIndexes(0, 0, 0); + jmodel_composite.setIndexes(0, 0, 0, 0); // TODO: correct LieGroup // test_joint_methods(jmodel_composite); @@ -93,7 +93,7 @@ struct TestJointMethods void operator()(const pinocchio::JointModelBase &) const { pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -101,7 +101,7 @@ struct TestJointMethods void operator()(const pinocchio::JointModelBase &) const { pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index 072188ed94..ff0c78d168 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -25,7 +25,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -40,7 +40,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -55,7 +55,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -70,7 +70,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -85,7 +85,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -102,23 +102,24 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; -template -struct init> +template class JointCollection> +struct init > { - typedef pinocchio::JointModelMimic JointModel; - + typedef pinocchio::JointModelMimicTpl JointModel; + static JointModel run() { - JointModel_ jmodel_ref = init::run(); - - JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); + + JointModel jmodel(jmodel_ref,1.,0.); + jmodel.setIndexes(0,0,0,0); return jmodel; } }; @@ -132,7 +133,7 @@ struct init> { JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -146,7 +147,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -161,7 +162,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -216,7 +217,7 @@ BOOST_AUTO_TEST_CASE(isEqual) BOOST_CHECK(joint_revolutex != joint_revolutey); JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0); + jmodelx.setIndexes(0, 0, 0, 0); TestJointModelIsEqual()(JointModel()); JointModel jmodel_any; diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index 95ade70874..d4a06ceb1a 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -69,7 +69,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -84,7 +84,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -99,7 +99,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -114,7 +114,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -129,7 +129,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -144,7 +144,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -158,7 +158,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -173,7 +173,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -190,22 +190,23 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -217,6 +218,10 @@ struct FiniteDiffJoint { } + void operator()(JointModelMimic & /*jmodel*/) const + { + } + template void operator()(JointModelBase & /*jmodel*/) const { diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 37971f419e..edf5045572 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -33,7 +33,7 @@ void test_joint_methods( JointData jdata = jmodel.createData(); JointDataComposite jdata_composite = jmodel_composite.createData(); - jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v()); + jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v(), jmodel.idx_j()); typedef typename JointModel::ConfigVector_t ConfigVector_t; typedef typename JointModel::TangentVector_t TangentVector_t; @@ -161,7 +161,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -179,7 +179,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } @@ -187,7 +187,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_methods(jmodel); } }; @@ -220,7 +220,7 @@ BOOST_AUTO_TEST_CASE(chain) BOOST_AUTO_TEST_CASE(vsZYX) { JointModelSphericalZYX jmodel_spherical; - jmodel_spherical.setIndexes(0, 0, 0); + jmodel_spherical.setIndexes(0, 0, 0, 0); JointModelComposite jmodel_composite((JointModelRZ())); jmodel_composite.addJoint(JointModelRY()); @@ -232,7 +232,7 @@ BOOST_AUTO_TEST_CASE(vsZYX) BOOST_AUTO_TEST_CASE(vsTranslation) { JointModelTranslation jmodel_translation; - jmodel_translation.setIndexes(0, 0, 0); + jmodel_translation.setIndexes(0, 0, 0, 0); JointModelComposite jmodel_composite((JointModelPX())); jmodel_composite.addJoint(JointModelPY()); @@ -259,7 +259,7 @@ BOOST_AUTO_TEST_CASE(test_copy) JointModelComposite jmodel_composite_planar((JointModelPX())); jmodel_composite_planar.addJoint(JointModelPY()); jmodel_composite_planar.addJoint(JointModelRZ()); - jmodel_composite_planar.setIndexes(0, 0, 0); + jmodel_composite_planar.setIndexes(0, 0, 0, 0); JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData(); diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 13242896ac..9385f5cfe6 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -137,7 +137,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -152,7 +152,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -167,7 +167,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -182,7 +182,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -197,7 +197,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -214,23 +214,23 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; - -template -struct init> +template class JointCollection> +struct init > { - typedef pinocchio::JointModelMimic JointModel; - + typedef pinocchio::JointModelMimicTpl JointModel; + static JointModel run() { - JointModel_ jmodel_ref = init::run(); - - JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); - + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); + + JointModel jmodel(jmodel_ref,1.,0.); + jmodel.setIndexes(0,0,0,0); + return jmodel; } }; @@ -245,7 +245,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -259,7 +259,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -274,7 +274,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -286,7 +286,7 @@ struct TestJoint void operator()(const JointModelBase &) const { JointModel jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); typename JointModel::JointDataDerived jdata = jmodel.createData(); test_joint_methods(jmodel, jdata); @@ -295,6 +295,10 @@ struct TestJoint void operator()(const pinocchio::JointModelComposite &) const { } + + void operator()(const pinocchio::JointModelMimic &) const + { + } }; namespace pinocchio @@ -328,7 +332,8 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, // Dynamic because unknown at compile time - NV = Eigen::Dynamic + NV = Eigen::Dynamic, + NJ = Eigen::Dynamic }; typedef _Scalar Scalar; @@ -419,7 +424,7 @@ BOOST_AUTO_TEST_CASE(isEqual) BOOST_CHECK(joint_revolutex != joint_revolutey); JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0); + jmodelx.setIndexes(0, 0, 0, 0); JointModel jmodelx_copy = jmodelx; BOOST_CHECK(jmodelx_copy == jmodelx); @@ -436,7 +441,7 @@ BOOST_AUTO_TEST_CASE(cast) JointModelRX joint_revolutex; JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0); + jmodelx.setIndexes(0, 0, 0, 0); BOOST_CHECK(jmodelx.cast() == jmodelx); BOOST_CHECK(jmodelx.cast().cast() == jmodelx); @@ -472,10 +477,10 @@ struct TestJointOperatorEqual test(jdata); } - template - void operator()(const pinocchio::JointModelMimic &) const - { - } + // template + // void operator()(const pinocchio::JointModelMimic &) const + // { + // } template static void test(const JointData & jdata) diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 094658c7b0..67fc9ac22a 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -12,7 +12,8 @@ using namespace pinocchio; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) -typedef Eigen::Matrix Matrix6x; +typedef Eigen::Matrix Matrix6x; + template void test_constraint_mimic(const JointModelBase & jmodel) @@ -20,90 +21,89 @@ void test_constraint_mimic(const JointModelBase & jmodel) typedef typename traits::JointDerived Joint; typedef typename traits::Constraint_t ConstraintType; typedef typename traits::JointDataDerived JointData; - typedef ScaledJointMotionSubspace ScaledJointMotionSubspaceType; - + typedef ScaledJointMotionSubspaceTpl ScaledConstraint; + typedef JointMotionSubspaceTpl ConstraintRef; + JointData jdata = jmodel.createData(); - + const double scaling_factor = 2.; - ConstraintType constraint_ref(jdata.S), constraint_ref_shared(jdata.S); - ScaledJointMotionSubspaceType scaled_constraint(constraint_ref_shared, scaling_factor); - + ConstraintRef constraint_ref(jdata.S.matrix()), constraint_ref_shared(jdata.S.matrix()); + ScaledConstraint scaled_constraint(constraint_ref_shared,scaling_factor); + BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv()); - + typedef typename JointModel::TangentVector_t TangentVector_t; TangentVector_t v = TangentVector_t::Random(); - + Motion m = scaled_constraint * v; Motion m_ref = scaling_factor * (Motion)(constraint_ref * v); - + BOOST_CHECK(m.isApprox(m_ref)); - + { SE3 M = SE3::Random(); - typename ScaledJointMotionSubspaceType::DenseBase S = M.act(scaled_constraint); - typename ScaledJointMotionSubspaceType::DenseBase S_ref = - scaling_factor * M.act(constraint_ref); - + typename ScaledConstraint::DenseBase S = M.act(scaled_constraint); + typename ScaledConstraint::DenseBase S_ref = scaling_factor * M.act(constraint_ref); + BOOST_CHECK(S.isApprox(S_ref)); } - + { - typename ScaledJointMotionSubspaceType::DenseBase S = scaled_constraint.matrix(); - typename ScaledJointMotionSubspaceType::DenseBase S_ref = - scaling_factor * constraint_ref.matrix(); - + typename ScaledConstraint::DenseBase S = scaled_constraint.matrix(); + typename ScaledConstraint::DenseBase S_ref = scaling_factor * constraint_ref.matrix(); + BOOST_CHECK(S.isApprox(S_ref)); } - + { Motion v = Motion::Random(); - typename ScaledJointMotionSubspaceType::DenseBase S = v.cross(scaled_constraint); - typename ScaledJointMotionSubspaceType::DenseBase S_ref = - scaling_factor * v.cross(constraint_ref); - + typename ScaledConstraint::DenseBase S = v.cross(scaled_constraint); + typename ScaledConstraint::DenseBase S_ref = scaling_factor * v.cross(constraint_ref); + BOOST_CHECK(S.isApprox(S_ref)); } - + // Test transpose operations { const Eigen::DenseIndex dim = 20; - const Matrix6x Fin = Matrix6x::Random(6, dim); + const Matrix6x Fin = Matrix6x::Random(6,dim); Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin; Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin); BOOST_CHECK(Fout.isApprox(Fout_ref)); - + Force force_in(Force::Random()); Eigen::MatrixXd Stf = (scaled_constraint.transpose() * force_in); Eigen::MatrixXd Stf_ref = scaling_factor * (constraint_ref.transpose() * force_in); BOOST_CHECK(Stf_ref.isApprox(Stf)); } - + // CRBA Y*S { Inertia Y = Inertia::Random(); Eigen::MatrixXd YS = Y * scaled_constraint; Eigen::MatrixXd YS_ref = scaling_factor * (Y * constraint_ref); - + BOOST_CHECK(YS.isApprox(YS_ref)); } + } struct TestJointConstraint { - - template + + template void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); - + jmodel.setIndexes(0,0,0,0); + test_constraint_mimic(jmodel); } - + void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0,0,0,0); test_constraint_mimic(jmodel); } @@ -111,21 +111,23 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0,0,0,0); test_constraint_mimic(jmodel); } + }; BOOST_AUTO_TEST_CASE(test_constraint) { using namespace pinocchio; typedef boost::variant< - JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, - JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelRUBX, JointModelRUBY, - JointModelRUBZ> - Variant; - + JointModelRX, JointModelRY, JointModelRZ + , JointModelRevoluteUnaligned + , JointModelPX, JointModelPY, JointModelPZ + , JointModelPrismaticUnaligned + > Variant; + boost::mpl::for_each(TestJointConstraint()); } @@ -134,68 +136,66 @@ void test_joint_mimic(const JointModelBase & jmodel) { typedef typename traits::JointDerived Joint; typedef typename traits::JointDataDerived JointData; - + JointData jdata = jmodel.createData(); - + const double scaling_factor = 1.; const double offset = 0.; - - typedef JointMimic JointMimicType; - typedef typename traits::JointModelDerived JointModelMimicType; - typedef typename traits::JointDataDerived JointDataMimicType; - + // test constructor - JointModelMimicType jmodel_mimic(jmodel.derived(), scaling_factor, offset); - JointDataMimicType jdata_mimic = jmodel_mimic.createData(); + JointModelMimic jmodel_mimic(jmodel.derived(),scaling_factor,offset); + JointDataMimic jdata_mimic = jmodel_mimic.createData(); + // Non-const ref accessors trigger asserts, usefull const ref to call const ref accessors... + const JointDataMimic & jdata_mimic_const_ref {jdata_mimic}; + BOOST_CHECK(jmodel_mimic.nq() == 0); BOOST_CHECK(jmodel_mimic.nv() == 0); - + BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q()); BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v()); - + BOOST_CHECK(jmodel_mimic.idx_q() == 0); BOOST_CHECK(jmodel_mimic.idx_v() == 0); - - typedef typename JointModelMimicType::ConfigVector_t ConfigVectorType; + + typedef typename JointModel::ConfigVector_t ConfigVectorType; typedef typename LieGroup::type LieGroupType; - ConfigVectorType q0 = - LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); + ConfigVectorType q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones()); + + jmodel.calc(jdata,q0); + jmodel_mimic.calc(jdata_mimic,q0); - jmodel.calc(jdata, q0); - jmodel_mimic.calc(jdata_mimic, q0); - - BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); + BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic_const_ref.M_accessor())); BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); - typedef typename JointModelMimicType::TangentVector_t TangentVectorType; - - q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); + typedef typename JointModel::TangentVector_t TangentVectorType; + + q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones()); TangentVectorType v0 = TangentVectorType::Random(); - jmodel.calc(jdata, q0, v0); - jmodel_mimic.calc(jdata_mimic, q0, v0); - - BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); + jmodel.calc(jdata,q0,v0); + jmodel_mimic.calc(jdata_mimic,q0,v0); + + BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic_const_ref.M())); BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); - BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v())); + BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic_const_ref.v())); } struct TestJointMimic { - - template + + template void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); - + jmodel.setIndexes(0,0,0,0); + test_joint_mimic(jmodel); } - + void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0,0,0,0); test_joint_mimic(jmodel); } @@ -203,21 +203,23 @@ struct TestJointMimic void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0,0,0,0); test_joint_mimic(jmodel); } + }; BOOST_AUTO_TEST_CASE(test_joint) { using namespace pinocchio; typedef boost::variant< - JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, - JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelRUBX, JointModelRUBY, - JointModelRUBZ> - Variant; - + JointModelRX, JointModelRY, JointModelRZ + , JointModelRevoluteUnaligned + , JointModelPX, JointModelPY, JointModelPZ + , JointModelPrismaticUnaligned + > Variant; + boost::mpl::for_each(TestJointMimic()); } @@ -225,14 +227,14 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_affine) { typedef JointModelRX::ConfigVector_t ConfigVectorType; double scaling = 1., offset = 0.; - + ConfigVectorType q0 = ConfigVectorType::Random(); ConfigVectorType q1; - LinearAffineTransform::run(q0, scaling, offset, q1); + LinearAffineTransform::run(q0,scaling,offset,q1); BOOST_CHECK(q0 == q1); - + offset = 2.; - LinearAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); + LinearAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1); BOOST_CHECK(q1 == ConfigVectorType::Constant(offset)); } @@ -240,32 +242,32 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_revolute) { typedef JointModelRUBX::ConfigVector_t ConfigVectorType; double scaling = 1., offset = 0.; - + ConfigVectorType q0 = ConfigVectorType::Random().normalized(); ConfigVectorType q1; - UnboundedRevoluteAffineTransform::run(q0, scaling, offset, q1); + UnboundedRevoluteAffineTransform::run(q0,scaling,offset,q1); BOOST_CHECK(q0.isApprox(q1)); - + offset = 2.; - UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); - BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset), math::sin(offset))); + UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1); + BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset),math::sin(offset))); } BOOST_AUTO_TEST_CASE(test_joint_generic_cast) { JointModelRX jmodel_ref; - jmodel_ref.setIndexes(1, 2, 3); - - JointModelMimic jmodel(jmodel_ref, 2., 1.); - jmodel.setIndexes(1, -1, -1); - + jmodel_ref.setIndexes(1,2,3,3); + + JointModelMimic jmodel(jmodel_ref,2.,1.); + jmodel.setIndexes(1,-1,-1,3); + BOOST_CHECK(jmodel.id() == jmodel_ref.id()); BOOST_CHECK(jmodel.idx_q() == jmodel_ref.idx_q()); BOOST_CHECK(jmodel.idx_v() == jmodel_ref.idx_v()); - + JointModel jmodel_generic(jmodel); - jmodel_generic.setIndexes(1, -2, -2); - + jmodel_generic.setIndexes(1,-2,-2,3); + BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id()); } -BOOST_AUTO_TEST_SUITE_END() +BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index ba1b90bc38..316793cd9e 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -75,8 +75,9 @@ void test_jmodel_nq_against_nq_ref(const JointModelBase & jmodel, co BOOST_CHECK(jmodel.nq() == nq_ref); } -template -void test_jmodel_nq_against_nq_ref(const JointModelMimic & jmodel, const int & nq_ref) +template class JointCollection> +void test_jmodel_nq_against_nq_ref(const JointModelMimicTpl & jmodel, + const int & nq_ref) { BOOST_CHECK(jmodel.jmodel().nq() == nq_ref); } @@ -89,9 +90,8 @@ void test_nv_against_jmodel( BOOST_CHECK(constraint.nv() == jmodel.nv()); } -template -void test_nv_against_jmodel( - const JointModelMimic & jmodel, +template class JointCollection, typename ConstraintDerived> +void test_nv_against_jmodel(const JointModelMimicTpl & jmodel, const JointMotionSubspaceBase & constraint) { BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv()); @@ -109,20 +109,20 @@ struct buildModel } }; -template -struct buildModel> -{ - typedef JointModelMimic JointModel_; +// template +// struct buildModel> +// { +// typedef JointModelMimic JointModel_; - static Model run(const JointModel_ & jmodel) - { - Model model; - model.addJoint(0, jmodel.jmodel(), SE3::Identity(), "joint"); - model.addJoint(0, jmodel, SE3::Identity(), "joint_mimic"); +// static Model run(const JointModel_ & jmodel) +// { +// Model model; +// model.addJoint(0, jmodel.jmodel(), SE3::Identity(), "joint"); +// model.addJoint(0, jmodel, SE3::Identity(), "joint_mimic"); - return model; - } -}; +// return model; +// } +// }; template void test_constraint_operations(const JointModelBase & jmodel) @@ -268,6 +268,11 @@ void test_constraint_operations(const JointModelBase & jmodel) } } +template class JointCollection> +void test_constraint_operations(const JointModelMimicTpl & /*jmodel*/) +{ } // Disable test for JointMimic + + template struct init; @@ -277,7 +282,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -292,7 +297,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -307,7 +312,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -322,7 +327,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -337,7 +342,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -351,7 +356,7 @@ struct init> { JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -371,23 +376,24 @@ struct init> jmodel.addJoint(JointModelRY(), SE3::Random()); jmodel.addJoint(JointModelRZ(), SE3::Random()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); - + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); + JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -402,7 +408,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -417,7 +423,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -429,7 +435,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModel jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test_constraint_operations(jmodel); } diff --git a/unittest/joint-prismatic.cpp b/unittest/joint-prismatic.cpp index 36918eb04d..eccc232c79 100644 --- a/unittest/joint-prismatic.cpp +++ b/unittest/joint-prismatic.cpp @@ -102,7 +102,7 @@ BOOST_AUTO_TEST_CASE(test_kinematics) JointDataPX joint_data; JointModelPX joint_model; - joint_model.setIndexes(0, 0, 0); + joint_model.setIndexes(0, 0, 0, 0); Eigen::VectorXd q(Eigen::VectorXd::Zero(1)); Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1)); diff --git a/unittest/liegroups.cpp b/unittest/liegroups.cpp index 3249470680..a72393a8ac 100644 --- a/unittest/liegroups.cpp +++ b/unittest/liegroups.cpp @@ -248,7 +248,7 @@ struct TestJoint void operator()(const T) const { T jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); typename T::JointDataDerived jdata = jmodel.createData(); run_tests(jmodel, jdata); @@ -257,7 +257,7 @@ struct TestJoint void operator()(const pinocchio::JointModelRevoluteUnaligned &) const { pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); pinocchio::JointModelRevoluteUnaligned::JointDataDerived jdata = jmodel.createData(); run_tests(jmodel, jdata); @@ -266,7 +266,7 @@ struct TestJoint void operator()(const pinocchio::JointModelPrismaticUnaligned &) const { pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); pinocchio::JointModelPrismaticUnaligned::JointDataDerived jdata = jmodel.createData(); run_tests(jmodel, jdata); diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 2ff9f8e57a..23c9a3f13f 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -397,17 +397,18 @@ struct init> } }; -template -struct init> +template class JointCollection> +struct init > { - typedef pinocchio::JointModelMimic JointModel; - + typedef pinocchio::JointModelMimicTpl JointModel; + static JointModel run() { - JointModel_ jmodel_ref = init::run(); - - JointModel jmodel(jmodel_ref, 1., 0.); - + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); + + JointModel jmodel(jmodel_ref,1.,0.); + return jmodel; } }; @@ -471,34 +472,37 @@ struct TestJointTransform // Do nothing } - template - void operator()(const pinocchio::JointModelMimic &) + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimic JointModelMimic; + typedef pinocchio::JointModelMimicTpl JointModelMimic; typedef typename JointModelMimic::JointDerived JointDerived; typedef typename pinocchio::traits::Transformation_t Transform; typedef typename pinocchio::traits::Constraint_t Constraint; typedef typename pinocchio::traits::JointDataDerived JointDataMimic; typedef pinocchio::JointDataBase JointDataBase; JointModelMimic jmodel_mimic = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModel; JointModel jmodel = init::run(); - + JointDataMimic jdata_mimic = jmodel_mimic.createData(); JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - + typedef typename pinocchio::LieGroup::type LieGroupType; LieGroupType lg; - - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.)); + + Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - - Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); - - jmodel_mimic.calc(jdata_mimic, q_random); - Transform & m = jdata_mimic_base.M(); + + Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); + + jmodel_mimic.calc(jdata_mimic,q_random); + // Copy Transform (and don't take ref) as non const ref of mimic buffers trigger assert + Transform m = const_cast(jdata_mimic_base).M(); test(m); - - Constraint & S = jdata_mimic_base.S(); + + // Copy Constraint (and don't take ref) as non const ref of mimic buffers trigger assert + Constraint S = const_cast(jdata_mimic_base).S(); test(S); } @@ -554,36 +558,39 @@ struct TestJointMotion // Do nothing } - template - void operator()(const pinocchio::JointModelMimic &) + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimic JointModelMimic; + typedef pinocchio::JointModelMimicTpl JointModelMimic; typedef typename JointModelMimic::JointDerived JointDerived; typedef typename pinocchio::traits::Motion_t Motion; typedef typename pinocchio::traits::Bias_t Bias; typedef typename pinocchio::traits::JointDataDerived JointDataMimic; typedef pinocchio::JointDataBase JointDataBase; JointModelMimic jmodel_mimic = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModel; JointModel jmodel = init::run(); - + JointDataMimic jdata_mimic = jmodel_mimic.createData(); JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - + typedef typename pinocchio::LieGroup::type LieGroupType; LieGroupType lg; - - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.)); + + Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - - Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); + + Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + + jmodel_mimic.calc(jdata_mimic,q_random,v_random); - jmodel_mimic.calc(jdata_mimic, q_random, v_random); - Motion & m = jdata_mimic_base.v(); - + // Copy Motion (and don't take ref) as non const ref of mimic buffers trigger assert + Motion m = const_cast(jdata_mimic_base).v(); test(m); - - Bias & b = jdata_mimic_base.c(); + + // Copy Bias (and don't take ref) as non const ref of mimic buffers trigger assert + Bias b = const_cast(jdata_mimic_base).c(); test(b); } @@ -654,29 +661,31 @@ struct TestJointData test(jdata); } - template - void operator()(const pinocchio::JointModelMimic &) + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimic JointModelMimic; + typedef pinocchio::JointModelMimicTpl JointModelMimic; typedef typename JointModelMimic::JointDerived JointDerived; typedef typename pinocchio::traits::JointDataDerived JointDataMimic; JointModelMimic jmodel_mimic = init::run(); + + typedef pinocchio::JointModelRevoluteTpl JointModel; JointModel jmodel = init::run(); - + JointDataMimic jdata_mimic = jmodel_mimic.createData(); typedef typename pinocchio::LieGroup::type LieGroupType; LieGroupType lg; - - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.)); + + Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - - Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); + + Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); - - jmodel_mimic.calc(jdata_mimic, q_random, v_random); + + jmodel_mimic.calc(jdata_mimic,q_random,v_random); pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); - jmodel_mimic.calc_aba(jdata_mimic, Eigen::VectorXd::Zero(jmodel.nv()), I, false); + jmodel_mimic.calc_aba(jdata_mimic,I,false); test(jdata_mimic); } diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp index 792cdfdf90..4af0e16956 100644 --- a/unittest/visitor.cpp +++ b/unittest/visitor.cpp @@ -253,16 +253,16 @@ struct init> } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; - static JointModel run(const pinocchio::Model & model) + static JointModel run(const pinocchio::Model & /* model*/) { - const pinocchio::JointIndex joint_id = model.getJointId(JointModel_::classname()); - - JointModel jmodel(boost::get(model.joints[joint_id]), 1., 0.); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + + JointModel jmodel(JointModelRX(), 1., 0.); return jmodel; } From a737c6e80a7d4f515d387fcc28a074dba11b1919 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 010/166] [unittest] Fix for mimic --- unittest/joint-generic.cpp | 9 +++++---- .../bindings_contact_inverse_dynamics.py | 2 +- .../python/bindings_geometry_model_urdf.py | 6 +++--- unittest/visitor.cpp | 19 +++++++++++-------- 4 files changed, 20 insertions(+), 16 deletions(-) diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 9385f5cfe6..52af7e4db3 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -477,10 +477,11 @@ struct TestJointOperatorEqual test(jdata); } - // template - // void operator()(const pinocchio::JointModelMimic &) const - // { - // } + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl & ) const + { + + } template static void test(const JointData & jdata) diff --git a/unittest/python/bindings_contact_inverse_dynamics.py b/unittest/python/bindings_contact_inverse_dynamics.py index 7886ed5a88..f4484c271e 100644 --- a/unittest/python/bindings_contact_inverse_dynamics.py +++ b/unittest/python/bindings_contact_inverse_dynamics.py @@ -19,7 +19,7 @@ def setUp(self): def load_model(self): self.model = pin.buildModelFromUrdf( - self.urdf_model_path, pin.JointModelFreeFlyer() + self.urdf_model_path, pin.JointModelFreeFlyer(), False ) pin.loadReferenceConfigurations(self.model, self.srdf_full_path) self.q0 = self.model.referenceConfigurations["half_sitting"] diff --git a/unittest/python/bindings_geometry_model_urdf.py b/unittest/python/bindings_geometry_model_urdf.py index 7ebb86b250..431d514be6 100644 --- a/unittest/python/bindings_geometry_model_urdf.py +++ b/unittest/python/bindings_geometry_model_urdf.py @@ -23,7 +23,7 @@ def test_load(self): self.model_dir / "romeo_description/meshes/V1/collision/LHipPitch.dae" ) - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -36,7 +36,7 @@ def test_load(self): def test_self_load(self): hint_list = [self.mesh_path] - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) collision_model_ref = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -82,7 +82,7 @@ def test_multi_load(self): self.model_dir / "romeo_description/meshes/V1/visual/LHipPitch.dae" ) - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp index 4af0e16956..c98f7b5041 100644 --- a/unittest/visitor.cpp +++ b/unittest/visitor.cpp @@ -253,17 +253,20 @@ struct init> } }; -template class JointCollection> -struct init> +template class JointCollection> +struct init > { - typedef pinocchio::JointModelMimicTpl JointModel; - - static JointModel run(const pinocchio::Model & /* model*/) + typedef pinocchio::JointModelMimicTpl JointModel; + + static JointModel run(const pinocchio::Model & model) { - typedef pinocchio::JointModelRevoluteTpl JointModelRX; + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(model); + jmodel_ref.setIndexes(0,0,0,0); + + JointModel jmodel(jmodel_ref,1.,0.); + jmodel.setIndexes(0,0,0,0); - JointModel jmodel(JointModelRX(), 1., 0.); - return jmodel; } }; From 37940774f3663e51e91b594e52826007a325f4ad Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 011/166] [Python] Add bindings --- include/pinocchio/bindings/python/multibody/model.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/pinocchio/bindings/python/multibody/model.hpp b/include/pinocchio/bindings/python/multibody/model.hpp index 091f5beb27..f6857c5abe 100644 --- a/include/pinocchio/bindings/python/multibody/model.hpp +++ b/include/pinocchio/bindings/python/multibody/model.hpp @@ -71,6 +71,7 @@ namespace pinocchio // Class Members .add_property("nq", &Model::nq, "Dimension of the configuration vector representation.") .add_property("nv", &Model::nv, "Dimension of the velocity vector space.") + .add_property("nj", &Model::nv, "Dimension of the jacobian matrix space.") .add_property("njoints", &Model::njoints, "Number of joints.") .add_property("nbodies", &Model::nbodies, "Number of bodies.") .add_property("nframes", &Model::nframes, "Number of frames.") @@ -89,6 +90,10 @@ namespace pinocchio "idx_vs", &Model::idx_vs, "Starting index of the *i*th joint in the tangent configuration space.") .add_property("nvs", &Model::nvs, "Dimension of the *i*th joint tangent subspace.") + .add_property( + "idx_js", &Model::idx_js, + "Starting index of the *i*th joint in the jacobian space.") + .add_property("njs", &Model::njs, "Dimension of the *i*th joint jacobian subspace.") .add_property( "parents", &Model::parents, "Vector of parent joint indexes. The parent of joint *i*, denoted *li*, " From 83598c9546ef757fb58c4b7647373c0b62cc7399 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 012/166] [Urdf] Add possibility to use mimic or not --- bindings/python/parsers/urdf/model.cpp | 93 +++++++++++++++++--------- bindings/python/pinocchio/shortcuts.py | 11 +-- examples/overview-urdf.py | 2 +- unittest/python/bindings_urdf.py | 16 ++--- 4 files changed, 77 insertions(+), 45 deletions(-) diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index a1003f1191..e9bfcabbbb 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -20,71 +20,74 @@ namespace pinocchio #ifdef PINOCCHIO_WITH_URDFDOM - Model buildModelFromUrdf(const bp::object & filename) + Model buildModelFromUrdf(const bp::object & filename, const bool mimic=false) { Model model; - pinocchio::urdf::buildModel(path(filename), model); + pinocchio::urdf::buildModel(path(filename), model, mimic); return model; } - Model & buildModelFromUrdf(const bp::object & filename, Model & model) + Model & buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic=false) { - return pinocchio::urdf::buildModel(path(filename), model); + return pinocchio::urdf::buildModel(path(filename), model, mimic); } - Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint) + Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, const bool mimic=false) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, model); + pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); return model; } Model buildModelFromUrdf( const bp::object & filename, const JointModel & root_joint, - const std::string & root_joint_name) + const std::string & root_joint_name, + const bool mimic=false) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); + pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); return model; } Model & - buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, Model & model) + buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, Model & model, const bool mimic=false) { - return pinocchio::urdf::buildModel(path(filename), root_joint, model); + return pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); } Model & buildModelFromUrdf( const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name, - Model & model) + Model & model, + const bool mimic = false) { - return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model); + return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); } - Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint) + Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, const bool mimic=false) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); return model; } Model buildModelFromXML( const std::string & xml_stream, const JointModel & root_joint, - const std::string & root_joint_name) + const std::string & root_joint_name, + const bool mimic = false) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); return model; } Model & - buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, Model & model) + buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, Model & model, const bool mimic = false) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); return model; } @@ -92,22 +95,23 @@ namespace pinocchio const std::string & xml_stream, const JointModel & root_joint, const std::string & root_joint_name, - Model & model) + Model & model, + const bool mimic = false) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); return model; } - Model buildModelFromXML(const std::string & xml_stream) + Model buildModelFromXML(const std::string & xml_stream, const bool mimic=true) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, model); + pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); return model; } - Model & buildModelFromXML(const std::string & xml_stream, Model & model) + Model & buildModelFromXML(const std::string & xml_stream, Model & model, const bool mimic=true) { - pinocchio::urdf::buildModelFromXML(xml_stream, model); + pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); return model; } @@ -120,7 +124,7 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "root_joint"), "Parse the URDF file given in input and return a pinocchio Model starting with the " @@ -136,13 +140,21 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast(pinocchio::python::buildModelFromUrdf), + static_cast( + pinocchio::python::buildModelFromUrdf), + bp::args("urdf_filename", "root_joint", "root_joint_name"), + "Parse the URDF file given in input and return a pinocchio Model starting with the " + "given root joint with its specified name."); + + bp::def( + "buildModelFromUrdf", + static_cast(pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename"), "Parse the URDF file given in input and return a pinocchio Model."); bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "model"), "Append to a given model a URDF structure given by its filename.", @@ -150,7 +162,7 @@ namespace pinocchio bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), bp::args("urdf_filename", "root_joint", "model"), "Append to a given model a URDF structure given by its filename and the root joint.\n" @@ -171,7 +183,7 @@ namespace pinocchio bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), bp::args("urdf_xml_stream", "root_joint"), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " @@ -187,7 +199,15 @@ namespace pinocchio bp::def( "buildModelFromXML", - static_cast( + static_cast( + pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name"), + "Parse the URDF XML stream given in input and return a pinocchio Model starting with " + "the given root joint with its specified name."); + + bp::def( + "buildModelFromXML", + static_cast( pinocchio::python::buildModelFromXML), bp::args("urdf_xml_stream", "root_joint", "model"), "Parse the URDF XML stream given in input and append it to the input model with the " @@ -205,13 +225,22 @@ namespace pinocchio bp::def( "buildModelFromXML", - static_cast(pinocchio::python::buildModelFromXML), + static_cast(pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model"), + "Parse the URDF XML stream given in input and append it to the input model with the " + "given interfacing joint with its specified name.", + bp::return_internal_reference<3>()); + + bp::def( + "buildModelFromXML", + static_cast(pinocchio::python::buildModelFromXML), bp::args("urdf_xml_stream"), "Parse the URDF XML stream given in input and return a pinocchio Model."); bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), bp::args("urdf_xml_stream", "model"), "Parse the URDF XML stream given in input and append it to the input model.", diff --git a/bindings/python/pinocchio/shortcuts.py b/bindings/python/pinocchio/shortcuts.py index 8d01baec0d..9efb5b00b8 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -25,6 +25,7 @@ def buildModelsFromUrdf( - verbose - print information of parsing (default - False) - meshLoader - object used to load meshes (default - hpp::fcl::MeshLoader) - geometry_types - Which geometry model to load. Can be pin.GeometryType.COLLISION, pin.GeometryType.VISUAL or both. (default - [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL]) + - mimic - If urdf mimic joints should be parsed or not (default - False) Return: Tuple of the models, in this order : model, collision model, and visual model. @@ -34,7 +35,7 @@ def buildModelsFromUrdf( Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons, it is treated as operational frame and not as a joint of the model. """ # Handle the switch from old to new api - arg_keys = ["package_dirs", "root_joint", "verbose", "meshLoader", "geometry_types"] + arg_keys = ["package_dirs", "root_joint", "verbose", "meshLoader", "geometry_types", "mimic"] if len(args) >= 3: if isinstance(args[2], str): arg_keys = [ @@ -44,6 +45,7 @@ def buildModelsFromUrdf( "verbose", "meshLoader", "geometry_types", + "mimic" ] for key, arg in zip(arg_keys, args): @@ -63,16 +65,17 @@ def _buildModelsFromUrdf( verbose=False, meshLoader=None, geometry_types=None, + mimic=False ) -> Tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]: if geometry_types is None: geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL] if root_joint is None: - model = pin.buildModelFromUrdf(filename) + model = pin.buildModelFromUrdf(filename, mimic) elif root_joint is not None and root_joint_name is None: - model = pin.buildModelFromUrdf(filename, root_joint) + model = pin.buildModelFromUrdf(filename, root_joint, mimic) else: - model = pin.buildModelFromUrdf(filename, root_joint, root_joint_name) + model = pin.buildModelFromUrdf(filename, root_joint, root_joint_name, mimic) if verbose and not WITH_HPP_FCL and meshLoader is not None: print( diff --git a/examples/overview-urdf.py b/examples/overview-urdf.py index 8aee4be196..9e01ccf037 100644 --- a/examples/overview-urdf.py +++ b/examples/overview-urdf.py @@ -15,7 +15,7 @@ ) # Load the urdf model -model = pinocchio.buildModelFromUrdf(urdf_filename) +model = pinocchio.buildModelFromUrdf(urdf_filename, False) print("model name: " + model.name) # Create data required by the algorithms diff --git a/unittest/python/bindings_urdf.py b/unittest/python/bindings_urdf.py index 45826f08cc..ef48ac58a2 100644 --- a/unittest/python/bindings_urdf.py +++ b/unittest/python/bindings_urdf.py @@ -12,25 +12,25 @@ def setUp(self): self.model_path = self.model_dir / "romeo_description/urdf/romeo.urdf" def test_load(self): - pin.buildModelFromUrdf(self.model_path) - pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) + pin.buildModelFromUrdf(self.model_path, False) + pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) def test_self_load(self): model = pin.Model() - pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), model) - pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) + pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), model, False) + pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) def test_xml(self): with self.model_path.open() as model: file_content = model.read() - model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) - model = pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer()) + model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) + model = pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer(), False) self.assertEqual(model, model_ref) model_self = pin.Model() - pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer(), model_self) + pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer(), model_self, False) self.assertEqual(model_self, model_ref) def test_pickle(self): @@ -39,7 +39,7 @@ def test_pickle(self): model_dir = self.current_dir / "../../models/example-robot-data/robots" model_path = model_dir / "ur_description/urdf/ur5_robot.urdf" - model = pin.buildModelFromUrdf(model_path) + model = pin.buildModelFromUrdf(model_path, False) filename = Path("model.pickle") with filename.open("wb") as f: pickle.dump(model, f) From 9f0191d0f6f8e64786b48218c83b32750d0ef9a0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 013/166] [Serialization] Fix Test and serialization --- include/pinocchio/multibody/model.hxx | 2 - .../pinocchio/serialization/joints-data.hpp | 16 ++ .../pinocchio/serialization/joints-model.hpp | 7 +- include/pinocchio/serialization/model.hpp | 3 + unittest/serialization.cpp | 146 +++++++++--------- 5 files changed, 96 insertions(+), 78 deletions(-) diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index 25e869cb48..5d174d0097 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -127,8 +127,6 @@ namespace pinocchio njs.push_back(joint_nj); idx_js.push_back(joint_idx_j); - - if (joint_nq > 0 && joint_nv > 0) { effortLimit.conservativeResize(nv); diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 616a67c595..599c10c509 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -61,6 +61,22 @@ namespace boost ar & make_nvp("UDinv", joint_data.UDinv()); ar & make_nvp("StU", joint_data.StU()); } + + template class JointCollection> + void serialize(Archive & ar, + pinocchio::JointDataBase> & joint_data, + const unsigned int /*version*/) + { + ar & make_nvp("S",joint_data.S()); + // ar & make_nvp("M",joint_data.M()); + // ar & make_nvp("v",joint_data.v()); + // ar & make_nvp("c",joint_data.c()); + + // ar & make_nvp("U",joint_data.U()); + // ar & make_nvp("Dinv",joint_data.Dinv()); + // ar & make_nvp("UDinv",joint_data.UDinv()); + } + } // namespace fix template diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index f565cf5bb1..01bc9792e5 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -289,11 +289,12 @@ namespace boost ar & make_nvp("base_variant", base_object(joint)); } - template + template + class JointCollectionTpl> void serialize( - Archive & ar, pinocchio::JointModelMimicTpl & joint, const unsigned int version) + Archive & ar, pinocchio::JointModelMimicTpl & joint, const unsigned int version) { - typedef pinocchio::JointModelMimicTpl JointType; + typedef pinocchio::JointModelMimicTpl JointType; // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); diff --git a/include/pinocchio/serialization/model.hpp b/include/pinocchio/serialization/model.hpp index 51a0cd1e63..65f82659ee 100644 --- a/include/pinocchio/serialization/model.hpp +++ b/include/pinocchio/serialization/model.hpp @@ -37,6 +37,9 @@ namespace boost ar & make_nvp("nv", model.nv); ar & make_nvp("nvs", model.nvs); ar & make_nvp("idx_vs", model.idx_vs); + ar & make_nvp("nj", model.nj); + ar & make_nvp("njs", model.njs); + ar & make_nvp("idx_js", model.idx_js); ar & make_nvp("njoints", model.njoints); ar & make_nvp("nbodies", model.nbodies); ar & make_nvp("nframes", model.nframes); diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 23c9a3f13f..daba688d3e 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -272,7 +272,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -287,7 +287,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -302,7 +302,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -317,7 +317,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -332,7 +332,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -346,7 +346,7 @@ struct init> { JointModel jmodel(static_cast(0.0)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -361,7 +361,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -375,7 +375,7 @@ struct init> { JointModel jmodel(pinocchio::XAxis::vector(), pinocchio::YAxis::vector()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -392,7 +392,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; @@ -475,35 +475,35 @@ struct TestJointTransform template class JointCollection> void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimicTpl JointModelMimic; - typedef typename JointModelMimic::JointDerived JointDerived; - typedef typename pinocchio::traits::Transformation_t Transform; - typedef typename pinocchio::traits::Constraint_t Constraint; - typedef typename pinocchio::traits::JointDataDerived JointDataMimic; - typedef pinocchio::JointDataBase JointDataBase; - JointModelMimic jmodel_mimic = init::run(); - typedef pinocchio::JointModelRevoluteTpl JointModel; - JointModel jmodel = init::run(); + // typedef pinocchio::JointModelMimicTpl JointModelMimic; + // typedef typename JointModelMimic::JointDerived JointDerived; + // typedef typename pinocchio::traits::Transformation_t Transform; + // typedef typename pinocchio::traits::Constraint_t Constraint; + // typedef typename pinocchio::traits::JointDataDerived JointDataMimic; + // typedef pinocchio::JointDataBase JointDataBase; + // JointModelMimic jmodel_mimic = init::run(); + // typedef pinocchio::JointModelRevoluteTpl JointModel; + // JointModel jmodel = init::run(); - JointDataMimic jdata_mimic = jmodel_mimic.createData(); - JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); + // JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - typedef typename pinocchio::LieGroup::type LieGroupType; - LieGroupType lg; + // typedef typename pinocchio::LieGroup::type LieGroupType; + // LieGroupType lg; - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); - Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); + // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); - jmodel_mimic.calc(jdata_mimic,q_random); - // Copy Transform (and don't take ref) as non const ref of mimic buffers trigger assert - Transform m = const_cast(jdata_mimic_base).M(); - test(m); + // jmodel_mimic.calc(jdata_mimic,q_random); + // // Copy Transform (and don't take ref) as non const ref of mimic buffers trigger assert + // Transform m = const_cast(jdata_mimic_base).M(); + // test(m); - // Copy Constraint (and don't take ref) as non const ref of mimic buffers trigger assert - Constraint S = const_cast(jdata_mimic_base).S(); - test(S); + // // Copy Constraint (and don't take ref) as non const ref of mimic buffers trigger assert + // Constraint S = const_cast(jdata_mimic_base).S(); + // test(S); } template @@ -561,37 +561,37 @@ struct TestJointMotion template class JointCollection> void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimicTpl JointModelMimic; - typedef typename JointModelMimic::JointDerived JointDerived; - typedef typename pinocchio::traits::Motion_t Motion; - typedef typename pinocchio::traits::Bias_t Bias; - typedef typename pinocchio::traits::JointDataDerived JointDataMimic; - typedef pinocchio::JointDataBase JointDataBase; - JointModelMimic jmodel_mimic = init::run(); - typedef pinocchio::JointModelRevoluteTpl JointModel; - JointModel jmodel = init::run(); + // typedef pinocchio::JointModelMimicTpl JointModelMimic; + // typedef typename JointModelMimic::JointDerived JointDerived; + // typedef typename pinocchio::traits::Motion_t Motion; + // typedef typename pinocchio::traits::Bias_t Bias; + // typedef typename pinocchio::traits::JointDataDerived JointDataMimic; + // typedef pinocchio::JointDataBase JointDataBase; + // JointModelMimic jmodel_mimic = init::run(); + // typedef pinocchio::JointModelRevoluteTpl JointModel; + // JointModel jmodel = init::run(); - JointDataMimic jdata_mimic = jmodel_mimic.createData(); - JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); + // JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - typedef typename pinocchio::LieGroup::type LieGroupType; - LieGroupType lg; + // typedef typename pinocchio::LieGroup::type LieGroupType; + // LieGroupType lg; - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); - Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); + // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); - Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); + // Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); - jmodel_mimic.calc(jdata_mimic,q_random,v_random); + // jmodel_mimic.calc(jdata_mimic,q_random,v_random); - // Copy Motion (and don't take ref) as non const ref of mimic buffers trigger assert - Motion m = const_cast(jdata_mimic_base).v(); - test(m); + // // Copy Motion (and don't take ref) as non const ref of mimic buffers trigger assert + // Motion m = const_cast(jdata_mimic_base).v(); + // test(m); - // Copy Bias (and don't take ref) as non const ref of mimic buffers trigger assert - Bias b = const_cast(jdata_mimic_base).c(); - test(b); + // // Copy Bias (and don't take ref) as non const ref of mimic buffers trigger assert + // Bias b = const_cast(jdata_mimic_base).c(); + // test(b); } template @@ -664,29 +664,29 @@ struct TestJointData template class JointCollection> void operator()(const pinocchio::JointModelMimicTpl &) { - typedef pinocchio::JointModelMimicTpl JointModelMimic; - typedef typename JointModelMimic::JointDerived JointDerived; - typedef typename pinocchio::traits::JointDataDerived JointDataMimic; - JointModelMimic jmodel_mimic = init::run(); + // typedef pinocchio::JointModelMimicTpl JointModelMimic; + // typedef typename JointModelMimic::JointDerived JointDerived; + // typedef typename pinocchio::traits::JointDataDerived JointDataMimic; + // JointModelMimic jmodel_mimic = init::run(); - typedef pinocchio::JointModelRevoluteTpl JointModel; - JointModel jmodel = init::run(); + // typedef pinocchio::JointModelRevoluteTpl JointModel; + // JointModel jmodel = init::run(); - JointDataMimic jdata_mimic = jmodel_mimic.createData(); + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); - typedef typename pinocchio::LieGroup::type LieGroupType; - LieGroupType lg; + // typedef typename pinocchio::LieGroup::type LieGroupType; + // LieGroupType lg; - Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); - Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); + // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); - Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); + // Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); - jmodel_mimic.calc(jdata_mimic,q_random,v_random); - pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); - jmodel_mimic.calc_aba(jdata_mimic,I,false); - test(jdata_mimic); + // jmodel_mimic.calc(jdata_mimic,q_random,v_random); + // pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); + // jmodel_mimic.calc_aba(jdata_mimic, Eigen::VectorXd::Zero(jmodel.nv()), I, false); + // test(jdata_mimic); } template From ed5650b0193aa3688ed28d43c8aba40ca7a59bd4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 014/166] [Mimic] Joint Tpl instead of JointBase --- .../pinocchio/multibody/joint/joint-mimic.hpp | 29 +++++++- include/pinocchio/parsers/urdf/model.hxx | 74 ++++++++++--------- 2 files changed, 65 insertions(+), 38 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index c08b39ae7a..ff6bd6bccd 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -397,7 +397,8 @@ struct JointCollectionMimicableTpl PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); // typedef typename boost::make_variant_over>::type>::type MimicableJointDataVariant; - typedef JointDataTpl<_Scalar, _Options, JointCollectionMimicableTpl> RefJointData; + typedef JointDataTpl<_Scalar, _Options, JointCollectionTpl> RefJointData; + typedef typename RefJointData::JointDataVariant RefJointDataVariant; JointDataMimicTpl() : m_scaling((Scalar)0) @@ -410,6 +411,18 @@ struct JointCollectionMimicableTpl // JointDataMimicTpl(const JointDataMimicTpl & other) // { *this = other; } + JointDataMimicTpl(const RefJointDataVariant & jdata, + const Scalar & scaling, + const Scalar & nq, + const Scalar & nv) + : m_jdata_ref(jdata) + , m_scaling(scaling) + , S(m_jdata_ref.S(),scaling) + { + m_q_transform.resize(nq, 1); + m_v_transform.resize(nv, 1); + } + JointDataMimicTpl(const RefJointData & jdata, const Scalar & scaling, const Scalar & nq, @@ -568,8 +581,8 @@ struct JointCollectionMimicableTpl PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); typedef JointCollectionTpl JointCollection; - typedef JointModelTpl JointModel; - typedef JointModel JointModelVariant; + typedef JointModelTpl JointModel; + typedef typename JointModel::JointModelVariant JointModelVariant; // typedef typename boost::make_variant_over>::type>::type MimicableJointModelVariant; // typedef JointModelTpl<_Scalar, _Options, JointCollectionMimicableTpl> MimicableJointModel; @@ -590,6 +603,16 @@ struct JointCollectionMimicableTpl JointModelMimicTpl() {} + + JointModelMimicTpl(const JointModelTpl & jmodel, + const Scalar & scaling, + const Scalar & offset) + : m_jmodel_ref(jmodel) + , m_scaling(scaling) + , m_offset(offset) + { } + + template JointModelMimicTpl(const JointModelBase & jmodel, const Scalar & scaling, diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index bb9c7fda88..001e25d50a 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -460,45 +460,49 @@ namespace pinocchio const Scalar multiplier, const Scalar offset) { - CartesianAxis axisType = extractCartesianAxis(axis); auto joint_mimic = model.joints[model.getJointId(mimic_name)]; - switch (axisType) - { - case AXIS_X: - return model.addJoint(frame.parent, - typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + return model.addJoint(frame.parentJoint, + typename JointCollection::JointModelMimic(joint_mimic.toVariant(), multiplier, offset), frame.placement * placement, joint_name, max_effort,max_velocity,min_config,max_config, friction, damping); - break; - - case AXIS_Y: - return model.addJoint(frame.parent, - typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), - frame.placement * placement, joint_name, - max_effort,max_velocity,min_config,max_config, - friction, damping); - break; - - case AXIS_Z: - return model.addJoint(frame.parent, - typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), - frame.placement * placement, joint_name, - max_effort,max_velocity,min_config,max_config, - friction, damping); - break; - - case AXIS_UNALIGNED: - return model.addJoint(frame.parent, - typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), - frame.placement * placement, joint_name, - max_effort,max_velocity,min_config,max_config, - friction, damping); - break; - default: - PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); - break; - } + // switch (axisType) + // { + // case AXIS_X: + // return model.addJoint(frame.parent, + // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + // frame.placement * placement, joint_name, + // max_effort,max_velocity,min_config,max_config, + // friction, damping); + // break; + + // case AXIS_Y: + // return model.addJoint(frame.parent, + // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + // frame.placement * placement, joint_name, + // max_effort,max_velocity,min_config,max_config, + // friction, damping); + // break; + + // case AXIS_Z: + // return model.addJoint(frame.parent, + // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + // frame.placement * placement, joint_name, + // max_effort,max_velocity,min_config,max_config, + // friction, damping); + // break; + + // case AXIS_UNALIGNED: + // return model.addJoint(frame.parent, + // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), + // frame.placement * placement, joint_name, + // max_effort,max_velocity,min_config,max_config, + // friction, damping); + // break; + // default: + // PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); + // break; + // } } From a0bcd853f2e3166645809df2a1379752c52dedcd Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 015/166] [Broken test] copy visitor from variant to variant --- .../pinocchio/multibody/joint/joint-mimic.hpp | 39 ++++++-- include/pinocchio/multibody/sample-models.hxx | 2 +- include/pinocchio/parsers/urdf/model.hxx | 93 ++----------------- src/parsers/urdf/model.cpp | 4 +- 4 files changed, 43 insertions(+), 95 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index ff6bd6bccd..da7ae7597f 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -11,6 +11,10 @@ #include "pinocchio/multibody/joint/joint-base.hpp" #include #include +#include +#include +#include +#include namespace pinocchio @@ -334,6 +338,28 @@ struct JointCollectionMimicableTpl > JointDataVariant; }; +// Helper to check if a type is in the target variant +template +struct is_type_in_variant : boost::mpl::contains {}; + +template +struct TransferVisitor : public boost::static_visitor { + TargetVariant& target; + + TransferVisitor(TargetVariant& targetVariant) : target(targetVariant) {} + + template + typename std::enable_if::value>::type + operator()(const T& value) const { + target = value; + } + + template + typename std::enable_if::value>::type + operator()(const T& value) const { + std::cout << "Type not supported in new variant\n"; + } +}; template class JointCollectionTpl> struct JointMimicTpl; template class JointCollectionTpl> struct JointModelMimicTpl; @@ -397,7 +423,7 @@ struct JointCollectionMimicableTpl PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); // typedef typename boost::make_variant_over>::type>::type MimicableJointDataVariant; - typedef JointDataTpl<_Scalar, _Options, JointCollectionTpl> RefJointData; + typedef JointDataTpl<_Scalar, _Options, JointCollectionMimicableTpl> RefJointData; typedef typename RefJointData::JointDataVariant RefJointDataVariant; JointDataMimicTpl() @@ -581,7 +607,7 @@ struct JointCollectionMimicableTpl PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); typedef JointCollectionTpl JointCollection; - typedef JointModelTpl JointModel; + typedef JointModelTpl JointModel; typedef typename JointModel::JointModelVariant JointModelVariant; // typedef typename boost::make_variant_over>::type>::type MimicableJointModelVariant; // typedef JointModelTpl<_Scalar, _Options, JointCollectionMimicableTpl> MimicableJointModel; @@ -607,17 +633,18 @@ struct JointCollectionMimicableTpl JointModelMimicTpl(const JointModelTpl & jmodel, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref(jmodel) - , m_scaling(scaling) + : m_scaling(scaling) , m_offset(offset) - { } + { + boost::apply_visitor(TransferVisitor(m_jmodel_ref), jmodel); + } template JointModelMimicTpl(const JointModelBase & jmodel, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref(jmodel.derived()) + : m_jmodel_ref((JointModelVariant)jmodel.derived()) , m_scaling(scaling) , m_offset(offset) { } diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 86a8d0c791..65a8dfb763 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -110,7 +110,7 @@ namespace pinocchio Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); - joint_id = addJointAndBody(model, typename JC::JointModelMimic(boost::get(model.joints[joint_id]), multiplier, offset), + joint_id = addJointAndBody(model, typename JC::JointModelMimic(model.joints[joint_id].derived(), multiplier, offset), model.names[joint_id], pre+"wrist1_joint_mimic", Id4); } else diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 001e25d50a..40b4fc8f1d 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -36,8 +36,7 @@ namespace pinocchio FLOATING, PLANAR, SPHERICAL, - MIMIC_REVOLUTE, - MIMIC_PRISMATIC + MIMIC }; typedef enum ::pinocchio::FrameType FrameType; @@ -265,28 +264,13 @@ namespace pinocchio frame.placement * placement, joint_name, max_effort, max_velocity, min_config, max_config, friction, damping); break; - case Base::MIMIC_REVOLUTE: - joint_id = addMimicJoint< - typename JointCollection::JointModelRX, - typename JointCollection::JointModelRY, - typename JointCollection::JointModelRZ, - typename JointCollection::JointModelRevoluteUnaligned> ( - axis, frame, placement, joint_name, - max_effort, max_velocity, min_config, max_config, - friction, damping, - mimic_name, multiplier, offset); + case Base::MIMIC: + joint_id = model.addJoint(frame.parentJoint, + typename JointCollection::JointModelMimic(model.joints[model.getJointId(mimic_name)], multiplier, offset), + frame.placement * placement, joint_name, + max_effort,max_velocity,min_config,max_config, + friction, damping); break; - case Base::MIMIC_PRISMATIC: - joint_id = addMimicJoint< - typename JointCollection::JointModelPX, - typename JointCollection::JointModelPY, - typename JointCollection::JointModelPZ, - typename JointCollection::JointModelPrismaticUnaligned> ( - axis, frame, placement, joint_name, - max_effort, max_velocity, min_config, max_config, - friction, damping, - mimic_name, multiplier, offset); - break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); }; @@ -443,69 +427,6 @@ namespace pinocchio } } - template - JointIndex addMimicJoint( - const Vector3& axis, - const Frame & frame, - const SE3 & placement, - const std::string & joint_name, - const VectorConstRef& max_effort, - const VectorConstRef& max_velocity, - const VectorConstRef& min_config, - const VectorConstRef& max_config, - const VectorConstRef& friction, - const VectorConstRef& damping, - const std::string &mimic_name, - const Scalar multiplier, - const Scalar offset) - { - auto joint_mimic = model.joints[model.getJointId(mimic_name)]; - return model.addJoint(frame.parentJoint, - typename JointCollection::JointModelMimic(joint_mimic.toVariant(), multiplier, offset), - frame.placement * placement, joint_name, - max_effort,max_velocity,min_config,max_config, - friction, damping); - // switch (axisType) - // { - // case AXIS_X: - // return model.addJoint(frame.parent, - // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), - // frame.placement * placement, joint_name, - // max_effort,max_velocity,min_config,max_config, - // friction, damping); - // break; - - // case AXIS_Y: - // return model.addJoint(frame.parent, - // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), - // frame.placement * placement, joint_name, - // max_effort,max_velocity,min_config,max_config, - // friction, damping); - // break; - - // case AXIS_Z: - // return model.addJoint(frame.parent, - // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), - // frame.placement * placement, joint_name, - // max_effort,max_velocity,min_config,max_config, - // friction, damping); - // break; - - // case AXIS_UNALIGNED: - // return model.addJoint(frame.parent, - // typename JointCollection::JointModelMimic(boost::get(joint_mimic), multiplier, offset), - // frame.placement * placement, joint_name, - // max_effort,max_velocity,min_config,max_config, - // friction, damping); - // break; - // default: - // PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); - // break; - // } - } - - private: /// /// \brief The four possible cartesian types of an 3D axis. diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index d119489f14..63baa97ef8 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -151,7 +151,7 @@ namespace pinocchio Scalar multiplier = joint->mimic->multiplier; Scalar offset = joint->mimic->offset; - model.addJointAndBody(UrdfVisitorBase::MIMIC_REVOLUTE, axis, + model.addJointAndBody(UrdfVisitorBase::MIMIC, axis, parentFrameId,jointPlacement,joint->name, Y,link->name, max_effort,max_velocity,min_config,max_config, @@ -225,7 +225,7 @@ namespace pinocchio Scalar multiplier = joint->mimic->multiplier; Scalar offset = joint->mimic->offset; - model.addJointAndBody(UrdfVisitorBase::MIMIC_PRISMATIC, axis, + model.addJointAndBody(UrdfVisitorBase::MIMIC, axis, parentFrameId,jointPlacement,joint->name, Y,link->name, max_effort,max_velocity,min_config,max_config, From 9501a72e5823a3122039937f81e4f3e2891ad3b3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 016/166] Add mimic to shortcuts --- bindings/python/pinocchio/shortcuts.py | 1 + unittest/python/bindings_geometry_model_urdf.py | 10 ++++++---- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/bindings/python/pinocchio/shortcuts.py b/bindings/python/pinocchio/shortcuts.py index 9efb5b00b8..0346619572 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -59,6 +59,7 @@ def buildModelsFromUrdf( def _buildModelsFromUrdf( filename, + mimic=False, package_dirs=None, root_joint=None, root_joint_name=None, diff --git a/unittest/python/bindings_geometry_model_urdf.py b/unittest/python/bindings_geometry_model_urdf.py index 431d514be6..dc1ca82d21 100644 --- a/unittest/python/bindings_geometry_model_urdf.py +++ b/unittest/python/bindings_geometry_model_urdf.py @@ -23,7 +23,7 @@ def test_load(self): self.model_dir / "romeo_description/meshes/V1/collision/LHipPitch.dae" ) - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), True) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -36,7 +36,7 @@ def test_load(self): def test_self_load(self): hint_list = [self.mesh_path] - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), True) collision_model_ref = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -82,7 +82,7 @@ def test_multi_load(self): self.model_dir / "romeo_description/meshes/V1/visual/LHipPitch.dae" ) - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), True) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list @@ -101,7 +101,7 @@ def test_multi_load(self): ) model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf( - self.model_path, hint_list, pin.JointModelFreeFlyer() + self.model_path, True, hint_list, pin.JointModelFreeFlyer() ) self.assertEqual(model, model_2) @@ -118,6 +118,7 @@ def test_multi_load(self): model_c, collision_model_c = pin.buildModelsFromUrdf( self.model_path, + True, hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.COLLISION, @@ -132,6 +133,7 @@ def test_multi_load(self): model_v, visual_model_v = pin.buildModelsFromUrdf( self.model_path, + True, hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.VISUAL, From affef7ea037e6cebe2bc64f928e6b4c5b7909850 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 017/166] wip --- .../pinocchio/multibody/joint/joint-mimic.hpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index da7ae7597f..f280e5d98a 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -437,17 +437,29 @@ struct TransferVisitor : public boost::static_visitor { // JointDataMimicTpl(const JointDataMimicTpl & other) // { *this = other; } - JointDataMimicTpl(const RefJointDataVariant & jdata, - const Scalar & scaling, - const Scalar & nq, - const Scalar & nv) - : m_jdata_ref(jdata) - , m_scaling(scaling) + JointDataMimicTpl(const JointDataTpl & jdata, + const Scalar & scaling, + const Scalar & nq, + const Scalar & nv) + : m_scaling(scaling) , S(m_jdata_ref.S(),scaling) - { + { m_q_transform.resize(nq, 1); m_v_transform.resize(nv, 1); + boost::apply_visitor(TransferVisitor(m_jdata_ref), jdata); } + + + // JointDataMimicTpl(const RefJointDataVariant & jdata, + // const Scalar & scaling, + // const Scalar & nq, + // const Scalar & nv) + // : m_jdata_ref(jdata) + // , m_scaling(scaling) + // , S(m_jdata_ref.S(),scaling) + // { + + // } JointDataMimicTpl(const RefJointData & jdata, const Scalar & scaling, From ff96d5c479694fa7e30436a729953961a68b73b6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 018/166] Copy constructor instead of = --- .../pinocchio/multibody/joint/joint-mimic.hpp | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index f280e5d98a..3397afc816 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -343,21 +343,19 @@ template struct is_type_in_variant : boost::mpl::contains {}; template -struct TransferVisitor : public boost::static_visitor { - TargetVariant& target; - - TransferVisitor(TargetVariant& targetVariant) : target(targetVariant) {} - +struct TransferVisitor : public boost::static_visitor { + template - typename std::enable_if::value>::type + typename std::enable_if::value, TargetVariant>::type operator()(const T& value) const { - target = value; + return TargetVariant(value); } template - typename std::enable_if::value>::type + typename std::enable_if::value, TargetVariant>::type operator()(const T& value) const { std::cout << "Type not supported in new variant\n"; + return TargetVariant(); } }; @@ -443,10 +441,11 @@ struct TransferVisitor : public boost::static_visitor { const Scalar & nv) : m_scaling(scaling) , S(m_jdata_ref.S(),scaling) + , m_jdata_ref(boost::apply_visitor(TransferVisitor(), jdata)) { m_q_transform.resize(nq, 1); m_v_transform.resize(nv, 1); - boost::apply_visitor(TransferVisitor(m_jdata_ref), jdata); + // boost::apply_visitor(TransferVisitor(m_jdata_ref), jdata); } @@ -647,8 +646,10 @@ struct TransferVisitor : public boost::static_visitor { const Scalar & offset) : m_scaling(scaling) , m_offset(offset) + , m_jmodel_ref(boost::apply_visitor(TransferVisitor(), jmodel)) { - boost::apply_visitor(TransferVisitor(m_jmodel_ref), jmodel); + std::cout << jmodel << std::endl; + // m_jmodel_ref = boost::apply_visitor(TransferVisitor<>(m_jmodel_ref), jmodel); } From a810252b5d7406e1548de53941d23cc77349bf25 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 019/166] [EtienneAr feedback] Add mimic constructor for different primay/secondary joint types --- .../pinocchio/multibody/joint/joint-mimic.hpp | 39 +++++++++++++++---- 1 file changed, 31 insertions(+), 8 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 3397afc816..e4c3bec7c1 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -644,24 +644,47 @@ struct TransferVisitor : public boost::static_visitor { JointModelMimicTpl(const JointModelTpl & jmodel, const Scalar & scaling, const Scalar & offset) + : JointModelMimicTpl(jmodel, jmodel, scaling, offset) + { } + + JointModelMimicTpl(const JointModelTpl & jmodel_mimicking, + const JointModelTpl & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) : m_scaling(scaling) , m_offset(offset) - , m_jmodel_ref(boost::apply_visitor(TransferVisitor(), jmodel)) - { - std::cout << jmodel << std::endl; - // m_jmodel_ref = boost::apply_visitor(TransferVisitor<>(m_jmodel_ref), jmodel); + , m_jmodel_ref(boost::apply_visitor(TransferVisitor(), jmodel_mimicking)) + { + assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); + assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); + assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); + + m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); } - template JointModelMimicTpl(const JointModelBase & jmodel, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref((JointModelVariant)jmodel.derived()) + : JointModelMimicTpl(jmodel, jmodel, scaling, offset) + { } + + template + JointModelMimicTpl(const JointModelBase & jmodel_mimicking, + const JointModelBase & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) + : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) , m_scaling(scaling) , m_offset(offset) - { } - + { + assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); + assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); + assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); + + m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); + } + Base & base() { return *static_cast(this); } const Base & base() const { return *static_cast(this); } From f39c03a29ed5ef77c58a44fa7debccc4038c26e7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 020/166] [EtienneAr feedback] Move transferVariant to joint-basic-visitors and document it --- .../multibody/joint/joint-basic-visitors.hpp | 11 +++++++ .../multibody/joint/joint-basic-visitors.hxx | 23 +++++++++++++++ .../pinocchio/multibody/joint/joint-mimic.hpp | 29 +++---------------- 3 files changed, 38 insertions(+), 25 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index 445ebf6585..818cb20a44 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -450,6 +450,17 @@ namespace pinocchio const JointDataTpl & jmodel_generic, const JointDataBase & jmodel); + /** + * @brief Transfer a value from one variant to another (given that the new variant contains the type of the value). + * For instance transfer a joint model (or data) from a generic joint model (or data) to another generic joint model (or data) with slightly different joint collections. + * + * @param[in] variant The value to transfer. + * + * @return The same value in the new variant. + */ + template + VariantDst transferToVariant(const VariantSrc & value); + } // namespace pinocchio /* --- Details -------------------------------------------------------------------- */ diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index b9b550cbaf..181e503681 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -915,6 +915,29 @@ namespace pinocchio return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived()))); } + template + struct TransferVisitor : public boost::static_visitor { + + template + typename boost::enable_if, TargetVariant>::type + operator()(const ValueType& value) const { + return TargetVariant(value); + } + + template + typename boost::disable_if, TargetVariant>::type + operator()(const ValueType& value) const { + assert(false && "Type not supported in new variant"); + return TargetVariant(); + } + }; + + template + VariantDst transferToVariant(const VariantSrc & value) + { + return boost::apply_visitor(TransferVisitor(), value); + } + /// @endcond } // namespace pinocchio diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index e4c3bec7c1..4ed3d0ae9e 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -9,6 +9,7 @@ #include "pinocchio/multibody/joint/joint-collection.hpp" #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" +#include "pinocchio/multibody/joint/joint-basic-visitors.hpp" #include #include #include @@ -338,27 +339,6 @@ struct JointCollectionMimicableTpl > JointDataVariant; }; -// Helper to check if a type is in the target variant -template -struct is_type_in_variant : boost::mpl::contains {}; - -template -struct TransferVisitor : public boost::static_visitor { - - template - typename std::enable_if::value, TargetVariant>::type - operator()(const T& value) const { - return TargetVariant(value); - } - - template - typename std::enable_if::value, TargetVariant>::type - operator()(const T& value) const { - std::cout << "Type not supported in new variant\n"; - return TargetVariant(); - } -}; - template class JointCollectionTpl> struct JointMimicTpl; template class JointCollectionTpl> struct JointModelMimicTpl; template class JointCollectionTpl> struct JointDataMimicTpl; @@ -441,7 +421,7 @@ struct TransferVisitor : public boost::static_visitor { const Scalar & nv) : m_scaling(scaling) , S(m_jdata_ref.S(),scaling) - , m_jdata_ref(boost::apply_visitor(TransferVisitor(), jdata)) + , m_jdata_ref(transferToVariant, RefJointData>(jdata)) { m_q_transform.resize(nq, 1); m_v_transform.resize(nv, 1); @@ -636,10 +616,9 @@ struct TransferVisitor : public boost::static_visitor { using Base::nv; using Base::nj; using Base::setIndexes; - + JointModelMimicTpl() {} - JointModelMimicTpl(const JointModelTpl & jmodel, const Scalar & scaling, @@ -653,7 +632,7 @@ struct TransferVisitor : public boost::static_visitor { const Scalar & offset) : m_scaling(scaling) , m_offset(offset) - , m_jmodel_ref(boost::apply_visitor(TransferVisitor(), jmodel_mimicking)) + , m_jmodel_ref(transferToVariant, JointModel>(jmodel_mimicking)) { assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); From 7e0c090209e6666134afa6dd9dde11a5f5dccf4d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 021/166] [EtienneAr feedback] PoC: fix appendModel --- include/pinocchio/algorithm/model.hxx | 31 ++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 472f9e5009..bb8cb0e2fb 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -153,6 +153,31 @@ namespace pinocchio GeometryModel &> ArgsType; + template + static typename std::enable_if< + !std::is_same>::value, + void>::type + updateMimicIds(JointModel & jmodel, const Model & old_model, const Model & new_model) + { + } + + template + static typename std::enable_if< + std::is_same>::value, + void>::type + updateMimicIds( + JointModelMimicTpl & jmodel, + const Model & old_model, + const Model & new_model) + { + const JointIndex mimicked_old_id = jmodel.jmodel().id(); + const std::string mimicked_name = old_model.names[mimicked_old_id]; + const JointIndex mimicked_new_id = new_model.getJointId(mimicked_name); + jmodel.jmodel().setIndexes( + mimicked_new_id, new_model.joints[mimicked_new_id].idx_q(), + new_model.joints[mimicked_new_id].idx_v(), new_model.joints[mimicked_new_id].idx_j()); + } + template static void algo( const JointModelBase & jmodel_in, @@ -185,7 +210,11 @@ namespace pinocchio model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]); - const typename Model::JointModel & jmodel_out = model.joints[joint_id_out]; + typename Model::JointModel & jmodel_out = model.joints[joint_id_out]; + + // For mimic joints, update the reference joint id + updateMimicIds(boost::get(jmodel_out), modelAB, model); + jmodel_out.jointVelocitySelector(model.rotorInertia) = jmodel_in.jointVelocitySelector(modelAB.rotorInertia); jmodel_out.jointVelocitySelector(model.rotorGearRatio) = From 72d7c8e925f4bbe5cd997d47cba43015f5768dbb Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 022/166] [EtienneAr feedback] Fix sample model for mimic in manipulator --- include/pinocchio/multibody/sample-models.hxx | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 65a8dfb763..d121d029da 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -105,31 +105,34 @@ namespace pinocchio model, typename JC::JointModelRX(), model.names[joint_id], pre + "wrist1", Marm); model.inertias[joint_id] = Ijoint; - if(mimic) + if (mimic) { Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); - - joint_id = addJointAndBody(model, typename JC::JointModelMimic(model.joints[joint_id].derived(), multiplier, offset), - model.names[joint_id], pre+"wrist1_joint_mimic", Id4); + + joint_id = addJointAndBody( + model, + typename JC::JointModelMimic(model.joints[joint_id].derived(), multiplier, offset), + model.names[joint_id], pre + "wrist1_joint_mimic", Id4); } else - { + { joint_id = addJointAndBody( model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4); } model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "effector_body", joint_id); + const int nq = mimic ? 5 : 6; const JointModel & base_joint = model.joints[root_joint_id]; const int idx_q = base_joint.idx_q(); const int idx_v = base_joint.idx_v(); - model.lowerPositionLimit.template segment<6>(idx_q).fill(qmin); - model.upperPositionLimit.template segment<6>(idx_q).fill(qmax); - model.velocityLimit.template segment<6>(idx_v).fill(vmax); - model.effortLimit.template segment<6>(idx_v).fill(taumax); + model.lowerPositionLimit.segment(idx_q, nq).fill(qmin); + model.upperPositionLimit.segment(idx_q, nq).fill(qmax); + model.velocityLimit.segment(idx_v, nq).fill(vmax); + model.effortLimit.segment(idx_v, nq).fill(taumax); } #ifdef PINOCCHIO_WITH_HPP_FCL From 424fd5234d8faf1a379dc6f47ce1c2ab47d369be Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 023/166] [EtienneAr feedback] Joint Mimic jointConfigSelector_impl follows conventions en return a 0 length vector (and apply pre-commit formatting) --- .../pinocchio/multibody/joint/joint-mimic.hpp | 1070 ++++++++++------- 1 file changed, 637 insertions(+), 433 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 4ed3d0ae9e..165a9b6245 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -17,18 +17,22 @@ #include #include - namespace pinocchio { - template struct ScaledJointMotionSubspaceTpl; - template - struct traits< ScaledJointMotionSubspaceTpl<_Scalar, _Options> > + struct ScaledJointMotionSubspaceTpl; + + template + struct traits> { typedef JointMotionSubspaceTpl RefJointMotionSubspace; typedef typename traits::Scalar Scalar; - enum { Options = traits::Options }; - enum { + enum + { + Options = traits::Options + }; + enum + { LINEAR = traits::LINEAR, ANGULAR = traits::ANGULAR }; @@ -38,69 +42,100 @@ namespace pinocchio typedef typename traits::MatrixReturnType MatrixReturnType; typedef typename traits::ConstMatrixReturnType ConstMatrixReturnType; }; // traits ScaledJointMotionSubspaceTpl - + template - struct SE3GroupAction< ScaledJointMotionSubspaceTpl<_Scalar, _Options> > - { typedef typename SE3GroupAction>::RefJointMotionSubspace>::ReturnType ReturnType; }; - + struct SE3GroupAction> + { + typedef + typename SE3GroupAction>:: + RefJointMotionSubspace>::ReturnType ReturnType; + }; + template - struct MotionAlgebraAction< ScaledJointMotionSubspaceTpl<_Scalar, _Options>, MotionDerived > - { typedef typename MotionAlgebraAction>::RefJointMotionSubspace, MotionDerived>::ReturnType ReturnType; }; - + struct MotionAlgebraAction, MotionDerived> + { + typedef typename MotionAlgebraAction< + typename traits>::RefJointMotionSubspace, + MotionDerived>::ReturnType ReturnType; + }; + template - struct ConstraintForceOp< ScaledJointMotionSubspaceTpl<_Scalar, _Options>, ForceDerived> + struct ConstraintForceOp, ForceDerived> { - typedef typename traits>::RefJointMotionSubspace::Scalar Scalar; - // typedef typename ConstraintForceOp>::RefJointMotionSubspace,ForceDerived>::ReturnType OriginalReturnType; + typedef typename traits< + ScaledJointMotionSubspaceTpl<_Scalar, _Options>>::RefJointMotionSubspace::Scalar Scalar; + // typedef typename ConstraintForceOp>::RefJointMotionSubspace,ForceDerived>::ReturnType OriginalReturnType; typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options> OriginalReturnType; - - typedef typename ScalarMatrixProduct::type IdealReturnType; - typedef Eigen::Matrix>::RefJointMotionSubspace::Options> ReturnType; + + typedef typename ScalarMatrixProduct::type IdealReturnType; + typedef Eigen::Matrix< + Scalar, + IdealReturnType::RowsAtCompileTime, + IdealReturnType::ColsAtCompileTime, + traits>::RefJointMotionSubspace::Options> + ReturnType; }; - + template - struct ConstraintForceSetOp< ScaledJointMotionSubspaceTpl<_Scalar, _Options>, ForceSet> + struct ConstraintForceSetOp, ForceSet> { - typedef typename traits>::RefJointMotionSubspace::Scalar Scalar; - // typedef typename ConstraintForceSetOp>::RefJointMotionSubspace, ForceSet>::ReturnType OriginalReturnType; + typedef typename traits< + ScaledJointMotionSubspaceTpl<_Scalar, _Options>>::RefJointMotionSubspace::Scalar Scalar; + // typedef typename ConstraintForceSetOp>::RefJointMotionSubspace, ForceSet>::ReturnType OriginalReturnType; typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options> OriginalReturnType; - typedef typename ScalarMatrixProduct::type IdealReturnType; - typedef Eigen::Matrix>::RefJointMotionSubspace::NV,ForceSet::ColsAtCompileTime, traits>::RefJointMotionSubspace::Options | Eigen::RowMajor> ReturnType; + typedef typename ScalarMatrixProduct::type IdealReturnType; + typedef Eigen::Matrix< + Scalar, + traits>::RefJointMotionSubspace::NV, + ForceSet::ColsAtCompileTime, + traits>::RefJointMotionSubspace::Options + | Eigen::RowMajor> + ReturnType; }; template struct ScaledJointMotionSubspaceTpl - : JointMotionSubspaceBase< ScaledJointMotionSubspaceTpl<_Scalar, _Options> > + : JointMotionSubspaceBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspaceTpl) - enum { NV = Eigen::Dynamic}; + enum + { + NV = Eigen::Dynamic + }; typedef JointMotionSubspaceBase Base; using Base::nv; - - typedef typename traits>::RefJointMotionSubspace RefJointMotionSubspace; + + typedef typename traits>::RefJointMotionSubspace + RefJointMotionSubspace; typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; - + ScaledJointMotionSubspaceTpl() : ScaledJointMotionSubspaceTpl(1.0) - {} - + { + } + explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor) : m_scaling_factor(scaling_factor) , m_constraint(0) - {} - - ScaledJointMotionSubspaceTpl(const RefJointMotionSubspace & constraint, - const Scalar & scaling_factor) + { + } + + ScaledJointMotionSubspaceTpl( + const RefJointMotionSubspace & constraint, const Scalar & scaling_factor) : m_constraint(constraint) , m_scaling_factor(scaling_factor) - {} + { + } ScaledJointMotionSubspaceTpl(const ScaledJointMotionSubspaceTpl & other) : m_constraint(other.m_constraint) , m_scaling_factor(other.m_scaling_factor) - {} + { + } ScaledJointMotionSubspaceTpl & operator=(const ScaledJointMotionSubspaceTpl & other) { @@ -108,7 +143,7 @@ namespace pinocchio m_scaling_factor = other.m_scaling_factor; return *this; } - + template JointMotion __mult__(const Eigen::MatrixBase & v) const { @@ -116,58 +151,68 @@ namespace pinocchio JointMotion jm = m_constraint * v; return jm * m_scaling_factor; } - + template - SE3ActionReturnType - se3Action(const SE3Tpl & m) const + SE3ActionReturnType se3Action(const SE3Tpl & m) const { SE3ActionReturnType res = m_constraint.se3Action(m); return m_scaling_factor * res; } - + template - SE3ActionReturnType - se3ActionInverse(const SE3Tpl & m) const + SE3ActionReturnType se3ActionInverse(const SE3Tpl & m) const { SE3ActionReturnType res = m_constraint.se3ActionInverse(m); return m_scaling_factor * res; } - - int nv_impl() const { return m_constraint.nv(); } - + + int nv_impl() const + { + return m_constraint.nv(); + } + struct TransposeConst { const ScaledJointMotionSubspaceTpl & ref; - TransposeConst(const ScaledJointMotionSubspaceTpl & ref) : ref(ref) {} - + TransposeConst(const ScaledJointMotionSubspaceTpl & ref) + : ref(ref) + { + } + template - typename ConstraintForceOp::ReturnType + typename ConstraintForceOp::ReturnType operator*(const ForceDense & f) const { - // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the evaluation level; - typedef typename ConstraintForceOp::ReturnType ReturnType; + // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the + // evaluation level; + typedef + typename ConstraintForceOp::ReturnType ReturnType; return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); } - + /// [CRBA] MatrixBase operator* (RefConstraint::Transpose S, ForceSet::Block) template - typename ConstraintForceSetOp::ReturnType + typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) const { - typedef typename ConstraintForceSetOp::ReturnType ReturnType; + typedef typename ConstraintForceSetOp::ReturnType + ReturnType; return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); } - + }; // struct TransposeConst - - TransposeConst transpose() const { return TransposeConst(*this); } - + + TransposeConst transpose() const + { + return TransposeConst(*this); + } + const DenseBase & matrix_impl() const { S = m_scaling_factor * m_constraint.matrix_impl(); return S; } - + DenseBase & matrix_impl() { S = m_scaling_factor * m_constraint.matrix_impl(); @@ -175,176 +220,208 @@ namespace pinocchio } template - typename MotionAlgebraAction::ReturnType + typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { - typedef typename MotionAlgebraAction::ReturnType ReturnType; + typedef typename MotionAlgebraAction::ReturnType + ReturnType; ReturnType res = m_scaling_factor * m_constraint.motionAction(m); return res; } - - inline const Scalar & scaling() const { return m_scaling_factor; } - inline Scalar & scaling() { return m_scaling_factor; } - - inline const RefJointMotionSubspace & constraint() const { return m_constraint.derived(); } - inline RefJointMotionSubspace & constraint() { return m_constraint.derived(); } - + + inline const Scalar & scaling() const + { + return m_scaling_factor; + } + inline Scalar & scaling() + { + return m_scaling_factor; + } + + inline const RefJointMotionSubspace & constraint() const + { + return m_constraint.derived(); + } + inline RefJointMotionSubspace & constraint() + { + return m_constraint.derived(); + } + bool isEqual(const ScaledJointMotionSubspaceTpl & other) const { - return m_constraint == other.m_constraint - && m_scaling_factor == other.m_scaling_factor; + return m_constraint == other.m_constraint && m_scaling_factor == other.m_scaling_factor; } - + protected: - RefJointMotionSubspace m_constraint; Scalar m_scaling_factor; mutable DenseBase S; }; // struct ScaledJointMotionSubspaceTpl - + template - struct MultiplicationOp, ScaledJointMotionSubspaceTpl > + struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef InertiaTpl Inertia; + typedef InertiaTpl Inertia; typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename Constraint::Scalar Scalar; - - // typedef typename MultiplicationOp::ReturnType OriginalReturnType; - typedef Eigen::Matrix ReturnType; + + // typedef typename MultiplicationOp::ReturnType + // OriginalReturnType; + typedef Eigen::Matrix ReturnType; // typedef typename ScalarMatrixProduct::type ReturnType; // typedef OriginalReturnType ReturnType; }; - + /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ namespace impl { template - struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl > + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef InertiaTpl Inertia; + typedef InertiaTpl Inertia; typedef ScaledJointMotionSubspaceTpl Constraint; - typedef typename MultiplicationOp::ReturnType ReturnType; - - static inline ReturnType run(const Inertia & Y, - const Constraint & scaled_constraint) + typedef typename MultiplicationOp::ReturnType ReturnType; + + static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) { return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); } }; } // namespace impl - + template - struct MultiplicationOp, ScaledJointMotionSubspaceTpl > + struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { typedef ScaledJointMotionSubspaceTpl Constraint; - typedef typename MultiplicationOp::ReturnType OriginalReturnType; + typedef typename MultiplicationOp::ReturnType OriginalReturnType; typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; }; - + /* [ABA] operator* (Inertia Y,Constraint S) */ namespace impl { template - struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl > + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> { typedef ScaledJointMotionSubspaceTpl Constraint; - typedef typename MultiplicationOp,Constraint>::ReturnType ReturnType; - - static inline ReturnType run(const Eigen::MatrixBase & Y, - const Constraint & scaled_constraint) + typedef + typename MultiplicationOp, Constraint>::ReturnType ReturnType; + + static inline ReturnType + run(const Eigen::MatrixBase & Y, const Constraint & scaled_constraint) { return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint()); } }; } // namespace impl - -template -struct JointCollectionMimicableTpl -{ - typedef _Scalar Scalar; - enum { Options = _Options }; - - // Joint Revolute - typedef JointModelRevoluteTpl JointModelRX; - typedef JointModelRevoluteTpl JointModelRY; - typedef JointModelRevoluteTpl JointModelRZ; - - // Joint Revolute Unaligned - typedef JointModelRevoluteUnalignedTpl JointModelRevoluteUnaligned; - - // Joint Revolute UBounded - typedef JointModelRevoluteUnboundedTpl JointModelRUBX; - typedef JointModelRevoluteUnboundedTpl JointModelRUBY; - typedef JointModelRevoluteUnboundedTpl JointModelRUBZ; - - // Joint Revolute Unbounded Unaligned - typedef JointModelRevoluteUnboundedUnalignedTpl JointModelRevoluteUnboundedUnaligned; - - // Joint Prismatic - typedef JointModelPrismaticTpl JointModelPX; - typedef JointModelPrismaticTpl JointModelPY; - typedef JointModelPrismaticTpl JointModelPZ; - - // Joint Prismatic Unaligned - typedef JointModelPrismaticUnalignedTpl JointModelPrismaticUnaligned; - - // Joint Translation - typedef JointModelTranslationTpl JointModelTranslation; - - typedef boost::variant< - JointModelRX, JointModelRY, JointModelRZ - , JointModelRevoluteUnaligned - , JointModelPX, JointModelPY, JointModelPZ - , JointModelPrismaticUnaligned - , JointModelTranslation - , JointModelRUBX, JointModelRUBY, JointModelRUBZ - , JointModelRevoluteUnboundedUnaligned - > JointModelVariant; - - // Joint Revolute - typedef JointDataRevoluteTpl JointDataRX; - typedef JointDataRevoluteTpl JointDataRY; - typedef JointDataRevoluteTpl JointDataRZ; - - // Joint Revolute Unaligned - typedef JointDataRevoluteUnalignedTpl JointDataRevoluteUnaligned; - - // Joint Revolute Unaligned - typedef JointDataRevoluteUnboundedUnalignedTpl JointDataRevoluteUnboundedUnaligned; - - // Joint Revolute UBounded - typedef JointDataRevoluteUnboundedTpl JointDataRUBX; - typedef JointDataRevoluteUnboundedTpl JointDataRUBY; - typedef JointDataRevoluteUnboundedTpl JointDataRUBZ; - - // Joint Prismatic - typedef JointDataPrismaticTpl JointDataPX; - typedef JointDataPrismaticTpl JointDataPY; - typedef JointDataPrismaticTpl JointDataPZ; - - // Joint Prismatic Unaligned - typedef JointDataPrismaticUnalignedTpl JointDataPrismaticUnaligned; - - // Joint Translation - typedef JointDataTranslationTpl JointDataTranslation; - - typedef boost::variant< - JointDataRX, JointDataRY, JointDataRZ - , JointDataRevoluteUnaligned - , JointDataPX, JointDataPY, JointDataPZ - , JointDataPrismaticUnaligned - , JointDataTranslation - , JointDataRUBX, JointDataRUBY, JointDataRUBZ - , JointDataRevoluteUnboundedUnaligned - > JointDataVariant; -}; - - template class JointCollectionTpl> struct JointMimicTpl; - template class JointCollectionTpl> struct JointModelMimicTpl; - template class JointCollectionTpl> struct JointDataMimicTpl; - + + template + struct JointCollectionMimicableTpl + { + typedef _Scalar Scalar; + enum + { + Options = _Options + }; + + // Joint Revolute + typedef JointModelRevoluteTpl JointModelRX; + typedef JointModelRevoluteTpl JointModelRY; + typedef JointModelRevoluteTpl JointModelRZ; + + // Joint Revolute Unaligned + typedef JointModelRevoluteUnalignedTpl JointModelRevoluteUnaligned; + + // Joint Revolute UBounded + typedef JointModelRevoluteUnboundedTpl JointModelRUBX; + typedef JointModelRevoluteUnboundedTpl JointModelRUBY; + typedef JointModelRevoluteUnboundedTpl JointModelRUBZ; + + // Joint Revolute Unbounded Unaligned + typedef JointModelRevoluteUnboundedUnalignedTpl + JointModelRevoluteUnboundedUnaligned; + + // Joint Prismatic + typedef JointModelPrismaticTpl JointModelPX; + typedef JointModelPrismaticTpl JointModelPY; + typedef JointModelPrismaticTpl JointModelPZ; + + // Joint Prismatic Unaligned + typedef JointModelPrismaticUnalignedTpl JointModelPrismaticUnaligned; + + // Joint Translation + typedef JointModelTranslationTpl JointModelTranslation; + + typedef boost::variant< + JointModelRX, + JointModelRY, + JointModelRZ, + JointModelRevoluteUnaligned, + JointModelPX, + JointModelPY, + JointModelPZ, + JointModelPrismaticUnaligned, + JointModelTranslation, + JointModelRUBX, + JointModelRUBY, + JointModelRUBZ, + JointModelRevoluteUnboundedUnaligned> + JointModelVariant; + + // Joint Revolute + typedef JointDataRevoluteTpl JointDataRX; + typedef JointDataRevoluteTpl JointDataRY; + typedef JointDataRevoluteTpl JointDataRZ; + + // Joint Revolute Unaligned + typedef JointDataRevoluteUnalignedTpl JointDataRevoluteUnaligned; + + // Joint Revolute Unaligned + typedef JointDataRevoluteUnboundedUnalignedTpl + JointDataRevoluteUnboundedUnaligned; + + // Joint Revolute UBounded + typedef JointDataRevoluteUnboundedTpl JointDataRUBX; + typedef JointDataRevoluteUnboundedTpl JointDataRUBY; + typedef JointDataRevoluteUnboundedTpl JointDataRUBZ; + + // Joint Prismatic + typedef JointDataPrismaticTpl JointDataPX; + typedef JointDataPrismaticTpl JointDataPY; + typedef JointDataPrismaticTpl JointDataPZ; + + // Joint Prismatic Unaligned + typedef JointDataPrismaticUnalignedTpl JointDataPrismaticUnaligned; + + // Joint Translation + typedef JointDataTranslationTpl JointDataTranslation; + + typedef boost::variant< + JointDataRX, + JointDataRY, + JointDataRZ, + JointDataRevoluteUnaligned, + JointDataPX, + JointDataPY, + JointDataPZ, + JointDataPrismaticUnaligned, + JointDataTranslation, + JointDataRUBX, + JointDataRUBY, + JointDataRUBZ, + JointDataRevoluteUnboundedUnaligned> + JointDataVariant; + }; + + template class JointCollectionTpl> + struct JointMimicTpl; + template class JointCollectionTpl> + struct JointModelMimicTpl; + template class JointCollectionTpl> + struct JointDataMimicTpl; + template class JointCollectionTpl> - struct traits< JointMimicTpl<_Scalar, _Options, JointCollectionTpl> > + struct traits> { typedef _Scalar Scalar; @@ -355,52 +432,54 @@ struct JointCollectionMimicableTpl NV = Eigen::Dynamic, NJ = Eigen::Dynamic }; - - typedef JointCollectionTpl JointCollection; - typedef JointDataMimicTpl JointDataDerived; - typedef JointModelMimicTpl JointModelDerived; + + typedef JointCollectionTpl JointCollection; + typedef JointDataMimicTpl JointDataDerived; + typedef JointModelMimicTpl JointModelDerived; typedef ScaledJointMotionSubspaceTpl Constraint_t; - typedef SE3Tpl Transformation_t; + typedef SE3Tpl Transformation_t; + + typedef MotionTpl Motion_t; + typedef MotionTpl Bias_t; - typedef MotionTpl Motion_t; - typedef MotionTpl Bias_t; - // [ABA] - typedef Eigen::Matrix U_t; - typedef Eigen::Matrix D_t; - typedef Eigen::Matrix UD_t; - - typedef Eigen::Matrix ConfigVector_t; - typedef Eigen::Matrix TangentVector_t; + typedef Eigen::Matrix U_t; + typedef Eigen::Matrix D_t; + typedef Eigen::Matrix UD_t; + + typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix TangentVector_t; PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; - + template class JointCollectionTpl> - struct traits< JointDataMimicTpl<_Scalar, Options, JointCollectionTpl> > - { - typedef JointMimicTpl<_Scalar,Options,JointCollectionTpl> JointDerived; + struct traits> + { + typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived; typedef _Scalar Scalar; }; - + template class JointCollectionTpl> - struct traits< JointModelMimicTpl<_Scalar, Options, JointCollectionTpl> > - { - typedef JointMimicTpl<_Scalar,Options,JointCollectionTpl> JointDerived; + struct traits> + { + typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived; typedef _Scalar Scalar; }; template class JointCollectionTpl> struct JointDataMimicTpl - : public JointDataBase< JointDataMimicTpl<_Scalar, _Options, JointCollectionTpl> > + : public JointDataBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointDataBase Base; - typedef JointMimicTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); - - // typedef typename boost::make_variant_over>::type>::type MimicableJointDataVariant; + + // typedef typename boost::make_variant_over>::type>::type + // MimicableJointDataVariant; typedef JointDataTpl<_Scalar, _Options, JointCollectionMimicableTpl> RefJointData; typedef typename RefJointData::JointDataVariant RefJointDataVariant; @@ -411,26 +490,27 @@ struct JointCollectionMimicableTpl m_q_transform.resize(0); m_v_transform.resize(0); } - + // JointDataMimicTpl(const JointDataMimicTpl & other) // { *this = other; } - - JointDataMimicTpl(const JointDataTpl & jdata, - const Scalar & scaling, - const Scalar & nq, - const Scalar & nv) + + JointDataMimicTpl( + const JointDataTpl & jdata, + const Scalar & scaling, + const Scalar & nq, + const Scalar & nv) : m_scaling(scaling) - , S(m_jdata_ref.S(),scaling) - , m_jdata_ref(transferToVariant, RefJointData>(jdata)) - { + , S(m_jdata_ref.S(), scaling) + , m_jdata_ref( + transferToVariant, RefJointData>(jdata)) + { m_q_transform.resize(nq, 1); m_v_transform.resize(nv, 1); // boost::apply_visitor(TransferVisitor(m_jdata_ref), jdata); } - // JointDataMimicTpl(const RefJointDataVariant & jdata, - // const Scalar & scaling, + // const Scalar & scaling, // const Scalar & nq, // const Scalar & nv) // : m_jdata_ref(jdata) @@ -440,134 +520,195 @@ struct JointCollectionMimicableTpl // } - JointDataMimicTpl(const RefJointData & jdata, - const Scalar & scaling, - const Scalar & nq, - const Scalar & nv) + JointDataMimicTpl( + const RefJointData & jdata, const Scalar & scaling, const Scalar & nq, const Scalar & nv) : m_jdata_ref(jdata.derived()) , m_scaling(scaling) - , S(m_jdata_ref.S(),scaling) + , S(m_jdata_ref.S(), scaling) { m_q_transform.resize(nq, 1); m_v_transform.resize(nv, 1); } - + JointDataMimicTpl & operator=(const JointDataMimicTpl & other) { m_jdata_ref = other.m_jdata_ref; m_scaling = other.m_scaling; m_q_transform = other.m_q_transform; m_v_transform = other.m_v_transform; - S = Constraint_t(m_jdata_ref.S(),other.m_scaling); + S = Constraint_t(m_jdata_ref.S(), other.m_scaling); return *this; } - + using Base::isEqual; bool isEqual(const JointDataMimicTpl & other) const { - return Base::isEqual(other) - && m_jdata_ref == other.m_jdata_ref - && m_scaling == other.m_scaling - && m_q_transform == other.m_q_transform - && m_v_transform == other.m_v_transform - ; - } - - static std::string classname() { return std::string("JointDataMimic"); } - - std::string shortname() const { return classname(); } - + return Base::isEqual(other) && m_jdata_ref == other.m_jdata_ref + && m_scaling == other.m_scaling && m_q_transform == other.m_q_transform + && m_v_transform == other.m_v_transform; + } + + static std::string classname() + { + return std::string("JointDataMimic"); + } + + std::string shortname() const + { + return classname(); + } + // // Accessors - ConstraintTypeConstRef S_accessor() const { return S; } - ConstraintTypeRef S_accessor() { return S; } - - TansformTypeConstRef M_accessor() const { + ConstraintTypeConstRef S_accessor() const + { + return S; + } + ConstraintTypeRef S_accessor() + { + return S; + } + + TansformTypeConstRef M_accessor() const + { M_ = m_jdata_ref.M(); - return M_; } - TansformTypeRef M_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + return M_; + } + TansformTypeRef M_accessor() + { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); M_ = m_jdata_ref.M(); - return M_; } + return M_; + } - MotionTypeConstRef v_accessor() const { + MotionTypeConstRef v_accessor() const + { v_ = m_jdata_ref.v(); - return v_; } - MotionTypeRef v_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + return v_; + } + MotionTypeRef v_accessor() + { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); v_ = m_jdata_ref.v(); - return v_; } + return v_; + } - BiasTypeConstRef c_accessor() const { + BiasTypeConstRef c_accessor() const + { c_ = m_jdata_ref.c(); - return c_; } - BiasTypeRef c_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + return c_; + } + BiasTypeRef c_accessor() + { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); c_ = m_jdata_ref.c(); - return c_; } + return c_; + } - UTypeConstRef U_accessor() const { + UTypeConstRef U_accessor() const + { U_ = m_jdata_ref.U(); - return U_; } - UTypeRef U_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + return U_; + } + UTypeRef U_accessor() + { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); U_ = m_jdata_ref.U(); - return U_; } + return U_; + } - DTypeConstRef Dinv_accessor() const { + DTypeConstRef Dinv_accessor() const + { Dinv_ = m_jdata_ref.Dinv(); - return Dinv_; } - DTypeRef Dinv_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + return Dinv_; + } + DTypeRef Dinv_accessor() + { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); Dinv_ = m_jdata_ref.Dinv(); - return Dinv_; } + return Dinv_; + } - UDTypeConstRef UDinv_accessor() const { + UDTypeConstRef UDinv_accessor() const + { UDinv_ = m_jdata_ref.UDinv(); - return UDinv_; } - UDTypeRef UDinv_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + return UDinv_; + } + UDTypeRef UDinv_accessor() + { + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); UDinv_ = m_jdata_ref.UDinv(); - return UDinv_; } + return UDinv_; + } - DTypeConstRef StU_accessor() const - { + DTypeConstRef StU_accessor() const + { StU_ = m_jdata_ref.StU(); return StU_; } DTypeRef StU_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use + // const ref"); StU_ = m_jdata_ref.StU(); return StU_; } friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>; - - const RefJointData & jdata() const { return m_jdata_ref; } - RefJointData & jdata() { return m_jdata_ref; } - - const Scalar & scaling() const { return m_scaling; } - Scalar & scaling() { return m_scaling; } - - ConfigVector_t & joint_q_accessor() { return m_q_transform; } - const ConfigVector_t & joint_q_accessor() const { return m_q_transform; } - - TangentVector_t & joint_v_accessor() { return m_v_transform; } - const TangentVector_t & joint_v_accessor() const { return m_v_transform; } - + + const RefJointData & jdata() const + { + return m_jdata_ref; + } + RefJointData & jdata() + { + return m_jdata_ref; + } + + const Scalar & scaling() const + { + return m_scaling; + } + Scalar & scaling() + { + return m_scaling; + } + + ConfigVector_t & joint_q_accessor() + { + return m_q_transform; + } + const ConfigVector_t & joint_q_accessor() const + { + return m_q_transform; + } + + TangentVector_t & joint_v_accessor() + { + return m_v_transform; + } + const TangentVector_t & joint_v_accessor() const + { + return m_v_transform; + } + protected: - RefJointData m_jdata_ref; Scalar m_scaling; /// \brief Transform configuration vector ConfigVector_t m_q_transform; /// \brief Transform velocity vector. - TangentVector_t m_v_transform; + TangentVector_t m_v_transform; public: // data - Constraint_t S; + Constraint_t S; protected: /// \brief Buffer variable for accessors to return references @@ -578,81 +719,96 @@ struct JointCollectionMimicableTpl mutable D_t Dinv_; mutable UD_t UDinv_; mutable D_t StU_; - + }; // struct JointDataMimicTpl - - template class JointCollectionTpl> - struct CastType< NewScalar, JointModelMimicTpl > + + template< + typename NewScalar, + typename Scalar, + int Options, + template + class JointCollectionTpl> + struct CastType> { - typedef JointModelMimicTpl type; + typedef JointModelMimicTpl type; }; - + template class JointCollectionTpl> struct JointModelMimicTpl - : public JointModelBase< JointModelMimicTpl<_Scalar,_Options,JointCollectionTpl> > + : public JointModelBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - + typedef JointModelBase Base; - typedef JointMimicTpl<_Scalar,_Options,JointCollectionTpl> JointDerived; + typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); - - typedef JointCollectionTpl JointCollection; - typedef JointModelTpl JointModel; - typedef typename JointModel::JointModelVariant JointModelVariant; - // typedef typename boost::make_variant_over>::type>::type MimicableJointModelVariant; - // typedef JointModelTpl<_Scalar, _Options, JointCollectionMimicableTpl> MimicableJointModel; + typedef JointCollectionTpl JointCollection; + typedef JointModelTpl JointModel; + typedef typename JointModel::JointModelVariant JointModelVariant; + // typedef typename boost::make_variant_over>::type>::type + // MimicableJointModelVariant; typedef JointModelTpl<_Scalar, _Options, + // JointCollectionMimicableTpl> MimicableJointModel; - typedef SE3Tpl SE3; - typedef MotionTpl Motion; - typedef InertiaTpl Inertia; + typedef SE3Tpl SE3; + typedef MotionTpl Motion; + typedef InertiaTpl Inertia; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; + using Base::nj; using Base::nq; using Base::nv; - using Base::nj; using Base::setIndexes; JointModelMimicTpl() - {} + { + } - JointModelMimicTpl(const JointModelTpl & jmodel, - const Scalar & scaling, - const Scalar & offset) + JointModelMimicTpl( + const JointModelTpl & jmodel, + const Scalar & scaling, + const Scalar & offset) : JointModelMimicTpl(jmodel, jmodel, scaling, offset) - { } + { + } - JointModelMimicTpl(const JointModelTpl & jmodel_mimicking, - const JointModelTpl & jmodel_mimicked, - const Scalar & scaling, - const Scalar & offset) + JointModelMimicTpl( + const JointModelTpl & jmodel_mimicking, + const JointModelTpl & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) : m_scaling(scaling) , m_offset(offset) - , m_jmodel_ref(transferToVariant, JointModel>(jmodel_mimicking)) + , m_jmodel_ref( + transferToVariant, JointModel>( + jmodel_mimicking)) { assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); + m_jmodel_ref.setIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_j()); } template - JointModelMimicTpl(const JointModelBase & jmodel, - const Scalar & scaling, - const Scalar & offset) + JointModelMimicTpl( + const JointModelBase & jmodel, const Scalar & scaling, const Scalar & offset) : JointModelMimicTpl(jmodel, jmodel, scaling, offset) - { } + { + } template - JointModelMimicTpl(const JointModelBase & jmodel_mimicking, - const JointModelBase & jmodel_mimicked, - const Scalar & scaling, - const Scalar & offset) + JointModelMimicTpl( + const JointModelBase & jmodel_mimicking, + const JointModelBase & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) , m_scaling(scaling) , m_offset(offset) @@ -661,19 +817,42 @@ struct JointCollectionMimicableTpl assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); + m_jmodel_ref.setIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_j()); + } + + Base & base() + { + return *static_cast(this); + } + const Base & base() const + { + return *static_cast(this); + } + + inline int nq_impl() const + { + return 0; + } + inline int nv_impl() const + { + return 0; + } + inline int nj_impl() const + { + return m_jmodel_ref.nj(); + } + + inline int idx_q_impl() const + { + return m_jmodel_ref.idx_q(); + } + inline int idx_v_impl() const + { + return m_jmodel_ref.idx_v(); } - Base & base() { return *static_cast(this); } - const Base & base() const { return *static_cast(this); } - - inline int nq_impl() const { return 0; } - inline int nv_impl() const { return 0; } - inline int nj_impl() const { return m_jmodel_ref.nj(); } - - inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); } - inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); } - void setIndexes_impl(JointIndex id, int q, int v, int j) { Base::i_id = id; // Only the id of the joint in the model is different. @@ -681,13 +860,14 @@ struct JointCollectionMimicableTpl Base::i_v = m_jmodel_ref.idx_v(); Base::i_j = j; } - + JointDataDerived createData() const { - return JointDataDerived(m_jmodel_ref.createData(),scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); + return JointDataDerived( + m_jmodel_ref.createData(), scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); } - + const std::vector hasConfigurationLimit() const { return m_jmodel_ref.hasConfigurationLimit(); @@ -699,33 +879,32 @@ struct JointCollectionMimicableTpl } template - EIGEN_DONT_INLINE - void calc(JointDataDerived & jdata, - const typename Eigen::MatrixBase & qs) const + EIGEN_DONT_INLINE void + calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const { typedef typename ConfigVectorAffineTransform::Type AffineTransform; - - AffineTransform::run(qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), - m_scaling,m_offset,jdata.m_q_transform); - m_jmodel_ref.calc(jdata.m_jdata_ref,qs); + + AffineTransform::run( + qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + jdata.m_q_transform); + m_jmodel_ref.calc(jdata.m_jdata_ref, qs); } - + template - EIGEN_DONT_INLINE - void calc(JointDataDerived & jdata, - const typename Eigen::MatrixBase & qs, - const typename Eigen::MatrixBase & vs) const + EIGEN_DONT_INLINE void calc( + JointDataDerived & jdata, + const typename Eigen::MatrixBase & qs, + const typename Eigen::MatrixBase & vs) const { typedef typename ConfigVectorAffineTransform::Type AffineTransform; - - AffineTransform::run(qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), - m_scaling,m_offset,jdata.m_q_transform); + + AffineTransform::run( + qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + jdata.m_q_transform); jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, - qs, - vs); + m_jmodel_ref.calc(jdata.m_jdata_ref, qs, vs); } - + template void calc_aba( JointDataDerived & data, @@ -734,89 +913,103 @@ struct JointCollectionMimicableTpl const bool update_I) const { // TODO: fixme - assert(false && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use this function"); - m_jmodel_ref.calc_aba(data.m_jdata_ref, - armature, - PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I), - update_I); - } - - static std::string classname() { return std::string("JointModelMimic"); } - - std::string shortname() const { return classname(); } - + assert( + false + && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use " + "this function"); + m_jmodel_ref.calc_aba( + data.m_jdata_ref, armature, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); + } + + static std::string classname() + { + return std::string("JointModelMimic"); + } + + std::string shortname() const + { + return classname(); + } + /// \returns An expression of *this with the Scalar type casted to NewScalar. template - typename CastType::type cast() const - { - typedef typename CastType::type ReturnType; - - ReturnType res(m_jmodel_ref.template cast(), - (NewScalar)m_scaling, - (NewScalar)m_offset); - res.setIndexes(id(),idx_q(),idx_v(), idx_j()); + typename CastType::type cast() const + { + typedef typename CastType::type ReturnType; + + ReturnType res( + m_jmodel_ref.template cast(), (NewScalar)m_scaling, (NewScalar)m_offset); + res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } - - const JointModel & jmodel() const { return m_jmodel_ref; } - JointModel & jmodel() { return m_jmodel_ref; } - - const Scalar & scaling() const { return m_scaling; } - Scalar & scaling() { return m_scaling; } - - const Scalar & offset() const { return m_offset; } - Scalar & offset() { return m_offset; } - + + const JointModel & jmodel() const + { + return m_jmodel_ref; + } + JointModel & jmodel() + { + return m_jmodel_ref; + } + + const Scalar & scaling() const + { + return m_scaling; + } + Scalar & scaling() + { + return m_scaling; + } + + const Scalar & offset() const + { + return m_offset; + } + Scalar & offset() + { + return m_offset; + } + protected: - // data JointModel m_jmodel_ref; Scalar m_scaling, m_offset; - + public: - /* Acces to dedicated segment in robot config space. */ // Const access template typename SizeDepType::template SegmentReturn::ConstType jointConfigSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), - m_jmodel_ref.idx_q(), - m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); } - + // Non-const access template typename SizeDepType::template SegmentReturn::Type jointConfigSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), - m_jmodel_ref.idx_q(), - m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); } - + /* Acces to dedicated segment in robot config velocity space. */ // Const access template typename SizeDepType::template SegmentReturn::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), - m_jmodel_ref.idx_v(), - m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); } - + // Non-const access template typename SizeDepType::template SegmentReturn::Type jointVelocitySelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), - m_jmodel_ref.idx_v(), - m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); } - + /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ // Const access // template @@ -827,7 +1020,7 @@ struct JointCollectionMimicableTpl // m_jmodel_ref.idx_v(), // m_jmodel_ref.nv()); // } - + // // Non-const access // template // typename SizeDepType::template ColsReturn::Type @@ -837,7 +1030,7 @@ struct JointCollectionMimicableTpl // m_jmodel_ref.idx_v(), // m_jmodel_ref.nv()); // } - + // /* Acces to dedicated rows in a matrix.*/ // // Const access // template @@ -848,7 +1041,7 @@ struct JointCollectionMimicableTpl // m_jmodel_ref.idx_v(), // m_jmodel_ref.nv()); // } - + // // Non-const access // template // typename SizeDepType::template RowsReturn::Type @@ -858,8 +1051,9 @@ struct JointCollectionMimicableTpl // m_jmodel_ref.idx_v(), // m_jmodel_ref.nv()); // } - - // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat + + // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the + // matrix Mat // // Const access // template // typename SizeDepType::template BlockReturn::ConstType @@ -869,7 +1063,7 @@ struct JointCollectionMimicableTpl // m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(), // m_jmodel_ref.nv(),m_jmodel_ref.nv()); // } - + // // Non-const access // template // typename SizeDepType::template BlockReturn::Type @@ -881,7 +1075,7 @@ struct JointCollectionMimicableTpl // } }; // struct JointModelMimicTpl - + } // namespace pinocchio #include @@ -889,20 +1083,30 @@ struct JointCollectionMimicableTpl namespace boost { template class JointCollectionTpl> - struct has_nothrow_constructor< ::pinocchio::JointModelMimicTpl > - : public integral_constant {}; - + struct has_nothrow_constructor< + ::pinocchio::JointModelMimicTpl> + : public integral_constant + { + }; + template class JointCollectionTpl> - struct has_nothrow_copy< ::pinocchio::JointModelMimicTpl > - : public integral_constant {}; - + struct has_nothrow_copy<::pinocchio::JointModelMimicTpl> + : public integral_constant + { + }; + template class JointCollectionTpl> - struct has_nothrow_constructor< ::pinocchio::JointDataMimicTpl > - : public integral_constant {}; - + struct has_nothrow_constructor< + ::pinocchio::JointDataMimicTpl> + : public integral_constant + { + }; + template class JointCollectionTpl> - struct has_nothrow_copy< ::pinocchio::JointDataMimicTpl > - : public integral_constant {}; -} + struct has_nothrow_copy<::pinocchio::JointDataMimicTpl> + : public integral_constant + { + }; +} // namespace boost -#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ \ No newline at end of file +#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ From 366f21f65d49fa4cd84254b2ff738723a22f6fb4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 024/166] [EtienneAr feedback] Fix appendToModel when mimic joint in model --- include/pinocchio/algorithm/model.hxx | 30 +++++++++++++++++---------- include/pinocchio/multibody/data.hxx | 2 ++ 2 files changed, 21 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index bb8cb0e2fb..3050cbbf4d 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -156,26 +156,30 @@ namespace pinocchio template static typename std::enable_if< !std::is_same>::value, - void>::type - updateMimicIds(JointModel & jmodel, const Model & old_model, const Model & new_model) + JointModel>::type + updateMimicIds(const JointModel & jmodel, const Model & old_model, const Model & new_model) { + JointModel res(jmodel); + return res; } template static typename std::enable_if< std::is_same>::value, - void>::type + JointModel>::type updateMimicIds( - JointModelMimicTpl & jmodel, + const JointModelMimicTpl & jmodel, const Model & old_model, const Model & new_model) { - const JointIndex mimicked_old_id = jmodel.jmodel().id(); + JointModel res(jmodel); + const JointIndex mimicked_old_id = res.jmodel().id(); const std::string mimicked_name = old_model.names[mimicked_old_id]; const JointIndex mimicked_new_id = new_model.getJointId(mimicked_name); - jmodel.jmodel().setIndexes( + res.jmodel().setIndexes( mimicked_new_id, new_model.joints[mimicked_new_id].idx_q(), new_model.joints[mimicked_new_id].idx_v(), new_model.joints[mimicked_new_id].idx_j()); + return res; } template @@ -198,9 +202,16 @@ namespace pinocchio !model.existJointName(modelAB.names[joint_id_in]), "The two models have conflicting joint names."); + // For mimic joints, update the reference joint id + JointModel jmodel_inter = updateMimicIds(jmodel_in.derived(), modelAB, model); + JointIndex joint_id_out = model.addJoint( - parent_id, jmodel_in, pMi * modelAB.jointPlacements[joint_id_in], - modelAB.names[joint_id_in], jmodel_in.jointVelocitySelector(modelAB.effortLimit), + parent_id, + jmodel_inter, // Use the intermediate joint (jmodel_inter) with updates id, idx, ... + pMi * modelAB.jointPlacements[joint_id_in], modelAB.names[joint_id_in], + jmodel_in.jointVelocitySelector( + modelAB + .effortLimit), // Need to select the vector base on the origin idxs, so use jmodel_in. jmodel_in.jointVelocitySelector(modelAB.velocityLimit), jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit), jmodel_in.jointConfigSelector(modelAB.upperPositionLimit), @@ -212,9 +223,6 @@ namespace pinocchio typename Model::JointModel & jmodel_out = model.joints[joint_id_out]; - // For mimic joints, update the reference joint id - updateMimicIds(boost::get(jmodel_out), modelAB, model); - jmodel_out.jointVelocitySelector(model.rotorInertia) = jmodel_in.jointVelocitySelector(modelAB.rotorInertia); jmodel_out.jointVelocitySelector(model.rotorGearRatio) = diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 8863485d77..5d15c2d933 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -223,6 +223,8 @@ namespace pinocchio for (Index joint = 1; joint < (Index)(model.njoints); joint++) { + if (boost::get(&model.joints[joint])) + continue; // Mimic joints should not override mimicked joint fromRow values const Index & parent = model.parents[joint]; const int nvj = model.joints[joint].nv(); const int idx_vj = model.joints[joint].idx_v(); From e9de47f99cfa9942d6a7fa71c8bbce21f4a3ab20 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 025/166] [EtienneAr feedback] Apply pre-commit --- include/pinocchio/multibody/data.hxx | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 5d15c2d933..f3b23cfcfb 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -201,17 +201,18 @@ namespace pinocchio lastChild[(Index)i] = i; const Index & parent = model.parents[(Index)i]; - lastChild[parent] = std::max(lastChild[(Index)i],lastChild[parent]); + lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); Scalar nv_; - if(boost::get(&model.joints[(Index)lastChild[(Index)i]]) && lastChild[(Index)i] != i) + if ( + boost::get(&model.joints[(Index)lastChild[(Index)i]]) + && lastChild[(Index)i] != i) nv_ = boost::get(model.joints[(Index)lastChild[(Index)i]]).jmodel().nv(); else nv_ = nv(model.joints[(Index)lastChild[(Index)i]]); - nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_v() - + nv_ - - model.joints[(Index)i].idx_v(); + nvSubtree[(Index)i] = + model.joints[(Index)lastChild[(Index)i]].idx_v() + nv_ - model.joints[(Index)i].idx_v(); } } From 3c630f54cab810627d1363058f90315ee201ede1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 026/166] [EtienneAr feedback] Turn on mimic joint in model, reachable-workspace, sample-models unittest --- unittest/model.cpp | 2 +- unittest/reachable-workspace.cpp | 2 +- unittest/sample-models.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/unittest/model.cpp b/unittest/model.cpp index 7e254fbb04..1a94dd4d28 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -174,7 +174,7 @@ BOOST_AUTO_TEST_CASE(append) Model manipulator, humanoid; GeometryModel geomManipulator, geomHumanoid; - buildModels::manipulator(manipulator); + buildModels::manipulator(manipulator, true); buildModels::manipulatorGeometries(manipulator, geomManipulator); geomManipulator.addAllCollisionPairs(); // Add prefix to joint and frame names diff --git a/unittest/reachable-workspace.cpp b/unittest/reachable-workspace.cpp index c4b56f83dc..1f19dbedd1 100644 --- a/unittest/reachable-workspace.cpp +++ b/unittest/reachable-workspace.cpp @@ -296,7 +296,7 @@ BOOST_AUTO_TEST_CASE(test_reachable_workspace) // Load the urdf model Model model; - pinocchio::buildModels::manipulator(model); + pinocchio::buildModels::manipulator(model, true); Eigen::VectorXd q = (model.upperPositionLimit + model.lowerPositionLimit) / 2; ReachableSetResults res; diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index 4ad4815988..8742a9c228 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -32,10 +32,10 @@ BOOST_AUTO_TEST_CASE(build_model_sample_humanoid_random) BOOST_AUTO_TEST_CASE(build_model_sample_manipulator) { pinocchio::Model model; - pinocchio::buildModels::manipulator(model); + pinocchio::buildModels::manipulator(model, true); - BOOST_CHECK(model.nq == 6); - BOOST_CHECK(model.nv == 6); + BOOST_CHECK(model.nq == 5); + BOOST_CHECK(model.nv == 5); #ifdef PINOCCHIO_WITH_HPP_FCL pinocchio::Data data(model); From 2176a5f6b8ad3208af54c3ba529ed09eb17962ad Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 027/166] [EtienneAr feedback] Split jointCols jointRows and jointBLock for full and reduced system --- .../pinocchio/algorithm/aba-derivatives.hxx | 44 +++---- include/pinocchio/algorithm/aba.hxx | 18 +-- .../algorithm/center-of-mass-derivatives.hxx | 2 +- .../pinocchio/algorithm/center-of-mass.hxx | 8 +- .../algorithm/centroidal-derivatives.hxx | 34 ++--- include/pinocchio/algorithm/centroidal.hxx | 12 +- .../pinocchio/algorithm/compute-all-terms.hxx | 12 +- .../constrained-dynamics-derivatives.hxx | 24 ++-- .../algorithm/constrained-dynamics.hxx | 20 +-- include/pinocchio/algorithm/crba.hxx | 8 +- include/pinocchio/algorithm/delassus.hxx | 14 +- .../impulse-dynamics-derivatives.hxx | 12 +- include/pinocchio/algorithm/jacobian.hxx | 18 ++- .../algorithm/kinematics-derivatives.hxx | 40 +++--- .../pinocchio/algorithm/rnea-derivatives.hxx | 38 +++--- .../rnea-second-order-derivatives.hxx | 9 +- include/pinocchio/algorithm/rnea.hxx | 22 ++-- .../multibody/joint/joint-composite.hpp | 44 +++++-- .../pinocchio/multibody/joint/joint-mimic.hpp | 98 +++++++------- .../multibody/joint/joint-model-base.hpp | 123 +++++++++++++++--- .../multibody/liegroup/liegroup-algo.hxx | 20 +-- unittest/joint-composite.cpp | 6 +- 22 files changed, 371 insertions(+), 255 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 2e2205c471..37cb503a7a 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -74,7 +74,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); } }; @@ -119,7 +119,7 @@ namespace pinocchio Force & fi = data.of[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - const ColBlock J_cols = jmodel.jointCols(data.J); + const ColBlock J_cols = jmodel.jointJacCols(data.J); jmodel.jointVelocitySelector(data.u).noalias() -= J_cols.transpose() * fi.toVector(); @@ -137,7 +137,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv_.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -204,7 +204,7 @@ namespace pinocchio typename Data::Motion & oa_gf = data.oa_gf[i]; typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); oa_gf += data.oa_gf[parent]; jmodel.jointVelocitySelector(data.ddq).noalias() = @@ -233,10 +233,10 @@ namespace pinocchio data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); motionSet::motionAction(ov, J_cols, dJ_cols); motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols); @@ -303,13 +303,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // dtau/dv motionSet::inertiaAction(data.oYcrb[i], dAdv_cols, dFdv_cols); @@ -578,7 +578,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; - const ColBlock J_cols = jmodel.jointCols(data.J); + const ColBlock J_cols = jmodel.jointJacCols(data.J); MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv); @@ -586,7 +586,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv_.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -649,7 +649,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - const ColsBlock J_cols = jmodel.jointCols(data.J); + const ColsBlock J_cols = jmodel.jointJacCols(data.J); // Already done in optimized::aba // oa = oa_gf + model.gravity; @@ -674,10 +674,10 @@ namespace pinocchio data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); motionSet::motionAction(ov, J_cols, dJ_cols); motionSet::motionAction(data.oa_gf[parent], J_cols, dAdq_cols); diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index e0798504d4..81756748a9 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -120,7 +120,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); ov = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -165,7 +165,7 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Force & fi = data.of[i]; @@ -214,7 +214,7 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock J_cols = jmodel.jointCols(data.J); + ColBlock J_cols = jmodel.jointJacCols(data.J); const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; @@ -636,7 +636,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); @@ -674,7 +674,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); jdata.U().noalias() = Ia * J_cols; jdata.StU().noalias() = J_cols.transpose() * jdata.U(); @@ -689,7 +689,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -748,13 +748,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - const ColsBlock J_cols = jmodel.jointCols(data.J); + const ColsBlock J_cols = jmodel.jointJacCols(data.J); Minv.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv()) = jdata.Dinv(); const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) .noalias() = @@ -804,7 +804,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); if (parent > 0) { diff --git a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx index 6bd999b8f6..be9d6a0a81 100644 --- a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx +++ b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx @@ -48,7 +48,7 @@ namespace pinocchio Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut, vcom_partial_dq); typename SizeDepType::template ColsReturn::Type - dvcom_dqi = jmodel.jointCols(dvcom_dq); + dvcom_dqi = jmodel.jointVelCols(dvcom_dq); Motion vpc = (parent > 0) ? (data.v[i] - (Motion)jdata.v()) : Motion::Zero(); vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ] diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index 11ff1cea47..087a5559ff 100644 --- a/include/pinocchio/algorithm/center-of-mass.hxx +++ b/include/pinocchio/algorithm/center-of-mass.hxx @@ -257,12 +257,12 @@ namespace pinocchio Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) { - jmodel.jointCols(Jcom_).col(col_id) = + jmodel.jointVelCols(Jcom_).col(col_id) = data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR) - data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); } @@ -366,12 +366,12 @@ namespace pinocchio Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) { - jmodel.jointCols(Jcom_).col(col_id) = + jmodel.jointVelCols(Jcom_).col(col_id) = Jcols.col(col_id).template segment<3>(Motion::LINEAR) - data.com[subtree_root_id].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); } diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 68dafb3a06..ce85aad3e8 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -94,11 +94,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(ov, J_cols, dJ_cols); @@ -158,14 +158,14 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dHdq_cols = jmodel.jointVelCols(data.dHdq); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // tau jmodel.jointVelocitySelector(data.tau).noalias() = @@ -375,10 +375,10 @@ namespace pinocchio typename Data::Motion & vtmp = data.v[0]; typename Data::Matrix6x & Ftmp = data.Fcrb[0]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); - ColsBlock Ftmp_cols = jmodel.jointCols(Ftmp); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dHdq_cols = jmodel.jointVelCols(data.dHdq); + ColsBlock Ftmp_cols = jmodel.jointVelCols(Ftmp); const Vector3 mg = data.oYcrb[i].mass() * model.gravity.linear(); for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 3e9f1aaafe..1effed60c6 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -136,10 +136,10 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); data.oYcrb[parent] += data.oYcrb[i]; } @@ -264,10 +264,10 @@ namespace pinocchio const Inertia & Y = data.oYcrb[i]; const typename Inertia::Matrix6 & doYcrb = data.doYcrb[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.oYcrb[parent] += Y; @@ -275,11 +275,11 @@ namespace pinocchio data.doYcrb[parent] += doYcrb; // Calc Ag - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); motionSet::inertiaAction(Y, J_cols, Ag_cols); // Calc dAg = Ivx + vxI - ColsBlock dAg_cols = jmodel.jointCols(data.dAg); + ColsBlock dAg_cols = jmodel.jointVelCols(data.dAg); dAg_cols.noalias() = doYcrb * J_cols; motionSet::inertiaAction(Y, dJ_cols, dAg_cols); } diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 3ca4fa2866..594baf9167 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -70,10 +70,10 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]); - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.a_gf[i] = data.a[i] = jdata.c() + (data.v[i] ^ jdata.v()); @@ -111,10 +111,10 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - ColsBlock dAg_cols = jmodel.jointCols(data.dAg); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock dAg_cols = jmodel.jointVelCols(data.dAg); // Calc Ag = Y * S motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index fb986b8295..a9e0334908 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -50,14 +50,14 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); if (ContactMode) { const Motion & ov = data.ov[i]; - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); motionSet::motionAction(ov, J_cols, dJ_cols); // TODO: make more efficient @@ -79,7 +79,7 @@ namespace pinocchio RNEAForwardStepType::addForceCrossMatrix(data.oh[i], data.doYcrb[i]); Motion & oa = data.oa[i]; Motion & oa_gf = data.oa_gf[i]; - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); const typename Data::TangentVectorType & a = data.ddq; data.a[i] = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); @@ -142,11 +142,11 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); typename Data::RowMatrixXs & dtau_dq = data.dtau_dq; @@ -185,8 +185,8 @@ namespace pinocchio if (ContactMode) { - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); typename Data::RowMatrixXs & dtau_dv = data.dtau_dv; dFdv_cols.noalias() = data.doYcrb[i] * J_cols; diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index f1bc7fbb3d..1ed65c384f 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -115,7 +115,7 @@ namespace pinocchio if (parent > 0) ov += data.ov[parent]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); oinertias = data.oMi[i].act(model.inertias[i]); data.oYcrb[i] = data.oinertias[i]; if (ContactMode) @@ -162,8 +162,8 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); - const ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); + const ColsBlock J_cols = jmodel.jointJacCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = @@ -562,7 +562,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); data.ov[i] = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -618,7 +618,7 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; @@ -680,7 +680,7 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; @@ -719,7 +719,7 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; @@ -1091,7 +1091,7 @@ namespace pinocchio // subtract the bias forces from the torque to get Mv_dot_free jmodel.jointVelocitySelector(data.tau).noalias() -= - jmodel.jointCols(data.J).transpose() * (data.of[i].toVector()); + jmodel.jointJacCols(data.J).transpose() * (data.of[i].toVector()); data.of_augmented[i].toVector().setZero(); } @@ -1115,7 +1115,7 @@ namespace pinocchio const JointModel & jmodel = model.joints[i]; data.oa_augmented[i].toVector().noalias() = data.oa_augmented[model.parents[i]].toVector() - + jmodel.jointCols(data.J) * jmodel.jointVelocitySelector(data.u); + + jmodel.jointJacCols(data.J) * jmodel.jointVelocitySelector(data.u); data.of_augmented[i].toVector().setZero(); } } @@ -1159,7 +1159,7 @@ namespace pinocchio data.of_augmented[parent] += data.of_augmented[i]; jmodel.jointVelocitySelector(data.tau).noalias() = - -jmodel.jointCols(data.J).transpose() * (data.of_augmented[i].toVector()); + -jmodel.jointJacCols(data.J).transpose() * (data.of_augmented[i].toVector()); } } diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index a97fefa1bd..d7d8193cc7 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -51,7 +51,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); } @@ -77,8 +77,8 @@ namespace pinocchio const JointIndex & i = jmodel.id(); // Centroidal momentum map - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock J_cols = jmodel.jointJacCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); // Joint Space Inertia Matrix @@ -153,7 +153,7 @@ namespace pinocchio /* F[1:6,i] = Y*S */ // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S(); - jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); + jmodel.jointJacCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = diff --git a/include/pinocchio/algorithm/delassus.hxx b/include/pinocchio/algorithm/delassus.hxx index 61c1efb033..4275c65510 100644 --- a/include/pinocchio/algorithm/delassus.hxx +++ b/include/pinocchio/algorithm/delassus.hxx @@ -48,7 +48,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); data.oYaba[i] = data.oYcrb[i].matrix(); } @@ -81,7 +81,7 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); @@ -437,7 +437,7 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointCols(data.J); + ColBlock Jcols = jmodel.jointJacCols(data.J); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); @@ -588,7 +588,7 @@ namespace pinocchio { // When propagating 6D constraints scratch_pad_vector.noalias() = - data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointJacCols(data.J); scratch_pad_vector2.noalias() = scratch_pad_vector * data.joints[i].Dinv().coeff(0, 0); data.spatial_inv_inertia[ad_i].noalias() += scratch_pad_vector2 * scratch_pad_vector.transpose(); @@ -598,7 +598,7 @@ namespace pinocchio // Propagating 3D constraints scratch_pad_vector.template topRows<3>().noalias() = data.extended_motion_propagator[i].template topRows<3>() - * model.joints[i].jointCols(data.J); + * model.joints[i].jointJacCols(data.J); scratch_pad_vector2.template topRows<3>().noalias() = scratch_pad_vector.template topRows<3>() * data.joints[i].Dinv().coeff(0, 0); data.spatial_inv_inertia[ad_i].template topLeftCorner<3, 3>().noalias() += @@ -609,7 +609,7 @@ namespace pinocchio else if (nv == 6) { scratch_pad1.noalias() = - data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointJacCols(data.J); scratch_pad2.noalias() = scratch_pad1 * data.joints[i].Dinv(); data.spatial_inv_inertia[ad_i].noalias() += scratch_pad2 * scratch_pad1.transpose(); } @@ -617,7 +617,7 @@ namespace pinocchio { // Joints with more than 1 DoF scratch_pad1.leftCols(nv).noalias() = - data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointJacCols(data.J); scratch_pad2.leftCols(nv).noalias() = scratch_pad1.leftCols(nv) * data.joints[i].Dinv(); data.spatial_inv_inertia[ad_i].noalias() += scratch_pad2.leftCols(nv) * scratch_pad1.leftCols(nv).transpose(); diff --git a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index e94b59830b..18cbe4b9d3 100644 --- a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx @@ -66,7 +66,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -78,13 +78,13 @@ namespace pinocchio // dvec/dv const int nv = jmodel.nv(); Eigen::Matrix v_spatial_partial_dv_cols(6, nv); - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); motionSet::se3ActionInverse(oMlast, Jcols, v_spatial_partial_dv_cols); v_partial_dv_cols = v_spatial_partial_dv_cols.template middleRows<3>(Motion::LINEAR); // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); #define FOR_NV() for (Eigen::DenseIndex j = 0; j < nv; ++j) #define GET_LINEAR(vec6) vec6.template segment<3>(Motion::LINEAR) @@ -175,7 +175,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -185,7 +185,7 @@ namespace pinocchio Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv); // dvec/dv - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); switch (rf) { @@ -200,7 +200,7 @@ namespace pinocchio } // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); const Scalar factor = Scalar(1) + r_coeff; switch (rf) { diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 8f91d2a57e..20e596b00a 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -56,8 +56,13 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; +<<<<<<< HEAD Matrix6xLike & J_ = J.const_cast_derived(); jmodel.jointCols(J_) = data.oMi[i].act(jdata.S()); +======= + Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); + jmodel.jointVelCols(J_) = data.oMi[i].act(jdata.S()); +>>>>>>> a55d3bf7 ([EtienneAr feedback] Split jointCols jointRows and jointBLock for full and reduced system) } }; @@ -113,7 +118,7 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; const JointIndex & i = jmodel.id(); - jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); } }; } // namespace impl @@ -318,8 +323,13 @@ namespace pinocchio data.liMi[i] = model.jointPlacements[i] * jdata.M(); data.iMf[parent] = data.liMi[i] * data.iMf[i]; +<<<<<<< HEAD Matrix6xLike & J_ = J.const_cast_derived(); jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); +======= + Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); + jmodel.jointVelCols(J_) = data.iMf[i].actInv(jdata.S()); +>>>>>>> a55d3bf7 ([EtienneAr feedback] Split jointCols jointRows and jointBLock for full and reduced system) } }; @@ -410,7 +420,7 @@ namespace pinocchio oMi = data.liMi[i]; } - jmodel.jointCols(data.J) = oMi.act(jdata.S()); + jmodel.jointJacCols(data.J) = oMi.act(jdata.S()); // Spatial velocity of joint i expressed in the global frame o data.ov[i] = oMi.act(vJ); @@ -418,8 +428,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); motionSet::motionAction(data.ov[i], Jcols, dJcols); } diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index 0e0e171bbe..6e0ba1fa12 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -83,8 +83,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); Jcols = oMi.act(jdata.S()); ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o @@ -181,7 +181,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -191,7 +191,7 @@ namespace pinocchio Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2, v_partial_dv); // dvec/dv: this result is then needed by dvec/dq - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); switch (rf) { case WORLD: @@ -208,7 +208,7 @@ namespace pinocchio } // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); switch (rf) { case WORLD: @@ -337,8 +337,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -353,10 +353,10 @@ namespace pinocchio ColsBlockOut4; Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4, a_partial_da); - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); - ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); - ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); - ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut2 a_partial_dq_cols = jmodel.jointVelCols(a_partial_dq_); + ColsBlockOut3 a_partial_dv_cols = jmodel.jointVelCols(a_partial_dv_); + ColsBlockOut4 a_partial_da_cols = jmodel.jointVelCols(a_partial_da_); // dacc/da switch (rf) @@ -588,7 +588,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -597,8 +597,8 @@ namespace pinocchio ColsBlockOut2; Matrix3xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut2, v_partial_dv); - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); - ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); const int nv = jmodel.nv(); Eigen::Matrix v_spatial_partial_dv_cols(6, nv); @@ -751,8 +751,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock dJcols = jmodel.jointCols(data.dJ); - ColsBlock Jcols = jmodel.jointCols(data.J); + ColsBlock dJcols = jmodel.jointJacCols(data.dJ); + ColsBlock Jcols = jmodel.jointJacCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -767,10 +767,10 @@ namespace pinocchio ColsBlockOut4; Matrix3xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut4, a_partial_da); - ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); - ColsBlockOut2 a_partial_dq_cols = jmodel.jointCols(a_partial_dq_); - ColsBlockOut3 a_partial_dv_cols = jmodel.jointCols(a_partial_dv_); - ColsBlockOut4 a_partial_da_cols = jmodel.jointCols(a_partial_da_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut2 a_partial_dq_cols = jmodel.jointVelCols(a_partial_dq_); + ColsBlockOut3 a_partial_dv_cols = jmodel.jointVelCols(a_partial_dv_); + ColsBlockOut4 a_partial_da_cols = jmodel.jointVelCols(a_partial_da_); const int nv = jmodel.nv(); Eigen::Matrix a_spatial_partial_da_cols(6, nv); diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index c9f4812bc5..ebaf0a855c 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -59,8 +59,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(minus_gravity, J_cols, dAdq_cols); } @@ -108,10 +108,10 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); @@ -308,11 +308,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(ov, J_cols, dJ_cols); @@ -390,14 +390,14 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; Matrix6x & dYtJ = data.Fcrb[0]; - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); - ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); - ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); - ColsBlock dFda_cols = jmodel.jointCols(data.dFda); // Also equals to Ag_cols - ColsBlock dYtJ_cols = jmodel.jointCols(dYtJ); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dFdq_cols = jmodel.jointVelCols(data.dFdq); + ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); + ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // Also equals to Ag_cols + ColsBlock dYtJ_cols = jmodel.jointVelCols(dYtJ); MatrixType1 & rnea_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq); MatrixType2 & rnea_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv); diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index b5c1fe412b..64202d4eaa 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -77,11 +77,12 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock J_cols = - jmodel.jointCols(data.J); // data.J has all the phi (in world frame) stacked in columns - ColsBlock psid_cols = jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame + jmodel.jointJacCols(data.J); // data.J has all the phi (in world frame) stacked in columns + ColsBlock psid_cols = + jmodel.jointVelCols(data.psid); // psid_cols is the psi_dot in world frame ColsBlock psidd_cols = - jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); // This here is phi_dot in world frame + jmodel.jointVelCols(data.psidd); // psidd_cols is the psi_dotdot in world frame + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); // This here is phi_dot in world frame J_cols.noalias() = data.oMi[i].act(jdata.S()); // J_cols is just the phi in world frame for a joint diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index f9ba3c7266..dee5287a15 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -530,11 +530,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointJacCols(data.J); J_cols = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame // computes vxS expressed at the world frame - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.B[i] = data.oYcrb[i].variation(Scalar(0.5) * data.ov[i]); @@ -582,12 +582,12 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointCols(data.dFdv)); - jmodel.jointCols(data.dFdv).noalias() += data.B[i] * J_cols; + motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); + jmodel.jointVelCols(data.dFdv).noalias() += data.B[i] * J_cols; data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); @@ -677,11 +677,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointCols(data.dJ); - ColsBlock J_cols = jmodel.jointCols(data.J); - ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); typename Data::Matrix6x & dFdv = data.Fcrb[0]; - ColsBlock dFdv_cols = jmodel.jointCols(dFdv); + ColsBlock dFdv_cols = jmodel.jointVelCols(dFdv); motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dFdv_cols); dFdv_cols.noalias() += data.B[i] * J_cols; diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index b8a4ffe0f5..574274f290 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -28,7 +28,7 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, - NV = Eigen::Dynamic, + NV = Eigen::Dynamic, NJ = Eigen::Dynamic }; @@ -194,12 +194,12 @@ namespace pinocchio typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; + using Base::nj; using Base::nq; using Base::nv; - using Base::nj; using Base::setIndexes; /// \brief Default contructor @@ -503,12 +503,25 @@ namespace pinocchio template typename SizeDepType::template ColsReturn::ConstType - jointCols(const Eigen::MatrixBase & A) const + jointVelCols(const Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_v, nv()); + } + template + typename SizeDepType::template ColsReturn::ConstType + jointJacCols(const Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } template - typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const + typename SizeDepType::template ColsReturn::Type + jointVelCols(Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_v, nv()); + } + template + typename SizeDepType::template ColsReturn::Type + jointJacCols(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } @@ -540,13 +553,25 @@ namespace pinocchio template typename SizeDepType::template ColsReturn::ConstType - jointCols_impl(const Eigen::MatrixBase & A) const + jointVelCols_impl(const Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_v, nv()); + } + template + typename SizeDepType::template ColsReturn::ConstType + jointJacCols_impl(const Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } template typename SizeDepType::template ColsReturn::Type - jointCols_impl(Eigen::MatrixBase & A) const + jointVelCols_impl(Eigen::MatrixBase & A) const + { + return A.middleCols(Base::i_v, nv()); + } + template + typename SizeDepType::template ColsReturn::Type + jointJacCols_impl(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } @@ -563,7 +588,7 @@ namespace pinocchio { int idx_q = this->idx_q(); int idx_v = this->idx_v(); - int idx_j = this->idx_j(); + int idx_j = this->idx_j(); m_idx_q.resize(joints.size()); m_idx_v.resize(joints.size()); @@ -578,7 +603,7 @@ namespace pinocchio m_idx_q[i] = idx_q; m_idx_v[i] = idx_v; - m_idx_j[i] = idx_j; + m_idx_j[i] = idx_j; ::pinocchio::setIndexes(joint, i, idx_q, idx_v, idx_j); m_nqs[i] = ::pinocchio::nq(joint); m_nvs[i] = ::pinocchio::nv(joint); @@ -607,7 +632,6 @@ namespace pinocchio /// \brief Dimension of the segment in the jacobian matrix std::vector m_njs; - public: /// \brief Number of joints contained in the JointModelComposite int njoints; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 165a9b6245..12be2b1ebe 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -1012,67 +1012,59 @@ namespace pinocchio /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ // Const access - // template - // typename SizeDepType::template ColsReturn::ConstType - // jointCols_impl(const Eigen::MatrixBase & A) const - // { - // return SizeDepType::middleCols(A.derived(), - // m_jmodel_ref.idx_v(), - // m_jmodel_ref.nv()); - // } + template + typename SizeDepType::template ColsReturn::ConstType + jointVelCols_impl(const Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + } - // // Non-const access - // template - // typename SizeDepType::template ColsReturn::Type - // jointCols_impl(Eigen::MatrixBase & A) const - // { - // return SizeDepType::middleCols(A.derived(), - // m_jmodel_ref.idx_v(), - // m_jmodel_ref.nv()); - // } + // Non-const access + template + typename SizeDepType::template ColsReturn::Type + jointVelCols_impl(Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + } - // /* Acces to dedicated rows in a matrix.*/ - // // Const access - // template - // typename SizeDepType::template RowsReturn::ConstType - // jointRows_impl(const Eigen::MatrixBase & A) const - // { - // return SizeDepType::middleRows(A.derived(), - // m_jmodel_ref.idx_v(), - // m_jmodel_ref.nv()); - // } + /* Acces to dedicated rows in a matrix.*/ + // Const access + template + typename SizeDepType::template RowsReturn::ConstType + joinVeltRows_impl(const Eigen::MatrixBase & A) const + { + return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + } - // // Non-const access - // template - // typename SizeDepType::template RowsReturn::Type - // jointRows_impl(Eigen::MatrixBase & A) const - // { - // return SizeDepType::middleRows(A.derived(), - // m_jmodel_ref.idx_v(), - // m_jmodel_ref.nv()); - // } + // Non-const access + template + typename SizeDepType::template RowsReturn::Type + jointVelRows_impl(Eigen::MatrixBase & A) const + { + return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + } // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the // matrix Mat // // Const access - // template - // typename SizeDepType::template BlockReturn::ConstType - // jointBlock_impl(const Eigen::MatrixBase & Mat) const - // { - // return SizeDepType::block(Mat.derived(), - // m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(), - // m_jmodel_ref.nv(),m_jmodel_ref.nv()); - // } + template + typename SizeDepType::template BlockReturn::ConstType + jointVelBlock_impl(const Eigen::MatrixBase & Mat) const + { + return SizeDepType::block( + Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), + m_jmodel_ref.nv()); + } - // // Non-const access - // template - // typename SizeDepType::template BlockReturn::Type - // jointBlock_impl(Eigen::MatrixBase & Mat) const - // { - // return SizeDepType::block(Mat.derived(), - // m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(), - // m_jmodel_ref.nv(),m_jmodel_ref.nv()); - // } + // Non-const access + template + typename SizeDepType::template BlockReturn::Type + jointVelBlock_impl(Eigen::MatrixBase & Mat) const + { + return SizeDepType::block( + Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), + m_jmodel_ref.nv()); + } }; // struct JointModelMimicTpl diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index fc29a2a345..e4eabad1c7 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -345,28 +345,58 @@ namespace pinocchio // Const access template typename SizeDepType::template ColsReturn::ConstType - jointCols(const Eigen::MatrixBase & A) const + jointVelCols(const Eigen::MatrixBase & A) const { - return derived().jointCols_impl(A.derived()); + return derived().jointVelCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::ConstType - jointCols_impl(const Eigen::MatrixBase & A) const + jointJacCols(const Eigen::MatrixBase & A) const + { + return derived().jointJacCols_impl(A.derived()); + } + + template + typename SizeDepType::template ColsReturn::ConstType + jointVelCols_impl(const Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), idx_v(), nv()); + } + + template + typename SizeDepType::template ColsReturn::ConstType + jointJacCols_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } // Non-const access + // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?) + template + typename SizeDepType::template ColsReturn::Type + jointVelCols(Eigen::MatrixBase & A) const + { + return derived().jointVelCols_impl(A.derived()); + } + template - typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const + typename SizeDepType::template ColsReturn::Type + jointJacCols(Eigen::MatrixBase & A) const { - return derived().jointCols_impl(A.derived()); + return derived().jointJacCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::Type - jointCols_impl(Eigen::MatrixBase & A) const + jointVelCols_impl(Eigen::MatrixBase & A) const + { + return SizeDepType::middleCols(A.derived(), idx_v(), nv()); + } + + template + typename SizeDepType::template ColsReturn::Type + jointJacCols_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } @@ -375,28 +405,57 @@ namespace pinocchio // Const access template typename SizeDepType::template RowsReturn::ConstType - jointRows(const Eigen::MatrixBase & A) const + jointVelRows(const Eigen::MatrixBase & A) const + { + return derived().jointVelRows_impl(A.derived()); + } + + template + typename SizeDepType::template RowsReturn::ConstType + jointJacRows(const Eigen::MatrixBase & A) const + { + return derived().jointJacRows_impl(A.derived()); + } + + template + typename SizeDepType::template RowsReturn::ConstType + jointVelRows_impl(const Eigen::MatrixBase & A) const { - return derived().jointRows_impl(A.derived()); + return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } template typename SizeDepType::template RowsReturn::ConstType - jointRows_impl(const Eigen::MatrixBase & A) const + jointJacRows_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } // Non-const access template - typename SizeDepType::template RowsReturn::Type jointRows(Eigen::MatrixBase & A) const + typename SizeDepType::template RowsReturn::Type + jointVelRows(Eigen::MatrixBase & A) const + { + return derived().jointVelRows_impl(A.derived()); + } + + template + typename SizeDepType::template RowsReturn::Type + jointJacRows(Eigen::MatrixBase & A) const + { + return derived().jointJacRows_impl(A.derived()); + } + + template + typename SizeDepType::template RowsReturn::Type + jointVelRows_impl(Eigen::MatrixBase & A) const { - return derived().jointRows_impl(A.derived()); + return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } template typename SizeDepType::template RowsReturn::Type - jointRows_impl(Eigen::MatrixBase & A) const + jointJacRows_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } @@ -406,14 +465,28 @@ namespace pinocchio // Const access template typename SizeDepType::template BlockReturn::ConstType - jointBlock(const Eigen::MatrixBase & Mat) const + jointVelBlock(const Eigen::MatrixBase & Mat) const + { + return derived().jointVelBlock_impl(Mat.derived()); + } + + template + typename SizeDepType::template BlockReturn::ConstType + jointJacBlock(const Eigen::MatrixBase & Mat) const { - return derived().jointBlock_impl(Mat.derived()); + return derived().jointJacBlock_impl(Mat.derived()); } template typename SizeDepType::template BlockReturn::ConstType - jointBlock_impl(const Eigen::MatrixBase & Mat) const + jointVelBlock_impl(const Eigen::MatrixBase & Mat) const + { + return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); + } + + template + typename SizeDepType::template BlockReturn::ConstType + jointJacBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } @@ -421,14 +494,28 @@ namespace pinocchio // Non-const access template typename SizeDepType::template BlockReturn::Type - jointBlock(Eigen::MatrixBase & Mat) const + jointVelBlock(Eigen::MatrixBase & Mat) const + { + return derived().jointVelBlock_impl(Mat.derived()); + } + + template + typename SizeDepType::template BlockReturn::Type + jointJacBlock(Eigen::MatrixBase & Mat) const + { + return derived().jointJacBlock_impl(Mat.derived()); + } + + template + typename SizeDepType::template BlockReturn::Type + jointVelBlock_impl(Eigen::MatrixBase & Mat) const { - return derived().jointBlock_impl(Mat.derived()); + return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); } template typename SizeDepType::template BlockReturn::Type - jointBlock_impl(Eigen::MatrixBase & Mat) const + jointJacBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index 72dacdd5a0..ef2e909f18 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -164,13 +164,14 @@ namespace pinocchio #define PINOCCHIO_DETAILS_JOINT_MIMIC_1(Algo) \ template \ - struct Algo> { \ + struct Algo> \ + { \ typedef typename Visitor::ArgsType ArgsType; \ - static void run (PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModelMimicTpl)) \ - {} \ + static void run(PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModelMimicTpl)) \ + { \ + } \ } - } // namespace details template @@ -253,7 +254,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrate( jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); + jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); } }; @@ -308,8 +309,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointRows(mat_in.derived()), - jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); + jmodel.jointJacRows(mat_in.derived()), + jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); } }; @@ -357,7 +358,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); + jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; @@ -401,7 +402,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dDifference( jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), - jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); + jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); } }; @@ -736,7 +737,6 @@ namespace pinocchio PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NeutralStepAlgo); PINOCCHIO_DETAILS_JOINT_MIMIC_1(NeutralStepAlgo); - template struct IntegrateCoeffWiseJacobianStepAlgo; diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index edf5045572..b2679f3402 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -150,8 +150,10 @@ void test_joint_methods( BOOST_CHECK( jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const)); - BOOST_CHECK(jmodel.jointCols(mat) == jmodel_composite.jointCols(mat)); - BOOST_CHECK(jmodel.jointCols(mat_const) == jmodel_composite.jointCols(mat_const)); + BOOST_CHECK(jmodel.jointVelCols(mat) == jmodel_composite.jointVelCols(mat)); + BOOST_CHECK(jmodel.jointVelCols(mat_const) == jmodel_composite.jointVelCols(mat_const)); + BOOST_CHECK(jmodel.jointJacCols(mat) == jmodel_composite.jointJacCols(mat)); + BOOST_CHECK(jmodel.jointJacCols(mat_const) == jmodel_composite.jointJacCols(mat_const)); } struct TestJointComposite From bce84b1d606fb81b6f63bdf037ea6fd777cb8d4c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 028/166] [URDF] Modified the way mimic joint are added (still broken for romeo....) --- .../pinocchio/multibody/joint/joint-mimic.hpp | 34 ++++- include/pinocchio/parsers/urdf/model.hxx | 120 +++++++++++++++--- src/parsers/urdf/model.cpp | 21 +-- 3 files changed, 144 insertions(+), 31 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 12be2b1ebe..f99d57de44 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -506,7 +506,6 @@ namespace pinocchio { m_q_transform.resize(nq, 1); m_v_transform.resize(nv, 1); - // boost::apply_visitor(TransferVisitor(m_jdata_ref), jdata); } // JointDataMimicTpl(const RefJointDataVariant & jdata, @@ -853,6 +852,35 @@ namespace pinocchio return m_jmodel_ref.idx_v(); } +<<<<<<< HEAD +======= + template + JointModelMimicTpl(const JointModelBase & jmodel_mimicking, + const JointModelTpl & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) + : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) + , m_scaling(scaling) + , m_offset(offset) + { + assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); + assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); + assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); + + m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); + } + + Base & base() { return *static_cast(this); } + const Base & base() const { return *static_cast(this); } + + inline int nq_impl() const { return 0; } + inline int nv_impl() const { return 0; } + inline int nj_impl() const { return m_jmodel_ref.nj(); } + + inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); } + inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); } + +>>>>>>> deb5d640 ([URDF] Modified the way mimic joint are added (still broken for romeo....)) void setIndexes_impl(JointIndex id, int q, int v, int j) { Base::i_id = id; // Only the id of the joint in the model is different. @@ -863,9 +891,13 @@ namespace pinocchio JointDataDerived createData() const { +<<<<<<< HEAD return JointDataDerived( m_jmodel_ref.createData(), scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); +======= + return JointDataDerived(m_jmodel_ref.createData(),scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); +>>>>>>> deb5d640 ([URDF] Modified the way mimic joint are added (still broken for romeo....)) } const std::vector hasConfigurationLimit() const diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 40b4fc8f1d..6b144c098f 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -15,6 +15,7 @@ #include #include #include +#include namespace pinocchio { @@ -24,6 +25,9 @@ namespace pinocchio { typedef double urdf_scalar_type; + template + struct MimicInfo; + template class UrdfVisitorBaseTpl { @@ -66,10 +70,7 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping, - const Scalar multiplier = 0, - const Scalar offset = 0, - const std::string mimic_name = "") = 0; + const VectorConstRef & damping) = 0; virtual void addJointAndBody( JointType type, @@ -85,10 +86,7 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping, - const Scalar multiplier = 0, - const Scalar offset = 0, - const std::string mimic_name = "") = 0; + const VectorConstRef & damping) = 0; virtual void addFixedJointAndBody( const FrameIndex & parentFrameId, @@ -115,6 +113,10 @@ namespace pinocchio virtual bool existFrame(const std::string & frame_name, const FrameType type) const = 0; + virtual void addMimicInfo(const std::string & mimic_name, const std::string & mimiced_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset, JointType jointType) = 0; + + virtual void convertMimicJoint(const std::string & mimic_name, const MimicInfo & mimic_info) = 0; + UrdfVisitorBaseTpl() : log(NULL) { @@ -129,6 +131,33 @@ namespace pinocchio } std::ostream * log; + std::unordered_map> mimicInfo_map; + }; + + template + struct MimicInfo + { + typedef _Scalar Scalar; + typedef Eigen::Matrix Vector3; + + Vector3 axis; + std::string mimiced_name; + Scalar multiplier; + Scalar offset; + + // Use the JointType from UrdfVisitorBaseTpl + typedef typename UrdfVisitorBaseTpl<_Scalar, Options>::JointType JointType; + JointType jointType; + + MimicInfo(){} + + MimicInfo(std::string _mimiced_name, Scalar _multiplier, Scalar _offset, Vector3 _axis, JointType _jointType): + mimiced_name(_mimiced_name), + multiplier(_multiplier), + offset(_offset), + axis(_axis), + jointType(_jointType) + {} }; template class JointCollectionTpl> @@ -187,15 +216,11 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping, - const Scalar multiplier = 0, - const Scalar offset = 0, - const std::string mimic_name = "") + const VectorConstRef & damping) { addJointAndBody( type, axis, parentFrameId, placement, joint_name, Y, SE3::Identity(), body_name, - max_effort, max_velocity, min_config, max_config, friction, damping, - multiplier, offset, mimic_name); + max_effort, max_velocity, min_config, max_config, friction, damping); } void addJointAndBody( @@ -212,14 +237,10 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping, - const Scalar multiplier = 0, - const Scalar offset = 0, - const std::string mimic_name = "") + const VectorConstRef & damping) { JointIndex joint_id; const Frame & frame = model.frames[parentFrameId]; - switch (type) { case Base::FLOATING: @@ -265,8 +286,9 @@ namespace pinocchio max_config, friction, damping); break; case Base::MIMIC: + std::cout << model.joints.back() << std::endl; joint_id = model.addJoint(frame.parentJoint, - typename JointCollection::JointModelMimic(model.joints[model.getJointId(mimic_name)], multiplier, offset), + typename JointCollection::JointModelMimic(model.joints.back(), 1, 0), frame.placement * placement, joint_name, max_effort,max_velocity,min_config,max_config, friction, damping); @@ -427,6 +449,64 @@ namespace pinocchio } } + void addMimicInfo(const std::string & mimic_name, const std::string & mimiced_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset, JointType jointType) + { + MimicInfo mimic_info(mimiced_name, multiplier, offset, axis, jointType); + this->mimicInfo_map.insert(std::make_pair(mimic_name, mimic_info)); + } + + void convertMimicJoint(const std::string & mimic_name, const MimicInfo & mimic_info) + { + switch(mimic_info.jointType) + { + case Base::REVOLUTE: + createMimicJoint(mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, mimic_info.offset); + break; + case Base::PRISMATIC: + createMimicJoint(mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, mimic_info.offset); + break; + + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); + } + } + + template + void createMimicJoint(const std::string & mimic_name, const std::string & mimiced_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset) + { + auto mimiced_joint = model.joints[getJointId(mimiced_name)]; + std::cout << mimiced_joint << std::endl; + auto mimic_joint = model.joints[getJointId(mimic_name)]; + + CartesianAxis axisType = extractCartesianAxis(axis); + switch (axisType) + { + case AXIS_X: + mimic_joint = typename JointCollection::JointModelMimic(TypeX(), mimiced_joint, multiplier, offset); + break; + + case AXIS_Y: + mimic_joint = typename JointCollection::JointModelMimic(TypeY(), mimiced_joint, multiplier, offset); + break; + + case AXIS_Z: + mimic_joint = typename JointCollection::JointModelMimic(TypeZ(), mimiced_joint, multiplier, offset); + break; + + case AXIS_UNALIGNED: + mimic_joint = typename JointCollection::JointModelMimic(TypeUnaligned(), mimiced_joint, multiplier, offset); + break; + + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); + break; + } + } + private: /// /// \brief The four possible cartesian types of an 3D axis. diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index 63baa97ef8..9c753257b5 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -147,16 +147,13 @@ namespace pinocchio friction = Vector::Constant(0, 0.); damping = Vector::Constant(0, 0.); - std::string mimic_name = joint->mimic->joint_name; - Scalar multiplier = joint->mimic->multiplier; - Scalar offset = joint->mimic->offset; + model.addMimicInfo(joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, joint->mimic->offset,UrdfVisitorBase::REVOLUTE); model.addJointAndBody(UrdfVisitorBase::MIMIC, axis, parentFrameId,jointPlacement,joint->name, Y,link->name, max_effort,max_velocity,min_config,max_config, - friction,damping, - multiplier, offset, mimic_name); + friction,damping); } else model.addJointAndBody( @@ -221,16 +218,15 @@ namespace pinocchio friction = Vector::Constant(0, 0.); damping = Vector::Constant(0, 0.); - std::string mimic_name = joint->mimic->joint_name; - Scalar multiplier = joint->mimic->multiplier; - Scalar offset = joint->mimic->offset; + + model.addMimicInfo(joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, joint->mimic->offset,UrdfVisitorBase::PRISMATIC); model.addJointAndBody(UrdfVisitorBase::MIMIC, axis, parentFrameId,jointPlacement,joint->name, Y,link->name, max_effort,max_velocity,min_config,max_config, - friction,damping, - multiplier, offset, mimic_name); + friction,damping); + } else model.addJointAndBody( @@ -315,6 +311,11 @@ namespace pinocchio { parseTree(child, model, mimic); } + if(mimic) + { + for (const auto & entry : model.mimicInfo_map) + model.convertMimicJoint(entry.first, entry.second); + } } void parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic) From 3d293cfce1bbc48e356a37d108d241355f90d5d6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 029/166] [urdf] Fix parsing of mimic joints --- .../pinocchio/multibody/joint/joint-mimic.hpp | 34 +++++++++++++++++++ include/pinocchio/parsers/urdf/model.hxx | 3 +- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index f99d57de44..afdae4aa91 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -349,8 +349,25 @@ namespace pinocchio // Joint Prismatic Unaligned typedef JointModelPrismaticUnalignedTpl JointModelPrismaticUnaligned; +<<<<<<< HEAD // Joint Translation typedef JointModelTranslationTpl JointModelTranslation; +======= + // Joint FreeFlyer + typedef JointModelFreeFlyerTpl JointModelFreeFlyer; + + + typedef boost::variant< + JointModelRX, JointModelRY, JointModelRZ + , JointModelRevoluteUnaligned + , JointModelPX, JointModelPY, JointModelPZ + , JointModelPrismaticUnaligned + , JointModelTranslation + , JointModelRUBX, JointModelRUBY, JointModelRUBZ + , JointModelRevoluteUnboundedUnaligned + , JointModelFreeFlyer + > JointModelVariant; +>>>>>>> 52ff6226 ([urdf] Fix parsing of mimic joints) typedef boost::variant< JointModelRX, @@ -393,6 +410,7 @@ namespace pinocchio // Joint Prismatic Unaligned typedef JointDataPrismaticUnalignedTpl JointDataPrismaticUnaligned; +<<<<<<< HEAD // Joint Translation typedef JointDataTranslationTpl JointDataTranslation; @@ -419,6 +437,22 @@ namespace pinocchio struct JointModelMimicTpl; template class JointCollectionTpl> struct JointDataMimicTpl; +======= + // Joint FreeFlyer + typedef JointDataFreeFlyerTpl JointDataFreeFlyer; + + typedef boost::variant< + JointDataRX, JointDataRY, JointDataRZ + , JointDataRevoluteUnaligned + , JointDataPX, JointDataPY, JointDataPZ + , JointDataPrismaticUnaligned + , JointDataTranslation + , JointDataRUBX, JointDataRUBY, JointDataRUBZ + , JointDataRevoluteUnboundedUnaligned + , JointDataFreeFlyer + > JointDataVariant; +}; +>>>>>>> 52ff6226 ([urdf] Fix parsing of mimic joints) template class JointCollectionTpl> struct traits> diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 6b144c098f..02795dce36 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -286,9 +286,8 @@ namespace pinocchio max_config, friction, damping); break; case Base::MIMIC: - std::cout << model.joints.back() << std::endl; joint_id = model.addJoint(frame.parentJoint, - typename JointCollection::JointModelMimic(model.joints.back(), 1, 0), + typename JointCollection::JointModelMimic(model.joints.at(1), 1, 0), frame.placement * placement, joint_name, max_effort,max_velocity,min_config,max_config, friction, damping); From 0496323ed3ebc2e1b6752eeb608d107380d54c1e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 030/166] [Example] Fix to include mimic --- examples/build-reduced-model.py | 2 +- examples/geometry-models.py | 2 +- examples/gepetto-viewer.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/build-reduced-model.py b/examples/build-reduced-model.py index 85269ae598..2916fdf27e 100644 --- a/examples/build-reduced-model.py +++ b/examples/build-reduced-model.py @@ -78,7 +78,7 @@ # reference_configuration is optional: if not provided, neutral configuration used # you can even mix joint names and joint ids mixed_jointsToLockIDs = [jointsToLockIDs[0], "wrist_2_joint", "wrist_3_joint"] -robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir) +robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, True, mesh_dir) reduced_robot = robot.buildReducedRobot( list_of_joints_to_lock=mixed_jointsToLockIDs, reference_configuration=initialJointConfig, diff --git a/examples/geometry-models.py b/examples/geometry-models.py index c1ae8d4579..046f27a2ba 100644 --- a/examples/geometry-models.py +++ b/examples/geometry-models.py @@ -14,7 +14,7 @@ # Load the urdf model model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( - urdf_model_path, mesh_dir + urdf_model_path, False, mesh_dir ) print("model name: " + model.name) diff --git a/examples/gepetto-viewer.py b/examples/gepetto-viewer.py index 2ba488e66f..cdc4628082 100644 --- a/examples/gepetto-viewer.py +++ b/examples/gepetto-viewer.py @@ -17,7 +17,7 @@ urdf_model_path = model_path / "talos_data/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() ) viz = GepettoVisualizer(model, collision_model, visual_model) From ef08565e11f138fda1e1d9122e7151b89ae61312 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 031/166] [robot_wrapper] Fix to include mimic --- examples/robot-wrapper-viewer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/robot-wrapper-viewer.py b/examples/robot-wrapper-viewer.py index 5388af21de..432461c2c4 100644 --- a/examples/robot-wrapper-viewer.py +++ b/examples/robot-wrapper-viewer.py @@ -34,7 +34,7 @@ urdf_filename = "talos_reduced.urdf" urdf_model_path = model_path / "talos_data/robots" / urdf_filename -robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()) +robot = RobotWrapper.BuildFromURDF(urdf_model_path, True, mesh_dir, pin.JointModelFreeFlyer()) # alias model = robot.model From f46fea140c18ead691e661eb04e09421e5e36186 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 032/166] Merge branch --- .../pinocchio/multibody/joint/joint-mimic.hpp | 67 ------------------- 1 file changed, 67 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index afdae4aa91..c37980ef47 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -349,25 +349,8 @@ namespace pinocchio // Joint Prismatic Unaligned typedef JointModelPrismaticUnalignedTpl JointModelPrismaticUnaligned; -<<<<<<< HEAD // Joint Translation typedef JointModelTranslationTpl JointModelTranslation; -======= - // Joint FreeFlyer - typedef JointModelFreeFlyerTpl JointModelFreeFlyer; - - - typedef boost::variant< - JointModelRX, JointModelRY, JointModelRZ - , JointModelRevoluteUnaligned - , JointModelPX, JointModelPY, JointModelPZ - , JointModelPrismaticUnaligned - , JointModelTranslation - , JointModelRUBX, JointModelRUBY, JointModelRUBZ - , JointModelRevoluteUnboundedUnaligned - , JointModelFreeFlyer - > JointModelVariant; ->>>>>>> 52ff6226 ([urdf] Fix parsing of mimic joints) typedef boost::variant< JointModelRX, @@ -410,7 +393,6 @@ namespace pinocchio // Joint Prismatic Unaligned typedef JointDataPrismaticUnalignedTpl JointDataPrismaticUnaligned; -<<<<<<< HEAD // Joint Translation typedef JointDataTranslationTpl JointDataTranslation; @@ -437,22 +419,6 @@ namespace pinocchio struct JointModelMimicTpl; template class JointCollectionTpl> struct JointDataMimicTpl; -======= - // Joint FreeFlyer - typedef JointDataFreeFlyerTpl JointDataFreeFlyer; - - typedef boost::variant< - JointDataRX, JointDataRY, JointDataRZ - , JointDataRevoluteUnaligned - , JointDataPX, JointDataPY, JointDataPZ - , JointDataPrismaticUnaligned - , JointDataTranslation - , JointDataRUBX, JointDataRUBY, JointDataRUBZ - , JointDataRevoluteUnboundedUnaligned - , JointDataFreeFlyer - > JointDataVariant; -}; ->>>>>>> 52ff6226 ([urdf] Fix parsing of mimic joints) template class JointCollectionTpl> struct traits> @@ -886,35 +852,6 @@ namespace pinocchio return m_jmodel_ref.idx_v(); } -<<<<<<< HEAD -======= - template - JointModelMimicTpl(const JointModelBase & jmodel_mimicking, - const JointModelTpl & jmodel_mimicked, - const Scalar & scaling, - const Scalar & offset) - : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) - , m_scaling(scaling) - , m_offset(offset) - { - assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); - assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); - assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - - m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); - } - - Base & base() { return *static_cast(this); } - const Base & base() const { return *static_cast(this); } - - inline int nq_impl() const { return 0; } - inline int nv_impl() const { return 0; } - inline int nj_impl() const { return m_jmodel_ref.nj(); } - - inline int idx_q_impl() const { return m_jmodel_ref.idx_q(); } - inline int idx_v_impl() const { return m_jmodel_ref.idx_v(); } - ->>>>>>> deb5d640 ([URDF] Modified the way mimic joint are added (still broken for romeo....)) void setIndexes_impl(JointIndex id, int q, int v, int j) { Base::i_id = id; // Only the id of the joint in the model is different. @@ -925,13 +862,9 @@ namespace pinocchio JointDataDerived createData() const { -<<<<<<< HEAD return JointDataDerived( m_jmodel_ref.createData(), scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); -======= - return JointDataDerived(m_jmodel_ref.createData(),scaling(), m_jmodel_ref.nq(), m_jmodel_ref.nv()); ->>>>>>> deb5d640 ([URDF] Modified the way mimic joint are added (still broken for romeo....)) } const std::vector hasConfigurationLimit() const From 33c7fa37a60b9241d93c6244c15cbb45f1111098 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 033/166] [RNEA] Modify to take mimic into account --- include/pinocchio/algorithm/rnea.hxx | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index dee5287a15..a54f4a3dda 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -63,7 +63,7 @@ namespace pinocchio data.v[i] = jdata.v(); if (parent > 0) data.v[i] += data.liMi[i].actInv(data.v[parent]); - + data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); @@ -99,9 +99,8 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - - jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose() * data.f[i]; - + jmodel.jointVelocitySelector(data.tau) += jdata.S().transpose() * data.f[i]; + if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); } From f353cab2e28e9456aaf42865d79ddb5b75672184 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 034/166] [Mimic] Modify to be able to mimic from base to Tpl --- .../pinocchio/multibody/joint/joint-mimic.hpp | 31 +++++++++++++++---- include/pinocchio/multibody/sample-models.hxx | 10 +++--- 2 files changed, 31 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index c37980ef47..e15a7f83ce 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -147,9 +147,10 @@ namespace pinocchio template JointMotion __mult__(const Eigen::MatrixBase & v) const { + assert(v.size() == nv()); JointMotion jm = m_constraint * v; - return jm * m_scaling_factor; + return m_scaling_factor * jm; } template @@ -821,6 +822,22 @@ namespace pinocchio jmodel_mimicked.idx_j()); } + template + JointModelMimicTpl(const JointModelBase & jmodel_mimicking, + const JointModelTpl & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) + : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) + , m_scaling(scaling) + , m_offset(offset) + { + assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); + assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); + assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); + + m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); + } + Base & base() { return *static_cast(this); @@ -886,6 +903,7 @@ namespace pinocchio AffineTransform::run( qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); + m_jmodel_ref.calc(jdata.m_jdata_ref, qs); } @@ -901,7 +919,8 @@ namespace pinocchio qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, qs, vs); + + m_jmodel_ref.calc(jdata.m_jdata_ref, qs , vs); } template @@ -981,7 +1000,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointConfigSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); } // Non-const access @@ -989,7 +1008,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointConfigSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); } /* Acces to dedicated segment in robot config velocity space. */ @@ -998,7 +1017,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); } // Non-const access @@ -1006,7 +1025,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointVelocitySelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); } /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index d121d029da..508b5ee315 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -107,13 +107,15 @@ namespace pinocchio if (mimic) { - Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); - Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); + // Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); + // Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); + Scalar multiplier = 0.5; + Scalar offset = 0; joint_id = addJointAndBody( model, - typename JC::JointModelMimic(model.joints[joint_id].derived(), multiplier, offset), - model.names[joint_id], pre + "wrist1_joint_mimic", Id4); + typename JC::JointModelMimic(typename JC::JointModelRY(), model.joints[joint_id].derived(), multiplier, offset), + model.names[joint_id], pre + "wrist1_mimic", Id4); } else { From 39a69187bd50645828f7f722853299053d7789b4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 035/166] [SampleModel] Test to compare a mimic and non mimic model --- unittest/sample-models.cpp | 98 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 98 insertions(+) diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index 8742a9c228..27f9e904ef 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -9,8 +9,27 @@ #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/geometry.hpp" #include "pinocchio/multibody/sample-models.hpp" +#include +#include #include +#include + + +// Helper functions to map reduced to full model +Eigen::VectorXd toFull(const Eigen::VectorXd &vec, int mimPrim, int mimSec, double scaling, double offset) { + Eigen::VectorXd vecFull(vec.size() + 1); + vecFull << vec.head(mimSec), scaling * vec[mimPrim] + offset, vec.tail(vec.size() - mimSec); + return vecFull; +} + +Eigen::MatrixXd create_G(const pinocchio::Model &model, const pinocchio::Model &modelMimic) { + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model.nv, modelMimic.nv); + for (int i = 0; i < modelMimic.nv; ++i) + G(i, i) = 1; + + return G; +} BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) @@ -67,4 +86,83 @@ BOOST_AUTO_TEST_CASE(build_model_sample_humanoid) * direct geometry with respect to reference values. */ } +BOOST_AUTO_TEST_CASE(compare_mimic) +{ + pinocchio::Model model, model_m; + pinocchio::buildModels::manipulator(model); + pinocchio::buildModels::manipulator(model_m, true); + + // Reduced model configuration (with the mimic joint) + Eigen::VectorXd q_reduced = pinocchio::neutral(model_m); + Eigen::VectorXd v_reduced = Eigen::VectorXd::Random(model_m.nv); + Eigen::VectorXd a_reduced = Eigen::VectorXd::Random(model_m.nv); + Eigen::VectorXd tau_reduced = Eigen::VectorXd::Random(model_m.nv); + + auto jointMimic = boost::get(model_m.joints[model_m.getJointId("wrist1_mimic_joint")]); + double scaling = jointMimic.scaling(); + double offset = jointMimic.offset(); + // Reduced model configuration (without the mimic joint) + Eigen::VectorXd q_full = toFull(q_reduced, 4, 5, scaling, offset); + Eigen::VectorXd v_full = toFull(v_reduced, 4, 5, scaling, 0); + Eigen::VectorXd a_full = toFull(a_reduced, 4, 5, scaling, 0); + + + pinocchio::Data dataFKFull(model); + pinocchio::forwardKinematics(model, dataFKFull, q_full, v_full, a_full); + + pinocchio::Data dataFKRed(model_m); + pinocchio::forwardKinematics(model_m, dataFKRed, q_reduced, v_reduced, a_reduced); + + for(int i = 0; i < model.njoints; i++) + { + BOOST_CHECK(dataFKRed.oMi[i].isApprox(dataFKFull.oMi[i])); + BOOST_CHECK(dataFKRed.liMi[i].isApprox(dataFKFull.liMi[i])); + BOOST_CHECK(model.inertias[i].isApprox(model_m.inertias[i])); + } + + // Compute M, and C, for the mimic model from the reduced model variable + pinocchio::Data dataBiasFull(model); + pinocchio::rnea(model, dataBiasFull, q_full, v_full, Eigen::VectorXd::Zero(model.nv)); + Eigen::VectorXd C_full = dataBiasFull.tau; + + pinocchio::Data dataCRBAFull(model); + pinocchio::crba(model, dataCRBAFull, q_full); + + Eigen::MatrixXd G = create_G(model, model_m); + G(model.nv - 1, model_m.nv - 1) = scaling; + + + // // Use equation of motion to compute tau from a_reduced + // std::cout << G.transpose() << std::endl; + Eigen::VectorXd tau_reduced_computed = (G.transpose() * dataCRBAFull.M * G) * a_reduced + (G.transpose() * C_full); + // Eigen::MatrixXd M_reduced_computed = (G.transpose() * dataCRBAFull.M * G); + // std::cout << " --------------------- " << std::endl; + pinocchio::Data dataRneaRed(model_m); + pinocchio::rnea(model_m, dataRneaRed, q_reduced, v_reduced, a_reduced); + + BOOST_CHECK((dataRneaRed.tau - tau_reduced_computed).isZero()); + std::cout << " --------------------- " << std::endl; + std::cout << tau_reduced_computed << std::endl; + std::cout << std::endl; + std::cout << dataRneaRed.tau << std::endl; + std::cout << std::endl; + + pinocchio::Data dataBiasF(model); + pinocchio::rnea(model, dataBiasF, q_full, v_full, a_full); + for(int i = 0; i < model.njoints; i++) + { + BOOST_CHECK(dataRneaRed.f[i].isApprox(dataBiasF.f[i])); + BOOST_CHECK(dataRneaRed.h[i].isApprox(dataBiasF.h[i])); + BOOST_CHECK(model.inertias[i].isApprox(model_m.inertias[i])); + } + + // // std::cout << dataBiasF.tau << std::endl; + + // // pinocchio::Data dataCrbaRed(model_m); + // // pinocchio::crba(model_m, dataCrbaRed, q_full); + // // BOOST_CHECK((M_reduced_computed - dataCrbaRed.M).isZero()); +} + + + BOOST_AUTO_TEST_SUITE_END() From e8df4c4a8790b418f6d2351c6845b275b694c3b9 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 036/166] [Data] Check on mimic is disable for now --- include/pinocchio/algorithm/check.hxx | 5 +++-- include/pinocchio/multibody/data.hxx | 3 +-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 4c54254ba1..401a48aac9 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -160,6 +160,8 @@ namespace pinocchio for (JointIndex j = 1; int(j) < model.njoints; ++j) { + if(boost::get(&model.joints[j])) + continue ; JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); int nv = model.joints[j].nv(); @@ -174,8 +176,7 @@ namespace pinocchio CHECK_DATA((model.parents[d] < j) || (model.parents[d] > c)); int row = model.joints[j].idx_v(); - if(boost::get(&model.joints[j])) - continue ; + CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]); diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index f3b23cfcfb..203a8f08e9 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -205,8 +205,7 @@ namespace pinocchio Scalar nv_; if ( - boost::get(&model.joints[(Index)lastChild[(Index)i]]) - && lastChild[(Index)i] != i) + boost::get(&model.joints[(Index)lastChild[(Index)i]])) nv_ = boost::get(model.joints[(Index)lastChild[(Index)i]]).jmodel().nv(); else nv_ = nv(model.joints[(Index)lastChild[(Index)i]]); From f31ac29f05b5fa6791ee8efe91012da4a16ac51d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 037/166] Working test --- include/pinocchio/multibody/sample-models.hxx | 2 +- unittest/sample-models.cpp | 14 ++++---------- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 508b5ee315..c250733991 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -110,7 +110,7 @@ namespace pinocchio // Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); // Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); - Scalar multiplier = 0.5; + Scalar multiplier = 1; Scalar offset = 0; joint_id = addJointAndBody( model, diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index 27f9e904ef..e9fd614694 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -106,7 +106,7 @@ BOOST_AUTO_TEST_CASE(compare_mimic) Eigen::VectorXd v_full = toFull(v_reduced, 4, 5, scaling, 0); Eigen::VectorXd a_full = toFull(a_reduced, 4, 5, scaling, 0); - + std::cout << v_full << std::endl; pinocchio::Data dataFKFull(model); pinocchio::forwardKinematics(model, dataFKFull, q_full, v_full, a_full); @@ -127,14 +127,13 @@ BOOST_AUTO_TEST_CASE(compare_mimic) pinocchio::Data dataCRBAFull(model); pinocchio::crba(model, dataCRBAFull, q_full); + Eigen::MatrixXd M = dataCRBAFull.M; Eigen::MatrixXd G = create_G(model, model_m); G(model.nv - 1, model_m.nv - 1) = scaling; - // // Use equation of motion to compute tau from a_reduced - // std::cout << G.transpose() << std::endl; - Eigen::VectorXd tau_reduced_computed = (G.transpose() * dataCRBAFull.M * G) * a_reduced + (G.transpose() * C_full); + Eigen::VectorXd tau_reduced_computed = (G.transpose() * M * G) * a_reduced + (G.transpose() * C_full); // Eigen::MatrixXd M_reduced_computed = (G.transpose() * dataCRBAFull.M * G); // std::cout << " --------------------- " << std::endl; pinocchio::Data dataRneaRed(model_m); @@ -149,12 +148,7 @@ BOOST_AUTO_TEST_CASE(compare_mimic) pinocchio::Data dataBiasF(model); pinocchio::rnea(model, dataBiasF, q_full, v_full, a_full); - for(int i = 0; i < model.njoints; i++) - { - BOOST_CHECK(dataRneaRed.f[i].isApprox(dataBiasF.f[i])); - BOOST_CHECK(dataRneaRed.h[i].isApprox(dataBiasF.h[i])); - BOOST_CHECK(model.inertias[i].isApprox(model_m.inertias[i])); - } + std::cout << dataBiasF.tau << "\n " << dataRneaRed.tau << std::endl; // // std::cout << dataBiasF.tau << std::endl; From cf8240852e0268a925918ac663cd81c14f8de3a0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 038/166] [EtienneAr feedback] Fix RNEA: initialize tau to zero before summing --- include/pinocchio/algorithm/rnea.hxx | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index a54f4a3dda..a92a24f6e2 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -63,7 +63,7 @@ namespace pinocchio data.v[i] = jdata.v(); if (parent > 0) data.v[i] += data.liMi[i].actInv(data.v[parent]); - + data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); @@ -100,7 +100,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; jmodel.jointVelocitySelector(data.tau) += jdata.S().transpose() * data.f[i]; - + if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); } @@ -130,6 +130,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; From 9db26dee1953a35d683d05f42dd7b056a6f4b3bf Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 039/166] [EtienneAr feedback] Fix crba for mimic joints --- include/pinocchio/algorithm/crba.hxx | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index d7d8193cc7..4a01f0e585 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -153,10 +153,10 @@ namespace pinocchio /* F[1:6,i] = Y*S */ // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S(); - jmodel.jointJacCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); + jmodel.jointVelCols(data.Fcrb[i]) += data.Ycrb[i] * jdata.S(); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); const JointIndex & parent = model.parents[i]; @@ -168,7 +168,7 @@ namespace pinocchio /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */ Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree[i]); Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); - forceSet::se3Action(data.liMi[i], iF, jF); + forceSet::se3Action(data.liMi[i], iF, jF); } } }; @@ -197,6 +197,11 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } + data.M.setZero(); + for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) + { + data.Fcrb[i].setZero(); + } typedef CrbaLocalConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { From 76a096977fc00481ab52c44db4b73c18d7aa4afd Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 040/166] [EtienneAr feedback] Test crba before rnea for mimic joints --- unittest/sample-models.cpp | 79 +++++++++++++++++--------------------- 1 file changed, 36 insertions(+), 43 deletions(-) diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index e9fd614694..4a956701e4 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -15,20 +15,22 @@ #include #include - // Helper functions to map reduced to full model -Eigen::VectorXd toFull(const Eigen::VectorXd &vec, int mimPrim, int mimSec, double scaling, double offset) { - Eigen::VectorXd vecFull(vec.size() + 1); - vecFull << vec.head(mimSec), scaling * vec[mimPrim] + offset, vec.tail(vec.size() - mimSec); - return vecFull; +Eigen::VectorXd +toFull(const Eigen::VectorXd & vec, int mimPrim, int mimSec, double scaling, double offset) +{ + Eigen::VectorXd vecFull(vec.size() + 1); + vecFull << vec.head(mimSec), scaling * vec[mimPrim] + offset, vec.tail(vec.size() - mimSec); + return vecFull; } -Eigen::MatrixXd create_G(const pinocchio::Model &model, const pinocchio::Model &modelMimic) { - Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model.nv, modelMimic.nv); - for (int i = 0; i < modelMimic.nv; ++i) - G(i, i) = 1; - - return G; +Eigen::MatrixXd create_G(const pinocchio::Model & model, const pinocchio::Model & modelMimic) +{ + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model.nv, modelMimic.nv); + for (int i = 0; i < modelMimic.nv; ++i) + G(i, i) = 1; + + return G; } BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) @@ -98,7 +100,8 @@ BOOST_AUTO_TEST_CASE(compare_mimic) Eigen::VectorXd a_reduced = Eigen::VectorXd::Random(model_m.nv); Eigen::VectorXd tau_reduced = Eigen::VectorXd::Random(model_m.nv); - auto jointMimic = boost::get(model_m.joints[model_m.getJointId("wrist1_mimic_joint")]); + auto jointMimic = boost::get( + model_m.joints[model_m.getJointId("wrist1_mimic_joint")]); double scaling = jointMimic.scaling(); double offset = jointMimic.offset(); // Reduced model configuration (without the mimic joint) @@ -106,57 +109,47 @@ BOOST_AUTO_TEST_CASE(compare_mimic) Eigen::VectorXd v_full = toFull(v_reduced, 4, 5, scaling, 0); Eigen::VectorXd a_full = toFull(a_reduced, 4, 5, scaling, 0); - std::cout << v_full << std::endl; pinocchio::Data dataFKFull(model); pinocchio::forwardKinematics(model, dataFKFull, q_full, v_full, a_full); pinocchio::Data dataFKRed(model_m); pinocchio::forwardKinematics(model_m, dataFKRed, q_reduced, v_reduced, a_reduced); - for(int i = 0; i < model.njoints; i++) + for (int i = 0; i < model.njoints; i++) { BOOST_CHECK(dataFKRed.oMi[i].isApprox(dataFKFull.oMi[i])); BOOST_CHECK(dataFKRed.liMi[i].isApprox(dataFKFull.liMi[i])); BOOST_CHECK(model.inertias[i].isApprox(model_m.inertias[i])); } - // Compute M, and C, for the mimic model from the reduced model variable - pinocchio::Data dataBiasFull(model); - pinocchio::rnea(model, dataBiasFull, q_full, v_full, Eigen::VectorXd::Zero(model.nv)); - Eigen::VectorXd C_full = dataBiasFull.tau; + // Build G matrix to go from full to reduced system + Eigen::MatrixXd G = create_G(model, model_m); + G(model.nv - 1, model_m.nv - 1) = scaling; + + // Test crba + pinocchio::Data dataCRBAMimic(model_m); + pinocchio::crba(model_m, dataCRBAMimic, q_reduced); pinocchio::Data dataCRBAFull(model); pinocchio::crba(model, dataCRBAFull, q_full); - Eigen::MatrixXd M = dataCRBAFull.M; - - Eigen::MatrixXd G = create_G(model, model_m); - G(model.nv - 1, model_m.nv - 1) = scaling; - // // Use equation of motion to compute tau from a_reduced - Eigen::VectorXd tau_reduced_computed = (G.transpose() * M * G) * a_reduced + (G.transpose() * C_full); - // Eigen::MatrixXd M_reduced_computed = (G.transpose() * dataCRBAFull.M * G); - // std::cout << " --------------------- " << std::endl; - pinocchio::Data dataRneaRed(model_m); - pinocchio::rnea(model_m, dataRneaRed, q_reduced, v_reduced, a_reduced); - - BOOST_CHECK((dataRneaRed.tau - tau_reduced_computed).isZero()); - std::cout << " --------------------- " << std::endl; - std::cout << tau_reduced_computed << std::endl; - std::cout << std::endl; - std::cout << dataRneaRed.tau << std::endl; - std::cout << std::endl; + Eigen::MatrixXd M_reduced_computed = (G.transpose() * dataCRBAFull.M * G); - pinocchio::Data dataBiasF(model); - pinocchio::rnea(model, dataBiasF, q_full, v_full, a_full); - std::cout << dataBiasF.tau << "\n " << dataRneaRed.tau << std::endl; + BOOST_CHECK((M_reduced_computed - dataCRBAMimic.M).isZero()); - // // std::cout << dataBiasF.tau << std::endl; + // Test rnea + pinocchio::Data dataBiasFull(model); + pinocchio::nonLinearEffects(model, dataBiasFull, q_full, v_full); + Eigen::VectorXd C_full = dataBiasFull.tau; - // // pinocchio::Data dataCrbaRed(model_m); - // // pinocchio::crba(model_m, dataCrbaRed, q_full); - // // BOOST_CHECK((M_reduced_computed - dataCrbaRed.M).isZero()); -} + // // Use equation of motion to compute tau from a_reduced + Eigen::VectorXd tau_reduced_computed = + (G.transpose() * dataCRBAFull.M * G) * a_reduced + (G.transpose() * C_full); + pinocchio::Data dataRneaRed(model_m); + pinocchio::rnea(model_m, dataRneaRed, q_reduced, v_reduced, a_reduced); + BOOST_CHECK((dataRneaRed.tau - tau_reduced_computed).isZero()); +} BOOST_AUTO_TEST_SUITE_END() From 218d155aadd9b2583272aa418fd2af3089a4610e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 041/166] [EtienneAr feedback] Port crba fix to algorithm with other convention --- include/pinocchio/algorithm/crba.hxx | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 4a01f0e585..37cdde469d 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -82,7 +82,7 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); // Joint Space Inertia Matrix - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); const JointIndex & parent = model.parents[i]; @@ -239,6 +239,11 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } + data.M.setZero(); + for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) + { + data.Fcrb[i].setZero(); + } typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { From 0c96e2a8c4aaca1cccafaee3494def95c5cafa74 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 042/166] [EtienneAr feedback] Port crba fix to rnea, compute-all-terms and constrained-dynamics --- include/pinocchio/algorithm/compute-all-terms.hxx | 2 +- include/pinocchio/algorithm/constrained-dynamics.hxx | 2 +- include/pinocchio/algorithm/rnea.hxx | 12 ++++++------ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 594baf9167..9af8717ec3 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -124,7 +124,7 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dAg_cols); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index 1ed65c384f..5ce44e30b8 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -166,7 +166,7 @@ namespace pinocchio const ColsBlock J_cols = jmodel.jointJacCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); - data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.dFda.middleCols(jmodel.idx_v(), data.nvSubtree[i]); data.oYcrb[parent] += data.oYcrb[i]; diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index a92a24f6e2..d8049f97a1 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -589,13 +589,13 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); jmodel.jointVelCols(data.dFdv).noalias() += data.B[i] * J_cols; - data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.C).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = + data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() += Ag_cols.transpose() * data.dJ.col(j); Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; @@ -641,6 +641,7 @@ namespace pinocchio typename Pass1::ArgsType(model, data, q.derived(), v.derived())); } + data.C.setZero(); typedef CoriolisMatrixBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -686,19 +687,18 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dFdv_cols); dFdv_cols.noalias() += data.B[i] * J_cols; - data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = + jmodel.jointVelRows(data.C).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = - Ag_cols.transpose() * data.dJ.col(j); + jmodel.jointVelRows(data.C).col(j).noalias() = Ag_cols.transpose() * data.dJ.col(j); Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j) += Mat_tmp * data.J.col(j); + jmodel.jointVelRows(data.C).col(j) += Mat_tmp * data.J.col(j); if (parent > 0) data.B[parent] += data.B[i]; From 339d8229b9a6f2671d79a4acfc90c5157eab9ee4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 043/166] [EtienneAr feedback] Revert useless change on Fcrb in crna.hxx --- include/pinocchio/algorithm/crba.hxx | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 37cdde469d..7189257feb 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -153,7 +153,7 @@ namespace pinocchio /* F[1:6,i] = Y*S */ // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S(); - jmodel.jointVelCols(data.Fcrb[i]) += data.Ycrb[i] * jdata.S(); + jmodel.jointVelCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += @@ -168,7 +168,7 @@ namespace pinocchio /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */ Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree[i]); Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); - forceSet::se3Action(data.liMi[i], iF, jF); + forceSet::se3Action(data.liMi[i], iF, jF); } } }; @@ -198,10 +198,6 @@ namespace pinocchio } data.M.setZero(); - for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) - { - data.Fcrb[i].setZero(); - } typedef CrbaLocalConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -240,10 +236,6 @@ namespace pinocchio } data.M.setZero(); - for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) - { - data.Fcrb[i].setZero(); - } typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { From 24e857dc2acfb88618aed32342eb1e7bf3207998 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 044/166] [EtienneAr feedback] Make M matrix symetric in test --- unittest/sample-models.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index 4a956701e4..e25b85c10a 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -129,9 +129,13 @@ BOOST_AUTO_TEST_CASE(compare_mimic) // Test crba pinocchio::Data dataCRBAMimic(model_m); pinocchio::crba(model_m, dataCRBAMimic, q_reduced); + dataCRBAMimic.M.triangularView() = + dataCRBAMimic.M.transpose().triangularView(); pinocchio::Data dataCRBAFull(model); pinocchio::crba(model, dataCRBAFull, q_full); + dataCRBAFull.M.triangularView() = + dataCRBAFull.M.transpose().triangularView(); Eigen::MatrixXd M_reduced_computed = (G.transpose() * dataCRBAFull.M * G); From 35261917e9120dc1d5712db5215587338941f5eb Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 045/166] [EtienneAr feedback] Properly use data.nle instead of rnea --- unittest/sample-models.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index e25b85c10a..e65d0a8971 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -144,11 +144,10 @@ BOOST_AUTO_TEST_CASE(compare_mimic) // Test rnea pinocchio::Data dataBiasFull(model); pinocchio::nonLinearEffects(model, dataBiasFull, q_full, v_full); - Eigen::VectorXd C_full = dataBiasFull.tau; + Eigen::VectorXd C_full = dataBiasFull.nle; // // Use equation of motion to compute tau from a_reduced - Eigen::VectorXd tau_reduced_computed = - (G.transpose() * dataCRBAFull.M * G) * a_reduced + (G.transpose() * C_full); + Eigen::VectorXd tau_reduced_computed = M_reduced_computed * a_reduced + (G.transpose() * C_full); pinocchio::Data dataRneaRed(model_m); pinocchio::rnea(model_m, dataRneaRed, q_reduced, v_reduced, a_reduced); From 3f9c3f2ee76e7edc0f5afbceeee9ff04c3a15f5e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 046/166] [EtienneAr feedback] Fix crba for mimic joints --- include/pinocchio/algorithm/crba.hxx | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 7189257feb..2f98b2ec20 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -153,7 +153,7 @@ namespace pinocchio /* F[1:6,i] = Y*S */ // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S(); - jmodel.jointVelCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); + jmodel.jointVelCols(data.Fcrb[i]) += data.Ycrb[i] * jdata.S(); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += @@ -168,7 +168,7 @@ namespace pinocchio /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */ Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree[i]); Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); - forceSet::se3Action(data.liMi[i], iF, jF); + forceSet::se3Action(data.liMi[i], iF, jF); } } }; @@ -198,6 +198,10 @@ namespace pinocchio } data.M.setZero(); + for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) + { + data.Fcrb[i].setZero(); + } typedef CrbaLocalConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { From 16c7c9631dccc1e75b9ae5d94a60b1777a9a4c40 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 047/166] [EtienneAr feedback] Fix RNEA after chage for mimic joints --- include/pinocchio/algorithm/rnea.hxx | 1 + 1 file changed, 1 insertion(+) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index d8049f97a1..98992a4043 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -184,6 +184,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; From 9a46d13fa815268d13e8350244c40d0fa70dd508 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 048/166] [example] Change buildModelUrdf - added mimic bool --- examples/collision-with-point-clouds.py | 2 +- examples/collisions.py | 2 +- examples/meshcat-viewer-dae.py | 2 +- examples/meshcat-viewer.py | 2 +- examples/simulation-contact-dynamics.py | 2 +- examples/static-contact-dynamics.py | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/collision-with-point-clouds.py b/examples/collision-with-point-clouds.py index dac0939b0d..ff024d89c1 100644 --- a/examples/collision-with-point-clouds.py +++ b/examples/collision-with-point-clouds.py @@ -22,7 +22,7 @@ urdf_model_path = model_path / "panda_description/urdf" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, mesh_dir + urdf_model_path, False, mesh_dir ) # Add point clouds diff --git a/examples/collisions.py b/examples/collisions.py index 1578efa4d9..d323f1fc1e 100644 --- a/examples/collisions.py +++ b/examples/collisions.py @@ -10,7 +10,7 @@ urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename # Load model -model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer()) +model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer(), False) # Load collision geometries geom_model = pin.buildGeomFromUrdf( diff --git a/examples/meshcat-viewer-dae.py b/examples/meshcat-viewer-dae.py index e80c08e4ff..2e96509ac3 100644 --- a/examples/meshcat-viewer-dae.py +++ b/examples/meshcat-viewer-dae.py @@ -19,7 +19,7 @@ urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() ) # Start a new MeshCat server and client. diff --git a/examples/meshcat-viewer.py b/examples/meshcat-viewer.py index 60c375e496..94e7f279f6 100644 --- a/examples/meshcat-viewer.py +++ b/examples/meshcat-viewer.py @@ -21,7 +21,7 @@ urdf_model_path = model_path / "solo_description/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() ) # Start a new MeshCat server and client. diff --git a/examples/simulation-contact-dynamics.py b/examples/simulation-contact-dynamics.py index 5194abacee..6dbdf5008d 100644 --- a/examples/simulation-contact-dynamics.py +++ b/examples/simulation-contact-dynamics.py @@ -18,7 +18,7 @@ srdf_full_path = model_path / "talos_data/srdf" / srdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() ) # Start a new MeshCat server and client. diff --git a/examples/static-contact-dynamics.py b/examples/static-contact-dynamics.py index e6757d5596..f512ff70b7 100644 --- a/examples/static-contact-dynamics.py +++ b/examples/static-contact-dynamics.py @@ -60,7 +60,7 @@ urdf_model_path = model_path / "solo_description/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() ) data = model.createData() From b087e76fba5eca58fbfa1d3648ee79e633951784 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 049/166] [mimic-joint] Added support for FreeFlyer --- include/pinocchio/multibody/joint/joint-mimic.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index e15a7f83ce..e3c95bdfa1 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -353,6 +353,9 @@ namespace pinocchio // Joint Translation typedef JointModelTranslationTpl JointModelTranslation; + // Joint FreeFlyer + typedef JointModelFreeFlyerTpl JointModelFreeFlyer; + typedef boost::variant< JointModelRX, JointModelRY, @@ -366,7 +369,8 @@ namespace pinocchio JointModelRUBX, JointModelRUBY, JointModelRUBZ, - JointModelRevoluteUnboundedUnaligned> + JointModelRevoluteUnboundedUnaligned, + JointModelFreeFlyer> JointModelVariant; // Joint Revolute @@ -397,6 +401,9 @@ namespace pinocchio // Joint Translation typedef JointDataTranslationTpl JointDataTranslation; + // Joint FreeFlyer + typedef JointDataFreeFlyerTpl JointDataFreeFlyer; + typedef boost::variant< JointDataRX, JointDataRY, @@ -410,7 +417,8 @@ namespace pinocchio JointDataRUBX, JointDataRUBY, JointDataRUBZ, - JointDataRevoluteUnboundedUnaligned> + JointDataRevoluteUnboundedUnaligned, + JointDataFreeFlyer> JointDataVariant; }; From c62da7ab679863217b8307b94341647384cb939c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 050/166] [unittest] Added bool mimic to build urdf --- unittest/python/bindings_build_geom_from_urdf_memorycheck.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittest/python/bindings_build_geom_from_urdf_memorycheck.py b/unittest/python/bindings_build_geom_from_urdf_memorycheck.py index e606a84e5c..3d517516da 100644 --- a/unittest/python/bindings_build_geom_from_urdf_memorycheck.py +++ b/unittest/python/bindings_build_geom_from_urdf_memorycheck.py @@ -15,7 +15,7 @@ def setUp(self): ) def test_load(self): - model = pin.buildModelFromUrdf(self.model_path) + model = pin.buildModelFromUrdf(self.model_path, False) for _ in range(2): pin.buildGeomFromUrdf( model, From ad639d4d9e48c8e263f2191dab6921f5c364fb0f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 051/166] [Joint] Split Joint ConfigSelector in two functions --- include/pinocchio/algorithm/model.hxx | 20 +++---- .../multibody/joint/joint-composite.hpp | 35 ++++++++++-- .../pinocchio/multibody/joint/joint-mimic.hpp | 20 ++++++- .../multibody/joint/joint-model-base.hpp | 42 ++++++++++++--- .../multibody/joint/joint-translation.hpp | 2 +- .../multibody/liegroup/liegroup-algo.hxx | 49 +++++++---------- include/pinocchio/multibody/model.hxx | 42 +++++++-------- unittest/joint-composite.cpp | 9 +++- unittest/model.cpp | 54 +++++++++++++------ 9 files changed, 180 insertions(+), 93 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 3050cbbf4d..dad5465d0a 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -213,8 +213,8 @@ namespace pinocchio modelAB .effortLimit), // Need to select the vector base on the origin idxs, so use jmodel_in. jmodel_in.jointVelocitySelector(modelAB.velocityLimit), - jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit), - jmodel_in.jointConfigSelector(modelAB.upperPositionLimit), + jmodel_in.jointConfigFromNqSelector(modelAB.lowerPositionLimit), + jmodel_in.jointConfigFromNqSelector(modelAB.upperPositionLimit), jmodel_in.jointVelocitySelector(modelAB.friction), jmodel_in.jointVelocitySelector(modelAB.damping)); assert(joint_id_out < model.joints.size()); @@ -420,8 +420,8 @@ namespace pinocchio const typename Model::JointModel & joint_model = model.joints[joint_id]; const typename Model::JointModel & joint_modelA = modelA.joints[joint_idA]; - joint_model.jointConfigSelector(config_vector) = - joint_modelA.jointConfigSelector(config_vectorA); + joint_model.jointConfigFromNqSelector(config_vector) = + joint_modelA.jointConfigFromNqSelector(config_vectorA); } model.referenceConfigurations.insert(std::make_pair(config_name, config_vector)); @@ -449,8 +449,8 @@ namespace pinocchio const typename Model::JointModel & joint_model = model.joints[joint_id]; const typename Model::JointModel & joint_modelB = modelB.joints[joint_idB]; - joint_model.jointConfigSelector(config_vector) = - joint_modelB.jointConfigSelector(config_vectorB); + joint_model.jointConfigFromNqSelector(config_vector) = + joint_modelB.jointConfigFromNqSelector(config_vectorB); } } @@ -644,8 +644,8 @@ namespace pinocchio parent_frame_placement * input_model.jointPlacements[joint_id], joint_name, joint_input_model.jointVelocitySelector(input_model.effortLimit), joint_input_model.jointVelocitySelector(input_model.velocityLimit), - joint_input_model.jointConfigSelector(input_model.lowerPositionLimit), - joint_input_model.jointConfigSelector(input_model.upperPositionLimit), + joint_input_model.jointConfigFromNqSelector(input_model.lowerPositionLimit), + joint_input_model.jointConfigFromNqSelector(input_model.upperPositionLimit), joint_input_model.jointVelocitySelector(input_model.friction), joint_input_model.jointVelocitySelector(input_model.damping)); // Append inertia @@ -679,8 +679,8 @@ namespace pinocchio const JointModel & input_joint_model = input_model.joints[input_joint_id]; const JointModel & reduced_joint_model = reduced_model.joints[reduced_joint_id]; - reduced_joint_model.jointConfigSelector(reduced_config_vector) = - input_joint_model.jointConfigSelector(input_config_vector); + reduced_joint_model.jointConfigFromNqSelector(reduced_config_vector) = + input_joint_model.jointConfigFromNqSelector(input_config_vector); } reduced_model.referenceConfigurations.insert( diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index 574274f290..b06f207d44 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -477,13 +477,26 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector(Eigen::MatrixBase & a) const + jointConfigFromDofSelector(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromNqSelector(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromNqSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } @@ -528,16 +541,30 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector_impl(const Eigen::MatrixBase & a) const + jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector_impl(Eigen::MatrixBase & a) const + jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_q, nq()); + } + template typename SizeDepType::template SegmentReturn::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase & a) const diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index e3c95bdfa1..4f4a76d524 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -1006,7 +1006,7 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector_impl(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); } @@ -1014,11 +1014,27 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector_impl(Eigen::MatrixBase & a) const + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); } + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); + } + /* Acces to dedicated segment in robot config velocity space. */ // Const access template diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index e4eabad1c7..8aad900677 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -283,14 +283,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector(const Eigen::MatrixBase & a) const { - return derived().jointConfigSelector_impl(a); + return derived().jointConfigFromDofSelector_impl(a); } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigSelector_impl(const Eigen::MatrixBase & a) const + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_q(), nq()); } @@ -298,14 +298,44 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector(Eigen::MatrixBase & a) const + jointConfigFromDofSelector(Eigen::MatrixBase & a) const { - return derived().jointConfigSelector_impl(a); + return derived().jointConfigFromDofSelector_impl(a); } template typename SizeDepType::template SegmentReturn::Type - jointConfigSelector_impl(Eigen::MatrixBase & a) const + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a, idx_q(), nq()); + } + + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromNqSelector(const Eigen::MatrixBase & a) const + { + return derived().jointConfigFromNqSelector_impl(a); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_q(), nq()); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromNqSelector(Eigen::MatrixBase & a) const + { + return derived().jointConfigFromNqSelector_impl(a); + } + + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a, idx_q(), nq()); } diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index bdfea32718..727af5aff9 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -599,7 +599,7 @@ namespace pinocchio template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - data.joint_q = this->jointConfigSelector(qs); + data.joint_q = this->jointConfigFromDofSelector(qs); data.M.translation() = data.joint_q; } diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index ef2e909f18..099c49181a 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -162,16 +162,6 @@ namespace pinocchio using Algo::run; \ }; -#define PINOCCHIO_DETAILS_JOINT_MIMIC_1(Algo) \ - template \ - struct Algo> \ - { \ - typedef typename Visitor::ArgsType ArgsType; \ - static void run(PINOCCHIO_DETAILS_WRITE_ARGS_1(JointModelMimicTpl)) \ - { \ - } \ - } - } // namespace details template @@ -207,8 +197,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.integrate( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -253,7 +243,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrate( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); } }; @@ -308,7 +298,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), jmodel.jointJacRows(mat_in.derived()), jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); } @@ -357,7 +347,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; @@ -401,7 +391,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dDifference( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), + jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived()), jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); } }; @@ -447,8 +437,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.interpolate( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), u, - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); + jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived()), u, + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -487,7 +477,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.difference( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), + jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived()), jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); } }; @@ -531,7 +521,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; DistanceVectorOut & distances_ = PINOCCHIO_EIGEN_CONST_CAST(DistanceVectorOut, distances); distances_[(Eigen::DenseIndex)i] += lgo.squaredDistance( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived())); + jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived())); } }; @@ -564,7 +554,7 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; squaredDistance += lgo.squaredDistance( - jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived())); + jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived())); } }; @@ -603,9 +593,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.randomConfiguration( - jmodel.jointConfigSelector(lowerLimits.derived()), - jmodel.jointConfigSelector(upperLimits.derived()), - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, q))); + jmodel.jointConfigFromNqSelector(lowerLimits.derived()), + jmodel.jointConfigFromNqSelector(upperLimits.derived()), + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, q))); } }; @@ -633,7 +623,7 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - lgo.normalize(jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); + lgo.normalize(jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); } }; @@ -664,7 +654,7 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - res &= lgo.isNormalized(jmodel.jointConfigSelector(q.derived()), prec); + res &= lgo.isNormalized(jmodel.jointConfigFromNqSelector(q.derived()), prec); } }; @@ -700,7 +690,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; isSame &= lgo.isSameConfiguration( - jmodel.jointConfigSelector(q1.derived()), jmodel.jointConfigSelector(q2.derived()), prec); + jmodel.jointConfigFromNqSelector(q1.derived()), jmodel.jointConfigFromNqSelector(q2.derived()), prec); } }; @@ -729,13 +719,12 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, neutral_elt)) = + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, neutral_elt)) = lgo.neutral(); } }; PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(NeutralStepAlgo); - PINOCCHIO_DETAILS_JOINT_MIMIC_1(NeutralStepAlgo); template struct IntegrateCoeffWiseJacobianStepAlgo; @@ -765,7 +754,7 @@ namespace pinocchio typedef typename LieGroupMap::template operation::type LieGroup; LieGroup lgo; lgo.integrateCoeffWiseJacobian( - jmodel.jointConfigSelector(q.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, jacobian) .template block( jmodel.idx_q(), jmodel.idx_v(), jmodel.nq(), jmodel.nv())); diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index 5d174d0097..3760756dc7 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -127,28 +127,26 @@ namespace pinocchio njs.push_back(joint_nj); idx_js.push_back(joint_idx_j); - if (joint_nq > 0 && joint_nv > 0) - { - effortLimit.conservativeResize(nv); - jmodel.jointVelocitySelector(effortLimit) = max_effort; - velocityLimit.conservativeResize(nv); - jmodel.jointVelocitySelector(velocityLimit) = max_velocity; - lowerPositionLimit.conservativeResize(nq); - jmodel.jointConfigSelector(lowerPositionLimit) = min_config; - upperPositionLimit.conservativeResize(nq); - jmodel.jointConfigSelector(upperPositionLimit) = max_config; - - armature.conservativeResize(nv); - jmodel.jointVelocitySelector(armature).setZero(); - rotorInertia.conservativeResize(nv); - jmodel.jointVelocitySelector(rotorInertia).setZero(); - rotorGearRatio.conservativeResize(nv); - jmodel.jointVelocitySelector(rotorGearRatio).setOnes(); - friction.conservativeResize(nv); - jmodel.jointVelocitySelector(friction) = joint_friction; - damping.conservativeResize(nv); - jmodel.jointVelocitySelector(damping) = joint_damping; - } + effortLimit.conservativeResize(nv); + jmodel.jointVelocitySelector(effortLimit) = max_effort; + velocityLimit.conservativeResize(nv); + jmodel.jointVelocitySelector(velocityLimit) = max_velocity; + lowerPositionLimit.conservativeResize(nq); + jmodel.jointConfigFromNqSelector(lowerPositionLimit) = min_config; + upperPositionLimit.conservativeResize(nq); + jmodel.jointConfigFromNqSelector(upperPositionLimit) = max_config; + + armature.conservativeResize(nv); + jmodel.jointVelocitySelector(armature).setZero(); + rotorInertia.conservativeResize(nv); + jmodel.jointVelocitySelector(rotorInertia).setZero(); + rotorGearRatio.conservativeResize(nv); + jmodel.jointVelocitySelector(rotorGearRatio).setOnes(); + friction.conservativeResize(nv); + jmodel.jointVelocitySelector(friction) = joint_friction; + damping.conservativeResize(nv); + jmodel.jointVelocitySelector(damping) = joint_damping; + // Init and add joint index to its parent subtrees. subtrees.push_back(IndexVector(1)); diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index b2679f3402..7120d06b4c 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -142,9 +142,14 @@ void test_joint_methods( Model::ConfigVectorType vec(Model::ConfigVectorType::Ones(m)); const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m)); - BOOST_CHECK(jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec)); + BOOST_CHECK(jmodel.jointConfigFromDofSelector(vec) == jmodel_composite.jointConfigFromDofSelector(vec)); BOOST_CHECK( - jmodel.jointConfigSelector(vec_const) == jmodel_composite.jointConfigSelector(vec_const)); + jmodel.jointConfigFromDofSelector(vec_const) == jmodel_composite.jointConfigFromDofSelector(vec_const)); + + BOOST_CHECK(jmodel.jointConfigFromNqSelector(vec) == jmodel_composite.jointConfigFromNqSelector(vec)); + BOOST_CHECK( + jmodel.jointConfigFromNqSelector(vec_const) == jmodel_composite.jointConfigFromNqSelector(vec_const)); + BOOST_CHECK(jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec)); BOOST_CHECK( diff --git a/unittest/model.cpp b/unittest/model.cpp index 1a94dd4d28..295fc735da 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -250,8 +250,11 @@ BOOST_AUTO_TEST_CASE(append) const JointModel & joint_model_humanoid = humanoid.joints[humanoid.getJointId(model1.names[joint_id])]; BOOST_CHECK( - joint_model_humanoid.jointConfigSelector(humanoid_config->second) - == joint_model1.jointConfigSelector(config_vector)); + joint_model_humanoid.jointConfigFromNqSelector(humanoid_config->second) + == joint_model1.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_humanoid.jointConfigFromDofSelector(humanoid_config->second) + == joint_model1.jointConfigFromDofSelector(config_vector)); // std::cerr<<"humanoid "<second) - == joint_model1.jointConfigSelector(config_vector)); + joint_model_manipulator.jointConfigFromNqSelector(manipulator_config->second) + == joint_model1.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_manipulator.jointConfigFromDofSelector(manipulator_config->second) + == joint_model1.jointConfigFromDofSelector(config_vector)); // std::cerr<<"manipulator "<second) - == joint_model.jointConfigSelector(config_vector)); + joint_model_humanoid.jointConfigFromNqSelector(humanoid_config->second) + == joint_model.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_humanoid.jointConfigFromDofSelector(humanoid_config->second) + == joint_model.jointConfigFromDofSelector(config_vector)); // std::cerr<<"humanoid "<second) - == joint_model.jointConfigSelector(config_vector)); + joint_model_manipulator.jointConfigFromNqSelector(manipulator_config->second) + == joint_model.jointConfigFromNqSelector(config_vector)); + BOOST_CHECK( + joint_model_manipulator.jointConfigFromDofSelector(manipulator_config->second) + == joint_model.jointConfigFromDofSelector(config_vector)); // std::cerr<<"manipulator "< Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 052/166] Apply pre-commit --- examples/robot-wrapper-viewer.py | 4 +- include/pinocchio/algorithm/check.hxx | 7 +- .../bindings/python/multibody/model.hpp | 3 +- include/pinocchio/multibody/data.hxx | 3 +- .../multibody/joint/joint-basic-visitors.hpp | 13 +- .../multibody/joint/joint-basic-visitors.hxx | 41 +++-- .../multibody/joint/joint-collection.hpp | 3 +- .../multibody/joint/joint-free-flyer.hpp | 2 +- .../multibody/joint/joint-generic.hpp | 2 +- .../joint/joint-helical-unaligned.hpp | 4 +- .../multibody/joint/joint-helical.hpp | 4 +- .../pinocchio/multibody/joint/joint-mimic.hpp | 23 +-- .../multibody/joint/joint-planar.hpp | 4 +- .../joint/joint-prismatic-unaligned.hpp | 4 +- .../multibody/joint/joint-prismatic.hpp | 2 +- .../joint/joint-revolute-unaligned.hpp | 2 +- .../joint-revolute-unbounded-unaligned.hpp | 4 +- .../joint/joint-revolute-unbounded.hpp | 4 +- .../multibody/joint/joint-revolute.hpp | 2 +- .../multibody/joint/joint-spherical-ZYX.hpp | 4 +- .../multibody/joint/joint-spherical.hpp | 2 +- .../multibody/joint/joint-translation.hpp | 2 +- .../multibody/joint/joint-universal.hpp | 2 +- .../multibody/liegroup/liegroup-algo.hxx | 21 ++- include/pinocchio/multibody/model.hpp | 1 - include/pinocchio/multibody/model.hxx | 4 +- include/pinocchio/multibody/sample-models.hpp | 3 +- include/pinocchio/multibody/sample-models.hxx | 3 +- include/pinocchio/multibody/sample-models.txx | 3 +- include/pinocchio/parsers/urdf/model.hxx | 113 ++++++++---- .../pinocchio/serialization/joints-data.hpp | 30 +++- .../pinocchio/serialization/joints-model.hpp | 10 +- src/multibody/sample-models.cpp | 3 +- src/parsers/urdf/model.cpp | 53 +++--- unittest/all-joints.cpp | 16 +- unittest/joint-composite.cpp | 13 +- unittest/joint-generic.cpp | 23 ++- unittest/joint-mimic.cpp | 163 +++++++++--------- unittest/joint-motion-subspace.cpp | 25 ++- unittest/python/bindings_urdf.py | 8 +- unittest/serialization.cpp | 56 +++--- unittest/visitor.cpp | 20 +-- 42 files changed, 398 insertions(+), 311 deletions(-) diff --git a/examples/robot-wrapper-viewer.py b/examples/robot-wrapper-viewer.py index 432461c2c4..6a9e9550d6 100644 --- a/examples/robot-wrapper-viewer.py +++ b/examples/robot-wrapper-viewer.py @@ -34,7 +34,9 @@ urdf_filename = "talos_reduced.urdf" urdf_model_path = model_path / "talos_data/robots" / urdf_filename -robot = RobotWrapper.BuildFromURDF(urdf_model_path, True, mesh_dir, pin.JointModelFreeFlyer()) +robot = RobotWrapper.BuildFromURDF( + urdf_model_path, True, mesh_dir, pin.JointModelFreeFlyer() +) # alias model = robot.model diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 401a48aac9..52042f9a6c 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -160,8 +160,8 @@ namespace pinocchio for (JointIndex j = 1; int(j) < model.njoints; ++j) { - if(boost::get(&model.joints[j])) - continue ; + if (boost::get(&model.joints[j])) + continue; JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); int nv = model.joints[j].nv(); @@ -176,8 +176,7 @@ namespace pinocchio CHECK_DATA((model.parents[d] < j) || (model.parents[d] > c)); int row = model.joints[j].idx_v(); - - + CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]); const JointModel & jparent = model.joints[model.parents[j]]; diff --git a/include/pinocchio/bindings/python/multibody/model.hpp b/include/pinocchio/bindings/python/multibody/model.hpp index f6857c5abe..e3fac073c7 100644 --- a/include/pinocchio/bindings/python/multibody/model.hpp +++ b/include/pinocchio/bindings/python/multibody/model.hpp @@ -91,8 +91,7 @@ namespace pinocchio "Starting index of the *i*th joint in the tangent configuration space.") .add_property("nvs", &Model::nvs, "Dimension of the *i*th joint tangent subspace.") .add_property( - "idx_js", &Model::idx_js, - "Starting index of the *i*th joint in the jacobian space.") + "idx_js", &Model::idx_js, "Starting index of the *i*th joint in the jacobian space.") .add_property("njs", &Model::njs, "Dimension of the *i*th joint jacobian subspace.") .add_property( "parents", &Model::parents, diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 203a8f08e9..4e101a3cea 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -204,8 +204,7 @@ namespace pinocchio lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); Scalar nv_; - if ( - boost::get(&model.joints[(Index)lastChild[(Index)i]])) + if (boost::get(&model.joints[(Index)lastChild[(Index)i]])) nv_ = boost::get(model.joints[(Index)lastChild[(Index)i]]).jmodel().nv(); else nv_ = nv(model.joints[(Index)lastChild[(Index)i]]); diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index 818cb20a44..a30f74ac27 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -155,7 +155,6 @@ namespace pinocchio template class JointCollectionTpl> inline int nj(const JointModelTpl & jmodel); - /** * @brief Visit a JointModelTpl through JointConfigurationLimitVisitor * to get the configurations limits @@ -216,7 +215,6 @@ namespace pinocchio template class JointCollectionTpl> inline int idx_j(const JointModelTpl & jmodel); - /** * @brief Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the * kinematic chain @@ -245,7 +243,11 @@ namespace pinocchio */ template class JointCollectionTpl> inline void setIndexes( - JointModelTpl & jmodel, JointIndex id, int q, int v, int j); + JointModelTpl & jmodel, + JointIndex id, + int q, + int v, + int j); /** * @brief Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the @@ -451,8 +453,9 @@ namespace pinocchio const JointDataBase & jmodel); /** - * @brief Transfer a value from one variant to another (given that the new variant contains the type of the value). - * For instance transfer a joint model (or data) from a generic joint model (or data) to another generic joint model (or data) with slightly different joint collections. + * @brief Transfer a value from one variant to another (given that the new variant contains the + * type of the value). For instance transfer a joint model (or data) from a generic joint model + * (or data) to another generic joint model (or data) with slightly different joint collections. * * @param[in] variant The value to transfer. * diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 181e503681..74dc52f173 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -265,7 +265,6 @@ namespace pinocchio return JointNjVisitor::run(jmodel); } - /** * @brief JointConfigurationLimitVisitor visitor */ @@ -439,8 +438,12 @@ namespace pinocchio } template class JointCollectionTpl> - static void - run(JointModelTpl & jmodel, JointIndex id, int q, int v, int j) + static void run( + JointModelTpl & jmodel, + JointIndex id, + int q, + int v, + int j) { return boost::apply_visitor(JointSetIndexesVisitor(id, q, v, j), jmodel); } @@ -915,21 +918,27 @@ namespace pinocchio return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived()))); } - template - struct TransferVisitor : public boost::static_visitor { + template + struct TransferVisitor : public boost::static_visitor + { - template - typename boost::enable_if, TargetVariant>::type - operator()(const ValueType& value) const { - return TargetVariant(value); - } + template + typename boost:: + enable_if, TargetVariant>::type + operator()(const ValueType & value) const + { + return TargetVariant(value); + } - template - typename boost::disable_if, TargetVariant>::type - operator()(const ValueType& value) const { - assert(false && "Type not supported in new variant"); - return TargetVariant(); - } + template + typename boost::disable_if< + boost::mpl::contains, + TargetVariant>::type + operator()(const ValueType & value) const + { + assert(false && "Type not supported in new variant"); + return TargetVariant(); + } }; template diff --git a/include/pinocchio/multibody/joint/joint-collection.hpp b/include/pinocchio/multibody/joint/joint-collection.hpp index 9f1fb36e57..540d8fb3fb 100644 --- a/include/pinocchio/multibody/joint/joint-collection.hpp +++ b/include/pinocchio/multibody/joint/joint-collection.hpp @@ -107,8 +107,7 @@ namespace pinocchio JointModelHelicalUnaligned, JointModelUniversal, boost::recursive_wrapper, - boost::recursive_wrapper - > + boost::recursive_wrapper> JointModelVariant; // Joint Revolute diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 3ee4285971..f8dc5398c7 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -288,9 +288,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index d68461a910..1ad504369b 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -29,7 +29,7 @@ namespace pinocchio { Options = _Options, NQ = Eigen::Dynamic, // Dynamic because unknown at compile time - NV = Eigen::Dynamic, + NV = Eigen::Dynamic, NJ = Eigen::Dynamic }; diff --git a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp index a2b70d4054..2d70d34a9e 100644 --- a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp @@ -524,7 +524,7 @@ namespace pinocchio enum { NQ = 1, - NV = 1, + NV = 1, NJ = 1 }; typedef _Scalar Scalar; @@ -637,9 +637,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointModelHelicalUnalignedTpl() diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index 4b308188b7..4285338e77 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -735,7 +735,7 @@ namespace pinocchio enum { NQ = 1, - NV = 1, + NV = 1, NJ = 1 }; typedef _Scalar Scalar; @@ -836,9 +836,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 4f4a76d524..c4919db555 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -369,7 +369,7 @@ namespace pinocchio JointModelRUBX, JointModelRUBY, JointModelRUBZ, - JointModelRevoluteUnboundedUnaligned, + JointModelRevoluteUnboundedUnaligned, JointModelFreeFlyer> JointModelVariant; @@ -417,7 +417,7 @@ namespace pinocchio JointDataRUBX, JointDataRUBY, JointDataRUBZ, - JointDataRevoluteUnboundedUnaligned, + JointDataRevoluteUnboundedUnaligned, JointDataFreeFlyer> JointDataVariant; }; @@ -831,10 +831,11 @@ namespace pinocchio } template - JointModelMimicTpl(const JointModelBase & jmodel_mimicking, - const JointModelTpl & jmodel_mimicked, - const Scalar & scaling, - const Scalar & offset) + JointModelMimicTpl( + const JointModelBase & jmodel_mimicking, + const JointModelTpl & jmodel_mimicked, + const Scalar & scaling, + const Scalar & offset) : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) , m_scaling(scaling) , m_offset(offset) @@ -843,7 +844,9 @@ namespace pinocchio assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes(jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), jmodel_mimicked.idx_j()); + m_jmodel_ref.setIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_j()); } Base & base() @@ -927,8 +930,8 @@ namespace pinocchio qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - - m_jmodel_ref.calc(jdata.m_jdata_ref, qs , vs); + + m_jmodel_ref.calc(jdata.m_jdata_ref, qs, vs); } template @@ -1041,7 +1044,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); } // Non-const access diff --git a/include/pinocchio/multibody/joint/joint-planar.hpp b/include/pinocchio/multibody/joint/joint-planar.hpp index a15fcc44e8..03d4c46c33 100644 --- a/include/pinocchio/multibody/joint/joint-planar.hpp +++ b/include/pinocchio/multibody/joint/joint-planar.hpp @@ -471,7 +471,7 @@ namespace pinocchio enum { NQ = 4, - NV = 3, + NV = 3, NJ = 3 }; enum @@ -565,9 +565,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const diff --git a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp index 2a932d63ef..1eca7ca90a 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp @@ -478,7 +478,7 @@ namespace pinocchio enum { NQ = 1, - NV = 1, + NV = 1, NJ = 1 }; typedef _Scalar Scalar; @@ -590,9 +590,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp index aa85e4ffc5..7731388304 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -672,9 +672,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; diff --git a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp index 2b8c3e9198..f5cb3cde84 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp @@ -616,9 +616,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointModelRevoluteUnalignedTpl() diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp index 757bc70952..57c986c98b 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -24,7 +24,7 @@ namespace pinocchio enum { NQ = 2, - NV = 1, + NV = 1, NJ = 1 }; typedef _Scalar Scalar; @@ -140,9 +140,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointModelRevoluteUnboundedUnalignedTpl() diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp index 26dc075b43..572001de2a 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -23,7 +23,7 @@ namespace pinocchio enum { NQ = 2, - NV = 1, + NV = 1, NJ = 1 }; typedef _Scalar Scalar; @@ -126,9 +126,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp index 5b1fcdf6f5..0b74e17695 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -760,9 +760,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; typedef Eigen::Matrix Vector3; diff --git a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp index 93f54ffad8..9b579f9074 100644 --- a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp @@ -279,7 +279,7 @@ namespace pinocchio enum { NQ = 3, - NV = 3, + NV = 3, NJ = 3 }; typedef _Scalar Scalar; @@ -378,9 +378,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const diff --git a/include/pinocchio/multibody/joint/joint-spherical.hpp b/include/pinocchio/multibody/joint/joint-spherical.hpp index abb5f06e34..4dcce705f7 100644 --- a/include/pinocchio/multibody/joint/joint-spherical.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical.hpp @@ -479,9 +479,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index 727af5aff9..cdab24e1ef 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -576,9 +576,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointDataDerived createData() const diff --git a/include/pinocchio/multibody/joint/joint-universal.hpp b/include/pinocchio/multibody/joint/joint-universal.hpp index 6ec5e12adb..a8310697d9 100644 --- a/include/pinocchio/multibody/joint/joint-universal.hpp +++ b/include/pinocchio/multibody/joint/joint-universal.hpp @@ -412,9 +412,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; + using Base::idx_j; using Base::idx_q; using Base::idx_v; - using Base::idx_j; using Base::setIndexes; JointModelUniversalTpl() diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index 099c49181a..9121e2cfcb 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -391,7 +391,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dDifference( - jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived()), + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived()), jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); } }; @@ -437,7 +438,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.interpolate( - jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived()), u, + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived()), u, jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -477,7 +479,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.difference( - jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived()), + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived()), jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); } }; @@ -521,7 +524,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; DistanceVectorOut & distances_ = PINOCCHIO_EIGEN_CONST_CAST(DistanceVectorOut, distances); distances_[(Eigen::DenseIndex)i] += lgo.squaredDistance( - jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived())); + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived())); } }; @@ -554,7 +558,8 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; squaredDistance += lgo.squaredDistance( - jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived())); + jmodel.jointConfigFromNqSelector(q0.derived()), + jmodel.jointConfigFromNqSelector(q1.derived())); } }; @@ -623,7 +628,8 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - lgo.normalize(jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); + lgo.normalize( + jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); } }; @@ -690,7 +696,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; isSame &= lgo.isSameConfiguration( - jmodel.jointConfigFromNqSelector(q1.derived()), jmodel.jointConfigFromNqSelector(q2.derived()), prec); + jmodel.jointConfigFromNqSelector(q1.derived()), + jmodel.jointConfigFromNqSelector(q2.derived()), prec); } }; diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index 0126004e1f..1936847e2f 100644 --- a/include/pinocchio/multibody/model.hpp +++ b/include/pinocchio/multibody/model.hpp @@ -139,7 +139,6 @@ namespace pinocchio /// \brief Dimension of the *i*th joint jacobian subspace. std::vector njs; - /// \brief Vector of parent joint indexes. The parent of joint *i*, denoted *li*, corresponds to /// li==parents[i]. std::vector parents; diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index 3760756dc7..2162654ce2 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -146,7 +146,6 @@ namespace pinocchio jmodel.jointVelocitySelector(friction) = joint_friction; damping.conservativeResize(nv); jmodel.jointVelocitySelector(damping) = joint_damping; - // Init and add joint index to its parent subtrees. subtrees.push_back(IndexVector(1)); @@ -300,7 +299,8 @@ namespace pinocchio && other.children == children && other.names == names && other.subtrees == subtrees && other.gravity == gravity && other.name == name; - res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs && other.idx_js == idx_js && other.njs == njs; + res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs + && other.idx_js == idx_js && other.njs == njs; if (other.referenceConfigurations.size() != referenceConfigurations.size()) return false; diff --git a/include/pinocchio/multibody/sample-models.hpp b/include/pinocchio/multibody/sample-models.hpp index 37e1c0a8ba..61fef617ae 100644 --- a/include/pinocchio/multibody/sample-models.hpp +++ b/include/pinocchio/multibody/sample-models.hpp @@ -18,7 +18,8 @@ namespace pinocchio * \param model: model, typically given empty, where the kinematic chain is added. */ template class JointCollectionTpl> - void manipulator(ModelTpl & model, const bool mimic = false); + void + manipulator(ModelTpl & model, const bool mimic = false); #ifdef PINOCCHIO_WITH_HPP_FCL /** \brief Create the geometries on top of the kinematic model created by manipulator function. diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index c250733991..73c74e9936 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -114,7 +114,8 @@ namespace pinocchio Scalar offset = 0; joint_id = addJointAndBody( model, - typename JC::JointModelMimic(typename JC::JointModelRY(), model.joints[joint_id].derived(), multiplier, offset), + typename JC::JointModelMimic( + typename JC::JointModelRY(), model.joints[joint_id].derived(), multiplier, offset), model.names[joint_id], pre + "wrist1_mimic", Id4); } else diff --git a/include/pinocchio/multibody/sample-models.txx b/include/pinocchio/multibody/sample-models.txx index 64ff36ac76..56458dcd78 100644 --- a/include/pinocchio/multibody/sample-models.txx +++ b/include/pinocchio/multibody/sample-models.txx @@ -12,7 +12,8 @@ namespace pinocchio namespace buildModels { extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void - manipulator(context::Model &, bool); + manipulator( + context::Model &, bool); extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void humanoid(context::Model &, bool); diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 02795dce36..fe1f8ff797 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -27,7 +27,7 @@ namespace pinocchio template struct MimicInfo; - + template class UrdfVisitorBaseTpl { @@ -113,9 +113,16 @@ namespace pinocchio virtual bool existFrame(const std::string & frame_name, const FrameType type) const = 0; - virtual void addMimicInfo(const std::string & mimic_name, const std::string & mimiced_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset, JointType jointType) = 0; + virtual void addMimicInfo( + const std::string & mimic_name, + const std::string & mimiced_name, + const Vector3 & axis, + const Scalar multiplier, + const Scalar offset, + JointType jointType) = 0; - virtual void convertMimicJoint(const std::string & mimic_name, const MimicInfo & mimic_info) = 0; + virtual void convertMimicJoint( + const std::string & mimic_name, const MimicInfo & mimic_info) = 0; UrdfVisitorBaseTpl() : log(NULL) @@ -144,20 +151,28 @@ namespace pinocchio std::string mimiced_name; Scalar multiplier; Scalar offset; - + // Use the JointType from UrdfVisitorBaseTpl typedef typename UrdfVisitorBaseTpl<_Scalar, Options>::JointType JointType; JointType jointType; - MimicInfo(){} + MimicInfo() + { + } - MimicInfo(std::string _mimiced_name, Scalar _multiplier, Scalar _offset, Vector3 _axis, JointType _jointType): - mimiced_name(_mimiced_name), - multiplier(_multiplier), - offset(_offset), - axis(_axis), - jointType(_jointType) - {} + MimicInfo( + std::string _mimiced_name, + Scalar _multiplier, + Scalar _offset, + Vector3 _axis, + JointType _jointType) + : mimiced_name(_mimiced_name) + , multiplier(_multiplier) + , offset(_offset) + , axis(_axis) + , jointType(_jointType) + { + } }; template class JointCollectionTpl> @@ -286,11 +301,11 @@ namespace pinocchio max_config, friction, damping); break; case Base::MIMIC: - joint_id = model.addJoint(frame.parentJoint, - typename JointCollection::JointModelMimic(model.joints.at(1), 1, 0), - frame.placement * placement, joint_name, - max_effort,max_velocity,min_config,max_config, - friction, damping); + joint_id = model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic(model.joints.at(1), 1, 0), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); @@ -448,34 +463,52 @@ namespace pinocchio } } - void addMimicInfo(const std::string & mimic_name, const std::string & mimiced_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset, JointType jointType) + void addMimicInfo( + const std::string & mimic_name, + const std::string & mimiced_name, + const Vector3 & axis, + const Scalar multiplier, + const Scalar offset, + JointType jointType) { MimicInfo mimic_info(mimiced_name, multiplier, offset, axis, jointType); this->mimicInfo_map.insert(std::make_pair(mimic_name, mimic_info)); } - void convertMimicJoint(const std::string & mimic_name, const MimicInfo & mimic_info) + void convertMimicJoint( + const std::string & mimic_name, const MimicInfo & mimic_info) { - switch(mimic_info.jointType) + switch (mimic_info.jointType) { - case Base::REVOLUTE: - createMimicJoint(mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, mimic_info.offset); + typename JointCollection::JointModelRevoluteUnaligned>( + mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, + mimic_info.offset); break; - case Base::PRISMATIC: - createMimicJoint(mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, mimic_info.offset); - break; + typename JointCollection::JointModelPrismaticUnaligned>( + mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, + mimic_info.offset); + break; - default: - PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); } } template - void createMimicJoint(const std::string & mimic_name, const std::string & mimiced_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset) + void createMimicJoint( + const std::string & mimic_name, + const std::string & mimiced_name, + const Vector3 & axis, + const Scalar multiplier, + const Scalar offset) { auto mimiced_joint = model.joints[getJointId(mimiced_name)]; std::cout << mimiced_joint << std::endl; @@ -485,19 +518,23 @@ namespace pinocchio switch (axisType) { case AXIS_X: - mimic_joint = typename JointCollection::JointModelMimic(TypeX(), mimiced_joint, multiplier, offset); + mimic_joint = + typename JointCollection::JointModelMimic(TypeX(), mimiced_joint, multiplier, offset); break; case AXIS_Y: - mimic_joint = typename JointCollection::JointModelMimic(TypeY(), mimiced_joint, multiplier, offset); + mimic_joint = + typename JointCollection::JointModelMimic(TypeY(), mimiced_joint, multiplier, offset); break; case AXIS_Z: - mimic_joint = typename JointCollection::JointModelMimic(TypeZ(), mimiced_joint, multiplier, offset); + mimic_joint = + typename JointCollection::JointModelMimic(TypeZ(), mimiced_joint, multiplier, offset); break; case AXIS_UNALIGNED: - mimic_joint = typename JointCollection::JointModelMimic(TypeUnaligned(), mimiced_joint, multiplier, offset); + mimic_joint = typename JointCollection::JointModelMimic( + TypeUnaligned(), mimiced_joint, multiplier, offset); break; default: @@ -582,14 +619,14 @@ namespace pinocchio typedef UrdfVisitorBaseTpl UrdfVisitorBase; - PINOCCHIO_PARSERS_DLLAPI void - parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = true); + PINOCCHIO_PARSERS_DLLAPI void parseRootTree( + const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = true); PINOCCHIO_PARSERS_DLLAPI void parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic = true); - PINOCCHIO_PARSERS_DLLAPI void - parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic = true); + PINOCCHIO_PARSERS_DLLAPI void parseRootTreeFromXML( + const std::string & xmlString, UrdfVisitorBase & model, const bool mimic = true); } // namespace details template class JointCollectionTpl> diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 599c10c509..8e87969317 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -62,12 +62,19 @@ namespace boost ar & make_nvp("StU", joint_data.StU()); } - template class JointCollection> - void serialize(Archive & ar, - pinocchio::JointDataBase> & joint_data, - const unsigned int /*version*/) + template< + class Archive, + typename Scalar, + int Options, + template + class JointCollection> + void serialize( + Archive & ar, + pinocchio::JointDataBase> & + joint_data, + const unsigned int /*version*/) { - ar & make_nvp("S",joint_data.S()); + ar & make_nvp("S", joint_data.S()); // ar & make_nvp("M",joint_data.M()); // ar & make_nvp("v",joint_data.v()); // ar & make_nvp("c",joint_data.c()); @@ -76,7 +83,7 @@ namespace boost // ar & make_nvp("Dinv",joint_data.Dinv()); // ar & make_nvp("UDinv",joint_data.UDinv()); } - + } // namespace fix template @@ -249,9 +256,16 @@ namespace boost ar & make_nvp("base_variant", base_object(joint)); } - template typename JointCollection> + template< + class Archive, + typename Scalar, + int Options, + template + typename JointCollection> void serialize( - Archive & ar, pinocchio::JointDataMimicTpl & joint, const unsigned int version) + Archive & ar, + pinocchio::JointDataMimicTpl & joint, + const unsigned int version) { typedef pinocchio::JointDataMimicTpl JointType; fix::serialize(ar, static_cast &>(joint), version); diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 01bc9792e5..f22cc758df 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -289,10 +289,16 @@ namespace boost ar & make_nvp("base_variant", base_object(joint)); } - template + template< + class Archive, + typename Scalar, + int Options, + template class JointCollectionTpl> void serialize( - Archive & ar, pinocchio::JointModelMimicTpl & joint, const unsigned int version) + Archive & ar, + pinocchio::JointModelMimicTpl & joint, + const unsigned int version) { typedef pinocchio::JointModelMimicTpl JointType; // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase diff --git a/src/multibody/sample-models.cpp b/src/multibody/sample-models.cpp index 3db04c7acb..2ad7f24976 100644 --- a/src/multibody/sample-models.cpp +++ b/src/multibody/sample-models.cpp @@ -13,7 +13,8 @@ namespace pinocchio namespace buildModels { template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void - manipulator(context::Model &, bool); + manipulator( + context::Model &, bool); template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void humanoid(context::Model &, bool); diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index 9c753257b5..e247702c58 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -137,23 +137,23 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } - if(joint->mimic && mimic) + if (joint->mimic && mimic) { - max_effort = Vector::Constant(0, infty); + max_effort = Vector::Constant(0, infty); max_velocity = Vector::Constant(0, infty); - min_config = Vector::Constant(0,-infty); - max_config = Vector::Constant(0, infty); + min_config = Vector::Constant(0, -infty); + max_config = Vector::Constant(0, infty); friction = Vector::Constant(0, 0.); damping = Vector::Constant(0, 0.); - - model.addMimicInfo(joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, joint->mimic->offset,UrdfVisitorBase::REVOLUTE); - - model.addJointAndBody(UrdfVisitorBase::MIMIC, axis, - parentFrameId,jointPlacement,joint->name, - Y,link->name, - max_effort,max_velocity,min_config,max_config, - friction,damping); + + model.addMimicInfo( + joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, + joint->mimic->offset, UrdfVisitorBase::REVOLUTE); + + model.addJointAndBody( + UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); } else model.addJointAndBody( @@ -209,24 +209,23 @@ namespace pinocchio friction << joint->dynamics->friction; damping << joint->dynamics->damping; } - if(joint->mimic && mimic) + if (joint->mimic && mimic) { - max_effort = Vector::Constant(0, infty); + max_effort = Vector::Constant(0, infty); max_velocity = Vector::Constant(0, infty); - min_config = Vector::Constant(0,-infty); - max_config = Vector::Constant(0, infty); + min_config = Vector::Constant(0, -infty); + max_config = Vector::Constant(0, infty); friction = Vector::Constant(0, 0.); damping = Vector::Constant(0, 0.); - model.addMimicInfo(joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, joint->mimic->offset,UrdfVisitorBase::PRISMATIC); + model.addMimicInfo( + joint->name, joint->mimic->joint_name, axis, joint->mimic->multiplier, + joint->mimic->offset, UrdfVisitorBase::PRISMATIC); - model.addJointAndBody(UrdfVisitorBase::MIMIC, axis, - parentFrameId,jointPlacement,joint->name, - Y,link->name, - max_effort,max_velocity,min_config,max_config, - friction,damping); - + model.addJointAndBody( + UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, + link->name, max_effort, max_velocity, min_config, max_config, friction, damping); } else model.addJointAndBody( @@ -300,7 +299,8 @@ namespace pinocchio /// \param[in] link The current URDF link. /// \param[in] model The model where the link must be added. /// - void parseRootTree(const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic) + void parseRootTree( + const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic) { model.setName(urdfTree->getName()); @@ -311,7 +311,7 @@ namespace pinocchio { parseTree(child, model, mimic); } - if(mimic) + if (mimic) { for (const auto & entry : model.mimicInfo_map) model.convertMimicJoint(entry.first, entry.second); @@ -330,7 +330,8 @@ namespace pinocchio "contain a valid URDF model."); } - void parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic) + void + parseRootTreeFromXML(const std::string & xmlString, UrdfVisitorBase & model, const bool mimic) { ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDF(xmlString); if (urdfTree) diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index ff0c78d168..de7bdee47a 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -107,19 +107,19 @@ struct init> } }; -template class JointCollection> -struct init > +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimicTpl JointModel; - + typedef pinocchio::JointModelMimicTpl JointModel; + static JointModel run() { - typedef pinocchio::JointModelRevoluteTpl JointModelRX; + typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModelRX jmodel_ref = init::run(); - - JointModel jmodel(jmodel_ref,1.,0.); - jmodel.setIndexes(0,0,0,0); + + JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } }; diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 7120d06b4c..19007d0c81 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -142,14 +142,17 @@ void test_joint_methods( Model::ConfigVectorType vec(Model::ConfigVectorType::Ones(m)); const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m)); - BOOST_CHECK(jmodel.jointConfigFromDofSelector(vec) == jmodel_composite.jointConfigFromDofSelector(vec)); BOOST_CHECK( - jmodel.jointConfigFromDofSelector(vec_const) == jmodel_composite.jointConfigFromDofSelector(vec_const)); - - BOOST_CHECK(jmodel.jointConfigFromNqSelector(vec) == jmodel_composite.jointConfigFromNqSelector(vec)); + jmodel.jointConfigFromDofSelector(vec) == jmodel_composite.jointConfigFromDofSelector(vec)); BOOST_CHECK( - jmodel.jointConfigFromNqSelector(vec_const) == jmodel_composite.jointConfigFromNqSelector(vec_const)); + jmodel.jointConfigFromDofSelector(vec_const) + == jmodel_composite.jointConfigFromDofSelector(vec_const)); + BOOST_CHECK( + jmodel.jointConfigFromNqSelector(vec) == jmodel_composite.jointConfigFromNqSelector(vec)); + BOOST_CHECK( + jmodel.jointConfigFromNqSelector(vec_const) + == jmodel_composite.jointConfigFromNqSelector(vec_const)); BOOST_CHECK(jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec)); BOOST_CHECK( diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 52af7e4db3..f6c9d58ef9 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -218,19 +218,19 @@ struct init> return jmodel; } }; -template class JointCollection> -struct init > +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimicTpl JointModel; - + typedef pinocchio::JointModelMimicTpl JointModel; + static JointModel run() { - typedef pinocchio::JointModelRevoluteTpl JointModelRX; + typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModelRX jmodel_ref = init::run(); - - JointModel jmodel(jmodel_ref,1.,0.); - jmodel.setIndexes(0,0,0,0); - + + JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); + return jmodel; } }; @@ -477,10 +477,9 @@ struct TestJointOperatorEqual test(jdata); } - template class JointCollection> - void operator()(const pinocchio::JointModelMimicTpl & ) const + template class JointCollection> + void operator()(const pinocchio::JointModelMimicTpl &) const { - } template diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 67fc9ac22a..821479f2f8 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -12,8 +12,7 @@ using namespace pinocchio; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) -typedef Eigen::Matrix Matrix6x; - +typedef Eigen::Matrix Matrix6x; template void test_constraint_mimic(const JointModelBase & jmodel) @@ -23,87 +22,86 @@ void test_constraint_mimic(const JointModelBase & jmodel) typedef typename traits::JointDataDerived JointData; typedef ScaledJointMotionSubspaceTpl ScaledConstraint; typedef JointMotionSubspaceTpl ConstraintRef; - + JointData jdata = jmodel.createData(); - + const double scaling_factor = 2.; ConstraintRef constraint_ref(jdata.S.matrix()), constraint_ref_shared(jdata.S.matrix()); - ScaledConstraint scaled_constraint(constraint_ref_shared,scaling_factor); - + ScaledConstraint scaled_constraint(constraint_ref_shared, scaling_factor); + BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv()); - + typedef typename JointModel::TangentVector_t TangentVector_t; TangentVector_t v = TangentVector_t::Random(); - + Motion m = scaled_constraint * v; Motion m_ref = scaling_factor * (Motion)(constraint_ref * v); - + BOOST_CHECK(m.isApprox(m_ref)); - + { SE3 M = SE3::Random(); typename ScaledConstraint::DenseBase S = M.act(scaled_constraint); typename ScaledConstraint::DenseBase S_ref = scaling_factor * M.act(constraint_ref); - + BOOST_CHECK(S.isApprox(S_ref)); } - + { typename ScaledConstraint::DenseBase S = scaled_constraint.matrix(); typename ScaledConstraint::DenseBase S_ref = scaling_factor * constraint_ref.matrix(); - + BOOST_CHECK(S.isApprox(S_ref)); } - + { Motion v = Motion::Random(); typename ScaledConstraint::DenseBase S = v.cross(scaled_constraint); typename ScaledConstraint::DenseBase S_ref = scaling_factor * v.cross(constraint_ref); - + BOOST_CHECK(S.isApprox(S_ref)); } - + // Test transpose operations { const Eigen::DenseIndex dim = 20; - const Matrix6x Fin = Matrix6x::Random(6,dim); + const Matrix6x Fin = Matrix6x::Random(6, dim); Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin; Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin); BOOST_CHECK(Fout.isApprox(Fout_ref)); - + Force force_in(Force::Random()); Eigen::MatrixXd Stf = (scaled_constraint.transpose() * force_in); Eigen::MatrixXd Stf_ref = scaling_factor * (constraint_ref.transpose() * force_in); BOOST_CHECK(Stf_ref.isApprox(Stf)); } - + // CRBA Y*S { Inertia Y = Inertia::Random(); Eigen::MatrixXd YS = Y * scaled_constraint; Eigen::MatrixXd YS_ref = scaling_factor * (Y * constraint_ref); - + BOOST_CHECK(YS.isApprox(YS_ref)); } - } struct TestJointConstraint { - - template + + template void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0,0,0,0); - + jmodel.setIndexes(0, 0, 0, 0); + test_constraint_mimic(jmodel); } - + void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0,0,0,0); + jmodel.setIndexes(0, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -111,23 +109,20 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0,0,0,0); + jmodel.setIndexes(0, 0, 0, 0); test_constraint_mimic(jmodel); } - }; BOOST_AUTO_TEST_CASE(test_constraint) { using namespace pinocchio; typedef boost::variant< - JointModelRX, JointModelRY, JointModelRZ - , JointModelRevoluteUnaligned - , JointModelPX, JointModelPY, JointModelPZ - , JointModelPrismaticUnaligned - > Variant; - + JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, + JointModelPY, JointModelPZ, JointModelPrismaticUnaligned> + Variant; + boost::mpl::for_each(TestJointConstraint()); } @@ -136,45 +131,46 @@ void test_joint_mimic(const JointModelBase & jmodel) { typedef typename traits::JointDerived Joint; typedef typename traits::JointDataDerived JointData; - + JointData jdata = jmodel.createData(); - + const double scaling_factor = 1.; const double offset = 0.; - + // test constructor - JointModelMimic jmodel_mimic(jmodel.derived(),scaling_factor,offset); + JointModelMimic jmodel_mimic(jmodel.derived(), scaling_factor, offset); JointDataMimic jdata_mimic = jmodel_mimic.createData(); // Non-const ref accessors trigger asserts, usefull const ref to call const ref accessors... - const JointDataMimic & jdata_mimic_const_ref {jdata_mimic}; - + const JointDataMimic & jdata_mimic_const_ref{jdata_mimic}; + BOOST_CHECK(jmodel_mimic.nq() == 0); BOOST_CHECK(jmodel_mimic.nv() == 0); - + BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q()); BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v()); - + BOOST_CHECK(jmodel_mimic.idx_q() == 0); BOOST_CHECK(jmodel_mimic.idx_v() == 0); - + typedef typename JointModel::ConfigVector_t ConfigVectorType; typedef typename LieGroup::type LieGroupType; - ConfigVectorType q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones()); - - jmodel.calc(jdata,q0); - jmodel_mimic.calc(jdata_mimic,q0); + ConfigVectorType q0 = + LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); + + jmodel.calc(jdata, q0); + jmodel_mimic.calc(jdata_mimic, q0); BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic_const_ref.M_accessor())); BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); typedef typename JointModel::TangentVector_t TangentVectorType; - - q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones()); + + q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); TangentVectorType v0 = TangentVectorType::Random(); - jmodel.calc(jdata,q0,v0); - jmodel_mimic.calc(jdata_mimic,q0,v0); - + jmodel.calc(jdata, q0, v0); + jmodel_mimic.calc(jdata_mimic, q0, v0); + BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic_const_ref.M())); BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic_const_ref.v())); @@ -182,20 +178,20 @@ void test_joint_mimic(const JointModelBase & jmodel) struct TestJointMimic { - - template + + template void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0,0,0,0); - + jmodel.setIndexes(0, 0, 0, 0); + test_joint_mimic(jmodel); } - + void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0,0,0,0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_mimic(jmodel); } @@ -203,23 +199,20 @@ struct TestJointMimic void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0,0,0,0); + jmodel.setIndexes(0, 0, 0, 0); test_joint_mimic(jmodel); } - }; BOOST_AUTO_TEST_CASE(test_joint) { using namespace pinocchio; typedef boost::variant< - JointModelRX, JointModelRY, JointModelRZ - , JointModelRevoluteUnaligned - , JointModelPX, JointModelPY, JointModelPZ - , JointModelPrismaticUnaligned - > Variant; - + JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, + JointModelPY, JointModelPZ, JointModelPrismaticUnaligned> + Variant; + boost::mpl::for_each(TestJointMimic()); } @@ -227,14 +220,14 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_affine) { typedef JointModelRX::ConfigVector_t ConfigVectorType; double scaling = 1., offset = 0.; - + ConfigVectorType q0 = ConfigVectorType::Random(); ConfigVectorType q1; - LinearAffineTransform::run(q0,scaling,offset,q1); + LinearAffineTransform::run(q0, scaling, offset, q1); BOOST_CHECK(q0 == q1); - + offset = 2.; - LinearAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1); + LinearAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); BOOST_CHECK(q1 == ConfigVectorType::Constant(offset)); } @@ -242,32 +235,32 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_revolute) { typedef JointModelRUBX::ConfigVector_t ConfigVectorType; double scaling = 1., offset = 0.; - + ConfigVectorType q0 = ConfigVectorType::Random().normalized(); ConfigVectorType q1; - UnboundedRevoluteAffineTransform::run(q0,scaling,offset,q1); + UnboundedRevoluteAffineTransform::run(q0, scaling, offset, q1); BOOST_CHECK(q0.isApprox(q1)); - + offset = 2.; - UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1); - BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset),math::sin(offset))); + UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); + BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset), math::sin(offset))); } BOOST_AUTO_TEST_CASE(test_joint_generic_cast) { JointModelRX jmodel_ref; - jmodel_ref.setIndexes(1,2,3,3); - - JointModelMimic jmodel(jmodel_ref,2.,1.); - jmodel.setIndexes(1,-1,-1,3); - + jmodel_ref.setIndexes(1, 2, 3, 3); + + JointModelMimic jmodel(jmodel_ref, 2., 1.); + jmodel.setIndexes(1, -1, -1, 3); + BOOST_CHECK(jmodel.id() == jmodel_ref.id()); BOOST_CHECK(jmodel.idx_q() == jmodel_ref.idx_q()); BOOST_CHECK(jmodel.idx_v() == jmodel_ref.idx_v()); - + JointModel jmodel_generic(jmodel); - jmodel_generic.setIndexes(1,-2,-2,3); - + jmodel_generic.setIndexes(1, -2, -2, 3); + BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id()); } -BOOST_AUTO_TEST_SUITE_END() \ No newline at end of file +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index 316793cd9e..1a1a55b625 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -76,8 +76,8 @@ void test_jmodel_nq_against_nq_ref(const JointModelBase & jmodel, co } template class JointCollection> -void test_jmodel_nq_against_nq_ref(const JointModelMimicTpl & jmodel, - const int & nq_ref) +void test_jmodel_nq_against_nq_ref( + const JointModelMimicTpl & jmodel, const int & nq_ref) { BOOST_CHECK(jmodel.jmodel().nq() == nq_ref); } @@ -90,8 +90,14 @@ void test_nv_against_jmodel( BOOST_CHECK(constraint.nv() == jmodel.nv()); } -template class JointCollection, typename ConstraintDerived> -void test_nv_against_jmodel(const JointModelMimicTpl & jmodel, +template< + typename Scalar, + int Options, + template + class JointCollection, + typename ConstraintDerived> +void test_nv_against_jmodel( + const JointModelMimicTpl & jmodel, const JointMotionSubspaceBase & constraint) { BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv()); @@ -269,9 +275,10 @@ void test_constraint_operations(const JointModelBase & jmodel) } template class JointCollection> -void test_constraint_operations(const JointModelMimicTpl & /*jmodel*/) -{ } // Disable test for JointMimic - +void test_constraint_operations( + const JointModelMimicTpl & /*jmodel*/) +{ +} // Disable test for JointMimic template struct init; @@ -389,9 +396,9 @@ struct init> static JointModel run() { - typedef pinocchio::JointModelRevoluteTpl JointModelRX; + typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModelRX jmodel_ref = init::run(); - + JointModel jmodel(jmodel_ref, 1., 0.); jmodel.setIndexes(0, 0, 0, 0); diff --git a/unittest/python/bindings_urdf.py b/unittest/python/bindings_urdf.py index ef48ac58a2..33b77e7e86 100644 --- a/unittest/python/bindings_urdf.py +++ b/unittest/python/bindings_urdf.py @@ -24,13 +24,17 @@ def test_xml(self): with self.model_path.open() as model: file_content = model.read() - model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) + model_ref = pin.buildModelFromUrdf( + self.model_path, pin.JointModelFreeFlyer(), False + ) model = pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer(), False) self.assertEqual(model, model_ref) model_self = pin.Model() - pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer(), model_self, False) + pin.buildModelFromXML( + file_content, pin.JointModelFreeFlyer(), model_self, False + ) self.assertEqual(model_self, model_ref) def test_pickle(self): diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index daba688d3e..5dac584921 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -397,18 +397,18 @@ struct init> } }; -template class JointCollection> -struct init > +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimicTpl JointModel; - + typedef pinocchio::JointModelMimicTpl JointModel; + static JointModel run() { - typedef pinocchio::JointModelRevoluteTpl JointModelRX; + typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModelRX jmodel_ref = init::run(); - - JointModel jmodel(jmodel_ref,1.,0.); - + + JointModel jmodel(jmodel_ref, 1., 0.); + return jmodel; } }; @@ -472,7 +472,7 @@ struct TestJointTransform // Do nothing } - template class JointCollection> + template class JointCollection> void operator()(const pinocchio::JointModelMimicTpl &) { // typedef pinocchio::JointModelMimicTpl JointModelMimic; @@ -484,23 +484,23 @@ struct TestJointTransform // JointModelMimic jmodel_mimic = init::run(); // typedef pinocchio::JointModelRevoluteTpl JointModel; // JointModel jmodel = init::run(); - + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); // JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - + // typedef typename pinocchio::LieGroup::type LieGroupType; // LieGroupType lg; - + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); - + // jmodel_mimic.calc(jdata_mimic,q_random); // // Copy Transform (and don't take ref) as non const ref of mimic buffers trigger assert // Transform m = const_cast(jdata_mimic_base).M(); // test(m); - + // // Copy Constraint (and don't take ref) as non const ref of mimic buffers trigger assert // Constraint S = const_cast(jdata_mimic_base).S(); // test(S); @@ -558,7 +558,7 @@ struct TestJointMotion // Do nothing } - template class JointCollection> + template class JointCollection> void operator()(const pinocchio::JointModelMimicTpl &) { // typedef pinocchio::JointModelMimicTpl JointModelMimic; @@ -570,25 +570,25 @@ struct TestJointMotion // JointModelMimic jmodel_mimic = init::run(); // typedef pinocchio::JointModelRevoluteTpl JointModel; // JointModel jmodel = init::run(); - + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); // JointDataBase & jdata_mimic_base = static_cast(jdata_mimic); - + // typedef typename pinocchio::LieGroup::type LieGroupType; // LieGroupType lg; - + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); // Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); - + // jmodel_mimic.calc(jdata_mimic,q_random,v_random); // // Copy Motion (and don't take ref) as non const ref of mimic buffers trigger assert // Motion m = const_cast(jdata_mimic_base).v(); // test(m); - + // // Copy Bias (and don't take ref) as non const ref of mimic buffers trigger assert // Bias b = const_cast(jdata_mimic_base).c(); // test(b); @@ -661,28 +661,28 @@ struct TestJointData test(jdata); } - template class JointCollection> + template class JointCollection> void operator()(const pinocchio::JointModelMimicTpl &) { // typedef pinocchio::JointModelMimicTpl JointModelMimic; // typedef typename JointModelMimic::JointDerived JointDerived; // typedef typename pinocchio::traits::JointDataDerived JointDataMimic; // JointModelMimic jmodel_mimic = init::run(); - + // typedef pinocchio::JointModelRevoluteTpl JointModel; // JointModel jmodel = init::run(); - + // JointDataMimic jdata_mimic = jmodel_mimic.createData(); // typedef typename pinocchio::LieGroup::type LieGroupType; // LieGroupType lg; - + // Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); // Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); - + // Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); // Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); - + // jmodel_mimic.calc(jdata_mimic,q_random,v_random); // pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); // jmodel_mimic.calc_aba(jdata_mimic, Eigen::VectorXd::Zero(jmodel.nv()), I, false); diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp index c98f7b5041..8b4db0d743 100644 --- a/unittest/visitor.cpp +++ b/unittest/visitor.cpp @@ -253,20 +253,20 @@ struct init> } }; -template class JointCollection> -struct init > +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimicTpl JointModel; - + typedef pinocchio::JointModelMimicTpl JointModel; + static JointModel run(const pinocchio::Model & model) { - typedef pinocchio::JointModelRevoluteTpl JointModelRX; + typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModelRX jmodel_ref = init::run(model); - jmodel_ref.setIndexes(0,0,0,0); - - JointModel jmodel(jmodel_ref,1.,0.); - jmodel.setIndexes(0,0,0,0); - + jmodel_ref.setIndexes(0, 0, 0, 0); + + JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); + return jmodel; } }; From 2fe8b5a0621ca6d38c2e8c96a91f9a6335302c23 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 053/166] Split JointVelocitySelector in two functions --- .../pinocchio/algorithm/aba-derivatives.hxx | 12 +++--- include/pinocchio/algorithm/aba.hxx | 26 ++++++------ .../algorithm/centroidal-derivatives.hxx | 4 +- .../pinocchio/algorithm/compute-all-terms.hxx | 3 +- .../constrained-dynamics-derivatives.hxx | 4 +- .../algorithm/constrained-dynamics.hxx | 28 ++++++------- include/pinocchio/algorithm/delassus.hxx | 4 +- include/pinocchio/algorithm/energy.hxx | 2 +- .../algorithm/kinematics-derivatives.hxx | 2 +- include/pinocchio/algorithm/kinematics.hxx | 2 +- include/pinocchio/algorithm/model.hxx | 32 +++++++------- include/pinocchio/algorithm/pv.hxx | 42 +++++++++---------- include/pinocchio/algorithm/regressor.hxx | 2 +- .../pinocchio/algorithm/rnea-derivatives.hxx | 6 +-- .../rnea-second-order-derivatives.hxx | 2 +- include/pinocchio/algorithm/rnea.hxx | 10 +++-- .../multibody/joint/joint-composite.hpp | 34 +++++++++++++-- .../pinocchio/multibody/joint/joint-mimic.hpp | 20 ++++++++- .../multibody/joint/joint-model-base.hpp | 42 ++++++++++++++++--- .../multibody/joint/joint-translation.hpp | 4 +- .../multibody/liegroup/liegroup-algo.hxx | 10 ++--- include/pinocchio/multibody/model.hxx | 14 +++---- unittest/joint-composite.cpp | 8 +++- 23 files changed, 196 insertions(+), 117 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 37cb503a7a..e129f08b66 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -121,12 +121,12 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; const ColBlock J_cols = jmodel.jointJacCols(data.J); - jmodel.jointVelocitySelector(data.u).noalias() -= J_cols.transpose() * fi.toVector(); + jmodel.jointVelocityFromNvSelector(data.u).noalias() -= J_cols.transpose() * fi.toVector(); jdata.U().noalias() = Ia * J_cols; jdata.StU().noalias() = J_cols.transpose() * jdata.U(); - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -163,7 +163,7 @@ namespace pinocchio Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); fi.toVector().noalias() += - Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -207,11 +207,11 @@ namespace pinocchio ColsBlock J_cols = jmodel.jointJacCols(data.J); oa_gf += data.oa_gf[parent]; - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromNvSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromNvSelector(data.u) - jdata.UDinv().transpose() * oa_gf.toVector(); - oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); + oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocityFromNvSelector(data.ddq); oa = oa_gf + model.gravity; of = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]); diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index 81756748a9..761a563a5a 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -169,13 +169,13 @@ namespace pinocchio Force & fi = data.of[i]; - jmodel.jointVelocitySelector(data.u).noalias() -= Jcols.transpose() * fi.toVector(); + jmodel.jointVelocityFromDofSelector(data.u).noalias() -= Jcols.transpose() * fi.toVector(); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromDofSelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -185,7 +185,7 @@ namespace pinocchio Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); fi.toVector().noalias() += - Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -220,10 +220,10 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.oa_gf[i] += data.oa_gf[parent]; // does take into account the gravity field - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromDofSelector(data.u) - jdata.UDinv().transpose() * data.oa_gf[i].toVector(); - data.oa_gf[i].toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); + data.oa_gf[i].toVector().noalias() += J_cols * jmodel.jointVelocityFromDofSelector(data.ddq); // Handle consistent output data.oa[i] = data.oa_gf[i] + model.gravity; @@ -423,15 +423,15 @@ namespace pinocchio const JointIndex parent = model.parents[i]; typename Inertia::Matrix6 & Ia = data.Yaba[i]; - jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityFromDofSelector(model.armature), Ia, parent > 0); if (parent > 0) { Force & pa = data.f[i]; pa.toVector().noalias() += - Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -461,10 +461,10 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromDofSelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); data.a[i] = data.a_gf[i]; data.a[i].linear().noalias() += data.oMi[i].rotation().transpose() * model.gravity.linear(); @@ -680,7 +680,7 @@ namespace pinocchio jdata.StU().noalias() = J_cols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromDofSelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index ce85aad3e8..4047ca92e5 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -78,7 +78,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -168,7 +168,7 @@ namespace pinocchio ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // tau - jmodel.jointVelocitySelector(data.tau).noalias() = + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = J_cols.transpose() * data.of[i].toVector(); // dtau/da similar to data.M diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 9af8717ec3..c4c32bba54 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -127,7 +127,7 @@ namespace pinocchio jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.nle) += jdata.S().transpose() * data.f[i]; data.oYcrb[parent] += data.oYcrb[i]; data.doYcrb[parent] += data.doYcrb[i]; @@ -160,6 +160,7 @@ namespace pinocchio typedef DataTpl Data; + data.nle.setZero(); data.v[0].setZero(); data.a[0].setZero(); data.h[0].setZero(); diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index a9e0334908..695bad7328 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -82,7 +82,7 @@ namespace pinocchio ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); const typename Data::TangentVectorType & a = data.ddq; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) data.a[i] += data.liMi[i].actInv(data.a[parent]); oa = data.oMi[i].act(data.a[i]); @@ -102,7 +102,7 @@ namespace pinocchio Motion & odvparent = data.oa[parent]; const typename Data::TangentVectorType & dimpulse = data.ddq; // Temporary calculation of J(dq_after) - odv = J_cols * jmodel.jointVelocitySelector(dimpulse); + odv = J_cols * jmodel.jointVelocityFromDofSelector(dimpulse); if (parent > 0) odv += odvparent; motionSet::motionAction(odvparent, J_cols, dAdq_cols); diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index 5ce44e30b8..f88ea81777 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -172,7 +172,7 @@ namespace pinocchio if (ContactMode) { - jmodel.jointVelocitySelector(data.nle).noalias() = + jmodel.jointVelocityFromNvSelector(data.nle).noalias() = J_cols.transpose() * data.of[i].toVector(); data.of[parent] += data.of[i]; } @@ -624,14 +624,14 @@ namespace pinocchio const Force & fi = data.of[i]; fi_augmented += fi; - jmodel.jointVelocitySelector(data.u).noalias() = - jmodel.jointVelocitySelector(tau) - Jcols.transpose() * fi_augmented.toVector(); + jmodel.jointVelocityFromNvSelector(data.u).noalias() = + jmodel.jointVelocityFromNvSelector(tau) - Jcols.transpose() * fi_augmented.toVector(); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -641,7 +641,7 @@ namespace pinocchio Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); fi_augmented.toVector().noalias() += - Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); data.oYaba[parent] += Ia; data.of_augmented[parent] += fi_augmented; } @@ -686,13 +686,13 @@ namespace pinocchio const Force & fi = data.of[i]; fi_augmented += fi; - jmodel.jointVelocitySelector(data.u).noalias() = - jmodel.jointVelocitySelector(tau) - Jcols.transpose() * fi_augmented.toVector(); + jmodel.jointVelocityFromNvSelector(data.u).noalias() = + jmodel.jointVelocityFromNvSelector(tau) - Jcols.transpose() * fi_augmented.toVector(); if (parent > 0) { fi_augmented.toVector().noalias() += - Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); data.of_augmented[parent] += fi_augmented; } } @@ -728,10 +728,10 @@ namespace pinocchio if (parent > 0) data.oa_augmented[i] += data.oa_augmented[parent]; // does not take into account the gravity field - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocitySelector(data.u) + jmodel.jointVelocityFromNvSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityFromNvSelector(data.u) - jdata.UDinv().transpose() * data.oa_augmented[i].toVector(); - data.oa_augmented[i].toVector() += Jcols * jmodel.jointVelocitySelector(data.ddq); + data.oa_augmented[i].toVector() += Jcols * jmodel.jointVelocityFromNvSelector(data.ddq); } }; @@ -1090,7 +1090,7 @@ namespace pinocchio data.of[parent] += data.of[i]; // subtract the bias forces from the torque to get Mv_dot_free - jmodel.jointVelocitySelector(data.tau).noalias() -= + jmodel.jointVelocityFromNvSelector(data.tau).noalias() -= jmodel.jointJacCols(data.J).transpose() * (data.of[i].toVector()); data.of_augmented[i].toVector().setZero(); } @@ -1115,7 +1115,7 @@ namespace pinocchio const JointModel & jmodel = model.joints[i]; data.oa_augmented[i].toVector().noalias() = data.oa_augmented[model.parents[i]].toVector() - + jmodel.jointJacCols(data.J) * jmodel.jointVelocitySelector(data.u); + + jmodel.jointJacCols(data.J) * jmodel.jointVelocityFromNvSelector(data.u); data.of_augmented[i].toVector().setZero(); } } @@ -1158,7 +1158,7 @@ namespace pinocchio const JointModel & jmodel = model.joints[i]; data.of_augmented[parent] += data.of_augmented[i]; - jmodel.jointVelocitySelector(data.tau).noalias() = + jmodel.jointVelocityFromNvSelector(data.tau).noalias() = -jmodel.jointJacCols(data.J).transpose() * (data.of_augmented[i].toVector()); } } diff --git a/include/pinocchio/algorithm/delassus.hxx b/include/pinocchio/algorithm/delassus.hxx index 4275c65510..5b07052aec 100644 --- a/include/pinocchio/algorithm/delassus.hxx +++ b/include/pinocchio/algorithm/delassus.hxx @@ -87,7 +87,7 @@ namespace pinocchio jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); @@ -443,7 +443,7 @@ namespace pinocchio jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx index a0bd50208a..4714ca7cd4 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -30,7 +30,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); data.kinetic_energy += - (jmodel.jointVelocitySelector(model.armature).array() * jdata.joint_v().array().square()) + (jmodel.jointVelocityFromDofSelector(model.armature).array() * jdata.joint_v().array().square()) .sum(); } }; diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index 6e0ba1fa12..a44324b3ae 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -76,7 +76,7 @@ namespace pinocchio if (parent > 0) vi += data.liMi[i].actInv(data.v[parent]); - ai = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (vi ^ jdata.v()); + ai = jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (vi ^ jdata.v()); if (parent > 0) ai += data.liMi[i].actInv(data.a[parent]); diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index fc7230cc6f..e2cb1dd3d1 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -233,7 +233,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); data.a[i] += data.liMi[i].actInv(data.a[parent]); } }; diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index dad5465d0a..e64a51a119 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -209,24 +209,24 @@ namespace pinocchio parent_id, jmodel_inter, // Use the intermediate joint (jmodel_inter) with updates id, idx, ... pMi * modelAB.jointPlacements[joint_id_in], modelAB.names[joint_id_in], - jmodel_in.jointVelocitySelector( + jmodel_in.jointVelocityFromNvSelector( modelAB .effortLimit), // Need to select the vector base on the origin idxs, so use jmodel_in. - jmodel_in.jointVelocitySelector(modelAB.velocityLimit), + jmodel_in.jointVelocityFromNvSelector(modelAB.velocityLimit), jmodel_in.jointConfigFromNqSelector(modelAB.lowerPositionLimit), jmodel_in.jointConfigFromNqSelector(modelAB.upperPositionLimit), - jmodel_in.jointVelocitySelector(modelAB.friction), - jmodel_in.jointVelocitySelector(modelAB.damping)); + jmodel_in.jointVelocityFromNvSelector(modelAB.friction), + jmodel_in.jointVelocityFromNvSelector(modelAB.damping)); assert(joint_id_out < model.joints.size()); model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]); typename Model::JointModel & jmodel_out = model.joints[joint_id_out]; - jmodel_out.jointVelocitySelector(model.rotorInertia) = - jmodel_in.jointVelocitySelector(modelAB.rotorInertia); - jmodel_out.jointVelocitySelector(model.rotorGearRatio) = - jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio); + jmodel_out.jointVelocityFromNvSelector(model.rotorInertia) = + jmodel_in.jointVelocityFromNvSelector(modelAB.rotorInertia); + jmodel_out.jointVelocityFromNvSelector(model.rotorGearRatio) = + jmodel_in.jointVelocityFromNvSelector(modelAB.rotorGearRatio); // Add all frames whose parent is this joint. for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) @@ -642,22 +642,22 @@ namespace pinocchio JointIndex reduced_joint_id = reduced_model.addJoint( reduced_parent_joint_index, joint_input_model, parent_frame_placement * input_model.jointPlacements[joint_id], joint_name, - joint_input_model.jointVelocitySelector(input_model.effortLimit), - joint_input_model.jointVelocitySelector(input_model.velocityLimit), + joint_input_model.jointVelocityFromNvSelector(input_model.effortLimit), + joint_input_model.jointVelocityFromNvSelector(input_model.velocityLimit), joint_input_model.jointConfigFromNqSelector(input_model.lowerPositionLimit), joint_input_model.jointConfigFromNqSelector(input_model.upperPositionLimit), - joint_input_model.jointVelocitySelector(input_model.friction), - joint_input_model.jointVelocitySelector(input_model.damping)); + joint_input_model.jointVelocityFromNvSelector(input_model.friction), + joint_input_model.jointVelocityFromNvSelector(input_model.damping)); // Append inertia reduced_model.appendBodyToJoint( reduced_joint_id, input_model.inertias[joint_id], SE3::Identity()); // Copy other kinematics and dynamics properties const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id]; - jmodel_out.jointVelocitySelector(reduced_model.rotorInertia) = - joint_input_model.jointVelocitySelector(input_model.rotorInertia); - jmodel_out.jointVelocitySelector(reduced_model.rotorGearRatio) = - joint_input_model.jointVelocitySelector(input_model.rotorGearRatio); + jmodel_out.jointVelocityFromNvSelector(reduced_model.rotorInertia) = + joint_input_model.jointVelocityFromNvSelector(input_model.rotorInertia); + jmodel_out.jointVelocityFromNvSelector(reduced_model.rotorGearRatio) = + joint_input_model.jointVelocityFromNvSelector(input_model.rotorGearRatio); } } diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx index eacc35523a..51deff8c9d 100644 --- a/include/pinocchio/algorithm/pv.hxx +++ b/include/pinocchio/algorithm/pv.hxx @@ -183,16 +183,16 @@ namespace pinocchio bias_and_force.toVector() -= data.Yaba[i] * data.a_bias[i].toVector(); - jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityFromNvSelector(model.armature), Ia, parent > 0); Force & pa = data.f[i]; if (parent > 0) { pa.toVector().noalias() += - Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -228,16 +228,16 @@ namespace pinocchio bias_and_force.toVector() -= data.Yaba[i] * data.a_bias[i].toVector(); - jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityFromDofSelector(model.armature), Ia, parent > 0); Force & pa = data.f[i]; if (parent > 0) { pa.toVector() += - Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -257,13 +257,13 @@ namespace pinocchio for (int ind = 0; ind < data.constraints_supported_dim[i]; ind++) { // Abusing previously unused data.tau for a role unrelated to its name below - jmodel.jointVelocitySelector(data.tau).noalias() = jdata.Dinv() * data.KAS[i].col(ind); + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = jdata.Dinv() * data.KAS[i].col(ind); for (int ind2 = ind; ind2 < data.constraints_supported_dim[i]; ind2++) { data.LA[parent](data.par_cons_ind[i] + ind2, data.par_cons_ind[i] + ind) = data.LA[i](ind2, ind) - + (data.KAS[i].col(ind2).dot(jmodel.jointVelocitySelector(data.tau))); + + (data.KAS[i].col(ind2).dot(jmodel.jointVelocityFromDofSelector(data.tau))); } } @@ -271,11 +271,11 @@ namespace pinocchio if (data.constraints_supported_dim[i] > 0) { // Abusing previously unused data.tau variable for a role unrelated to its name below - jmodel.jointVelocitySelector(data.tau).noalias() = + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = (jdata.Dinv() - * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocitySelector(data.u))) + * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocityFromDofSelector(data.u))) .eval(); - const Motion a_bf = jdata.S() * jmodel.jointVelocitySelector(data.tau); + const Motion a_bf = jdata.S() * jmodel.jointVelocityFromDofSelector(data.tau); const Motion a_bf_motion = a_bf + data.a_bias[i]; for (int ind = 0; ind < data.constraints_supported_dim[i]; ind++) { @@ -310,9 +310,9 @@ namespace pinocchio const JointIndex parent = model.parents[i]; // Abusing otherwise unused data.tau below. - jmodel.jointVelocitySelector(data.tau).noalias() = jdata.S().transpose() * data.f[i]; - jmodel.jointVelocitySelector(data.u) -= jmodel.jointVelocitySelector(data.tau); - data.f[i].toVector().noalias() -= jdata.UDinv() * jmodel.jointVelocitySelector(data.tau); + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromNvSelector(data.u) -= jmodel.jointVelocityFromNvSelector(data.tau); + data.f[i].toVector().noalias() -= jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.tau); if (parent > 0) { @@ -343,11 +343,11 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a[i] = data.liMi[i].actInv(data.a[parent]) + data.a_bias[i]; - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocitySelector(data.u)) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocityFromDofSelector(data.u)) - jdata.UDinv().transpose() * data.a[i].toVector(); - data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); } }; @@ -373,18 +373,18 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a[i] = data.liMi[i].actInv(data.a[parent]) + data.a_bias[i]; - jmodel.jointVelocitySelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocitySelector(data.u)) + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocityFromDofSelector(data.u)) - jdata.UDinv().transpose() * data.a[i].toVector(); data.lambdaA[i].noalias() = data.lambdaA[parent].segment(data.par_cons_ind[i], data.lambdaA[i].size()); for (int j = 0; j < data.constraints_supported_dim[i]; j++) { - jmodel.jointVelocitySelector(data.ddq).noalias() -= + jmodel.jointVelocityFromDofSelector(data.ddq).noalias() -= data.lambdaA[i][j] * jdata.Dinv() * (data.KAS[i].col(j)); } - data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); } }; diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index 88f7cffa51..5c85c42537 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -377,7 +377,7 @@ namespace pinocchio data.v[i] += data.liMi[i].actInv(data.v[parent]); data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(a); data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); } }; diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index ebaf0a855c..793971d4bd 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -128,7 +128,7 @@ namespace pinocchio gravity_partial_dq_.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = Ag_cols.transpose() * data.dAdq.col(j); - jmodel.jointVelocitySelector(g).noalias() = J_cols.transpose() * data.of[i].toVector(); + jmodel.jointVelocityFromDofSelector(g).noalias() = J_cols.transpose() * data.of[i].toVector(); if (parent > 0) { data.oYcrb[parent] += data.oYcrb[i]; @@ -291,7 +291,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -404,7 +404,7 @@ namespace pinocchio MatrixType3 & rnea_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da); // tau - jmodel.jointVelocitySelector(data.tau).noalias() = + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = J_cols.transpose() * data.of[i].toVector(); const Eigen::DenseIndex nv_subtree = data.nvSubtree[i]; diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index 64202d4eaa..bfefbd52c5 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -93,7 +93,7 @@ namespace pinocchio ov, psid_cols, psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs ov += vJ; - oa += (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c()); + oa += (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c()); motionSet::motionAction(ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i)) // + vJ Composite rigid body inertia Inertia & oY = data.oYcrb[i]; diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 98992a4043..be1ed9c76e 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -65,7 +65,7 @@ namespace pinocchio data.v[i] += data.liMi[i].actInv(data.v[parent]); data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); - data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(a); data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); // // data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]); @@ -99,7 +99,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - jmodel.jointVelocitySelector(data.tau) += jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.tau) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); @@ -280,7 +280,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(data.nle) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); } @@ -307,6 +307,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.nle.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -388,7 +389,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - jmodel.jointVelocitySelector(g) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(g) = jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); } @@ -412,6 +413,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + data.g.setZero(); data.a_gf[0] = -model.gravity; typedef ComputeGeneralizedGravityForwardStep< diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index b06f207d44..f56bafc927 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -503,13 +503,26 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector(Eigen::MatrixBase & a) const + jointVelocityFromDofSelector(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromNvSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } @@ -567,13 +580,26 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector_impl(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return a.segment(Base::i_v, nv()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector_impl(Eigen::MatrixBase & a) const + jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index c4919db555..3ae7228631 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -1042,7 +1042,7 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector_impl(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); } @@ -1050,11 +1050,27 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector_impl(Eigen::MatrixBase & a) const + jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); } + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); + } + /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ // Const access template diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index 8aad900677..cc606149b9 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -344,14 +344,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector(const Eigen::MatrixBase & a) const { - return derived().jointVelocitySelector_impl(a.derived()); + return derived().jointVelocityFromDofSelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocitySelector_impl(const Eigen::MatrixBase & a) const + jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nv()); } @@ -359,14 +359,44 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector(Eigen::MatrixBase & a) const + jointVelocityFromDofSelector(Eigen::MatrixBase & a) const { - return derived().jointVelocitySelector_impl(a.derived()); + return derived().jointVelocityFromDofSelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocitySelector_impl(Eigen::MatrixBase & a) const + jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_v(), nv()); + } + + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector(const Eigen::MatrixBase & a) const + { + return derived().jointVelocityFromNvSelector_impl(a.derived()); + } + + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), idx_v(), nv()); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromNvSelector(Eigen::MatrixBase & a) const + { + return derived().jointVelocityFromNvSelector_impl(a.derived()); + } + + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nv()); } diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index cdab24e1ef..0e67aa81a2 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -608,7 +608,7 @@ namespace pinocchio calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase & vs) const { - data.joint_v = this->jointVelocitySelector(vs); + data.joint_v = this->jointVelocityFromDofSelector(vs); data.v.linear() = data.joint_v; } @@ -620,7 +620,7 @@ namespace pinocchio { calc(data, qs.derived()); - data.joint_v = this->jointVelocitySelector(vs); + data.joint_v = this->jointVelocityFromDofSelector(vs); data.v.linear() = data.joint_v; } diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index 9121e2cfcb..f9a8d92926 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -197,7 +197,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.integrate( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -243,7 +243,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrate( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); } }; @@ -298,7 +298,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacRows(mat_in.derived()), jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); } @@ -347,7 +347,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; @@ -481,7 +481,7 @@ namespace pinocchio lgo.difference( jmodel.jointConfigFromNqSelector(q0.derived()), jmodel.jointConfigFromNqSelector(q1.derived()), - jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); + jmodel.jointVelocityFromNvSelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); } }; diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index 2162654ce2..243c53f303 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -128,24 +128,24 @@ namespace pinocchio idx_js.push_back(joint_idx_j); effortLimit.conservativeResize(nv); - jmodel.jointVelocitySelector(effortLimit) = max_effort; + jmodel.jointVelocityFromNvSelector(effortLimit) = max_effort; velocityLimit.conservativeResize(nv); - jmodel.jointVelocitySelector(velocityLimit) = max_velocity; + jmodel.jointVelocityFromNvSelector(velocityLimit) = max_velocity; lowerPositionLimit.conservativeResize(nq); jmodel.jointConfigFromNqSelector(lowerPositionLimit) = min_config; upperPositionLimit.conservativeResize(nq); jmodel.jointConfigFromNqSelector(upperPositionLimit) = max_config; armature.conservativeResize(nv); - jmodel.jointVelocitySelector(armature).setZero(); + jmodel.jointVelocityFromNvSelector(armature).setZero(); rotorInertia.conservativeResize(nv); - jmodel.jointVelocitySelector(rotorInertia).setZero(); + jmodel.jointVelocityFromNvSelector(rotorInertia).setZero(); rotorGearRatio.conservativeResize(nv); - jmodel.jointVelocitySelector(rotorGearRatio).setOnes(); + jmodel.jointVelocityFromNvSelector(rotorGearRatio).setOnes(); friction.conservativeResize(nv); - jmodel.jointVelocitySelector(friction) = joint_friction; + jmodel.jointVelocityFromNvSelector(friction) = joint_friction; damping.conservativeResize(nv); - jmodel.jointVelocitySelector(damping) = joint_damping; + jmodel.jointVelocityFromNvSelector(damping) = joint_damping; // Init and add joint index to its parent subtrees. subtrees.push_back(IndexVector(1)); diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 19007d0c81..c4c428fca2 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -154,9 +154,13 @@ void test_joint_methods( jmodel.jointConfigFromNqSelector(vec_const) == jmodel_composite.jointConfigFromNqSelector(vec_const)); - BOOST_CHECK(jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec)); + BOOST_CHECK(jmodel.jointVelocityFromNvSelector(vec) == jmodel_composite.jointVelocityFromNvSelector(vec)); BOOST_CHECK( - jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const)); + jmodel.jointVelocityFromNvSelector(vec_const) == jmodel_composite.jointVelocityFromNvSelector(vec_const)); + + BOOST_CHECK(jmodel.jointVelocityFromDofSelector(vec) == jmodel_composite.jointVelocityFromDofSelector(vec)); + BOOST_CHECK( + jmodel.jointVelocityFromDofSelector(vec_const) == jmodel_composite.jointVelocityFromDofSelector(vec_const)); BOOST_CHECK(jmodel.jointVelCols(mat) == jmodel_composite.jointVelCols(mat)); BOOST_CHECK(jmodel.jointVelCols(mat_const) == jmodel_composite.jointVelCols(mat_const)); From 167c23f17cc5218323a8acbd599fffed3e8093fa Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 054/166] Apply pre-commit --- include/pinocchio/algorithm/aba-derivatives.hxx | 4 ++-- include/pinocchio/algorithm/aba.hxx | 11 ++++++----- include/pinocchio/algorithm/energy.hxx | 6 +++--- include/pinocchio/algorithm/pv.hxx | 14 ++++++++------ include/pinocchio/algorithm/rnea-derivatives.hxx | 3 ++- .../algorithm/rnea-second-order-derivatives.hxx | 3 ++- .../pinocchio/multibody/liegroup/liegroup-algo.hxx | 13 ++++++++----- unittest/joint-composite.cpp | 12 ++++++++---- 8 files changed, 39 insertions(+), 27 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index e129f08b66..1d2198a740 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -162,8 +162,8 @@ namespace pinocchio { Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); - fi.toVector().noalias() += - Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); + fi.toVector().noalias() += Ia * data.oa_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index 761a563a5a..fc75b85cd1 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -184,8 +184,8 @@ namespace pinocchio { Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); - fi.toVector().noalias() += - Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); + fi.toVector().noalias() += Ia * data.oa_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -223,7 +223,8 @@ namespace pinocchio jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = jdata.Dinv() * jmodel.jointVelocityFromDofSelector(data.u) - jdata.UDinv().transpose() * data.oa_gf[i].toVector(); - data.oa_gf[i].toVector().noalias() += J_cols * jmodel.jointVelocityFromDofSelector(data.ddq); + data.oa_gf[i].toVector().noalias() += + J_cols * jmodel.jointVelocityFromDofSelector(data.ddq); // Handle consistent output data.oa[i] = data.oa_gf[i] + model.gravity; @@ -430,8 +431,8 @@ namespace pinocchio if (parent > 0) { Force & pa = data.f[i]; - pa.toVector().noalias() += - Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); + pa.toVector().noalias() += Ia * data.a_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx index 4714ca7cd4..b321be13ca 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -29,9 +29,9 @@ namespace pinocchio { const JointIndex & i = jmodel.id(); data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); - data.kinetic_energy += - (jmodel.jointVelocityFromDofSelector(model.armature).array() * jdata.joint_v().array().square()) - .sum(); + data.kinetic_energy += (jmodel.jointVelocityFromDofSelector(model.armature).array() + * jdata.joint_v().array().square()) + .sum(); } }; diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx index 51deff8c9d..9ffcd16b9a 100644 --- a/include/pinocchio/algorithm/pv.hxx +++ b/include/pinocchio/algorithm/pv.hxx @@ -191,8 +191,8 @@ namespace pinocchio if (parent > 0) { - pa.toVector().noalias() += - Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); + pa.toVector().noalias() += Ia * data.a_bias[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -236,8 +236,8 @@ namespace pinocchio if (parent > 0) { - pa.toVector() += - Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); + pa.toVector() += Ia * data.a_bias[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -257,7 +257,8 @@ namespace pinocchio for (int ind = 0; ind < data.constraints_supported_dim[i]; ind++) { // Abusing previously unused data.tau for a role unrelated to its name below - jmodel.jointVelocityFromDofSelector(data.tau).noalias() = jdata.Dinv() * data.KAS[i].col(ind); + jmodel.jointVelocityFromDofSelector(data.tau).noalias() = + jdata.Dinv() * data.KAS[i].col(ind); for (int ind2 = ind; ind2 < data.constraints_supported_dim[i]; ind2++) { @@ -312,7 +313,8 @@ namespace pinocchio // Abusing otherwise unused data.tau below. jmodel.jointVelocityFromDofSelector(data.tau).noalias() = jdata.S().transpose() * data.f[i]; jmodel.jointVelocityFromNvSelector(data.u) -= jmodel.jointVelocityFromNvSelector(data.tau); - data.f[i].toVector().noalias() -= jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.tau); + data.f[i].toVector().noalias() -= + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.tau); if (parent > 0) { diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index 793971d4bd..11376912d9 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -128,7 +128,8 @@ namespace pinocchio gravity_partial_dq_.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = Ag_cols.transpose() * data.dAdq.col(j); - jmodel.jointVelocityFromDofSelector(g).noalias() = J_cols.transpose() * data.of[i].toVector(); + jmodel.jointVelocityFromDofSelector(g).noalias() = + J_cols.transpose() * data.of[i].toVector(); if (parent > 0) { data.oYcrb[parent] += data.oYcrb[i]; diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index bfefbd52c5..ef6b606b0a 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -93,7 +93,8 @@ namespace pinocchio ov, psid_cols, psidd_cols); // This ov here is v(p(i)) , psi_dotdot calcs ov += vJ; - oa += (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c()); + oa += + (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c()); motionSet::motionAction(ov, J_cols, dJ_cols); // This here is phi_dot, here ov used is v(p(i)) // + vJ Composite rigid body inertia Inertia & oY = data.oYcrb[i]; diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index f9a8d92926..d5dfd2b6d2 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -197,7 +197,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.integrate( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -243,7 +244,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrate( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); } }; @@ -298,8 +300,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), - jmodel.jointJacRows(mat_in.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacRows(mat_in.derived()), jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); } }; @@ -347,7 +349,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigFromNqSelector(q.derived()), jmodel.jointVelocityFromNvSelector(v.derived()), + jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index c4c428fca2..6440a87741 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -154,13 +154,17 @@ void test_joint_methods( jmodel.jointConfigFromNqSelector(vec_const) == jmodel_composite.jointConfigFromNqSelector(vec_const)); - BOOST_CHECK(jmodel.jointVelocityFromNvSelector(vec) == jmodel_composite.jointVelocityFromNvSelector(vec)); BOOST_CHECK( - jmodel.jointVelocityFromNvSelector(vec_const) == jmodel_composite.jointVelocityFromNvSelector(vec_const)); + jmodel.jointVelocityFromNvSelector(vec) == jmodel_composite.jointVelocityFromNvSelector(vec)); + BOOST_CHECK( + jmodel.jointVelocityFromNvSelector(vec_const) + == jmodel_composite.jointVelocityFromNvSelector(vec_const)); - BOOST_CHECK(jmodel.jointVelocityFromDofSelector(vec) == jmodel_composite.jointVelocityFromDofSelector(vec)); BOOST_CHECK( - jmodel.jointVelocityFromDofSelector(vec_const) == jmodel_composite.jointVelocityFromDofSelector(vec_const)); + jmodel.jointVelocityFromDofSelector(vec) == jmodel_composite.jointVelocityFromDofSelector(vec)); + BOOST_CHECK( + jmodel.jointVelocityFromDofSelector(vec_const) + == jmodel_composite.jointVelocityFromDofSelector(vec_const)); BOOST_CHECK(jmodel.jointVelCols(mat) == jmodel_composite.jointVelCols(mat)); BOOST_CHECK(jmodel.jointVelCols(mat_const) == jmodel_composite.jointVelCols(mat_const)); From 185ff558966b4878106afd91de82c6ce6c8a1fbf Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 055/166] [EtienneAr feedback] Add more test for joint mimic in non trivial cases --- unittest/joint-mimic.cpp | 43 +++++++++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 16 deletions(-) diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 821479f2f8..8b05d47a9d 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -120,13 +120,15 @@ BOOST_AUTO_TEST_CASE(test_constraint) using namespace pinocchio; typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, - JointModelPY, JointModelPZ, JointModelPrismaticUnaligned> + JointModelPY, JointModelPZ, JointModelPrismaticUnaligned + //, JointModelRUBX, JointModelRUBY, JointModelRUBZ + > Variant; boost::mpl::for_each(TestJointConstraint()); } -template +template void test_joint_mimic(const JointModelBase & jmodel) { typedef typename traits::JointDerived Joint; @@ -134,8 +136,8 @@ void test_joint_mimic(const JointModelBase & jmodel) JointData jdata = jmodel.createData(); - const double scaling_factor = 1.; - const double offset = 0.; + const double scaling_factor = 2.; + const double offset = 1.; // test constructor JointModelMimic jmodel_mimic(jmodel.derived(), scaling_factor, offset); @@ -157,25 +159,30 @@ void test_joint_mimic(const JointModelBase & jmodel) typedef typename LieGroup::type LieGroupType; ConfigVectorType q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); + ConfigVectorType q0_mimic; + MimicConfigurationTransform::run(q0, scaling_factor, offset, q0_mimic); - jmodel.calc(jdata, q0); + jmodel.calc(jdata, q0_mimic); jmodel_mimic.calc(jdata_mimic, q0); - BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic_const_ref.M_accessor())); - BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); + BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); + BOOST_CHECK((scaling_factor * jdata.S.matrix()).isApprox(jdata_mimic.S.matrix())); typedef typename JointModel::TangentVector_t TangentVectorType; q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(), ConfigVectorType::Ones()); + MimicConfigurationTransform::run(q0, scaling_factor, offset, q0_mimic); TangentVectorType v0 = TangentVectorType::Random(); - jmodel.calc(jdata, q0, v0); + TangentVectorType v0_mimic = v0 * scaling_factor; + jmodel.calc(jdata, q0_mimic, v0_mimic); jmodel_mimic.calc(jdata_mimic, q0, v0); - BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic_const_ref.M())); - BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); - BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic_const_ref.v())); + BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); + BOOST_CHECK((scaling_factor * jdata.S.matrix()).isApprox(jdata_mimic.S.matrix())); + BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v())); } +template struct TestJointMimic { @@ -185,7 +192,7 @@ struct TestJointMimic JointModel jmodel; jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic(jmodel); } void operator()(const JointModelBase &) const @@ -193,7 +200,7 @@ struct TestJointMimic JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic(jmodel); } void operator()(const JointModelBase &) const @@ -201,7 +208,7 @@ struct TestJointMimic JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic(jmodel); } }; @@ -211,9 +218,13 @@ BOOST_AUTO_TEST_CASE(test_joint) typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned> - Variant; + VariantLinear; + + typedef boost::variant VariantUnboundedRevolute; - boost::mpl::for_each(TestJointMimic()); + boost::mpl::for_each(TestJointMimic()); + boost::mpl::for_each( + TestJointMimic()); } BOOST_AUTO_TEST_CASE(test_transform_linear_affine) From f670aa065fe800ccc223ccb37388cec4c529ce9f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 056/166] [EtienneAr feedback] Fix scaling and offset in mimic joint calc --- include/pinocchio/multibody/joint/joint-mimic.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 3ae7228631..d45fcfb382 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -915,7 +915,7 @@ namespace pinocchio qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); - m_jmodel_ref.calc(jdata.m_jdata_ref, qs); + m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform); } template @@ -931,7 +931,7 @@ namespace pinocchio jdata.m_q_transform); jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, qs, vs); + m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform, jdata.m_v_transform); } template From eba619af09b80d71065327f71d6a547fe3f49dd6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 057/166] [EtienneAr feedback] WiP: Fix ConfigVectorAffineTransform template specialization --- .../joint/joint-common-operations.hpp | 41 ++++++++++++++++++- .../multibody/joint/joint-planar.hpp | 5 +++ .../joint/joint-prismatic-unaligned.hpp | 5 +++ .../multibody/joint/joint-prismatic.hpp | 5 +++ .../joint/joint-revolute-unaligned.hpp | 5 +++ .../joint-revolute-unbounded-unaligned.hpp | 5 +++ .../joint/joint-revolute-unbounded.hpp | 34 +++------------ .../multibody/joint/joint-revolute.hpp | 5 +++ .../multibody/joint/joint-translation.hpp | 5 +++ 9 files changed, 80 insertions(+), 30 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index 4401914414..26e1a0e982 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -62,6 +62,45 @@ namespace pinocchio } }; + struct UnboundedRevoluteAffineTransform + { + template + static void run( + const Eigen::MatrixBase & q, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & dest) + { + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn, 2); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut, 2); + + const typename ConfigVectorIn::Scalar & ca = q(0); + const typename ConfigVectorIn::Scalar & sa = q(1); + + const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca); + const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset; + + ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest); + SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0)); + } + }; + + struct NoAffineTransform + { + template + static void run( + const Eigen::MatrixBase & q, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & dest) + { + assert( + scaling == 1.0 && offset == 0. + && "No ConfigVectorAffineTransform specialized for this joint type"); + PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest).noalias() = q; + } + }; + /// /// \brief Assign the correct configuration vector space affine transformation according to the /// joint type. @@ -69,7 +108,7 @@ namespace pinocchio template struct ConfigVectorAffineTransform { - typedef LinearAffineTransform Type; + typedef NoAffineTransform Type; }; } // namespace pinocchio diff --git a/include/pinocchio/multibody/joint/joint-planar.hpp b/include/pinocchio/multibody/joint/joint-planar.hpp index 03d4c46c33..5bda1719c5 100644 --- a/include/pinocchio/multibody/joint/joint-planar.hpp +++ b/include/pinocchio/multibody/joint/joint-planar.hpp @@ -680,6 +680,11 @@ namespace pinocchio }; // struct JointModelPlanarTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp index 1eca7ca90a..5c7719b629 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp @@ -708,6 +708,11 @@ namespace pinocchio Vector3 axis; }; // struct JointModelPrismaticUnalignedTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp index 7731388304..0521df216b 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -786,6 +786,11 @@ namespace pinocchio typedef JointDataPrismaticTpl JointDataPZ; typedef JointModelPrismaticTpl JointModelPZ; + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp index f5cb3cde84..772b969483 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp @@ -732,6 +732,11 @@ namespace pinocchio Vector3 axis; }; // struct JointModelRevoluteUnalignedTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp index 57c986c98b..252e203c57 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -259,6 +259,11 @@ namespace pinocchio Vector3 axis; }; // struct JointModelRevoluteUnboundedUnalignedTpl + template + struct ConfigVectorAffineTransform> + { + typedef UnboundedRevoluteAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp index 572001de2a..fed0cf7c98 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -232,35 +232,6 @@ namespace pinocchio }; // struct JointModelRevoluteUnboundedTpl - struct UnboundedRevoluteAffineTransform - { - template - static void run( - const Eigen::MatrixBase & q, - const Scalar & scaling, - const Scalar & offset, - const Eigen::MatrixBase & dest) - { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn, 2); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut, 2); - - const typename ConfigVectorIn::Scalar & ca = q(0); - const typename ConfigVectorIn::Scalar & sa = q(1); - - const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca); - const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset; - - ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest); - SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0)); - } - }; - - template - struct ConfigVectorAffineTransform> - { - typedef UnboundedRevoluteAffineTransform Type; - }; - typedef JointRevoluteUnboundedTpl JointRUBX; typedef JointDataRevoluteUnboundedTpl JointDataRUBX; typedef JointModelRevoluteUnboundedTpl JointModelRUBX; @@ -273,6 +244,11 @@ namespace pinocchio typedef JointDataRevoluteUnboundedTpl JointDataRUBZ; typedef JointModelRevoluteUnboundedTpl JointModelRUBZ; + template + struct ConfigVectorAffineTransform> + { + typedef UnboundedRevoluteAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp index 0b74e17695..2c5396211e 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -882,6 +882,11 @@ namespace pinocchio typedef JointDataRevoluteTpl JointDataRZ; typedef JointModelRevoluteTpl JointModelRZ; + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index 0e67aa81a2..56a880a430 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -665,6 +665,11 @@ namespace pinocchio }; // struct JointModelTranslationTpl + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; } // namespace pinocchio #include From d75eedcedd993d60c89c531b8c00fe031476aa8b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 058/166] [EtienneAr feedback] Make visitor for config vector affine transform --- .../joint/joint-common-operations.hpp | 37 +++++++++++++++++-- .../pinocchio/multibody/joint/joint-mimic.hpp | 20 +++++----- 2 files changed, 44 insertions(+), 13 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index 26e1a0e982..c896db3013 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -9,6 +9,7 @@ #include "pinocchio/math/matrix.hpp" #include +#include namespace pinocchio { @@ -71,8 +72,8 @@ namespace pinocchio const Scalar & offset, const Eigen::MatrixBase & dest) { - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn, 2); - EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut, 2); + assert(q.size() == 2); + assert(dest.size() == 2); const typename ConfigVectorIn::Scalar & ca = q(0); const typename ConfigVectorIn::Scalar & sa = q(1); @@ -103,7 +104,7 @@ namespace pinocchio /// /// \brief Assign the correct configuration vector space affine transformation according to the - /// joint type. + /// joint type. Must be specialized for every joint type. /// template struct ConfigVectorAffineTransform @@ -111,6 +112,36 @@ namespace pinocchio typedef NoAffineTransform Type; }; + template + struct ConfigVectorAffineTransformVisitor : public boost::static_visitor + { + public: + const Eigen::MatrixBase & q; + const Scalar & scaling; + const Scalar & offset; + const Eigen::MatrixBase & dest; + + ConfigVectorAffineTransformVisitor( + const Eigen::MatrixBase & q, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & dest) + : q(q) + , scaling(scaling) + , offset(offset) + , dest(dest) + { + } + + template + void operator()(const JointModel & /*jmodel*/) const + { + typedef typename ConfigVectorAffineTransform::Type + AffineTransform; + AffineTransform::run(q, scaling, offset, dest); + } + }; + } // namespace pinocchio #endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__ diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index d45fcfb382..a2f02410e4 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -909,11 +909,11 @@ namespace pinocchio EIGEN_DONT_INLINE void calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const { - typedef typename ConfigVectorAffineTransform::Type AffineTransform; - - AffineTransform::run( - qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, - jdata.m_q_transform); + boost::apply_visitor( + ConfigVectorAffineTransformVisitor( + qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + jdata.m_q_transform), + m_jmodel_ref); m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform); } @@ -924,13 +924,13 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { - typedef typename ConfigVectorAffineTransform::Type AffineTransform; + boost::apply_visitor( + ConfigVectorAffineTransformVisitor( + qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + jdata.m_q_transform), + m_jmodel_ref); - AffineTransform::run( - qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, - jdata.m_q_transform); jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform, jdata.m_v_transform); } From a4023a90cea82084679404002d7e3559de943005 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 059/166] [EtienneAr feedback] Move configVectorAffineTransform code to joint-basic-visitor --- .../multibody/joint/joint-basic-visitors.hpp | 29 +++++++++ .../multibody/joint/joint-basic-visitors.hxx | 47 +++++++++++++++ .../joint/joint-common-operations.hpp | 60 +++++-------------- .../pinocchio/multibody/joint/joint-mimic.hpp | 18 ++---- 4 files changed, 97 insertions(+), 57 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index a30f74ac27..7cb2d00418 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -464,6 +464,35 @@ namespace pinocchio template VariantDst transferToVariant(const VariantSrc & value); + /** + * @brief Apply the correct affine transform, on a joint configuration, depending on the joint + * type. + * + * @tparam Scalar Type of scaling and offset scalars. + * @tparam Options + * @tparam JointCollectionTpl Collection of Joint types + * @tparam ConfigVectorIn Type of the input joint configuration vector. + * @tparam ConfigVectorOut Type of the ouptut joint configuration vector. + * @param jmodel Joint variant to determine the correct affine transform to use. + * @param qIn Input configuration vector + * @param scaling scaling factor + * @param offset Offset value + * @param qOut Ouptut joint configuration vector + */ + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorIn, + typename ConfigVectorOut> + void configVectorAffineTransform( + const JointModelTpl & jmodel, + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut); + } // namespace pinocchio /* --- Details -------------------------------------------------------------------- */ diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 74dc52f173..68a3e49e1b 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -947,6 +947,53 @@ namespace pinocchio return boost::apply_visitor(TransferVisitor(), value); } + template + struct ConfigVectorAffineTransformVisitor : public boost::static_visitor + { + public: + const Eigen::MatrixBase & qIn; + const Scalar & scaling; + const Scalar & offset; + const Eigen::MatrixBase & qOut; + + ConfigVectorAffineTransformVisitor( + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut) + : qIn(qIn) + , scaling(scaling) + , offset(offset) + , qOut(qOut) + { + } + + template + void operator()(const JointModel & /*jmodel*/) const + { + typedef typename ConfigVectorAffineTransform::Type + AffineTransform; + AffineTransform::run(qIn, scaling, offset, qOut); + } + }; + + template< + typename Scalar, + int Options, + template + class JointCollectionTpl, + typename ConfigVectorIn, + typename ConfigVectorOut> + void configVectorAffineTransform( + const JointModelTpl & jmodel, + const Eigen::MatrixBase & qIn, + const Scalar & scaling, + const Scalar & offset, + const Eigen::MatrixBase & qOut) + { + boost::apply_visitor(ConfigVectorAffineTransformVisitor(qIn, scaling, offset, qOut), jmodel); + } + /// @endcond } // namespace pinocchio diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index c896db3013..eb86773c50 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -52,14 +52,14 @@ namespace pinocchio { template static void run( - const Eigen::MatrixBase & q, + const Eigen::MatrixBase & qIn, const Scalar & scaling, const Scalar & offset, - const Eigen::MatrixBase & dest) + const Eigen::MatrixBase & qOut) { - assert(q.size() == dest.size()); - PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest).noalias() = - scaling * q + ConfigVectorOut::Constant(dest.size(), offset); + assert(qIn.size() == qOut.size()); + PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = + scaling * qIn + ConfigVectorOut::Constant(qOut.size(), offset); } }; @@ -67,21 +67,21 @@ namespace pinocchio { template static void run( - const Eigen::MatrixBase & q, + const Eigen::MatrixBase & qIn, const Scalar & scaling, const Scalar & offset, - const Eigen::MatrixBase & dest) + const Eigen::MatrixBase & qOut) { - assert(q.size() == 2); - assert(dest.size() == 2); + assert(qIn.size() == 2); + assert(qOut.size() == 2); - const typename ConfigVectorIn::Scalar & ca = q(0); - const typename ConfigVectorIn::Scalar & sa = q(1); + const typename ConfigVectorIn::Scalar & ca = qIn(0); + const typename ConfigVectorIn::Scalar & sa = qIn(1); const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca); const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset; - ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest); + ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut); SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0)); } }; @@ -90,15 +90,15 @@ namespace pinocchio { template static void run( - const Eigen::MatrixBase & q, + const Eigen::MatrixBase & qIn, const Scalar & scaling, const Scalar & offset, - const Eigen::MatrixBase & dest) + const Eigen::MatrixBase & qOut) { assert( scaling == 1.0 && offset == 0. && "No ConfigVectorAffineTransform specialized for this joint type"); - PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest).noalias() = q; + PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = qIn; } }; @@ -112,36 +112,6 @@ namespace pinocchio typedef NoAffineTransform Type; }; - template - struct ConfigVectorAffineTransformVisitor : public boost::static_visitor - { - public: - const Eigen::MatrixBase & q; - const Scalar & scaling; - const Scalar & offset; - const Eigen::MatrixBase & dest; - - ConfigVectorAffineTransformVisitor( - const Eigen::MatrixBase & q, - const Scalar & scaling, - const Scalar & offset, - const Eigen::MatrixBase & dest) - : q(q) - , scaling(scaling) - , offset(offset) - , dest(dest) - { - } - - template - void operator()(const JointModel & /*jmodel*/) const - { - typedef typename ConfigVectorAffineTransform::Type - AffineTransform; - AffineTransform::run(q, scaling, offset, dest); - } - }; - } // namespace pinocchio #endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__ diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index a2f02410e4..bd9798fe73 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -909,12 +909,9 @@ namespace pinocchio EIGEN_DONT_INLINE void calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const { - boost::apply_visitor( - ConfigVectorAffineTransformVisitor( - qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, - jdata.m_q_transform), - m_jmodel_ref); - + configVectorAffineTransform( + m_jmodel_ref, qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + jdata.m_q_transform); m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform); } @@ -924,12 +921,9 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { - boost::apply_visitor( - ConfigVectorAffineTransformVisitor( - qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, - jdata.m_q_transform), - m_jmodel_ref); - + configVectorAffineTransform( + m_jmodel_ref, qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + jdata.m_q_transform); jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform, jdata.m_v_transform); } From 1dc3138c4ecf59b366ee3127aebb9e36a69ca672 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 060/166] [EtienneAr feedback] Add more test cases for config vector affine transforms --- unittest/joint-mimic.cpp | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 8b05d47a9d..84499c525a 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -237,12 +237,16 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_affine) LinearAffineTransform::run(q0, scaling, offset, q1); BOOST_CHECK(q0 == q1); - offset = 2.; + scaling = 2.5; + offset = 1.5; LinearAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); BOOST_CHECK(q1 == ConfigVectorType::Constant(offset)); + + LinearAffineTransform::run(q0, scaling, offset, q1); + BOOST_CHECK((scaling * q0 + ConfigVectorType::Ones() * offset) == q1); } -BOOST_AUTO_TEST_CASE(test_transform_linear_revolute) +BOOST_AUTO_TEST_CASE(test_transform_linear_revolute_unbounded) { typedef JointModelRUBX::ConfigVector_t ConfigVectorType; double scaling = 1., offset = 0.; @@ -252,9 +256,24 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_revolute) UnboundedRevoluteAffineTransform::run(q0, scaling, offset, q1); BOOST_CHECK(q0.isApprox(q1)); - offset = 2.; - UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(), scaling, offset, q1); - BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset), math::sin(offset))); + scaling = 2.5; + offset = 1.5; + UnboundedRevoluteAffineTransform::run(q0, scaling, offset, q1); + const double theta = atan2(q0[1], q0[0]); + BOOST_CHECK( + q1 + == ConfigVectorType(math::cos(theta * scaling + offset), math::sin(theta * scaling + offset))); +} + +BOOST_AUTO_TEST_CASE(test_transform_no_affine) +{ + typedef JointModelRX::ConfigVector_t ConfigVectorType; + double scaling = 1., offset = 0.; + + ConfigVectorType q0 = ConfigVectorType::Random(); + ConfigVectorType q1; + NoAffineTransform::run(q0, scaling, offset, q1); + BOOST_CHECK(q0 == q1); } BOOST_AUTO_TEST_CASE(test_joint_generic_cast) From c1aa0ca7847627b4205c491690faf45214b915ea Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 061/166] [EtienneAr feedback] Test affine transform for identity and non identity cases --- unittest/joint-mimic.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 84499c525a..5dc9d6896d 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -128,7 +128,7 @@ BOOST_AUTO_TEST_CASE(test_constraint) boost::mpl::for_each(TestJointConstraint()); } -template +template void test_joint_mimic(const JointModelBase & jmodel) { typedef typename traits::JointDerived Joint; @@ -136,8 +136,8 @@ void test_joint_mimic(const JointModelBase & jmodel) JointData jdata = jmodel.createData(); - const double scaling_factor = 2.; - const double offset = 1.; + const double scaling_factor = MimicIdentity ? 1. : 2.5; + const double offset = MimicIdentity ? 0 : 0.75; // test constructor JointModelMimic jmodel_mimic(jmodel.derived(), scaling_factor, offset); @@ -182,7 +182,7 @@ void test_joint_mimic(const JointModelBase & jmodel) BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v())); } -template +template struct TestJointMimic { @@ -192,7 +192,7 @@ struct TestJointMimic JointModel jmodel; jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic(jmodel); } void operator()(const JointModelBase &) const @@ -200,7 +200,8 @@ struct TestJointMimic JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic( + jmodel); } void operator()(const JointModelBase &) const @@ -208,7 +209,8 @@ struct TestJointMimic JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); jmodel.setIndexes(0, 0, 0, 0); - test_joint_mimic(jmodel); + test_joint_mimic( + jmodel); } }; @@ -222,9 +224,15 @@ BOOST_AUTO_TEST_CASE(test_joint) typedef boost::variant VariantUnboundedRevolute; - boost::mpl::for_each(TestJointMimic()); + // Test all the joints with scaling == 1.0 and offset == 0.0 + boost::mpl::for_each(TestJointMimic()); + boost::mpl::for_each( + TestJointMimic()); + + // Test specific transforms for non trivial affine values + boost::mpl::for_each(TestJointMimic()); boost::mpl::for_each( - TestJointMimic()); + TestJointMimic()); } BOOST_AUTO_TEST_CASE(test_transform_linear_affine) From b176c5eb4df273372c969306e51b5cb1f0fe23e7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 062/166] [EtienneAr feedback] Cleaner floating point comparison for NoAffineTransform assert --- include/pinocchio/multibody/joint/joint-common-operations.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index eb86773c50..d10215d171 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -96,7 +96,8 @@ namespace pinocchio const Eigen::MatrixBase & qOut) { assert( - scaling == 1.0 && offset == 0. + fabs(scaling - 1.0) < Eigen::NumTraits::dummy_precision() + && fabs(offset) < Eigen::NumTraits::dummy_precision() && "No ConfigVectorAffineTransform specialized for this joint type"); PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = qIn; } From 0b3d70a0b4f47fe2b059347cf16c46717e4af99f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 063/166] [EtienneAr feedback] keep original q and v in JointDataMimic --- .../pinocchio/multibody/joint/joint-mimic.hpp | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index bd9798fe73..1e91be8523 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -496,7 +496,9 @@ namespace pinocchio : m_scaling((Scalar)0) , S((Scalar)0) { + joint_q.resize(0); m_q_transform.resize(0); + joint_v.resize(0); m_v_transform.resize(0); } @@ -513,7 +515,9 @@ namespace pinocchio , m_jdata_ref( transferToVariant, RefJointData>(jdata)) { + joint_q.resize(nq, 1); m_q_transform.resize(nq, 1); + joint_v.resize(nv, 1); m_v_transform.resize(nv, 1); } @@ -534,7 +538,9 @@ namespace pinocchio , m_scaling(scaling) , S(m_jdata_ref.S(), scaling) { + joint_q.resize(nq, 1); m_q_transform.resize(nq, 1); + joint_v.resize(nv, 1); m_v_transform.resize(nv, 1); } @@ -542,7 +548,9 @@ namespace pinocchio { m_jdata_ref = other.m_jdata_ref; m_scaling = other.m_scaling; + joint_q = other.joint_q; m_q_transform = other.m_q_transform; + joint_v = other.joint_v; m_v_transform = other.m_v_transform; S = Constraint_t(m_jdata_ref.S(), other.m_scaling); return *this; @@ -552,7 +560,8 @@ namespace pinocchio bool isEqual(const JointDataMimicTpl & other) const { return Base::isEqual(other) && m_jdata_ref == other.m_jdata_ref - && m_scaling == other.m_scaling && m_q_transform == other.m_q_transform + && m_scaling == other.m_scaling && joint_q == other.joint_q + && m_q_transform == other.m_q_transform && joint_v == other.joint_v && m_v_transform == other.m_v_transform; } @@ -689,28 +698,32 @@ namespace pinocchio ConfigVector_t & joint_q_accessor() { - return m_q_transform; + return joint_q; } const ConfigVector_t & joint_q_accessor() const { - return m_q_transform; + return joint_q; } TangentVector_t & joint_v_accessor() { - return m_v_transform; + return joint_v; } const TangentVector_t & joint_v_accessor() const { - return m_v_transform; + return joint_v; } protected: RefJointData m_jdata_ref; Scalar m_scaling; - /// \brief Transform configuration vector + /// \brief original configuration vector + ConfigVector_t joint_q; + /// \brief Transformed configuration vector ConfigVector_t m_q_transform; + /// \brief original velocity vector + TangentVector_t joint_v; /// \brief Transform velocity vector. TangentVector_t m_v_transform; @@ -909,6 +922,7 @@ namespace pinocchio EIGEN_DONT_INLINE void calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const { + jdata.joint_q = qs.segment(idx_q(), nq()); configVectorAffineTransform( m_jmodel_ref, qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); @@ -921,6 +935,8 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { + jdata.joint_q = qs.segment(idx_q(), nq()); + jdata.joint_v = vs.segment(idx_v(), nv()); configVectorAffineTransform( m_jmodel_ref, qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); From 75ec986a836b3e8f365679ac60573030a4e3ace8 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 064/166] [EtienneAr feedback] [Mimic] Set indexes of ref joint to 0 --- .../pinocchio/multibody/joint/joint-mimic.hpp | 72 ++++++++----------- unittest/joint-mimic.cpp | 3 - 2 files changed, 31 insertions(+), 44 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 1e91be8523..871be61544 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -812,9 +812,9 @@ namespace pinocchio assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes( - jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), - jmodel_mimicked.idx_j()); + m_jmodel_ref.setIndexes(0, 0, 0, 0); + Base::i_q = jmodel_mimicked.idx_q(); + Base::i_v = jmodel_mimicked.idx_v(); } template @@ -838,9 +838,9 @@ namespace pinocchio assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes( - jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), - jmodel_mimicked.idx_j()); + m_jmodel_ref.setIndexes(0, 0, 0, 0); + Base::i_q = jmodel_mimicked.idx_q(); + Base::i_v = jmodel_mimicked.idx_v(); } template @@ -857,9 +857,9 @@ namespace pinocchio assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes( - jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), - jmodel_mimicked.idx_j()); + m_jmodel_ref.setIndexes(0, 0, 0, 0); + Base::i_q = jmodel_mimicked.idx_q(); + Base::i_v = jmodel_mimicked.idx_v(); } Base & base() @@ -884,20 +884,12 @@ namespace pinocchio return m_jmodel_ref.nj(); } - inline int idx_q_impl() const - { - return m_jmodel_ref.idx_q(); - } - inline int idx_v_impl() const - { - return m_jmodel_ref.idx_v(); - } - void setIndexes_impl(JointIndex id, int q, int v, int j) { - Base::i_id = id; // Only the id of the joint in the model is different. - Base::i_q = m_jmodel_ref.idx_q(); - Base::i_v = m_jmodel_ref.idx_v(); + Base::i_id = id; + // When setting the indexes q and v should remain on the mimicked joint + // Base::i_q = q; + // Base::i_v = v; Base::i_j = j; } @@ -924,7 +916,7 @@ namespace pinocchio { jdata.joint_q = qs.segment(idx_q(), nq()); configVectorAffineTransform( - m_jmodel_ref, qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + m_jmodel_ref, qs.segment(idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform); } @@ -938,9 +930,9 @@ namespace pinocchio jdata.joint_q = qs.segment(idx_q(), nq()); jdata.joint_v = vs.segment(idx_v(), nv()); configVectorAffineTransform( - m_jmodel_ref, qs.segment(m_jmodel_ref.idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, + m_jmodel_ref, qs.segment(idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, jdata.m_q_transform); - jdata.m_v_transform = m_scaling * vs.segment(m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + jdata.m_v_transform = m_scaling * vs.segment(idx_v(), m_jmodel_ref.nv()); m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform, jdata.m_v_transform); } @@ -1021,7 +1013,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); } // Non-const access @@ -1029,7 +1021,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); } // Const access @@ -1037,7 +1029,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); + return SizeDepType::segment(a.derived(), idx_q(), 0); } // Non-const access @@ -1045,7 +1037,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_q(), 0); + return SizeDepType::segment(a.derived(), idx_q(), 0); } /* Acces to dedicated segment in robot config velocity space. */ @@ -1054,7 +1046,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), idx_v(), m_jmodel_ref.nv()); } // Non-const access @@ -1062,7 +1054,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::segment(a.derived(), idx_v(), m_jmodel_ref.nv()); } // Const access @@ -1070,7 +1062,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); + return SizeDepType::segment(a.derived(), idx_v(), 0); } // Non-const access @@ -1078,7 +1070,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), m_jmodel_ref.idx_v(), 0); + return SizeDepType::segment(a.derived(), idx_v(), 0); } /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ @@ -1087,7 +1079,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointVelCols_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); } // Non-const access @@ -1095,16 +1087,16 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type jointVelCols_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); } /* Acces to dedicated rows in a matrix.*/ // Const access template typename SizeDepType::template RowsReturn::ConstType - joinVeltRows_impl(const Eigen::MatrixBase & A) const + joinVelRows_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleRows(A.derived(), idx_v(), m_jmodel_ref.nv()); } // Non-const access @@ -1112,7 +1104,7 @@ namespace pinocchio typename SizeDepType::template RowsReturn::Type jointVelRows_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleRows(A.derived(), idx_v(), m_jmodel_ref.nv()); } // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the @@ -1123,8 +1115,7 @@ namespace pinocchio jointVelBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block( - Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), - m_jmodel_ref.nv()); + Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); } // Non-const access @@ -1133,8 +1124,7 @@ namespace pinocchio jointVelBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block( - Mat.derived(), m_jmodel_ref.idx_v(), m_jmodel_ref.idx_v(), m_jmodel_ref.nv(), - m_jmodel_ref.nv()); + Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); } }; // struct JointModelMimicTpl diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 5dc9d6896d..d457ee2420 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -152,9 +152,6 @@ void test_joint_mimic(const JointModelBase & jmodel) BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q()); BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v()); - BOOST_CHECK(jmodel_mimic.idx_q() == 0); - BOOST_CHECK(jmodel_mimic.idx_v() == 0); - typedef typename JointModel::ConfigVector_t ConfigVectorType; typedef typename LieGroup::type LieGroupType; ConfigVectorType q0 = From 78da384f97e339ac58d8e2c8c3dd3bf9cfb6796c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 065/166] [EtienneAr feedback] Fix mimic test for slightly new convention of ref joint idx_q, idx_v set to 0 --- include/pinocchio/algorithm/model.hxx | 2 +- .../pinocchio/multibody/joint/joint-mimic.hpp | 31 +++++++++++++------ 2 files changed, 22 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index e64a51a119..8d414ad394 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -176,7 +176,7 @@ namespace pinocchio const JointIndex mimicked_old_id = res.jmodel().id(); const std::string mimicked_name = old_model.names[mimicked_old_id]; const JointIndex mimicked_new_id = new_model.getJointId(mimicked_name); - res.jmodel().setIndexes( + res.setMimicIndexes( mimicked_new_id, new_model.joints[mimicked_new_id].idx_q(), new_model.joints[mimicked_new_id].idx_v(), new_model.joints[mimicked_new_id].idx_j()); return res; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 871be61544..40ae598f0b 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -812,9 +812,9 @@ namespace pinocchio assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes(0, 0, 0, 0); - Base::i_q = jmodel_mimicked.idx_q(); - Base::i_v = jmodel_mimicked.idx_v(); + setMimicIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_j()); } template @@ -838,9 +838,9 @@ namespace pinocchio assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes(0, 0, 0, 0); - Base::i_q = jmodel_mimicked.idx_q(); - Base::i_v = jmodel_mimicked.idx_v(); + setMimicIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_j()); } template @@ -857,9 +857,9 @@ namespace pinocchio assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - m_jmodel_ref.setIndexes(0, 0, 0, 0); - Base::i_q = jmodel_mimicked.idx_q(); - Base::i_v = jmodel_mimicked.idx_v(); + setMimicIndexes( + jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), + jmodel_mimicked.idx_j()); } Base & base() @@ -884,7 +884,7 @@ namespace pinocchio return m_jmodel_ref.nj(); } - void setIndexes_impl(JointIndex id, int q, int v, int j) + void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int j) { Base::i_id = id; // When setting the indexes q and v should remain on the mimicked joint @@ -893,6 +893,17 @@ namespace pinocchio Base::i_j = j; } + // Specific way for mimic joints to set the mimicked q,v indexes. + // Used for manipulating tree (e.g. appendModel) + void setMimicIndexes(JointIndex id, int q, int v, int j) + { + // Set idx_q, idx_v to zero so that only sub segment of q,v can be passed to ref joint + m_jmodel_ref.setIndexes(id, 0, 0, j); + // idx_q, idx_v kept separately + Base::i_q = q; + Base::i_v = v; + } + JointDataDerived createData() const { From 3a5107830c5cdf610f21aa4c9e8fbc3b160357e3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 066/166] [EtienneAr feedback] Fix serialization for mimic joint with setIndexes --- .../pinocchio/serialization/joints-model.hpp | 33 ++++++++++++++++++- 1 file changed, 32 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index f22cc758df..c0379e5e9c 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -65,6 +65,37 @@ namespace boost ar & make_nvp("i_j", i_j); } + template + class SetJointIndexes + { + Derived & joint; + + public: + SetJointIndexes(Derived & joint) + : joint(joint) {}; + + void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) + { + joint.setIndexes(i_id, i_q, i_v, i_j); + } + }; + + template class JointCollectionTpl> + class SetJointIndexes> + { + pinocchio::JointModelMimicTpl & joint; + + public: + SetJointIndexes(pinocchio::JointModelMimicTpl & joint) + : joint(joint) {}; + + void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) + { + joint.setIndexes(i_id, 0, 0, i_j); + joint.setMimicIndexes(0, i_q, i_v, 0); + } + }; + template void load(Archive & ar, pinocchio::JointModelBase & joint, const unsigned int /*version*/) @@ -77,7 +108,7 @@ namespace boost ar & make_nvp("i_v", i_v); ar & make_nvp("i_j", i_j); - joint.setIndexes(i_id, i_q, i_v, i_j); + SetJointIndexes(joint.derived()).run(i_id, i_q, i_v, i_j); } template From 0c4c36b3cfb08c58092963a3115708dccaa56d8e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 067/166] [bindings] Added mimic to context --- include/pinocchio/bindings/python/context/generic.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index 9f3eb37616..cf2020aa8e 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -138,6 +138,9 @@ namespace pinocchio typedef JointModelCompositeTpl JointModelComposite; typedef JointDataCompositeTpl JointDataComposite; + typedef JointModelMimicTpl JointModelMimic; + typedef JointDataMimicTpl JointDataMimic; + // Algorithm typedef ProximalSettingsTpl ProximalSettings; typedef ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; From a3c962258d73551ef7cb2c867d1442f4bd608978 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 068/166] [parser] remove std::cout --- include/pinocchio/parsers/urdf/model.hxx | 1 - 1 file changed, 1 deletion(-) diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index fe1f8ff797..af00a86b47 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -511,7 +511,6 @@ namespace pinocchio const Scalar offset) { auto mimiced_joint = model.joints[getJointId(mimiced_name)]; - std::cout << mimiced_joint << std::endl; auto mimic_joint = model.joints[getJointId(mimic_name)]; CartesianAxis axisType = extractCartesianAxis(axis); From f7b0fa31f39190c3bcc30cd5db01cecf3881f135 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 069/166] [bindings] BROKEN - Added specialization for mimic joint in bindings --- .../python/multibody/joint/joints-models.hpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index c6253ade15..366fe59018 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -360,6 +360,44 @@ namespace pinocchio ; } + + // Specialization for JointModelMimic + struct JointModelMimicConstructorVisitor + : public boost::static_visitor + { + const context::Scalar & m_scaling; + const context::Scalar & m_offset; + + JointModelMimicConstructorVisitor(const context::Scalar & scaling, const context::Scalar & offset) + : m_scaling(scaling) + , m_offset(offset) + { + } + + template + context::JointModelMimic * operator()(JointModelDerived & jmodel) const + { + return new context::JointModelMimic(jmodel, m_scaling, m_offset); + } + }; // struct JointModelMimicConstructorVisitor + + static context::JointModelMimic * init_proxy(const context::JointModel & jmodel, const context::Scalar & scaling, const context::Scalar & offset) + { + return boost::apply_visitor( + JointModelMimicConstructorVisitor(scaling, offset), jmodel); + } + + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def( + "__init__", + bp::make_constructor(init_proxy, bp::default_call_policies(), bp::args("joint_model", "scaling", "offset")), + "Init JointModelMimic from an existing joint with scaling and offset."); + } + } // namespace python } // namespace pinocchio From efea7f8867d70870fb9b310260e53dd366de32c8 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 070/166] [python] Added jointModelMimicable to context --- include/pinocchio/bindings/python/context/generic.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index cf2020aa8e..1eb7a151d3 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -64,6 +64,7 @@ namespace pinocchio typedef DataTpl Data; typedef JointCollectionDefaultTpl JointCollectionDefault; + typedef JointCollectionMimicableTpl JointCollectionMimicable; // Joints typedef JointModelTpl JointModel; @@ -141,6 +142,8 @@ namespace pinocchio typedef JointModelMimicTpl JointModelMimic; typedef JointDataMimicTpl JointDataMimic; + typedef JointModelTpl JointModelMimicable; + // Algorithm typedef ProximalSettingsTpl ProximalSettings; typedef ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; From 745b0a73cd8b239b499535c3b536f6663125d544 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 071/166] [bindings] Fix mimic specialization for constructor --- .../bindings/python/multibody/joint/joints-models.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index 366fe59018..9a0a69943f 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -10,6 +10,7 @@ #include "pinocchio/multibody/joint/joint-collection.hpp" #include "pinocchio/multibody/joint/joint-composite.hpp" #include "pinocchio/multibody/joint/joint-generic.hpp" +#include "pinocchio/multibody/joint/joint-mimic.hpp" #include @@ -357,7 +358,6 @@ namespace pinocchio .def(bp::self == bp::self) .def(bp::self != bp::self) #endif - ; } @@ -375,16 +375,18 @@ namespace pinocchio } template - context::JointModelMimic * operator()(JointModelDerived & jmodel) const + context::JointModelMimic * operator()(const JointModelDerived & jmodel) const { + return new context::JointModelMimic(jmodel, m_scaling, m_offset); } + }; // struct JointModelMimicConstructorVisitor static context::JointModelMimic * init_proxy(const context::JointModel & jmodel, const context::Scalar & scaling, const context::Scalar & offset) { return boost::apply_visitor( - JointModelMimicConstructorVisitor(scaling, offset), jmodel); + JointModelMimicConstructorVisitor(scaling, offset), transferToVariant(jmodel)); } template<> From e508e4c428a66f7c6ad2c78d5f5fc1d45a3c46c5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 072/166] [context] Added mimicable joint --- include/pinocchio/multibody/joint/fwd.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index 774111bb1e..159afb6ded 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -134,6 +134,10 @@ namespace pinocchio struct JointCollectionDefaultTpl; typedef JointCollectionDefaultTpl JointCollectionDefault; + template + struct JointCollectionMimicableTpl; + typedef JointCollectionMimicableTpl JointCollectionMimicable; + template< typename Scalar, int Options = context::Options, @@ -169,6 +173,8 @@ namespace pinocchio struct JointModelTpl; typedef JointModelTpl JointModel; + typedef JointModelTpl JointModelMimicable; + template< typename Scalar, int Options = context::Options, From be5ac35532af587973e53ccb3a71e77bd10bc819 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 073/166] [bindings] Finished specialization for mimic --- .../python/multibody/joint/joints-models.hpp | 14 +++++++++++++- include/pinocchio/multibody/joint/joint-mimic.hpp | 6 +++--- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index 9a0a69943f..33510d6a0b 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -389,6 +389,16 @@ namespace pinocchio JointModelMimicConstructorVisitor(scaling, offset), transferToVariant(jmodel)); } + static context::Scalar get_scaling(context::JointModelMimic & jmodel) + { + return jmodel.scaling(); + } + + static context::Scalar get_offset(context::JointModelMimic & jmodel) + { + return jmodel.offset(); + } + template<> bp::class_ & expose_joint_model(bp::class_ & cl) @@ -397,7 +407,9 @@ namespace pinocchio .def( "__init__", bp::make_constructor(init_proxy, bp::default_call_policies(), bp::args("joint_model", "scaling", "offset")), - "Init JointModelMimic from an existing joint with scaling and offset."); + "Init JointModelMimic from an existing joint with scaling and offset.") + .add_property("scaling",&get_scaling) + .add_property("offset", &get_offset); } } // namespace python diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 40ae598f0b..9c03e7fb6c 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -805,7 +805,7 @@ namespace pinocchio : m_scaling(scaling) , m_offset(offset) , m_jmodel_ref( - transferToVariant, JointModel>( + transferToVariant, JointModelVariant>( jmodel_mimicking)) { assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); @@ -830,7 +830,7 @@ namespace pinocchio const JointModelBase & jmodel_mimicked, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) + : m_jmodel_ref((JointModel)jmodel_mimicking.derived()) , m_scaling(scaling) , m_offset(offset) { @@ -849,7 +849,7 @@ namespace pinocchio const JointModelTpl & jmodel_mimicked, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref((JointModelVariant)jmodel_mimicking.derived()) + : m_jmodel_ref((JointModel)jmodel_mimicking.derived()) , m_scaling(scaling) , m_offset(offset) { From f789dd6a46d034ddf26810376eaed9f6d345de03 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 074/166] {Joints@ Added mimicable trait --- include/pinocchio/multibody/joint/joint-composite.hpp | 2 ++ include/pinocchio/multibody/joint/joint-free-flyer.hpp | 2 ++ include/pinocchio/multibody/joint/joint-generic.hpp | 2 ++ include/pinocchio/multibody/joint/joint-helical-unaligned.hpp | 2 ++ include/pinocchio/multibody/joint/joint-helical.hpp | 4 +++- include/pinocchio/multibody/joint/joint-model-base.hpp | 1 + include/pinocchio/multibody/joint/joint-planar.hpp | 2 ++ .../pinocchio/multibody/joint/joint-prismatic-unaligned.hpp | 2 ++ include/pinocchio/multibody/joint/joint-prismatic.hpp | 2 ++ .../pinocchio/multibody/joint/joint-revolute-unaligned.hpp | 2 ++ .../multibody/joint/joint-revolute-unbounded-unaligned.hpp | 2 ++ .../pinocchio/multibody/joint/joint-revolute-unbounded.hpp | 2 ++ include/pinocchio/multibody/joint/joint-revolute.hpp | 2 ++ include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp | 2 ++ include/pinocchio/multibody/joint/joint-spherical.hpp | 2 ++ include/pinocchio/multibody/joint/joint-translation.hpp | 2 ++ include/pinocchio/multibody/joint/joint-universal.hpp | 2 ++ unittest/joint-generic.cpp | 3 +++ 18 files changed, 37 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index f56bafc927..b94d43a1f6 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -48,6 +48,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index f8dc5398c7..5d3ca7b47b 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -191,6 +191,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index 1ad504369b..7dd75683ef 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -70,6 +70,8 @@ namespace pinocchio typedef ConfigVector_t ConfigVectorTypeRef; typedef TangentVector_t TangentVectorTypeConstRef; typedef TangentVector_t TangentVectorTypeRef; + + typedef boost::mpl::false_ is_mimicable_t; }; template class JointCollectionTpl> diff --git a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp index 2d70d34a9e..55d2989e89 100644 --- a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp @@ -547,6 +547,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index 4285338e77..0eb68703e1 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -757,7 +757,9 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; - + + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index cc606149b9..de33fc91fd 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -25,6 +25,7 @@ typedef TYPENAME traits::U_t U_t; \ typedef TYPENAME traits::D_t D_t; \ typedef TYPENAME traits::UD_t UD_t; \ + typedef TYPENAME traits::is_mimicable_t is_mimicable_t; \ enum \ { \ Options = traits::Options, \ diff --git a/include/pinocchio/multibody/joint/joint-planar.hpp b/include/pinocchio/multibody/joint/joint-planar.hpp index 5bda1719c5..96e7c56f3c 100644 --- a/include/pinocchio/multibody/joint/joint-planar.hpp +++ b/include/pinocchio/multibody/joint/joint-planar.hpp @@ -494,6 +494,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp index 5c7719b629..614abb07b1 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp @@ -501,6 +501,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp index 0521df216b..08c33a63c8 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -593,6 +593,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp index 772b969483..eb039f9a3e 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp @@ -526,6 +526,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp index 252e203c57..8211eaa935 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -49,6 +49,8 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp index fed0cf7c98..619ae467c2 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -46,6 +46,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp index 2c5396211e..7be27ad828 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -682,6 +682,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::true_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp index 9b579f9074..63bf442979 100644 --- a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp @@ -302,6 +302,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-spherical.hpp b/include/pinocchio/multibody/joint/joint-spherical.hpp index 4dcce705f7..9ce0600f8b 100644 --- a/include/pinocchio/multibody/joint/joint-spherical.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical.hpp @@ -405,6 +405,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index 56a880a430..f4702ec097 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -502,6 +502,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; // traits JointTranslationTpl diff --git a/include/pinocchio/multibody/joint/joint-universal.hpp b/include/pinocchio/multibody/joint/joint-universal.hpp index a8310697d9..54e797a1ab 100644 --- a/include/pinocchio/multibody/joint/joint-universal.hpp +++ b/include/pinocchio/multibody/joint/joint-universal.hpp @@ -336,6 +336,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index f6c9d58ef9..8e9e61cf6b 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -351,6 +351,9 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + + typedef boost::mpl::false_ is_mimicable_t; + }; template class JointCollectionTpl> From 4d70402d252c23b6c8ed745a2e169201e95dd9b7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 075/166] [Visitor] Change TransferVariant visitor into checkMimic visitor --- .../multibody/joint/joint-basic-visitors.hxx | 39 ++++++++++--------- 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 68a3e49e1b..6105ab2cd5 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -918,33 +918,36 @@ namespace pinocchio return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived()))); } - template - struct TransferVisitor : public boost::static_visitor - { - template - typename boost:: - enable_if, TargetVariant>::type - operator()(const ValueType & value) const + // Meta-function to check is_mimicable_t trait + template + struct is_mimicable { + static constexpr bool value = traits::is_mimicable_t::value; + }; + + template + struct CheckMimicVisitor : public boost::static_visitor + { + template + typename boost::enable_if_c::value, JointModel>::type + operator()(const T & value) const { - return TargetVariant(value); + return value; } - template - typename boost::disable_if< - boost::mpl::contains, - TargetVariant>::type - operator()(const ValueType & value) const + template + typename boost::disable_if_c::value, JointModel>::type + operator()(const T & value) const { - assert(false && "Type not supported in new variant"); - return TargetVariant(); + assert(false && "Type not supported in new variant"); + return value; } }; - template - VariantDst transferToVariant(const VariantSrc & value) + template + JointModel checkMimic(const JointModel & value) { - return boost::apply_visitor(TransferVisitor(), value); + return boost::apply_visitor(CheckMimicVisitor(), value); } template From 59c610f895a844a5cf5c917d12299dfca7f487e3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 076/166] [Joint] Remove mimicable joint collection --- .../pinocchio/multibody/joint/joint-mimic.hpp | 105 ------------------ 1 file changed, 105 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 9c03e7fb6c..180769f23a 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -316,111 +316,6 @@ namespace pinocchio }; } // namespace impl - template - struct JointCollectionMimicableTpl - { - typedef _Scalar Scalar; - enum - { - Options = _Options - }; - - // Joint Revolute - typedef JointModelRevoluteTpl JointModelRX; - typedef JointModelRevoluteTpl JointModelRY; - typedef JointModelRevoluteTpl JointModelRZ; - - // Joint Revolute Unaligned - typedef JointModelRevoluteUnalignedTpl JointModelRevoluteUnaligned; - - // Joint Revolute UBounded - typedef JointModelRevoluteUnboundedTpl JointModelRUBX; - typedef JointModelRevoluteUnboundedTpl JointModelRUBY; - typedef JointModelRevoluteUnboundedTpl JointModelRUBZ; - - // Joint Revolute Unbounded Unaligned - typedef JointModelRevoluteUnboundedUnalignedTpl - JointModelRevoluteUnboundedUnaligned; - - // Joint Prismatic - typedef JointModelPrismaticTpl JointModelPX; - typedef JointModelPrismaticTpl JointModelPY; - typedef JointModelPrismaticTpl JointModelPZ; - - // Joint Prismatic Unaligned - typedef JointModelPrismaticUnalignedTpl JointModelPrismaticUnaligned; - - // Joint Translation - typedef JointModelTranslationTpl JointModelTranslation; - - // Joint FreeFlyer - typedef JointModelFreeFlyerTpl JointModelFreeFlyer; - - typedef boost::variant< - JointModelRX, - JointModelRY, - JointModelRZ, - JointModelRevoluteUnaligned, - JointModelPX, - JointModelPY, - JointModelPZ, - JointModelPrismaticUnaligned, - JointModelTranslation, - JointModelRUBX, - JointModelRUBY, - JointModelRUBZ, - JointModelRevoluteUnboundedUnaligned, - JointModelFreeFlyer> - JointModelVariant; - - // Joint Revolute - typedef JointDataRevoluteTpl JointDataRX; - typedef JointDataRevoluteTpl JointDataRY; - typedef JointDataRevoluteTpl JointDataRZ; - - // Joint Revolute Unaligned - typedef JointDataRevoluteUnalignedTpl JointDataRevoluteUnaligned; - - // Joint Revolute Unaligned - typedef JointDataRevoluteUnboundedUnalignedTpl - JointDataRevoluteUnboundedUnaligned; - - // Joint Revolute UBounded - typedef JointDataRevoluteUnboundedTpl JointDataRUBX; - typedef JointDataRevoluteUnboundedTpl JointDataRUBY; - typedef JointDataRevoluteUnboundedTpl JointDataRUBZ; - - // Joint Prismatic - typedef JointDataPrismaticTpl JointDataPX; - typedef JointDataPrismaticTpl JointDataPY; - typedef JointDataPrismaticTpl JointDataPZ; - - // Joint Prismatic Unaligned - typedef JointDataPrismaticUnalignedTpl JointDataPrismaticUnaligned; - - // Joint Translation - typedef JointDataTranslationTpl JointDataTranslation; - - // Joint FreeFlyer - typedef JointDataFreeFlyerTpl JointDataFreeFlyer; - - typedef boost::variant< - JointDataRX, - JointDataRY, - JointDataRZ, - JointDataRevoluteUnaligned, - JointDataPX, - JointDataPY, - JointDataPZ, - JointDataPrismaticUnaligned, - JointDataTranslation, - JointDataRUBX, - JointDataRUBY, - JointDataRUBZ, - JointDataRevoluteUnboundedUnaligned, - JointDataFreeFlyer> - JointDataVariant; - }; template class JointCollectionTpl> struct JointMimicTpl; From 263f8075dfb8bdf6ebc19b26ea0a7c0685013fe2 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 077/166] [Joint] Use check mimic for jointMimic constructor --- .../pinocchio/multibody/joint/joint-mimic.hpp | 93 ++----------------- 1 file changed, 9 insertions(+), 84 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 180769f23a..0f72ed429a 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -316,7 +316,6 @@ namespace pinocchio }; } // namespace impl - template class JointCollectionTpl> struct JointMimicTpl; template class JointCollectionTpl> @@ -354,6 +353,8 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; + typedef boost::mpl::false_ is_mimicable_t; + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; @@ -381,10 +382,7 @@ namespace pinocchio typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); - // typedef typename boost::make_variant_over>::type>::type - // MimicableJointDataVariant; - typedef JointDataTpl<_Scalar, _Options, JointCollectionMimicableTpl> RefJointData; + typedef JointDataTpl<_Scalar, _Options, JointCollectionTpl> RefJointData; typedef typename RefJointData::JointDataVariant RefJointDataVariant; JointDataMimicTpl() @@ -401,37 +399,14 @@ namespace pinocchio // { *this = other; } JointDataMimicTpl( - const JointDataTpl & jdata, + const RefJointData & jdata, const Scalar & scaling, const Scalar & nq, const Scalar & nv) : m_scaling(scaling) , S(m_jdata_ref.S(), scaling) , m_jdata_ref( - transferToVariant, RefJointData>(jdata)) - { - joint_q.resize(nq, 1); - m_q_transform.resize(nq, 1); - joint_v.resize(nv, 1); - m_v_transform.resize(nv, 1); - } - - // JointDataMimicTpl(const RefJointDataVariant & jdata, - // const Scalar & scaling, - // const Scalar & nq, - // const Scalar & nv) - // : m_jdata_ref(jdata) - // , m_scaling(scaling) - // , S(m_jdata_ref.S(),scaling) - // { - - // } - - JointDataMimicTpl( - const RefJointData & jdata, const Scalar & scaling, const Scalar & nq, const Scalar & nv) - : m_jdata_ref(jdata.derived()) - , m_scaling(scaling) - , S(m_jdata_ref.S(), scaling) + checkMimic(jdata)) { joint_q.resize(nq, 1); m_q_transform.resize(nq, 1); @@ -660,13 +635,9 @@ namespace pinocchio PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); typedef JointCollectionTpl JointCollection; - typedef JointModelTpl JointModel; + typedef JointModelTpl JointModel; typedef typename JointModel::JointModelVariant JointModelVariant; - // typedef typename boost::make_variant_over>::type>::type - // MimicableJointModelVariant; typedef JointModelTpl<_Scalar, _Options, - // JointCollectionMimicableTpl> MimicableJointModel; - + typedef SE3Tpl SE3; typedef MotionTpl Motion; typedef InertiaTpl Inertia; @@ -684,34 +655,6 @@ namespace pinocchio { } - JointModelMimicTpl( - const JointModelTpl & jmodel, - const Scalar & scaling, - const Scalar & offset) - : JointModelMimicTpl(jmodel, jmodel, scaling, offset) - { - } - - JointModelMimicTpl( - const JointModelTpl & jmodel_mimicking, - const JointModelTpl & jmodel_mimicked, - const Scalar & scaling, - const Scalar & offset) - : m_scaling(scaling) - , m_offset(offset) - , m_jmodel_ref( - transferToVariant, JointModelVariant>( - jmodel_mimicking)) - { - assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); - assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); - assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - - setMimicIndexes( - jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), - jmodel_mimicked.idx_j()); - } - template JointModelMimicTpl( const JointModelBase & jmodel, const Scalar & scaling, const Scalar & offset) @@ -725,32 +668,14 @@ namespace pinocchio const JointModelBase & jmodel_mimicked, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref((JointModel)jmodel_mimicking.derived()) - , m_scaling(scaling) - , m_offset(offset) - { - assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); - assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); - assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - - setMimicIndexes( - jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), - jmodel_mimicked.idx_j()); - } - - template - JointModelMimicTpl( - const JointModelBase & jmodel_mimicking, - const JointModelTpl & jmodel_mimicked, - const Scalar & scaling, - const Scalar & offset) - : m_jmodel_ref((JointModel)jmodel_mimicking.derived()) + : m_jmodel_ref(checkMimic((JointModel)jmodel_mimicking.derived())) , m_scaling(scaling) , m_offset(offset) { assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); + setMimicIndexes( jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), From adaa1ff14be3efde70f5f0aaba24dbf9ffe2b2fd Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 078/166] [bindings] Make bindings conform to new jointMimic --- .../pinocchio/bindings/python/multibody/joint/joints-models.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index 33510d6a0b..eea5973584 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -386,7 +386,7 @@ namespace pinocchio static context::JointModelMimic * init_proxy(const context::JointModel & jmodel, const context::Scalar & scaling, const context::Scalar & offset) { return boost::apply_visitor( - JointModelMimicConstructorVisitor(scaling, offset), transferToVariant(jmodel)); + JointModelMimicConstructorVisitor(scaling, offset), jmodel); } static context::Scalar get_scaling(context::JointModelMimic & jmodel) From 9b81b89a6945a37a6545a6369fca359e50f11527 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 079/166] [EtienneAr feedback] Make random humanoid with mimic joint --- bindings/python/multibody/sample-models.cpp | 26 +++++++++++++++++++ include/pinocchio/multibody/sample-models.hpp | 5 +++- include/pinocchio/multibody/sample-models.hxx | 25 ++++++++++++++---- include/pinocchio/multibody/sample-models.txx | 2 +- src/multibody/sample-models.cpp | 2 +- unittest/python/bindings_data.py | 2 +- unittest/python/bindings_frame.py | 2 +- unittest/python/bindings_joints.py | 2 +- unittest/python/bindings_kinematics.py | 2 +- unittest/python/bindings_model.py | 2 +- unittest/python/bindings_rnea.py | 2 +- unittest/python/bindings_sample_models.py | 2 +- unittest/python/serialization.py | 6 ++--- 13 files changed, 62 insertions(+), 18 deletions(-) diff --git a/bindings/python/multibody/sample-models.cpp b/bindings/python/multibody/sample-models.cpp index 295e04f723..d47338a474 100644 --- a/bindings/python/multibody/sample-models.cpp +++ b/bindings/python/multibody/sample-models.cpp @@ -26,6 +26,13 @@ namespace pinocchio return model; } + Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic) + { + Model model; + buildModels::humanoidRandom(model, usingFF, mimic); + return model; + } + Model buildSampleModelManipulator() { Model model; @@ -33,6 +40,13 @@ namespace pinocchio return model; } + Model buildSampleModelManipulator(bool mimic) + { + Model model; + buildModels::manipulator(model, mimic); + return model; + } + #ifdef PINOCCHIO_WITH_HPP_FCL GeometryModel buildSampleGeometryModelManipulator(const Model & model) { @@ -80,11 +94,23 @@ namespace pinocchio "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint " "placements.\nOnly meant for unit tests."); + bp::def( + "buildSampleModelHumanoidRandom", + static_cast(pinocchio::python::buildSampleModelHumanoidRandom), + bp::args("using_free_flyer", "mimic"), + "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint " + "placements.\nOnly meant for unit tests."); + bp::def( "buildSampleModelManipulator", static_cast(pinocchio::python::buildSampleModelManipulator), "Generate a (hard-coded) model of a simple manipulator."); + bp::def( + "buildSampleModelManipulator", + static_cast(pinocchio::python::buildSampleModelManipulator), + bp::args("mimic"), "Generate a (hard-coded) model of a simple manipulator."); + #ifdef PINOCCHIO_WITH_HPP_FCL bp::def( "buildSampleGeometryModelManipulator", diff --git a/include/pinocchio/multibody/sample-models.hpp b/include/pinocchio/multibody/sample-models.hpp index 61fef617ae..f2a15060f1 100644 --- a/include/pinocchio/multibody/sample-models.hpp +++ b/include/pinocchio/multibody/sample-models.hpp @@ -72,7 +72,10 @@ namespace pinocchio * This changes the size of the configuration space (33 vs 32). */ template class JointCollectionTpl> - void humanoidRandom(ModelTpl & model, bool usingFF = true); + void humanoidRandom( + ModelTpl & model, + bool usingFF = true, + bool mimic = false); } // namespace buildModels } // namespace pinocchio diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 73c74e9936..4fac645a23 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -110,8 +110,8 @@ namespace pinocchio // Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); // Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); - Scalar multiplier = 1; - Scalar offset = 0; + Scalar multiplier = 2.5; + Scalar offset = 0.75; joint_id = addJointAndBody( model, typename JC::JointModelMimic( @@ -233,7 +233,8 @@ namespace pinocchio #endif template class JointCollectionTpl> - void humanoidRandom(ModelTpl & model, bool usingFF) + void + humanoidRandom(ModelTpl & model, bool usingFF, bool mimic) { typedef JointCollectionTpl JC; typedef ModelTpl Model; @@ -288,8 +289,22 @@ namespace pinocchio addJointAndBody(model, typename JC::JointModelRY(), "larm1_joint", "larm2"); addJointAndBody(model, typename JC::JointModelRZ(), "larm2_joint", "larm3"); addJointAndBody(model, typename JC::JointModelRY(), "larm3_joint", "larm4"); - addJointAndBody(model, typename JC::JointModelRY(), "larm4_joint", "larm5"); - addJointAndBody(model, typename JC::JointModelRX(), "larm5_joint", "larm6"); + Index joint_id = addJointAndBody(model, typename JC::JointModelRY(), "larm4_joint", "larm5"); + + if (mimic) + { + Scalar multiplier = 2.5; + Scalar offset = 0.75; + addJointAndBody( + model, + typename JC::JointModelMimic( + typename JC::JointModelRX(), model.joints[joint_id].derived(), multiplier, offset), + "larm5_joint", "larm6"); + } + else + { + addJointAndBody(model, typename JC::JointModelRX(), "larm5_joint", "larm6"); + } } template class JointCollectionTpl> diff --git a/include/pinocchio/multibody/sample-models.txx b/include/pinocchio/multibody/sample-models.txx index 56458dcd78..39a108e264 100644 --- a/include/pinocchio/multibody/sample-models.txx +++ b/include/pinocchio/multibody/sample-models.txx @@ -20,7 +20,7 @@ namespace pinocchio extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void humanoidRandom( - context::Model &, bool); + context::Model &, bool, bool); } // namespace buildModels } // namespace pinocchio diff --git a/src/multibody/sample-models.cpp b/src/multibody/sample-models.cpp index 2ad7f24976..f5a376d52f 100644 --- a/src/multibody/sample-models.cpp +++ b/src/multibody/sample-models.cpp @@ -21,7 +21,7 @@ namespace pinocchio template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void humanoidRandom( - context::Model &, bool); + context::Model &, bool, bool); } // namespace buildModels } // namespace pinocchio diff --git a/unittest/python/bindings_data.py b/unittest/python/bindings_data.py index 7a2c961916..3fc7b24c19 100644 --- a/unittest/python/bindings_data.py +++ b/unittest/python/bindings_data.py @@ -7,7 +7,7 @@ class TestData(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) self.data = self.model.createData() def test_copy(self): diff --git a/unittest/python/bindings_frame.py b/unittest/python/bindings_frame.py index d0c545eb3b..23ac326f9c 100644 --- a/unittest/python/bindings_frame.py +++ b/unittest/python/bindings_frame.py @@ -8,7 +8,7 @@ class TestFrameBindings(PinocchioTestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) self.parent_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index f82778be9c..1a27e92abf 100644 --- a/unittest/python/bindings_joints.py +++ b/unittest/python/bindings_joints.py @@ -7,7 +7,7 @@ class TestJointsAlgo(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) diff --git a/unittest/python/bindings_kinematics.py b/unittest/python/bindings_kinematics.py index 402e712f1b..721dfa4fde 100644 --- a/unittest/python/bindings_kinematics.py +++ b/unittest/python/bindings_kinematics.py @@ -7,7 +7,7 @@ class TestKinematicsBindings(PinocchioTestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) self.joint_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_model.py b/unittest/python/bindings_model.py index e6b9474d7e..25d257f0ea 100644 --- a/unittest/python/bindings_model.py +++ b/unittest/python/bindings_model.py @@ -8,7 +8,7 @@ class TestModel(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) def test_empty_model_sizes(self): model = pin.Model() diff --git a/unittest/python/bindings_rnea.py b/unittest/python/bindings_rnea.py index 88c54a0f8c..a6c9111707 100644 --- a/unittest/python/bindings_rnea.py +++ b/unittest/python/bindings_rnea.py @@ -7,7 +7,7 @@ class TestRNEA(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom() + self.model = pin.buildSampleModelHumanoidRandom(True, True) self.data = self.model.createData() qmax = np.full((self.model.nq, 1), np.pi) diff --git a/unittest/python/bindings_sample_models.py b/unittest/python/bindings_sample_models.py index 159a5f6139..f55d75b5fb 100644 --- a/unittest/python/bindings_sample_models.py +++ b/unittest/python/bindings_sample_models.py @@ -16,7 +16,7 @@ def test_all_sampled_models(self): self.assertTrue(huamnoid_1 != huamnoid_2) self.assertTrue(huamnoid_1 != huamnoid_3) - manipulator_1 = pin.buildSampleModelManipulator() + manipulator_1 = pin.buildSampleModelManipulator(True) if pin.WITH_HPP_FCL: _geometry_manipulator_1 = pin.buildSampleGeometryModelManipulator( diff --git a/unittest/python/serialization.py b/unittest/python/serialization.py index 66c7d37b34..7adc0a8f3e 100644 --- a/unittest/python/serialization.py +++ b/unittest/python/serialization.py @@ -8,7 +8,7 @@ class TestSerialization(TestCase): def testTXT(self): - model = pin.buildSampleModelHumanoidRandom() + model = pin.buildSampleModelHumanoidRandom(True, True) filename = main_path + "/model.txt" model.saveToText(filename) @@ -18,7 +18,7 @@ def testTXT(self): self.assertTrue(model == model2) def testXML(self): - model = pin.buildSampleModelHumanoidRandom() + model = pin.buildSampleModelHumanoidRandom(True, True) filename = main_path + "/model.xml" tag_name = "Model" model.saveToXML(filename, tag_name) @@ -29,7 +29,7 @@ def testXML(self): self.assertTrue(model == model2) def testBIN(self): - model = pin.buildSampleModelHumanoidRandom() + model = pin.buildSampleModelHumanoidRandom(True, True) filename = main_path + "/model.bin" model.saveToBinary(filename) From 30948c3f9440fe3bbdafecabb22021a9c8166a94 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 080/166] [EtienneAr feedback] Fix size in jointBlock --- include/pinocchio/multibody/joint/joint-model-base.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index de33fc91fd..3c50d940a8 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -549,7 +549,7 @@ namespace pinocchio typename SizeDepType::template BlockReturn::ConstType jointJacBlock_impl(const Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); + return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } // Non-const access @@ -578,7 +578,7 @@ namespace pinocchio typename SizeDepType::template BlockReturn::Type jointJacBlock_impl(Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); + return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } protected: From d5cf0a3623fae22572de301c31902e6217e308af Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 081/166] [EtienneAr feedback] Fix mimic serialization for joint_q joint_v --- .../pinocchio/multibody/joint/joint-mimic.hpp | 84 ++++++++++++++----- .../pinocchio/serialization/joints-data.hpp | 21 +++-- unittest/serialization.cpp | 6 +- 3 files changed, 79 insertions(+), 32 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 0f72ed429a..4ca8290ff3 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -389,10 +389,10 @@ namespace pinocchio : m_scaling((Scalar)0) , S((Scalar)0) { - joint_q.resize(0); - m_q_transform.resize(0); - joint_v.resize(0); - m_v_transform.resize(0); + joint_q.resize(0, 1); + joint_q_transformed.resize(0, 1); + joint_v.resize(0, 1); + joint_v_transformed.resize(0, 1); } // JointDataMimicTpl(const JointDataMimicTpl & other) @@ -406,12 +406,36 @@ namespace pinocchio : m_scaling(scaling) , S(m_jdata_ref.S(), scaling) , m_jdata_ref( + transferToVariant, RefJointData>(jdata)) + { + joint_q.resize(nq, 1); + joint_q_transformed.resize(nq, 1); + joint_v.resize(nv, 1); + joint_v_transformed.resize(nv, 1); + } + + // JointDataMimicTpl(const RefJointDataVariant & jdata, + // const Scalar & scaling, + // const Scalar & nq, + // const Scalar & nv) + // : m_jdata_ref(jdata) + // , m_scaling(scaling) + // , S(m_jdata_ref.S(),scaling) + // { + + // } + + JointDataMimicTpl( + const RefJointData & jdata, const Scalar & scaling, const Scalar & nq, const Scalar & nv) + : m_jdata_ref(jdata.derived()) + , m_scaling(scaling) + , S(m_jdata_ref.S(), scaling) checkMimic(jdata)) { joint_q.resize(nq, 1); - m_q_transform.resize(nq, 1); + joint_q_transformed.resize(nq, 1); joint_v.resize(nv, 1); - m_v_transform.resize(nv, 1); + joint_v_transformed.resize(nv, 1); } JointDataMimicTpl & operator=(const JointDataMimicTpl & other) @@ -419,9 +443,9 @@ namespace pinocchio m_jdata_ref = other.m_jdata_ref; m_scaling = other.m_scaling; joint_q = other.joint_q; - m_q_transform = other.m_q_transform; + joint_q_transformed = other.joint_q_transformed; joint_v = other.joint_v; - m_v_transform = other.m_v_transform; + joint_v_transformed = other.joint_v_transformed; S = Constraint_t(m_jdata_ref.S(), other.m_scaling); return *this; } @@ -431,8 +455,8 @@ namespace pinocchio { return Base::isEqual(other) && m_jdata_ref == other.m_jdata_ref && m_scaling == other.m_scaling && joint_q == other.joint_q - && m_q_transform == other.m_q_transform && joint_v == other.joint_v - && m_v_transform == other.m_v_transform; + && joint_q_transformed == other.joint_q_transformed && joint_v == other.joint_v + && joint_v_transformed == other.joint_v_transformed; } static std::string classname() @@ -575,6 +599,15 @@ namespace pinocchio return joint_q; } + ConfigVector_t & q_transformed() + { + return joint_q_transformed; + } + const ConfigVector_t & q_transformed() const + { + return joint_q_transformed; + } + TangentVector_t & joint_v_accessor() { return joint_v; @@ -584,6 +617,15 @@ namespace pinocchio return joint_v; } + TangentVector_t & v_transformed() + { + return joint_v_transformed; + } + const TangentVector_t & v_transformed() const + { + return joint_v_transformed; + } + protected: RefJointData m_jdata_ref; Scalar m_scaling; @@ -591,11 +633,11 @@ namespace pinocchio /// \brief original configuration vector ConfigVector_t joint_q; /// \brief Transformed configuration vector - ConfigVector_t m_q_transform; + ConfigVector_t joint_q_transformed; /// \brief original velocity vector TangentVector_t joint_v; /// \brief Transform velocity vector. - TangentVector_t m_v_transform; + TangentVector_t joint_v_transformed; public: // data @@ -745,11 +787,10 @@ namespace pinocchio EIGEN_DONT_INLINE void calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const { - jdata.joint_q = qs.segment(idx_q(), nq()); + jdata.joint_q = qs.segment(idx_q(), m_jmodel_ref.nq()); configVectorAffineTransform( - m_jmodel_ref, qs.segment(idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, - jdata.m_q_transform); - m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform); + m_jmodel_ref, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q_transformed); } template @@ -758,13 +799,12 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { - jdata.joint_q = qs.segment(idx_q(), nq()); - jdata.joint_v = vs.segment(idx_v(), nv()); + jdata.joint_q = qs.segment(idx_q(), m_jmodel_ref.nq()); + jdata.joint_v = vs.segment(idx_v(), m_jmodel_ref.nv()); configVectorAffineTransform( - m_jmodel_ref, qs.segment(idx_q(), m_jmodel_ref.nq()), m_scaling, m_offset, - jdata.m_q_transform); - jdata.m_v_transform = m_scaling * vs.segment(idx_v(), m_jmodel_ref.nv()); - m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.m_q_transform, jdata.m_v_transform); + m_jmodel_ref, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + jdata.joint_v_transformed = m_scaling * jdata.joint_v; + m_jmodel_ref.calc(jdata.m_jdata_ref, jdata.joint_q_transformed, jdata.joint_v_transformed); } template diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 8e87969317..9cf657a6d4 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -74,14 +74,18 @@ namespace boost joint_data, const unsigned int /*version*/) { - ar & make_nvp("S", joint_data.S()); - // ar & make_nvp("M",joint_data.M()); - // ar & make_nvp("v",joint_data.v()); - // ar & make_nvp("c",joint_data.c()); + ar & make_nvp("joint_q", joint_data.joint_q()); + ar & make_nvp("joint_v", joint_data.joint_v()); - // ar & make_nvp("U",joint_data.U()); - // ar & make_nvp("Dinv",joint_data.Dinv()); - // ar & make_nvp("UDinv",joint_data.UDinv()); + ar & make_nvp("S", joint_data.S()); + // ar & make_nvp("M", joint_data.M()); + // ar & make_nvp("v", joint_data.v()); + // ar & make_nvp("c", joint_data.c()); + + // ar & make_nvp("U", joint_data.U()); + // ar & make_nvp("Dinv", joint_data.Dinv()); + // ar & make_nvp("UDinv", joint_data.UDinv()); + // ar & make_nvp("StU", joint_data.StU()); } } // namespace fix @@ -270,6 +274,9 @@ namespace boost typedef pinocchio::JointDataMimicTpl JointType; fix::serialize(ar, static_cast &>(joint), version); + ar & make_nvp("m_q_transform", joint.q_transformed()); + ar & make_nvp("m_v_transform", joint.v_transformed()); + ar & make_nvp("jdata", joint.jdata()); ar & make_nvp("scaling", joint.scaling()); } diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 5dac584921..e181d8faff 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -707,7 +707,7 @@ BOOST_AUTO_TEST_CASE(test_model_serialization) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); generic_test(model, TEST_SERIALIZATION_FOLDER "/Model", "Model"); } @@ -717,7 +717,7 @@ BOOST_AUTO_TEST_CASE(test_throw_extension) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); const std::string & fake_filename = "this_is_a_fake_filename"; @@ -752,7 +752,7 @@ BOOST_AUTO_TEST_CASE(test_data_serialization) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); Data data(model); From c5854f1fef0a8b9ea07748cbef05a167ccc5197b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 082/166] [EtienneAr feedback] Remove problematic testcase for mimic in python --- unittest/python/bindings_data.py | 2 +- unittest/python/bindings_frame.py | 2 +- unittest/python/bindings_joints.py | 2 +- unittest/python/bindings_kinematics.py | 2 +- unittest/python/bindings_model.py | 2 +- unittest/python/bindings_rnea.py | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/unittest/python/bindings_data.py b/unittest/python/bindings_data.py index 3fc7b24c19..2c55b55d90 100644 --- a/unittest/python/bindings_data.py +++ b/unittest/python/bindings_data.py @@ -7,7 +7,7 @@ class TestData(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) self.data = self.model.createData() def test_copy(self): diff --git a/unittest/python/bindings_frame.py b/unittest/python/bindings_frame.py index 23ac326f9c..20531f2889 100644 --- a/unittest/python/bindings_frame.py +++ b/unittest/python/bindings_frame.py @@ -8,7 +8,7 @@ class TestFrameBindings(PinocchioTestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) self.parent_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index 1a27e92abf..69be539214 100644 --- a/unittest/python/bindings_joints.py +++ b/unittest/python/bindings_joints.py @@ -7,7 +7,7 @@ class TestJointsAlgo(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) diff --git a/unittest/python/bindings_kinematics.py b/unittest/python/bindings_kinematics.py index 721dfa4fde..07faa508ed 100644 --- a/unittest/python/bindings_kinematics.py +++ b/unittest/python/bindings_kinematics.py @@ -7,7 +7,7 @@ class TestKinematicsBindings(PinocchioTestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) self.joint_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_model.py b/unittest/python/bindings_model.py index 25d257f0ea..912596e752 100644 --- a/unittest/python/bindings_model.py +++ b/unittest/python/bindings_model.py @@ -8,7 +8,7 @@ class TestModel(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) def test_empty_model_sizes(self): model = pin.Model() diff --git a/unittest/python/bindings_rnea.py b/unittest/python/bindings_rnea.py index a6c9111707..a3e4f611b6 100644 --- a/unittest/python/bindings_rnea.py +++ b/unittest/python/bindings_rnea.py @@ -7,7 +7,7 @@ class TestRNEA(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) self.data = self.model.createData() qmax = np.full((self.model.nq, 1), np.pi) From ce7c2d31c72ee204fae7b1c302f8b82d939b6dcc Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 083/166] [EtienneAr feedback] Add slightly more test on sample models for mimic --- unittest/sample-models.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index e65d0a8971..b716b43b5c 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -48,6 +48,18 @@ BOOST_AUTO_TEST_CASE(build_model_sample_humanoid_random) BOOST_CHECK(modelff.nq == 32); BOOST_CHECK(modelff.nv == 32); + + pinocchio::Model modelMimic; + pinocchio::buildModels::humanoidRandom(modelMimic, true, true); + + BOOST_CHECK(modelMimic.nq == 32); + BOOST_CHECK(modelMimic.nv == 31); + + pinocchio::Model modelMimicff; + pinocchio::buildModels::humanoidRandom(modelMimicff, false, true); + + BOOST_CHECK(modelMimicff.nq == 31); + BOOST_CHECK(modelMimicff.nv == 31); } BOOST_AUTO_TEST_CASE(build_model_sample_manipulator) From 68bc0c4d907ba461ea2a21e47e7325e4a413d840 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 084/166] [EtienneAr feedback] Fix rebase conflict --- .../pinocchio/multibody/joint/joint-mimic.hpp | 22 ++----------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 4ca8290ff3..89f8ac126b 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -398,22 +398,6 @@ namespace pinocchio // JointDataMimicTpl(const JointDataMimicTpl & other) // { *this = other; } - JointDataMimicTpl( - const RefJointData & jdata, - const Scalar & scaling, - const Scalar & nq, - const Scalar & nv) - : m_scaling(scaling) - , S(m_jdata_ref.S(), scaling) - , m_jdata_ref( - transferToVariant, RefJointData>(jdata)) - { - joint_q.resize(nq, 1); - joint_q_transformed.resize(nq, 1); - joint_v.resize(nv, 1); - joint_v_transformed.resize(nv, 1); - } - // JointDataMimicTpl(const RefJointDataVariant & jdata, // const Scalar & scaling, // const Scalar & nq, @@ -427,10 +411,9 @@ namespace pinocchio JointDataMimicTpl( const RefJointData & jdata, const Scalar & scaling, const Scalar & nq, const Scalar & nv) - : m_jdata_ref(jdata.derived()) + : m_jdata_ref(checkMimic(jdata.derived())) , m_scaling(scaling) , S(m_jdata_ref.S(), scaling) - checkMimic(jdata)) { joint_q.resize(nq, 1); joint_q_transformed.resize(nq, 1); @@ -679,7 +662,7 @@ namespace pinocchio typedef JointCollectionTpl JointCollection; typedef JointModelTpl JointModel; typedef typename JointModel::JointModelVariant JointModelVariant; - + typedef SE3Tpl SE3; typedef MotionTpl Motion; typedef InertiaTpl Inertia; @@ -717,7 +700,6 @@ namespace pinocchio assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); - setMimicIndexes( jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), From 06976239209c07690d98e035bea746bdf864d2b0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 085/166] [EtienneAr feedback] Activate mimic for joint-jacobian test --- unittest/joint-jacobian.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/unittest/joint-jacobian.cpp b/unittest/joint-jacobian.cpp index 4dc54ffff4..913760a731 100644 --- a/unittest/joint-jacobian.cpp +++ b/unittest/joint-jacobian.cpp @@ -31,7 +31,7 @@ BOOST_AUTO_TEST_CASE(test_jacobian) using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); pinocchio::Data data(model); VectorXd q = VectorXd::Zero(model.nq); @@ -77,7 +77,7 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); pinocchio::Data data(model); pinocchio::Data data_ref(model); @@ -234,7 +234,7 @@ BOOST_AUTO_TEST_CASE(test_timings) using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); pinocchio::Data data(model); long flag = BOOST_BINARY(1111); From 251ec2b8f1724018c4ae1b51577d7fd134fe88e8 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 086/166] [EtienneAr feedback] Fix some size for mimic col/row selection --- .../multibody/joint/joint-model-base.hpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index 3c50d940a8..18fd002ed2 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -412,7 +412,7 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::ConstType + typename SizeDepType::template ColsReturn::ConstType jointJacCols(const Eigen::MatrixBase & A) const { return derived().jointJacCols_impl(A.derived()); @@ -426,10 +426,10 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::ConstType + typename SizeDepType::template ColsReturn::ConstType jointJacCols_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_j(), nj()); + return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } // Non-const access @@ -442,7 +442,7 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::Type + typename SizeDepType::template ColsReturn::Type jointJacCols(Eigen::MatrixBase & A) const { return derived().jointJacCols_impl(A.derived()); @@ -456,10 +456,10 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::Type + typename SizeDepType::template ColsReturn::Type jointJacCols_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_j(), nj()); + return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } /* Acces to dedicated rows in a matrix.*/ @@ -472,7 +472,7 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::ConstType + typename SizeDepType::template RowsReturn::ConstType jointJacRows(const Eigen::MatrixBase & A) const { return derived().jointJacRows_impl(A.derived()); @@ -486,10 +486,10 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::ConstType + typename SizeDepType::template RowsReturn::ConstType jointJacRows_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_j(), nj()); + return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } // Non-const access @@ -501,7 +501,7 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::Type + typename SizeDepType::template RowsReturn::Type jointJacRows(Eigen::MatrixBase & A) const { return derived().jointJacRows_impl(A.derived()); @@ -515,10 +515,10 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::Type + typename SizeDepType::template RowsReturn::Type jointJacRows_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_j(), nj()); + return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the @@ -532,7 +532,7 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::ConstType + typename SizeDepType::template BlockReturn::ConstType jointJacBlock(const Eigen::MatrixBase & Mat) const { return derived().jointJacBlock_impl(Mat.derived()); @@ -546,7 +546,7 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::ConstType + typename SizeDepType::template BlockReturn::ConstType jointJacBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); @@ -561,7 +561,7 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::Type + typename SizeDepType::template BlockReturn::Type jointJacBlock(Eigen::MatrixBase & Mat) const { return derived().jointJacBlock_impl(Mat.derived()); @@ -575,7 +575,7 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::Type + typename SizeDepType::template BlockReturn::Type jointJacBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); From 54c4af23bb0169cecb4e29587c047cbc4989a54f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 087/166] [EtienneAr feedback] Fix joint jacobians computation for mimic --- include/pinocchio/algorithm/jacobian.hxx | 69 ++++++++++++++++++------ 1 file changed, 54 insertions(+), 15 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 20e596b00a..a211254856 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -61,8 +61,12 @@ namespace pinocchio jmodel.jointCols(J_) = data.oMi[i].act(jdata.S()); ======= Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); +<<<<<<< HEAD jmodel.jointVelCols(J_) = data.oMi[i].act(jdata.S()); >>>>>>> a55d3bf7 ([EtienneAr feedback] Split jointCols jointRows and jointBLock for full and reduced system) +======= + jmodel.jointJacCols(J_) = data.oMi[i].act(jdata.S()); +>>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) } }; @@ -189,50 +193,73 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.rows(), 6); - PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nv); + PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nj); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); +<<<<<<< HEAD Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); +======= + Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); + Jout_.fill(0); +>>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) + typedef typename ModelTpl::JointIndex JointIndex; typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; typedef typename Matrix6xLikeOut::ColXpr ColXprOut; typedef MotionRef MotionOut; - const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1; switch (rf) { case WORLD: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + MotionIn v_in(Jin.col(col_in)); + MotionOut v_out(Jout_.col(col_out)); - v_out = v_in; + v_out += v_in; + } } break; } case LOCAL_WORLD_ALIGNED: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + MotionIn v_in(Jin.col(col_in)); + MotionOut v_out(Jout_.col(col_out)); - v_out = v_in; - v_out.linear() -= placement.translation().cross(v_in.angular()); + v_out += v_in; + v_out.linear() -= placement.translation().cross(v_in.angular()); + } } break; } case LOCAL: { - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - MotionIn v_in(Jin.col(j)); - MotionOut v_out(Jout_.col(j)); + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + MotionIn v_in(Jin.col(col_in)); + MotionOut v_out(Jout_.col(col_out)); - v_out = placement.actInv(v_in); + v_out += placement.actInv(v_in); + } } break; } @@ -328,8 +355,12 @@ namespace pinocchio jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); ======= Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); +<<<<<<< HEAD jmodel.jointVelCols(J_) = data.iMf[i].actInv(jdata.S()); >>>>>>> a55d3bf7 ([EtienneAr feedback] Split jointCols jointRows and jointBLock for full and reduced system) +======= + jmodel.jointVelCols(J_) += data.iMf[i].actInv(jdata.S()); +>>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) } }; @@ -354,14 +385,22 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; data.iMf[jointId].setIdentity(); + + Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); + J_.fill(0); + typedef JointJacobianForwardStep< Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> Pass; for (JointIndex i = jointId; i > 0; i = model.parents[i]) { Pass::run( +<<<<<<< HEAD model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); +======= + model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J_)); +>>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) } } From c797831f462510dab6f43deb85fcab386d683404 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 088/166] [EtienneAr feedback] WiP fix crba malloc --- .../joint-motion-subspace-generic.hpp | 203 ++++++++++++++++++ .../multibody/joint-motion-subspace.hpp | 3 + .../pinocchio/multibody/joint/joint-mimic.hpp | 38 ++-- unittest/crba.cpp | 8 +- 4 files changed, 227 insertions(+), 25 deletions(-) diff --git a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp index 7ef1026b50..c09876a695 100644 --- a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp @@ -207,6 +207,209 @@ namespace pinocchio }; } // namespace details + template + struct traits> + { + typedef _Scalar Scalar; + enum + { + LINEAR = 0, + ANGULAR = 3, + Options = _Options, + MaxDim = _MaxDim + }; + + typedef MotionTpl JointMotion; + typedef Eigen::Matrix JointForce; + typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix + ReducedSquaredMatrix; + + typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType; + typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType; + + typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; + }; // traits JointMotionSubspacePreallocTpl + + template + struct SE3GroupAction> + { + typedef Eigen::Matrix ReturnType; + }; + + template + struct MotionAlgebraAction, MotionDerived> + { + typedef Eigen::Matrix ReturnType; + }; + + template + struct JointMotionSubspacePreallocTpl + : public JointMotionSubspaceBase> + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef JointMotionSubspaceBase Base; + + friend class JointMotionSubspaceBase; + PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspacePreallocTpl) + + enum + { + NV = Eigen::Dynamic + }; + + using Base::nv; + + JointMotionSubspacePreallocTpl() + : S(0) + { + } + + template + explicit JointMotionSubspacePreallocTpl(const Eigen::MatrixBase & _S) + : S(_S) + { + // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace + // path + // TODO + EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D); + } + + // It is only valid for dynamics size + explicit JointMotionSubspacePreallocTpl(const int dim) + : S(6, dim) + { + assert(dim <= _MaxDim); + } + + template + JointMotionSubspacePreallocTpl(const JointMotionSubspaceTpl & subspace) + : S(subspace.matrix()) + { + } + + static JointMotionSubspacePreallocTpl Zero(const int dim) + { + return JointMotionSubspacePreallocTpl(dim); + } + + template + JointMotion __mult__(const Eigen::MatrixBase & vj) const + { + return JointMotion(S * vj); + } + + struct Transpose : JointMotionSubspaceTransposeBase + { + const JointMotionSubspacePreallocTpl & ref; + Transpose(const JointMotionSubspacePreallocTpl & ref) + : ref(ref) + { + } + + template + JointForce operator*(const ForceDense & f) const + { + return (ref.S.transpose() * f.toVector()).eval(); + } + + template + typename Eigen::Matrix + operator*(const Eigen::MatrixBase & F) + { + return ref.S.transpose() * F; + } + }; + + Transpose transpose() const + { + return Transpose(*this); + } + + MatrixReturnType matrix_impl() + { + return S; + } + ConstMatrixReturnType matrix_impl() const + { + return S; + } + + int nv_impl() const + { + return (int)S.cols(); + } + + template + friend typename JointMotionSubspacePreallocTpl<_MaxDim, _Scalar, _Options>::DenseBase + operator*(const InertiaTpl & Y, const JointMotionSubspacePreallocTpl & S) + { + typedef typename JointMotionSubspacePreallocTpl::DenseBase ReturnType; + ReturnType res(6, S.nv()); + motionSet::inertiaAction(Y, S.S, res); + return res; + } + + template + friend Eigen::Matrix<_Scalar, 6, Eigen::Dynamic, 6, _MaxDim> + operator*(const Eigen::Matrix & Ymatrix, const JointMotionSubspacePreallocTpl & S) + { + typedef Eigen::Matrix<_Scalar, 6, Eigen::Dynamic, 6, _MaxDim> ReturnType; + return ReturnType(Ymatrix * S.matrix()); + } + + DenseBase se3Action(const SE3Tpl & m) const + { + DenseBase res(6, nv()); + motionSet::se3Action(m, S, res); + return res; + } + + DenseBase se3ActionInverse(const SE3Tpl & m) const + { + DenseBase res(6, nv()); + motionSet::se3ActionInverse(m, S, res); + return res; + } + + template + DenseBase motionAction(const MotionDense & v) const + { + DenseBase res(6, nv()); + motionSet::motionAction(v, S, res); + return res; + } + + void disp_impl(std::ostream & os) const + { + os << "S =\n" << S << std::endl; + } + + bool isEqual(const JointMotionSubspacePreallocTpl & other) const + { + return S == other.S; + } + + protected: + DenseBase S; + }; // class JointMotionSubspacePreallocTpl + + namespace details + { + template + struct StDiagonalMatrixSOperation> + { + typedef JointMotionSubspacePreallocTpl Constraint; + typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; + + static ReturnType run(const JointMotionSubspaceBase & constraint) + { + return constraint.matrix().transpose() * constraint.matrix(); + } + }; + } // namespace details + } // namespace pinocchio #endif // ifndef __pinocchio_multibody_constraint_generic_hpp__ diff --git a/include/pinocchio/multibody/joint-motion-subspace.hpp b/include/pinocchio/multibody/joint-motion-subspace.hpp index f5129138a5..a8dd9d78f7 100644 --- a/include/pinocchio/multibody/joint-motion-subspace.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace.hpp @@ -13,6 +13,9 @@ namespace pinocchio template struct JointMotionSubspaceTpl; + template + struct JointMotionSubspacePreallocTpl; + typedef JointMotionSubspaceTpl<1, context::Scalar, context::Options> JointMotionSubspace1d; typedef JointMotionSubspaceTpl<3, context::Scalar, context::Options> JointMotionSubspace3d; typedef JointMotionSubspaceTpl<6, context::Scalar, context::Options> JointMotionSubspace6d; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 89f8ac126b..693bdbee81 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -25,7 +25,7 @@ namespace pinocchio template struct traits> { - typedef JointMotionSubspaceTpl RefJointMotionSubspace; + typedef JointMotionSubspacePreallocTpl<6, _Scalar, _Options> RefJointMotionSubspace; typedef typename traits::Scalar Scalar; enum { @@ -64,9 +64,8 @@ namespace pinocchio { typedef typename traits< ScaledJointMotionSubspaceTpl<_Scalar, _Options>>::RefJointMotionSubspace::Scalar Scalar; - // typedef typename ConstraintForceOp>::RefJointMotionSubspace,ForceDerived>::ReturnType OriginalReturnType; - typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options> OriginalReturnType; + typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options, 6, 6> + OriginalReturnType; typedef typename ScalarMatrixProduct::type IdealReturnType; typedef Eigen::Matrix< @@ -82,16 +81,15 @@ namespace pinocchio { typedef typename traits< ScaledJointMotionSubspaceTpl<_Scalar, _Options>>::RefJointMotionSubspace::Scalar Scalar; - // typedef typename ConstraintForceSetOp>::RefJointMotionSubspace, ForceSet>::ReturnType OriginalReturnType; - typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options> OriginalReturnType; - typedef typename ScalarMatrixProduct::type IdealReturnType; + typedef Eigen::Matrix< Scalar, - traits>::RefJointMotionSubspace::NV, - ForceSet::ColsAtCompileTime, + Eigen::Dynamic, + Eigen::Dynamic, traits>::RefJointMotionSubspace::Options - | Eigen::RowMajor> + | Eigen::RowMajor, + 6, + 6> ReturnType; }; @@ -124,8 +122,8 @@ namespace pinocchio { } - ScaledJointMotionSubspaceTpl( - const RefJointMotionSubspace & constraint, const Scalar & scaling_factor) + template + ScaledJointMotionSubspaceTpl(const ConstraintTpl & constraint, const Scalar & scaling_factor) : m_constraint(constraint) , m_scaling_factor(scaling_factor) { @@ -184,8 +182,6 @@ namespace pinocchio typename ConstraintForceOp::ReturnType operator*(const ForceDense & f) const { - // TODO: I don't know why, but we should a dense a return type, otherwise it failes at the - // evaluation level; typedef typename ConstraintForceOp::ReturnType ReturnType; return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); @@ -198,7 +194,8 @@ namespace pinocchio { typedef typename ConstraintForceSetOp::ReturnType ReturnType; - return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F)); + ReturnType tmp = ref.m_constraint.transpose() * F; + return ReturnType(ref.m_scaling_factor * tmp); } }; // struct TransposeConst @@ -268,7 +265,7 @@ namespace pinocchio // typedef typename MultiplicationOp::ReturnType // OriginalReturnType; - typedef Eigen::Matrix ReturnType; + typedef Eigen::Matrix ReturnType; // typedef typename ScalarMatrixProduct::type ReturnType; // typedef OriginalReturnType ReturnType; }; @@ -285,7 +282,8 @@ namespace pinocchio static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) { - return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); + ReturnType tmp = (Y * scaled_constraint.constraint()); + return scaled_constraint.scaling() * tmp; } }; } // namespace impl @@ -293,9 +291,7 @@ namespace pinocchio template struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef ScaledJointMotionSubspaceTpl Constraint; - typedef typename MultiplicationOp::ReturnType OriginalReturnType; - typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType; + typedef Eigen::Matrix ReturnType; }; /* [ABA] operator* (Inertia Y,Constraint S) */ diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 19da9b9e35..1fa345fed5 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -64,7 +64,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); pinocchio::Data data(model); #ifdef NDEBUG @@ -128,7 +128,7 @@ BOOST_AUTO_TEST_CASE(test_crba) BOOST_AUTO_TEST_CASE(test_minimal_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); pinocchio::Data data(model), data_ref(model); model.lowerPositionLimit.head<7>().fill(-1.); @@ -166,7 +166,7 @@ BOOST_AUTO_TEST_CASE(test_minimal_crba) BOOST_AUTO_TEST_CASE(test_roto_inertia_effects) { pinocchio::Model model, model_ref; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); model_ref = model; BOOST_CHECK(model == model_ref); @@ -194,7 +194,7 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) { using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model); + pinocchio::buildModels::humanoidRandom(model, true, true); model.addJoint( size_t(model.njoints - 1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), From 4ee2ecf7b47fae76997611d16133850570739541 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 089/166] [EtienneAr feedback] Remove temporary variables before return for joint mimic --- .../pinocchio/multibody/joint/joint-mimic.hpp | 27 +++++-------------- 1 file changed, 6 insertions(+), 21 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 693bdbee81..78187408b9 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -154,15 +154,13 @@ namespace pinocchio template SE3ActionReturnType se3Action(const SE3Tpl & m) const { - SE3ActionReturnType res = m_constraint.se3Action(m); - return m_scaling_factor * res; + return m_scaling_factor * m_constraint.se3Action(m); } template SE3ActionReturnType se3ActionInverse(const SE3Tpl & m) const { - SE3ActionReturnType res = m_constraint.se3ActionInverse(m); - return m_scaling_factor * res; + return m_scaling_factor * m_constraint.se3ActionInverse(m); } int nv_impl() const @@ -182,9 +180,7 @@ namespace pinocchio typename ConstraintForceOp::ReturnType operator*(const ForceDense & f) const { - typedef - typename ConstraintForceOp::ReturnType ReturnType; - return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f)); + return ref.m_scaling_factor * (ref.m_constraint.transpose() * f); } /// [CRBA] MatrixBase operator* (RefConstraint::Transpose S, ForceSet::Block) @@ -192,10 +188,7 @@ namespace pinocchio typename ConstraintForceSetOp::ReturnType operator*(const Eigen::MatrixBase & F) const { - typedef typename ConstraintForceSetOp::ReturnType - ReturnType; - ReturnType tmp = ref.m_constraint.transpose() * F; - return ReturnType(ref.m_scaling_factor * tmp); + return ref.m_scaling_factor * (ref.m_constraint.transpose() * F); } }; // struct TransposeConst @@ -221,10 +214,7 @@ namespace pinocchio typename MotionAlgebraAction::ReturnType motionAction(const MotionDense & m) const { - typedef typename MotionAlgebraAction::ReturnType - ReturnType; - ReturnType res = m_scaling_factor * m_constraint.motionAction(m); - return res; + return m_scaling_factor * m_constraint.motionAction(m); } inline const Scalar & scaling() const @@ -263,11 +253,7 @@ namespace pinocchio typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename Constraint::Scalar Scalar; - // typedef typename MultiplicationOp::ReturnType - // OriginalReturnType; typedef Eigen::Matrix ReturnType; - // typedef typename ScalarMatrixProduct::type ReturnType; - // typedef OriginalReturnType ReturnType; }; /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ @@ -282,8 +268,7 @@ namespace pinocchio static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) { - ReturnType tmp = (Y * scaled_constraint.constraint()); - return scaled_constraint.scaling() * tmp; + return scaled_constraint.scaling() * (Y * scaled_constraint.constraint()); } }; } // namespace impl From 28335c6794892e264f46e2d37b9381274ed4be60 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 090/166] [EtienneAr feedback] Add stack allocation feature for JointMotionSubspaceTpl class and remove dedicated class --- .../joint-motion-subspace-generic.hpp | 261 +++--------------- .../multibody/joint-motion-subspace.hpp | 5 +- .../pinocchio/multibody/joint/joint-mimic.hpp | 2 +- 3 files changed, 38 insertions(+), 230 deletions(-) diff --git a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp index c09876a695..462203a687 100644 --- a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp @@ -9,8 +9,8 @@ namespace pinocchio { - template - struct traits> + template + struct traits> { typedef _Scalar Scalar; enum @@ -22,9 +22,9 @@ namespace pinocchio }; typedef MotionTpl JointMotion; - typedef Eigen::Matrix JointForce; - typedef Eigen::Matrix DenseBase; - typedef Eigen::Matrix ReducedSquaredMatrix; + typedef Eigen::Matrix JointForce; + typedef Eigen::Matrix DenseBase; + typedef Eigen::Matrix ReducedSquaredMatrix; typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType; typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType; @@ -32,21 +32,21 @@ namespace pinocchio typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; }; // traits JointMotionSubspaceTpl - template - struct SE3GroupAction> + template + struct SE3GroupAction> { - typedef Eigen::Matrix ReturnType; + typedef Eigen::Matrix ReturnType; }; - template - struct MotionAlgebraAction, MotionDerived> + template + struct MotionAlgebraAction, MotionDerived> { - typedef Eigen::Matrix ReturnType; + typedef Eigen::Matrix ReturnType; }; - template + template struct JointMotionSubspaceTpl - : public JointMotionSubspaceBase> + : public JointMotionSubspaceBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -60,6 +60,8 @@ namespace pinocchio NV = _Dim }; + constexpr static int MaxNV = NV < 0 ? _MaxDim : NV; + using Base::nv; template @@ -84,7 +86,18 @@ namespace pinocchio : S(6, dim) { EIGEN_STATIC_ASSERT( - _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR) + _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR); + assert(_MaxDim < 0 || dim <= _MaxDim); + } + + // It is only valid for dynamics size + template + explicit JointMotionSubspaceTpl(const JointMotionSubspaceTpl subspace) + : S(subspace.matrix()) + { + EIGEN_STATIC_ASSERT( + _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR); + assert(_MaxDim < 0 || subspace.matrix().cols() <= _MaxDim); } static JointMotionSubspaceTpl Zero(const int dim) @@ -113,9 +126,10 @@ namespace pinocchio } template - typename Eigen::Matrix operator*(const Eigen::MatrixBase & F) + typename Eigen::Matrix + operator*(const Eigen::MatrixBase & F) { - return (ref.S.transpose() * F).eval(); + return ref.S.transpose() * F; } }; @@ -139,7 +153,7 @@ namespace pinocchio } template - friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options>::DenseBase + friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options, _MaxDim>::DenseBase operator*(const InertiaTpl & Y, const JointMotionSubspaceTpl & S) { typedef typename JointMotionSubspaceTpl::DenseBase ReturnType; @@ -149,10 +163,10 @@ namespace pinocchio } template - friend Eigen::Matrix<_Scalar, 6, _Dim> + friend Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim> operator*(const Eigen::Matrix & Ymatrix, const JointMotionSubspaceTpl & S) { - typedef Eigen::Matrix<_Scalar, 6, _Dim> ReturnType; + typedef Eigen::Matrix<_Scalar, 6, _Dim, _Options, 6, _MaxDim> ReturnType; return ReturnType(Ymatrix * S.matrix()); } @@ -194,213 +208,10 @@ namespace pinocchio namespace details { - template - struct StDiagonalMatrixSOperation> - { - typedef JointMotionSubspaceTpl Constraint; - typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; - - static ReturnType run(const JointMotionSubspaceBase & constraint) - { - return constraint.matrix().transpose() * constraint.matrix(); - } - }; - } // namespace details - - template - struct traits> - { - typedef _Scalar Scalar; - enum - { - LINEAR = 0, - ANGULAR = 3, - Options = _Options, - MaxDim = _MaxDim - }; - - typedef MotionTpl JointMotion; - typedef Eigen::Matrix JointForce; - typedef Eigen::Matrix DenseBase; - typedef Eigen::Matrix - ReducedSquaredMatrix; - - typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType; - typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType; - - typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; - }; // traits JointMotionSubspacePreallocTpl - - template - struct SE3GroupAction> - { - typedef Eigen::Matrix ReturnType; - }; - - template - struct MotionAlgebraAction, MotionDerived> - { - typedef Eigen::Matrix ReturnType; - }; - - template - struct JointMotionSubspacePreallocTpl - : public JointMotionSubspaceBase> - { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef JointMotionSubspaceBase Base; - - friend class JointMotionSubspaceBase; - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspacePreallocTpl) - - enum - { - NV = Eigen::Dynamic - }; - - using Base::nv; - - JointMotionSubspacePreallocTpl() - : S(0) - { - } - - template - explicit JointMotionSubspacePreallocTpl(const Eigen::MatrixBase & _S) - : S(_S) - { - // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace - // path - // TODO - EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D); - } - - // It is only valid for dynamics size - explicit JointMotionSubspacePreallocTpl(const int dim) - : S(6, dim) - { - assert(dim <= _MaxDim); - } - - template - JointMotionSubspacePreallocTpl(const JointMotionSubspaceTpl & subspace) - : S(subspace.matrix()) - { - } - - static JointMotionSubspacePreallocTpl Zero(const int dim) - { - return JointMotionSubspacePreallocTpl(dim); - } - - template - JointMotion __mult__(const Eigen::MatrixBase & vj) const - { - return JointMotion(S * vj); - } - - struct Transpose : JointMotionSubspaceTransposeBase - { - const JointMotionSubspacePreallocTpl & ref; - Transpose(const JointMotionSubspacePreallocTpl & ref) - : ref(ref) - { - } - - template - JointForce operator*(const ForceDense & f) const - { - return (ref.S.transpose() * f.toVector()).eval(); - } - - template - typename Eigen::Matrix - operator*(const Eigen::MatrixBase & F) - { - return ref.S.transpose() * F; - } - }; - - Transpose transpose() const - { - return Transpose(*this); - } - - MatrixReturnType matrix_impl() - { - return S; - } - ConstMatrixReturnType matrix_impl() const - { - return S; - } - - int nv_impl() const - { - return (int)S.cols(); - } - - template - friend typename JointMotionSubspacePreallocTpl<_MaxDim, _Scalar, _Options>::DenseBase - operator*(const InertiaTpl & Y, const JointMotionSubspacePreallocTpl & S) - { - typedef typename JointMotionSubspacePreallocTpl::DenseBase ReturnType; - ReturnType res(6, S.nv()); - motionSet::inertiaAction(Y, S.S, res); - return res; - } - - template - friend Eigen::Matrix<_Scalar, 6, Eigen::Dynamic, 6, _MaxDim> - operator*(const Eigen::Matrix & Ymatrix, const JointMotionSubspacePreallocTpl & S) - { - typedef Eigen::Matrix<_Scalar, 6, Eigen::Dynamic, 6, _MaxDim> ReturnType; - return ReturnType(Ymatrix * S.matrix()); - } - - DenseBase se3Action(const SE3Tpl & m) const - { - DenseBase res(6, nv()); - motionSet::se3Action(m, S, res); - return res; - } - - DenseBase se3ActionInverse(const SE3Tpl & m) const - { - DenseBase res(6, nv()); - motionSet::se3ActionInverse(m, S, res); - return res; - } - - template - DenseBase motionAction(const MotionDense & v) const - { - DenseBase res(6, nv()); - motionSet::motionAction(v, S, res); - return res; - } - - void disp_impl(std::ostream & os) const - { - os << "S =\n" << S << std::endl; - } - - bool isEqual(const JointMotionSubspacePreallocTpl & other) const - { - return S == other.S; - } - - protected: - DenseBase S; - }; // class JointMotionSubspacePreallocTpl - - namespace details - { - template - struct StDiagonalMatrixSOperation> + template + struct StDiagonalMatrixSOperation> { - typedef JointMotionSubspacePreallocTpl Constraint; + typedef JointMotionSubspaceTpl Constraint; typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; static ReturnType run(const JointMotionSubspaceBase & constraint) diff --git a/include/pinocchio/multibody/joint-motion-subspace.hpp b/include/pinocchio/multibody/joint-motion-subspace.hpp index a8dd9d78f7..ece1448736 100644 --- a/include/pinocchio/multibody/joint-motion-subspace.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace.hpp @@ -10,12 +10,9 @@ namespace pinocchio { - template + template struct JointMotionSubspaceTpl; - template - struct JointMotionSubspacePreallocTpl; - typedef JointMotionSubspaceTpl<1, context::Scalar, context::Options> JointMotionSubspace1d; typedef JointMotionSubspaceTpl<3, context::Scalar, context::Options> JointMotionSubspace3d; typedef JointMotionSubspaceTpl<6, context::Scalar, context::Options> JointMotionSubspace6d; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 78187408b9..5d64496fe0 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -25,7 +25,7 @@ namespace pinocchio template struct traits> { - typedef JointMotionSubspacePreallocTpl<6, _Scalar, _Options> RefJointMotionSubspace; + typedef JointMotionSubspaceTpl RefJointMotionSubspace; typedef typename traits::Scalar Scalar; enum { From d8efd15b05c9082027496352dee42343def40424 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 091/166] [CRBA] Update computation of Ag --- include/pinocchio/algorithm/centroidal.hxx | 2 +- include/pinocchio/algorithm/crba.hxx | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 1effed60c6..ed888cbd3f 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -140,7 +140,7 @@ namespace pinocchio J_cols = data.oMi[i].act(jdata.S()); ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); data.oYcrb[parent] += data.oYcrb[i]; } diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 2f98b2ec20..d84bf297f7 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -79,8 +79,8 @@ namespace pinocchio // Centroidal momentum map ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); ColsBlock J_cols = jmodel.jointJacCols(data.J); - motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); - + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + // Joint Space Inertia Matrix jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); @@ -240,6 +240,7 @@ namespace pinocchio } data.M.setZero(); + data.Ag.setZero(); typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -336,4 +337,4 @@ namespace pinocchio /// @endcond -#endif // ifndef __pinocchio_crba_hxx__ +#endif // ifndef __pinocchio_crba_hxx__ \ No newline at end of file From 40857956f4fd91b1925d7c9d88b263dc72d1d545 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 092/166] [EtienneAr feedback] Small serialization fix after changes in JoitMotionSubspaceTpl --- include/pinocchio/serialization/joints-motion-subspace.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/serialization/joints-motion-subspace.hpp b/include/pinocchio/serialization/joints-motion-subspace.hpp index 154ce4110c..73778bf1a0 100644 --- a/include/pinocchio/serialization/joints-motion-subspace.hpp +++ b/include/pinocchio/serialization/joints-motion-subspace.hpp @@ -101,10 +101,10 @@ namespace boost ar & make_nvp("angularSubspace", S.angularSubspace()); } - template + template void serialize( Archive & ar, - pinocchio::JointMotionSubspaceTpl & S, + pinocchio::JointMotionSubspaceTpl & S, const unsigned int /*version*/) { ar & make_nvp("matrix", S.matrix()); From b0e395545db00c9aa6328884f652046da0d9cd93 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 093/166] [EtienneAr feedback] Fix on Jcom for mimic --- include/pinocchio/algorithm/center-of-mass.hxx | 6 ++++-- include/pinocchio/algorithm/check.hxx | 2 +- include/pinocchio/multibody/data.hxx | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index 087a5559ff..e09ed85a5c 100644 --- a/include/pinocchio/algorithm/center-of-mass.hxx +++ b/include/pinocchio/algorithm/center-of-mass.hxx @@ -256,13 +256,14 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); + Jcom_.fill(0); ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) { - jmodel.jointVelCols(Jcom_).col(col_id) = + jmodel.jointVelCols(Jcom_).col(col_id) += data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR) - data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); } @@ -365,13 +366,14 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); + Jcom_.fill(0); ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) { - jmodel.jointVelCols(Jcom_).col(col_id) = + jmodel.jointVelCols(Jcom_).col(col_id) += Jcols.col(col_id).template segment<3>(Motion::LINEAR) - data.com[subtree_root_id].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR)); } diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 52042f9a6c..94783bf975 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -125,7 +125,7 @@ namespace pinocchio CHECK_DATA(data.D.size() == model.nv); CHECK_DATA(data.tmp.size() >= model.nv); CHECK_DATA(data.J.cols() == model.nj); - CHECK_DATA(data.Jcom.cols() == model.nj); + CHECK_DATA(data.Jcom.cols() == model.nv); CHECK_DATA(data.torque_residual.size() == model.nv); CHECK_DATA(data.dq_after.size() == model.nv); // CHECK_DATA( data.impulse_c.size()== model.nv ); diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 4e101a3cea..80f2eb28dd 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -97,7 +97,7 @@ namespace pinocchio , vcom((std::size_t)model.njoints, Vector3::Zero()) , acom((std::size_t)model.njoints, Vector3::Zero()) , mass((std::size_t)model.njoints, (Scalar)(-1)) - , Jcom(Matrix3x::Zero(3, model.nj)) + , Jcom(Matrix3x::Zero(3, model.nv)) , kinetic_energy(Scalar(0)) , potential_energy(Scalar(0)) , mechanical_energy(Scalar(0)) From 653a9cb2746f458133fa8db4d690f4c5d48366f5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 094/166] [EtienneAr feedback] Fix corriolis computation for mimic leading --- include/pinocchio/algorithm/rnea.hxx | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index be1ed9c76e..d7d34809d7 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -580,7 +580,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6); + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nj(), 6); typedef typename SizeDepType::template ColsReturn::Type @@ -598,14 +598,12 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() += - Ag_cols.transpose() * data.dJ.col(j); + jmodel.jointVelRows(data.C).col(j).noalias() += Ag_cols.transpose() * data.dJ.col(j); - Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; + Mat_tmp.topRows(jmodel.nj()).noalias() = J_cols.transpose() * data.B[i]; for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() += - Mat_tmp * data.J.col(j); + jmodel.jointVelRows(data.C).col(j).noalias() += Mat_tmp * data.J.col(j); if (parent > 0) { @@ -676,7 +674,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6); + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nj(), 6); typedef typename SizeDepType::template ColsReturn::Type @@ -698,7 +696,7 @@ namespace pinocchio j = data.parents_fromRow[(JointIndex)j]) jmodel.jointVelRows(data.C).col(j).noalias() = Ag_cols.transpose() * data.dJ.col(j); - Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; + Mat_tmp.topRows(jmodel.nj()).noalias() = J_cols.transpose() * data.B[i]; for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) jmodel.jointVelRows(data.C).col(j) += Mat_tmp * data.J.col(j); From e38611326637db34d214cf61ae03f592e7599add Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 095/166] [EtienneAr feedback] Enable mimic in rnea tests --- unittest/rnea.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 51b7842c63..ba7c688511 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE(test_rnea) using namespace pinocchio; pinocchio::Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -79,7 +79,7 @@ BOOST_AUTO_TEST_CASE(test_nle_vs_rnea) using namespace pinocchio; pinocchio::Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -136,7 +136,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_with_fext) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -179,7 +179,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_with_armature) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); model.lowerPositionLimit.head<3>().fill(-1.); @@ -209,7 +209,7 @@ BOOST_AUTO_TEST_CASE(test_compute_gravity) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -239,7 +239,7 @@ BOOST_AUTO_TEST_CASE(test_compute_static_torque) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -285,7 +285,7 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis) const double prec = Eigen::NumTraits::dummy_precision(); Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, true, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); From d1aa428dbf6f084baa6725c144c52c4df06dda8c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 096/166] [EtienneAr feedback] Define MaxDim in ScaledJointMotionSubspace and fix mimic test with it --- .../pinocchio/multibody/joint/joint-mimic.hpp | 45 ++++++++++++++----- unittest/joint-mimic.cpp | 2 +- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 5d64496fe0..6e4c41e183 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -25,7 +25,12 @@ namespace pinocchio template struct traits> { - typedef JointMotionSubspaceTpl RefJointMotionSubspace; + enum + { + MaxDim = 6 + }; + typedef JointMotionSubspaceTpl + RefJointMotionSubspace; typedef typename traits::Scalar Scalar; enum { @@ -64,7 +69,13 @@ namespace pinocchio { typedef typename traits< ScaledJointMotionSubspaceTpl<_Scalar, _Options>>::RefJointMotionSubspace::Scalar Scalar; - typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options, 6, 6> + typedef Eigen::Matrix< + _Scalar, + Eigen::Dynamic, + Eigen::Dynamic, + _Options, + traits>::MaxDim, + traits>::MaxDim> OriginalReturnType; typedef typename ScalarMatrixProduct::type IdealReturnType; @@ -79,17 +90,16 @@ namespace pinocchio template struct ConstraintForceSetOp, ForceSet> { - typedef typename traits< - ScaledJointMotionSubspaceTpl<_Scalar, _Options>>::RefJointMotionSubspace::Scalar Scalar; + typedef ScaledJointMotionSubspaceTpl<_Scalar, _Options> MotionSubspace; + typedef typename traits::RefJointMotionSubspace::Scalar Scalar; typedef Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, - traits>::RefJointMotionSubspace::Options - | Eigen::RowMajor, - 6, - 6> + traits::RefJointMotionSubspace::Options | Eigen::RowMajor, + traits::MaxDim, + traits::MaxDim> ReturnType; }; @@ -109,6 +119,11 @@ namespace pinocchio typedef typename traits>::RefJointMotionSubspace RefJointMotionSubspace; + enum + { + MaxDim = traits < ScaledJointMotionSubspaceTpl < _Scalar, + _Options >> ::MaxDim + }; typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; ScaledJointMotionSubspaceTpl() @@ -253,7 +268,9 @@ namespace pinocchio typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename Constraint::Scalar Scalar; - typedef Eigen::Matrix ReturnType; + typedef Eigen:: + Matrix::MaxDim, traits::MaxDim> + ReturnType; }; /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ @@ -276,7 +293,15 @@ namespace pinocchio template struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef Eigen::Matrix ReturnType; + typedef ScaledJointMotionSubspaceTpl MotionSubspace; + typedef Eigen::Matrix< + S2, + 6, + Eigen::Dynamic, + O2, + traits::MaxDim, + traits::MaxDim> + ReturnType; }; /* [ABA] operator* (Inertia Y,Constraint S) */ diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index d457ee2420..8da9a7880c 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -64,7 +64,7 @@ void test_constraint_mimic(const JointModelBase & jmodel) // Test transpose operations { - const Eigen::DenseIndex dim = 20; + const Eigen::DenseIndex dim = ScaledConstraint::MaxDim; const Matrix6x Fin = Matrix6x::Random(6, dim); Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin; Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin); From 5ddbb7c408c8e25af4a9876bbae217fe45041c90 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 097/166] [EtienneAr feedback] Add dedicated mimic test for crba --- unittest/crba.cpp | 55 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 1fa345fed5..f2867c62f6 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -125,6 +125,61 @@ BOOST_AUTO_TEST_CASE(test_crba) #endif // ifndef NDEBUG } +BOOST_AUTO_TEST_CASE(test_crba_mimic) +{ + pinocchio::Model model_mimic, model_full; + pinocchio::buildModels::humanoidRandom(model_mimic, true, true); + pinocchio::buildModels::humanoidRandom(model_full, true, false); + + // HumanoidRandom constants + const int primary_idxq = model_mimic.nq - 1; + const int primary_idxv = model_mimic.nv - 1; + const int secondary_idxq = model_full.nq - 1; + const int secondary_idxv = model_full.nv - 1; + const double ratio = 2.5; + const double offset = 0.75; + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); + G.topLeftCorner(model_mimic.nv, model_mimic.nv).setIdentity(); + G(secondary_idxv, primary_idxv) = ratio; + + // Make both model match + for (size_t i = 0; i < model_full.njoints; i++) + { + model_full.inertias[i] = model_mimic.inertias[i]; + model_full.jointPlacements[i] = model_mimic.jointPlacements[i]; + } + + pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_full(model_full); + + // Prepare test data + Eigen::VectorXd q_mimic = Eigen::VectorXd::Ones(model_mimic.nq); + q_mimic.segment<4>(3).normalize(); + Eigen::VectorXd v_mimic = Eigen::VectorXd::Zero(model_mimic.nv); + Eigen::VectorXd a_mimic = Eigen::VectorXd::Zero(model_mimic.nv); + + Eigen::VectorXd q_full(model_full.nq), v_full(model_full.nv), a_full(model_full.nv); + q_full.head(model_mimic.nq) = q_mimic; + v_full.head(model_mimic.nv) = v_mimic; + a_full.head(model_mimic.nv) = a_mimic; + + q_full[secondary_idxq] = q_mimic[primary_idxq] * ratio + offset; + v_full[secondary_idxv] = v_mimic[primary_idxv] * ratio; + a_full[secondary_idxv] = a_mimic[primary_idxv] * ratio; + + // Run crba + pinocchio::crba(model_mimic, data_mimic, q_mimic); + pinocchio::crba(model_full, data_full, q_full); + + // Compute other half of matrix + data_mimic.M.triangularView() = + data_mimic.M.transpose().triangularView(); + data_full.M.triangularView() = + data_full.M.transpose().triangularView(); + + BOOST_CHECK(data_mimic.M.isApprox(G.transpose() * data_full.M * G, 1e-12)); +} + BOOST_AUTO_TEST_CASE(test_minimal_crba) { pinocchio::Model model; From 1a42e7210ff0309a05ce9e91f32cf8181c153ff5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 098/166] [EtienneAr feedback] Template ScaledMotionSubspace on MaxDim --- .../pinocchio/multibody/joint/joint-mimic.hpp | 116 ++++++++---------- .../serialization/joints-motion-subspace.hpp | 4 +- unittest/joint-mimic.cpp | 2 +- 3 files changed, 54 insertions(+), 68 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 6e4c41e183..3a9a0fbbe7 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -19,17 +19,17 @@ namespace pinocchio { - template + template struct ScaledJointMotionSubspaceTpl; - template - struct traits> + template + struct traits> { enum { - MaxDim = 6 + MaxDim = _MaxDim }; - typedef JointMotionSubspaceTpl + typedef JointMotionSubspaceTpl RefJointMotionSubspace; typedef typename traits::Scalar Scalar; enum @@ -48,34 +48,31 @@ namespace pinocchio typedef typename traits::ConstMatrixReturnType ConstMatrixReturnType; }; // traits ScaledJointMotionSubspaceTpl - template - struct SE3GroupAction> + template + struct SE3GroupAction> { - typedef - typename SE3GroupAction>:: - RefJointMotionSubspace>::ReturnType ReturnType; + typedef typename SE3GroupAction>::RefJointMotionSubspace>::ReturnType + ReturnType; }; - template - struct MotionAlgebraAction, MotionDerived> + template + struct MotionAlgebraAction< + ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, + MotionDerived> { typedef typename MotionAlgebraAction< - typename traits>::RefJointMotionSubspace, + typename traits< + ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace, MotionDerived>::ReturnType ReturnType; }; - template - struct ConstraintForceOp, ForceDerived> + template + struct ConstraintForceOp, ForceDerived> { - typedef typename traits< - ScaledJointMotionSubspaceTpl<_Scalar, _Options>>::RefJointMotionSubspace::Scalar Scalar; - typedef Eigen::Matrix< - _Scalar, - Eigen::Dynamic, - Eigen::Dynamic, - _Options, - traits>::MaxDim, - traits>::MaxDim> + typedef typename traits>:: + RefJointMotionSubspace::Scalar Scalar; + typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options, _MaxDim, _MaxDim> OriginalReturnType; typedef typename ScalarMatrixProduct::type IdealReturnType; @@ -83,14 +80,15 @@ namespace pinocchio Scalar, IdealReturnType::RowsAtCompileTime, IdealReturnType::ColsAtCompileTime, - traits>::RefJointMotionSubspace::Options> + traits< + ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace::Options> ReturnType; }; - template - struct ConstraintForceSetOp, ForceSet> + template + struct ConstraintForceSetOp, ForceSet> { - typedef ScaledJointMotionSubspaceTpl<_Scalar, _Options> MotionSubspace; + typedef ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim> MotionSubspace; typedef typename traits::RefJointMotionSubspace::Scalar Scalar; typedef Eigen::Matrix< @@ -98,32 +96,28 @@ namespace pinocchio Eigen::Dynamic, Eigen::Dynamic, traits::RefJointMotionSubspace::Options | Eigen::RowMajor, - traits::MaxDim, - traits::MaxDim> + _MaxDim, + _MaxDim> ReturnType; }; - template + template struct ScaledJointMotionSubspaceTpl - : JointMotionSubspaceBase> + : JointMotionSubspaceBase> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspaceTpl) enum { - NV = Eigen::Dynamic + NV = Eigen::Dynamic, + MaxDim = _MaxDim }; typedef JointMotionSubspaceBase Base; using Base::nv; - typedef typename traits>::RefJointMotionSubspace - RefJointMotionSubspace; - enum - { - MaxDim = traits < ScaledJointMotionSubspaceTpl < _Scalar, - _Options >> ::MaxDim - }; + typedef typename traits>:: + RefJointMotionSubspace RefJointMotionSubspace; typedef typename SE3GroupAction::ReturnType SE3ActionReturnType; ScaledJointMotionSubspaceTpl() @@ -261,26 +255,24 @@ namespace pinocchio mutable DenseBase S; }; // struct ScaledJointMotionSubspaceTpl - template - struct MultiplicationOp, ScaledJointMotionSubspaceTpl> + template + struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { typedef InertiaTpl Inertia; - typedef ScaledJointMotionSubspaceTpl Constraint; + typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename Constraint::Scalar Scalar; - typedef Eigen:: - Matrix::MaxDim, traits::MaxDim> - ReturnType; + typedef Eigen::Matrix ReturnType; }; /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ namespace impl { - template - struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> + template + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> { typedef InertiaTpl Inertia; - typedef ScaledJointMotionSubspaceTpl Constraint; + typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename MultiplicationOp::ReturnType ReturnType; static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint) @@ -290,27 +282,20 @@ namespace pinocchio }; } // namespace impl - template - struct MultiplicationOp, ScaledJointMotionSubspaceTpl> + template + struct MultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef ScaledJointMotionSubspaceTpl MotionSubspace; - typedef Eigen::Matrix< - S2, - 6, - Eigen::Dynamic, - O2, - traits::MaxDim, - traits::MaxDim> - ReturnType; + typedef ScaledJointMotionSubspaceTpl MotionSubspace; + typedef Eigen::Matrix ReturnType; }; /* [ABA] operator* (Inertia Y,Constraint S) */ namespace impl { - template - struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> + template + struct LhsMultiplicationOp, ScaledJointMotionSubspaceTpl> { - typedef ScaledJointMotionSubspaceTpl Constraint; + typedef ScaledJointMotionSubspaceTpl Constraint; typedef typename MultiplicationOp, Constraint>::ReturnType ReturnType; @@ -339,13 +324,14 @@ namespace pinocchio Options = _Options, NQ = Eigen::Dynamic, NV = Eigen::Dynamic, - NJ = Eigen::Dynamic + NJ = Eigen::Dynamic, + MaxNJ = 6 }; typedef JointCollectionTpl JointCollection; typedef JointDataMimicTpl JointDataDerived; typedef JointModelMimicTpl JointModelDerived; - typedef ScaledJointMotionSubspaceTpl Constraint_t; + typedef ScaledJointMotionSubspaceTpl Constraint_t; typedef SE3Tpl Transformation_t; typedef MotionTpl Motion_t; diff --git a/include/pinocchio/serialization/joints-motion-subspace.hpp b/include/pinocchio/serialization/joints-motion-subspace.hpp index 73778bf1a0..6491cc5c0a 100644 --- a/include/pinocchio/serialization/joints-motion-subspace.hpp +++ b/include/pinocchio/serialization/joints-motion-subspace.hpp @@ -110,10 +110,10 @@ namespace boost ar & make_nvp("matrix", S.matrix()); } - template + template void serialize( Archive & ar, - pinocchio::ScaledJointMotionSubspaceTpl & S, + pinocchio::ScaledJointMotionSubspaceTpl & S, const unsigned int /*version*/) { ar & make_nvp("scaling", S.scaling()); diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 8da9a7880c..d5cbe2c5ae 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -20,7 +20,7 @@ void test_constraint_mimic(const JointModelBase & jmodel) typedef typename traits::JointDerived Joint; typedef typename traits::Constraint_t ConstraintType; typedef typename traits::JointDataDerived JointData; - typedef ScaledJointMotionSubspaceTpl ScaledConstraint; + typedef ScaledJointMotionSubspaceTpl ScaledConstraint; typedef JointMotionSubspaceTpl ConstraintRef; JointData jdata = jmodel.createData(); From 0f1a75b034ce9fbba3570bdaa353259d3c9f4a3d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 099/166] [EtienneAr feedback] Add getRelativePlacement method for kinematics --- include/pinocchio/algorithm/kinematics.hpp | 24 +++++++++++ include/pinocchio/algorithm/kinematics.hxx | 46 ++++++++++++++++++++++ include/pinocchio/algorithm/kinematics.txx | 10 +++++ src/algorithm/kinematics.cpp | 10 +++++ 4 files changed, 90 insertions(+) diff --git a/include/pinocchio/algorithm/kinematics.hpp b/include/pinocchio/algorithm/kinematics.hpp index cc0bfe8ebe..4193e697fc 100644 --- a/include/pinocchio/algorithm/kinematics.hpp +++ b/include/pinocchio/algorithm/kinematics.hpp @@ -101,6 +101,30 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a); + /** + * @brief Returns the relative placement of two joints expressed in the desired reference + * frame. You must first call pinocchio::forwardKinematics to update placement values in data + * structure. + * + * @param[in] model The kinematic model + * @param[in] data Data associated to model + * @param[in] jointId Id of the reference joint + * @param[in] jointId Id of the target joint + * @param[in] convention Convention to use (compuputation is done using data.liMi if LOCAL, and + * data.oMi if WORLD). + * + * @return The relative placement of the target joint wrt to the refence joint, expressed in + * the desired reference frame. + * + */ + template class JointCollectionTpl> + SE3Tpl getRelativePlacement( + const ModelTpl & model, + const DataTpl & data, + const JointIndex jointIdRef, + const JointIndex jointIdTarget, + const Convention convention = LOCAL); + /** * @brief Returns the spatial velocity of the joint expressed in the desired reference frame. * You must first call pinocchio::forwardKinematics to update placement and velocity diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index e2cb1dd3d1..fa61d1b49c 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -276,6 +276,52 @@ namespace pinocchio } } } // namespace impl + + template class JointCollectionTpl> + SE3Tpl getRelativePlacement( + const ModelTpl & model, + const DataTpl & data, + const JointIndex jointIdRef, + const JointIndex jointIdTarget, + const Convention convention) + { + assert(model.check(data) && "data is not consistent with model."); + assert(jointIdRef >= 0 && jointIdRef < model.njoints && "invalid joint id"); + assert(jointIdTarget >= 0 && jointIdTarget < model.njoints && "invalid joint id"); + switch (convention) + { + case Convention::LOCAL: { + SE3Tpl result; + const JointIndex child_id = std::max(jointIdRef, jointIdTarget); + const JointIndex parent_id = std::min(jointIdRef, jointIdTarget); + result.setIdentity(); + + // Traverse the kinematic chain from "tip" to "root" + JointIndex i = child_id; + while (i > parent_id) + { + result = data.liMi[i].act(result); + i = model.parents[i]; + } + + // The parent have been overshot, meaning it is in a different sub kinematic chain as the + // child + assert(i == parent_id && "Joints are not direct ancestor / descendants. Unsupported case."); + + // Inverse the result if necessary + if (child_id == jointIdRef) + { + result = result.inverse(); + } + return result; + } + case Convention::WORLD: + return data.oMi[jointIdRef].actInv(data.oMi[jointIdTarget]); + default: + throw std::invalid_argument("Bad convention."); + } + } + template class JointCollectionTpl> MotionTpl getVelocity( const ModelTpl & model, diff --git a/include/pinocchio/algorithm/kinematics.txx b/include/pinocchio/algorithm/kinematics.txx index 70bbf95e54..2e78f792fd 100644 --- a/include/pinocchio/algorithm/kinematics.txx +++ b/include/pinocchio/algorithm/kinematics.txx @@ -45,6 +45,16 @@ namespace pinocchio const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); } // namespace impl + + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI + SE3Tpl + getRelativePlacement( + const context::Model &, + const context::Data &, + const JointIndex, + const JointIndex, + const Convention); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI MotionTpl getVelocity( diff --git a/src/algorithm/kinematics.cpp b/src/algorithm/kinematics.cpp index 55b6072ee5..77025a86d7 100644 --- a/src/algorithm/kinematics.cpp +++ b/src/algorithm/kinematics.cpp @@ -45,6 +45,16 @@ namespace pinocchio const Eigen::MatrixBase> &, const Eigen::MatrixBase> &); } // namespace impl + + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI + SE3Tpl + getRelativePlacement( + const context::Model &, + const context::Data &, + const JointIndex, + const JointIndex, + const Convention); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI MotionTpl getVelocity( From 4f944942764d6fd954bf1f0b16ee1b5e8c5032b1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 100/166] [EtienneAr feedback] add patch to crba for mimic joints --- include/pinocchio/algorithm/crba.hxx | 37 +++++++++++++++++++++++++--- 1 file changed, 34 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index d84bf297f7..0a36c89ea7 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -80,7 +80,7 @@ namespace pinocchio ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); ColsBlock J_cols = jmodel.jointJacCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); - + // Joint Space Inertia Matrix jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); @@ -122,6 +122,35 @@ namespace pinocchio } }; + template + static inline void mimic_patch_CrbaLocalConventionBackwardStep( + const JointModelBase &, const Model &, Data &) + { + } + + template class JointCollectionTpl> + static inline void mimic_patch_CrbaLocalConventionBackwardStep( + const JointModelBase> & jmodel, + const Model & model, + Data & data) + { + // For mimic joint, if i is the primary and j the scondary joint + // the resulting inertia of both joints is + // M = Mii + 2* Mij + Mjj + // But in this current implementation only M = Mii + Mij + Mjj is computed + // So an extra Mij is added here. + const JointIndex secondary_id = jmodel.id(); + const JointIndex primary_id = jmodel.derived().jmodel().id(); + + // TODO do the transform in the general case + const SE3 jMi = + getRelativePlacement(model, data, primary_id, secondary_id, Convention::LOCAL); + Data::Matrix6x::ColsBlockXpr iF = jmodel.jointVelCols(data.Fcrb[secondary_id]); + Eigen::Matrix jF(6, jmodel.nj()); + forceSet::se3Action(jMi, iF, jF); + jmodel.jointVelBlock(data.M).noalias() += data.joints[primary_id].S().transpose() * jF; + } + template class JointCollectionTpl> struct CrbaLocalConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -140,7 +169,7 @@ namespace pinocchio Data & data) { /* - * F[1:6,i] = Y*S + * F[1:6,i] += Y*S * M[i,SUBTREE] = S'*F[1:6,SUBTREE] * if li>0 * Yli += liXi Yi @@ -170,6 +199,8 @@ namespace pinocchio Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); forceSet::se3Action(data.liMi[i], iF, jF); } + + mimic_patch_CrbaLocalConventionBackwardStep(jmodel, model, data); } }; @@ -337,4 +368,4 @@ namespace pinocchio /// @endcond -#endif // ifndef __pinocchio_crba_hxx__ \ No newline at end of file +#endif // ifndef __pinocchio_crba_hxx__ From 8e34295df3499af931e135fe242549d71a6c5567 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 101/166] [EtienneAr feedback] Update comment and remove hardcoded max size for crba patch --- include/pinocchio/algorithm/crba.hxx | 16 +++++++++------- .../pinocchio/multibody/joint/joint-mimic.hpp | 4 ++++ 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 0a36c89ea7..3d6ea2abaa 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -122,31 +122,33 @@ namespace pinocchio } }; + /// \brief Patch to the crba algorithm for joint mimic (in local convention) template static inline void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase &, const Model &, Data &) { } + /// \note For the case of a joint j mimicking a joint i + /// the resulting inertia of both joints is + /// M = Mii + 2* Mij + Mjj + /// But in this current implementation only M = Mii + Mij + Mjj is computed + /// So an extra Mij is added here. template class JointCollectionTpl> static inline void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase> & jmodel, const Model & model, Data & data) { - // For mimic joint, if i is the primary and j the scondary joint - // the resulting inertia of both joints is - // M = Mii + 2* Mij + Mjj - // But in this current implementation only M = Mii + Mij + Mjj is computed - // So an extra Mij is added here. + typedef JointModelMimicTpl JointModel; const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); - // TODO do the transform in the general case const SE3 jMi = getRelativePlacement(model, data, primary_id, secondary_id, Convention::LOCAL); Data::Matrix6x::ColsBlockXpr iF = jmodel.jointVelCols(data.Fcrb[secondary_id]); - Eigen::Matrix jF(6, jmodel.nj()); + Eigen::Matrix jF( + 6, jmodel.nj()); forceSet::se3Action(jMi, iF, jF); jmodel.jointVelBlock(data.M).noalias() += data.joints[primary_id].S().transpose() * jF; } diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 3a9a0fbbe7..b9b0b3d627 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -650,6 +650,10 @@ namespace pinocchio typedef JointModelBase Base; typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); + enum + { + MaxNJ = traits::MaxNJ + }; typedef JointCollectionTpl JointCollection; typedef JointModelTpl JointModel; From eadd0055e5b1dd1d1324cc6764a06fc77af47b3f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 102/166] [EtienneAr feedback] Make random humanoid with mimic joint --- unittest/python/bindings_data.py | 4 ++++ unittest/python/bindings_frame.py | 4 ++++ unittest/python/bindings_joints.py | 4 ++++ unittest/python/bindings_kinematics.py | 4 ++++ unittest/python/bindings_model.py | 4 ++++ unittest/python/bindings_rnea.py | 4 ++++ 6 files changed, 24 insertions(+) diff --git a/unittest/python/bindings_data.py b/unittest/python/bindings_data.py index 2c55b55d90..bfc574ced1 100644 --- a/unittest/python/bindings_data.py +++ b/unittest/python/bindings_data.py @@ -7,7 +7,11 @@ class TestData(TestCase): def setUp(self): +<<<<<<< HEAD self.model = pin.buildSampleModelHumanoidRandom(True) +======= + self.model = pin.buildSampleModelHumanoidRandom(True, True) +>>>>>>> 289a65bc (Make random humanoid with mimic joint) self.data = self.model.createData() def test_copy(self): diff --git a/unittest/python/bindings_frame.py b/unittest/python/bindings_frame.py index 20531f2889..066eec41a0 100644 --- a/unittest/python/bindings_frame.py +++ b/unittest/python/bindings_frame.py @@ -8,7 +8,11 @@ class TestFrameBindings(PinocchioTestCase): def setUp(self): +<<<<<<< HEAD self.model = pin.buildSampleModelHumanoidRandom(True) +======= + self.model = pin.buildSampleModelHumanoidRandom(True, True) +>>>>>>> 289a65bc (Make random humanoid with mimic joint) self.parent_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index 69be539214..03a109aa23 100644 --- a/unittest/python/bindings_joints.py +++ b/unittest/python/bindings_joints.py @@ -7,7 +7,11 @@ class TestJointsAlgo(TestCase): def setUp(self): +<<<<<<< HEAD self.model = pin.buildSampleModelHumanoidRandom(True) +======= + self.model = pin.buildSampleModelHumanoidRandom(True, True) +>>>>>>> 289a65bc (Make random humanoid with mimic joint) qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) diff --git a/unittest/python/bindings_kinematics.py b/unittest/python/bindings_kinematics.py index 07faa508ed..0360a863c3 100644 --- a/unittest/python/bindings_kinematics.py +++ b/unittest/python/bindings_kinematics.py @@ -7,7 +7,11 @@ class TestKinematicsBindings(PinocchioTestCase): def setUp(self): +<<<<<<< HEAD self.model = pin.buildSampleModelHumanoidRandom(True) +======= + self.model = pin.buildSampleModelHumanoidRandom(True, True) +>>>>>>> 289a65bc (Make random humanoid with mimic joint) self.joint_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_model.py b/unittest/python/bindings_model.py index 912596e752..6fea852c09 100644 --- a/unittest/python/bindings_model.py +++ b/unittest/python/bindings_model.py @@ -8,7 +8,11 @@ class TestModel(TestCase): def setUp(self): +<<<<<<< HEAD self.model = pin.buildSampleModelHumanoidRandom(True) +======= + self.model = pin.buildSampleModelHumanoidRandom(True, True) +>>>>>>> 289a65bc (Make random humanoid with mimic joint) def test_empty_model_sizes(self): model = pin.Model() diff --git a/unittest/python/bindings_rnea.py b/unittest/python/bindings_rnea.py index a3e4f611b6..8478e77c98 100644 --- a/unittest/python/bindings_rnea.py +++ b/unittest/python/bindings_rnea.py @@ -7,7 +7,11 @@ class TestRNEA(TestCase): def setUp(self): +<<<<<<< HEAD self.model = pin.buildSampleModelHumanoidRandom(True) +======= + self.model = pin.buildSampleModelHumanoidRandom(True, True) +>>>>>>> 289a65bc (Make random humanoid with mimic joint) self.data = self.model.createData() qmax = np.full((self.model.nq, 1), np.pi) From d180773024eb8bb20c0eb1a48edb7133aec73916 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 103/166] [algorithm/model] Added a function to transform a joint into a mimic --- include/pinocchio/algorithm/model.hpp | 22 +++++ include/pinocchio/algorithm/model.hxx | 122 ++++++++++++++++++++++++++ include/pinocchio/algorithm/model.txx | 9 ++ src/algorithm/model.cpp | 9 ++ 4 files changed, 162 insertions(+) diff --git a/include/pinocchio/algorithm/model.hpp b/include/pinocchio/algorithm/model.hpp index 697e7a5ad2..95823d67e2 100644 --- a/include/pinocchio/algorithm/model.hpp +++ b/include/pinocchio/algorithm/model.hpp @@ -203,6 +203,28 @@ namespace pinocchio ModelTpl & reduced_model, std::vector & list_of_reduced_geom_models); + /** + * + * \brief Transform of a joint of the model into a mimic joint. Keep the type of the joint as it + * was previously. + * + * \param[in] model the input model to take joints from. + * \param[in] index_primary index of the joint to mimic + * \param[in] index_secondary index of the joint that will mimic + * \param[in] scaling Scaling of joint velocity and configuration + * \param[in] offset Offset of joint configuration + * \param[out] output_model Model with the joint mimic + * + */ + template class JointCollectionTpl> + void transformJointIntoMimic( + const ModelTpl & input_model, + const JointIndex & index_primary, + const JointIndex & index_secondary, + const Scalar & scaling, + const Scalar & offset, + ModelTpl & output_model); + /** * * \brief Computes the common ancestor between two joints belonging to the same kinematic tree. diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 8d414ad394..0f12ed318d 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -825,6 +825,128 @@ namespace pinocchio } } + template class JointCollectionTpl> + void transformJointIntoMimic( + const ModelTpl & input_model, + const JointIndex & index_primary, + const JointIndex & index_secondary, + const Scalar & scaling, + const Scalar & offset, + ModelTpl & output_model) + { + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_primary <= (size_t)input_model.njoints, + "index of primary is greater than the total of joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_secondary <= (size_t)input_model.njoints, + "index of primary is greater than the total of joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( + index_primary < index_secondary, "index of primary is greater than secondary"); + + typedef ModelTpl Model; + typedef typename Model::JointModel JointModel; + + output_model = input_model; + + output_model.joints[index_secondary] = JointModelMimic( + input_model.joints.at(index_secondary), output_model.joints.at(index_primary), scaling, + offset); + + Scalar old_nq = input_model.joints.at(index_secondary).nq(); + Scalar old_nv = input_model.joints.at(index_secondary).nv(); + output_model.nq = input_model.nq - old_nq; + output_model.nv = input_model.nv - old_nv; + Scalar nq = output_model.nq; + Scalar nv = output_model.nv; + + // Resize limits + output_model.effortLimit.resize(nv); + output_model.velocityLimit.resize(nv); + output_model.lowerPositionLimit.resize(nq); + output_model.upperPositionLimit.resize(nq); + output_model.armature.resize(nv); + output_model.rotorInertia.resize(nv); + output_model.rotorGearRatio.resize(nv); + output_model.friction.resize(nv); + output_model.damping.resize(nv); + + // Move indexes and limits + for (JointIndex joint_id = 1; joint_id < (JointIndex)index_secondary; ++joint_id) + { + const JointModel & jmodel_input = input_model.joints[joint_id]; + const JointModel & jmodel_output = output_model.joints[joint_id]; + jmodel_output.jointVelocityFromNvSelector(output_model.effortLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.effortLimit); + jmodel_output.jointVelocityFromNvSelector(output_model.velocityLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.velocityLimit); + + jmodel_output.jointConfigFromNqSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigFromNqSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocityFromNvSelector(output_model.armature) = + jmodel_input.jointVelocityFromNvSelector(input_model.armature); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorInertia) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorInertia); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorGearRatio); + jmodel_output.jointVelocityFromNvSelector(output_model.friction) = + jmodel_input.jointVelocityFromNvSelector(input_model.friction); + jmodel_output.jointVelocityFromNvSelector(output_model.damping) = + jmodel_input.jointVelocityFromNvSelector(input_model.damping); + } + + // Move indexes and limits + Scalar idx_q = output_model.idx_qs[index_secondary]; + Scalar idx_v = output_model.idx_vs[index_secondary]; + for (JointIndex joint_id = index_secondary; joint_id < (JointIndex)input_model.njoints; + ++joint_id) + { + const JointModel & jmodel_input = input_model.joints[joint_id]; + JointModel & jmodel_output = output_model.joints[joint_id]; + jmodel_output.setIndexes(jmodel_input.id(), idx_q, idx_v, jmodel_input.idx_j()); + + output_model.idx_qs[joint_id] = jmodel_output.idx_q(); + output_model.nqs[joint_id] = jmodel_output.nq(); + output_model.idx_vs[joint_id] = jmodel_output.idx_v(); + output_model.nvs[joint_id] = jmodel_output.nv(); + + idx_q += jmodel_output.nq(); + idx_v += jmodel_output.nv(); + if (joint_id == index_secondary) + { + output_model.idx_qs[index_secondary] = output_model.joints[index_secondary].idx_q(); + output_model.idx_vs[index_secondary] = output_model.joints[index_secondary].idx_v(); + output_model.nqs[index_secondary] = 0; + output_model.nvs[index_secondary] = 0; + } + else + { + jmodel_output.jointVelocityFromNvSelector(output_model.effortLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.effortLimit); + jmodel_output.jointVelocityFromNvSelector(output_model.velocityLimit) = + jmodel_input.jointVelocityFromNvSelector(input_model.velocityLimit); + + jmodel_output.jointConfigFromNqSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigFromNqSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigFromNqSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocityFromNvSelector(output_model.armature) = + jmodel_input.jointVelocityFromNvSelector(input_model.armature); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorInertia) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorInertia); + jmodel_output.jointVelocityFromNvSelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocityFromNvSelector(input_model.rotorGearRatio); + jmodel_output.jointVelocityFromNvSelector(output_model.friction) = + jmodel_input.jointVelocityFromNvSelector(input_model.friction); + jmodel_output.jointVelocityFromNvSelector(output_model.damping) = + jmodel_input.jointVelocityFromNvSelector(input_model.damping); + } + } + } + template class JointCollectionTpl> JointIndex findCommonAncestor( const ModelTpl & model, diff --git a/include/pinocchio/algorithm/model.txx b/include/pinocchio/algorithm/model.txx index b9a34f639b..03f9596ed0 100644 --- a/include/pinocchio/algorithm/model.txx +++ b/include/pinocchio/algorithm/model.txx @@ -81,6 +81,15 @@ namespace pinocchio context::Model &, std::vector> &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void + transformJointIntoMimic( + const context::Model &, + const JointIndex &, + const JointIndex &, + const context::Scalar &, + const context::Scalar &, + context::Model &); + extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI JointIndex findCommonAncestor( const context::Model &, JointIndex, JointIndex, size_t &, size_t &); diff --git a/src/algorithm/model.cpp b/src/algorithm/model.cpp index 5c8dd7065a..44b37ac98d 100644 --- a/src/algorithm/model.cpp +++ b/src/algorithm/model.cpp @@ -81,6 +81,15 @@ namespace pinocchio context::Model &, std::vector> &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void + transformJointIntoMimic( + const context::Model &, + const JointIndex &, + const JointIndex &, + const context::Scalar &, + const context::Scalar &, + context::Model &); + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI JointIndex findCommonAncestor( const context::Model &, JointIndex, JointIndex, size_t &, size_t &); From 43f995916bcfeb934442abfd878743d35ff6ad77 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 104/166] [unittest/model] Added test for transformIntoJointMimic --- unittest/model.cpp | 65 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) diff --git a/unittest/model.cpp b/unittest/model.cpp index 295fc735da..8e7c11e982 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -885,4 +885,69 @@ BOOST_AUTO_TEST_CASE(test_has_configuration_limit) BOOST_CHECK((model_cf_limits_tangent == expected_cf_limits_tangent_model)); } +BOOST_AUTO_TEST_CASE(test_has_transform_to_mimic) +{ + Model humanoid_model, humanoid_mimic; + buildModels::humanoid(humanoid_model); + + JointIndex index_p = humanoid_model.getJointId("rleg_shoulder3_joint"); + JointIndex index_s = humanoid_model.getJointId("lleg_shoulder3_joint"); + + transformJointIntoMimic(humanoid_model, index_p, index_s, 2.0, 0.4, humanoid_mimic); + + BOOST_CHECK(humanoid_mimic.nq == (humanoid_model.nq - humanoid_model.nqs[index_s])); + BOOST_CHECK(humanoid_mimic.nv == (humanoid_model.nv - humanoid_model.nvs[index_s])); + BOOST_CHECK(humanoid_model.nj == humanoid_mimic.nj); + BOOST_CHECK(humanoid_mimic.njoints == humanoid_model.njoints); + + for (int i = 1; i < humanoid_model.names.size(); i++) + { + JointIndex full_id = humanoid_model.getJointId(humanoid_model.names[i]); + JointIndex mim_id = humanoid_mimic.getJointId(humanoid_model.names[i]); + + BOOST_CHECK(full_id == mim_id); + + const JointModel & jmodel_mim = humanoid_mimic.joints[mim_id]; + const JointModel & jmodel_full = humanoid_model.joints[full_id]; + + BOOST_CHECK(jmodel_mim.idx_v() == humanoid_mimic.idx_vs[mim_id]); + BOOST_CHECK(jmodel_mim.nv() == humanoid_mimic.nvs[mim_id]); + BOOST_CHECK(jmodel_mim.idx_q() == humanoid_mimic.idx_qs[mim_id]); + BOOST_CHECK(jmodel_mim.nq() == humanoid_mimic.nqs[mim_id]); + + if (mim_id != index_s) + { + BOOST_CHECK( + jmodel_mim.jointVelocityFromNvSelector(humanoid_mimic.effortLimit) + == jmodel_full.jointVelocityFromNvSelector(humanoid_model.effortLimit)); + BOOST_CHECK( + jmodel_mim.jointVelocityFromNvSelector(humanoid_mimic.velocityLimit) + == jmodel_full.jointVelocityFromNvSelector(humanoid_model.velocityLimit)); + + BOOST_CHECK( + jmodel_mim.jointConfigFromNqSelector(humanoid_mimic.lowerPositionLimit) + == jmodel_full.jointConfigFromNqSelector(humanoid_model.lowerPositionLimit)); + BOOST_CHECK( + jmodel_mim.jointConfigFromNqSelector(humanoid_mimic.upperPositionLimit) + == jmodel_full.jointConfigFromNqSelector(humanoid_model.upperPositionLimit)); + + BOOST_CHECK( + jmodel_mim.jointVelocityFromNvSelector(humanoid_mimic.armature) + == jmodel_full.jointVelocityFromNvSelector(humanoid_model.armature)); + BOOST_CHECK( + jmodel_mim.jointVelocityFromNvSelector(humanoid_mimic.rotorInertia) + == jmodel_full.jointVelocityFromNvSelector(humanoid_model.rotorInertia)); + BOOST_CHECK( + jmodel_mim.jointVelocityFromNvSelector(humanoid_mimic.rotorGearRatio) + == jmodel_full.jointVelocityFromNvSelector(humanoid_model.rotorGearRatio)); + BOOST_CHECK( + jmodel_mim.jointVelocityFromNvSelector(humanoid_mimic.friction) + == jmodel_full.jointVelocityFromNvSelector(humanoid_model.friction)); + BOOST_CHECK( + jmodel_mim.jointVelocityFromNvSelector(humanoid_mimic.damping) + == jmodel_full.jointVelocityFromNvSelector(humanoid_model.damping)); + } + } +} + BOOST_AUTO_TEST_SUITE_END() From 0263aca9861ae5353bd9ff8f9a338bb6e96ec0f6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 105/166] Apply pre-commit --- .../python/multibody/joint/joints-models.hpp | 18 +++++++++++------- include/pinocchio/multibody/joint/fwd.hpp | 3 ++- .../multibody/joint/joint-basic-visitors.hxx | 12 ++++++------ .../multibody/joint/joint-helical.hpp | 4 ++-- unittest/joint-generic.cpp | 1 - 5 files changed, 21 insertions(+), 17 deletions(-) diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index eea5973584..77cca4d02d 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -368,7 +368,8 @@ namespace pinocchio const context::Scalar & m_scaling; const context::Scalar & m_offset; - JointModelMimicConstructorVisitor(const context::Scalar & scaling, const context::Scalar & offset) + JointModelMimicConstructorVisitor( + const context::Scalar & scaling, const context::Scalar & offset) : m_scaling(scaling) , m_offset(offset) { @@ -380,13 +381,15 @@ namespace pinocchio return new context::JointModelMimic(jmodel, m_scaling, m_offset); } - + }; // struct JointModelMimicConstructorVisitor - static context::JointModelMimic * init_proxy(const context::JointModel & jmodel, const context::Scalar & scaling, const context::Scalar & offset) + static context::JointModelMimic * init_proxy( + const context::JointModel & jmodel, + const context::Scalar & scaling, + const context::Scalar & offset) { - return boost::apply_visitor( - JointModelMimicConstructorVisitor(scaling, offset), jmodel); + return boost::apply_visitor(JointModelMimicConstructorVisitor(scaling, offset), jmodel); } static context::Scalar get_scaling(context::JointModelMimic & jmodel) @@ -406,9 +409,10 @@ namespace pinocchio return cl .def( "__init__", - bp::make_constructor(init_proxy, bp::default_call_policies(), bp::args("joint_model", "scaling", "offset")), + bp::make_constructor( + init_proxy, bp::default_call_policies(), bp::args("joint_model", "scaling", "offset")), "Init JointModelMimic from an existing joint with scaling and offset.") - .add_property("scaling",&get_scaling) + .add_property("scaling", &get_scaling) .add_property("offset", &get_offset); } diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index 159afb6ded..ebc395c102 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -173,7 +173,8 @@ namespace pinocchio struct JointModelTpl; typedef JointModelTpl JointModel; - typedef JointModelTpl JointModelMimicable; + typedef JointModelTpl + JointModelMimicable; template< typename Scalar, diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 6105ab2cd5..1787a9bac4 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -918,11 +918,11 @@ namespace pinocchio return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived()))); } - // Meta-function to check is_mimicable_t trait template - struct is_mimicable { - static constexpr bool value = traits::is_mimicable_t::value; + struct is_mimicable + { + static constexpr bool value = traits::is_mimicable_t::value; }; template @@ -932,15 +932,15 @@ namespace pinocchio typename boost::enable_if_c::value, JointModel>::type operator()(const T & value) const { - return value; + return value; } template typename boost::disable_if_c::value, JointModel>::type operator()(const T & value) const { - assert(false && "Type not supported in new variant"); - return value; + assert(false && "Type not supported in new variant"); + return value; } }; diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index 0eb68703e1..94ea970661 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -757,9 +757,9 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; - + typedef boost::mpl::true_ is_mimicable_t; - + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 8e9e61cf6b..844f2a6376 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -353,7 +353,6 @@ namespace pinocchio typedef Eigen::Matrix TangentVector_t; typedef boost::mpl::false_ is_mimicable_t; - }; template class JointCollectionTpl> From 8f9e6b3270dcfc4f3038b895051c48405a6dbba3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 106/166] Fix rebase --- unittest/python/bindings_data.py | 4 ---- unittest/python/bindings_frame.py | 4 ---- unittest/python/bindings_joints.py | 4 ---- unittest/python/bindings_kinematics.py | 4 ---- unittest/python/bindings_model.py | 4 ---- unittest/python/bindings_rnea.py | 4 ---- 6 files changed, 24 deletions(-) diff --git a/unittest/python/bindings_data.py b/unittest/python/bindings_data.py index bfc574ced1..3fc7b24c19 100644 --- a/unittest/python/bindings_data.py +++ b/unittest/python/bindings_data.py @@ -7,11 +7,7 @@ class TestData(TestCase): def setUp(self): -<<<<<<< HEAD - self.model = pin.buildSampleModelHumanoidRandom(True) -======= self.model = pin.buildSampleModelHumanoidRandom(True, True) ->>>>>>> 289a65bc (Make random humanoid with mimic joint) self.data = self.model.createData() def test_copy(self): diff --git a/unittest/python/bindings_frame.py b/unittest/python/bindings_frame.py index 066eec41a0..23ac326f9c 100644 --- a/unittest/python/bindings_frame.py +++ b/unittest/python/bindings_frame.py @@ -8,11 +8,7 @@ class TestFrameBindings(PinocchioTestCase): def setUp(self): -<<<<<<< HEAD - self.model = pin.buildSampleModelHumanoidRandom(True) -======= self.model = pin.buildSampleModelHumanoidRandom(True, True) ->>>>>>> 289a65bc (Make random humanoid with mimic joint) self.parent_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index 03a109aa23..1a27e92abf 100644 --- a/unittest/python/bindings_joints.py +++ b/unittest/python/bindings_joints.py @@ -7,11 +7,7 @@ class TestJointsAlgo(TestCase): def setUp(self): -<<<<<<< HEAD - self.model = pin.buildSampleModelHumanoidRandom(True) -======= self.model = pin.buildSampleModelHumanoidRandom(True, True) ->>>>>>> 289a65bc (Make random humanoid with mimic joint) qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) diff --git a/unittest/python/bindings_kinematics.py b/unittest/python/bindings_kinematics.py index 0360a863c3..721dfa4fde 100644 --- a/unittest/python/bindings_kinematics.py +++ b/unittest/python/bindings_kinematics.py @@ -7,11 +7,7 @@ class TestKinematicsBindings(PinocchioTestCase): def setUp(self): -<<<<<<< HEAD - self.model = pin.buildSampleModelHumanoidRandom(True) -======= self.model = pin.buildSampleModelHumanoidRandom(True, True) ->>>>>>> 289a65bc (Make random humanoid with mimic joint) self.joint_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_model.py b/unittest/python/bindings_model.py index 6fea852c09..25d257f0ea 100644 --- a/unittest/python/bindings_model.py +++ b/unittest/python/bindings_model.py @@ -8,11 +8,7 @@ class TestModel(TestCase): def setUp(self): -<<<<<<< HEAD - self.model = pin.buildSampleModelHumanoidRandom(True) -======= self.model = pin.buildSampleModelHumanoidRandom(True, True) ->>>>>>> 289a65bc (Make random humanoid with mimic joint) def test_empty_model_sizes(self): model = pin.Model() diff --git a/unittest/python/bindings_rnea.py b/unittest/python/bindings_rnea.py index 8478e77c98..a6c9111707 100644 --- a/unittest/python/bindings_rnea.py +++ b/unittest/python/bindings_rnea.py @@ -7,11 +7,7 @@ class TestRNEA(TestCase): def setUp(self): -<<<<<<< HEAD - self.model = pin.buildSampleModelHumanoidRandom(True) -======= self.model = pin.buildSampleModelHumanoidRandom(True, True) ->>>>>>> 289a65bc (Make random humanoid with mimic joint) self.data = self.model.createData() qmax = np.full((self.model.nq, 1), np.pi) From d9af1baed6a2e4a4dbb9b15b222d95517f77bfd4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 107/166] [Algorithm/model] Remove useless if --- include/pinocchio/algorithm/model.hxx | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 0f12ed318d..1e7c272b5d 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -914,14 +914,7 @@ namespace pinocchio idx_q += jmodel_output.nq(); idx_v += jmodel_output.nv(); - if (joint_id == index_secondary) - { - output_model.idx_qs[index_secondary] = output_model.joints[index_secondary].idx_q(); - output_model.idx_vs[index_secondary] = output_model.joints[index_secondary].idx_v(); - output_model.nqs[index_secondary] = 0; - output_model.nvs[index_secondary] = 0; - } - else + if (joint_id != index_secondary) { jmodel_output.jointVelocityFromNvSelector(output_model.effortLimit) = jmodel_input.jointVelocityFromNvSelector(input_model.effortLimit); From df1c2f99aec38b35a47f68c283bfc12ead4a8d6b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 108/166] [EtienneAr feedback] Fix crba in local convention to prevent heap allocation --- include/pinocchio/algorithm/crba.hxx | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 3d6ea2abaa..ad4f3efee9 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -129,6 +129,22 @@ namespace pinocchio { } + template + struct GetMotionSubspaceTplNoMalloc : public boost::static_visitor + { + template + ReturnType operator()(const JointDataBase & jdata) const + { + assert(jdata.S().nv() <= ReturnType::MaxColsAtCompileTime); + return ReturnType(jdata.S().matrix()); + } + + static ReturnType run(const JointDataTpl & jdata) + { + return boost::apply_visitor(GetMotionSubspaceTplNoMalloc(), jdata); + } + }; + /// \note For the case of a joint j mimicking a joint i /// the resulting inertia of both joints is /// M = Mii + 2* Mij + Mjj @@ -150,7 +166,13 @@ namespace pinocchio Eigen::Matrix jF( 6, jmodel.nj()); forceSet::se3Action(jMi, iF, jF); - jmodel.jointVelBlock(data.M).noalias() += data.joints[primary_id].S().transpose() * jF; + + typedef typename Eigen::Matrix + MotionSubspace; + jmodel.jointVelBlock(data.M).noalias() += + GetMotionSubspaceTplNoMalloc::run(data.joints[primary_id]) + .transpose() + * jF; } template class JointCollectionTpl> From 10930172e27c1249ad8933265e7647b400262b8a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 109/166] [EtienneAr feedback] Add assert for crba in world convention with mimic joints --- include/pinocchio/algorithm/crba.hxx | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index ad4f3efee9..ad2ec509b0 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -70,6 +70,11 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { + assert( + (std::is_same>::value + == false) + && "WORLD convention not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename SizeDepType::template ColsReturn::Type From 47e54822fe3be49efc70acada9ae319d957f0214 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 110/166] [EtienneAr feedback] More exhaustive testing of crba for mimic --- unittest/crba.cpp | 83 ++++++++++++++++++++++++++++------------------- 1 file changed, 49 insertions(+), 34 deletions(-) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index f2867c62f6..3e15402135 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -16,6 +16,7 @@ #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" +#include "pinocchio/algorithm/model.hpp" #include "pinocchio/algorithm/crba.hpp" #include "pinocchio/algorithm/centroidal.hpp" #include "pinocchio/algorithm/rnea.hpp" @@ -64,7 +65,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, true, true); + pinocchio::buildModels::humanoidRandom(model, true); pinocchio::Data data(model); #ifdef NDEBUG @@ -125,47 +126,45 @@ BOOST_AUTO_TEST_CASE(test_crba) #endif // ifndef NDEBUG } -BOOST_AUTO_TEST_CASE(test_crba_mimic) +void test_mimic_against_full_model( + const pinocchio::Model & model_full, int primary_id, int secondary_id) { - pinocchio::Model model_mimic, model_full; - pinocchio::buildModels::humanoidRandom(model_mimic, true, true); - pinocchio::buildModels::humanoidRandom(model_full, true, false); - - // HumanoidRandom constants - const int primary_idxq = model_mimic.nq - 1; - const int primary_idxv = model_mimic.nv - 1; - const int secondary_idxq = model_full.nq - 1; - const int secondary_idxv = model_full.nv - 1; + // constants + const int primary_idxq = model_full.joints[primary_id].idx_q(); + const int primary_idxv = model_full.joints[primary_id].idx_v(); + const int secondary_idxq = model_full.joints[secondary_id].idx_q(); + const int secondary_idxv = model_full.joints[secondary_id].idx_v(); const double ratio = 2.5; const double offset = 0.75; - Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); - G.topLeftCorner(model_mimic.nv, model_mimic.nv).setIdentity(); - G(secondary_idxv, primary_idxv) = ratio; - // Make both model match - for (size_t i = 0; i < model_full.njoints; i++) - { - model_full.inertias[i] = model_mimic.inertias[i]; - model_full.jointPlacements[i] = model_mimic.jointPlacements[i]; - } - - pinocchio::Data data_mimic(model_mimic); + // Build mimic model + pinocchio::Model model_mimic; + pinocchio::transformJointIntoMimic( + model_full, primary_id, secondary_id, ratio, offset, model_mimic); pinocchio::Data data_full(model_full); + pinocchio::Data data_mimic(model_mimic); // Prepare test data - Eigen::VectorXd q_mimic = Eigen::VectorXd::Ones(model_mimic.nq); - q_mimic.segment<4>(3).normalize(); - Eigen::VectorXd v_mimic = Eigen::VectorXd::Zero(model_mimic.nv); - Eigen::VectorXd a_mimic = Eigen::VectorXd::Zero(model_mimic.nv); + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); + G.topLeftCorner(model_mimic.nv, model_mimic.nv).setIdentity(); + G(secondary_idxv, primary_idxv) = ratio; + + Eigen::VectorXd q_mimic = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v_mimic = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd a_mimic = pinocchio::randomConfiguration(model_mimic); - Eigen::VectorXd q_full(model_full.nq), v_full(model_full.nv), a_full(model_full.nv); - q_full.head(model_mimic.nq) = q_mimic; - v_full.head(model_mimic.nv) = v_mimic; - a_full.head(model_mimic.nv) = a_mimic; + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); + Eigen::VectorXd v_full = G.transpose() * v_mimic; + Eigen::VectorXd a_full = G.transpose() * a_mimic; - q_full[secondary_idxq] = q_mimic[primary_idxq] * ratio + offset; - v_full[secondary_idxv] = v_mimic[primary_idxv] * ratio; - a_full[secondary_idxv] = a_mimic[primary_idxv] * ratio; + for (int n = 1; n < model_full.njoints; n++) + { + const double joint_ratio = ((n == secondary_id) ? ratio : 1.0); + const double joint_offset = ((n == secondary_id) ? offset : 0.0); + Eigen::VectorXd aaa = model_mimic.joints[n].jointConfigFromDofSelector(q_mimic); + model_full.joints[n].jointConfigFromDofSelector(q_full) = + joint_ratio * aaa + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } // Run crba pinocchio::crba(model_mimic, data_mimic, q_mimic); @@ -177,13 +176,29 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) data_full.M.triangularView() = data_full.M.transpose().triangularView(); + // Check full model against reduced BOOST_CHECK(data_mimic.M.isApprox(G.transpose() * data_full.M * G, 1e-12)); } +BOOST_AUTO_TEST_CASE(test_crba_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model, true); + + // Test for direct parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg4_joint"), + humanoid_model.getJointId("rleg5_joint")); + // Test for parallel joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg4_joint"), + humanoid_model.getJointId("lleg4_joint")); +} + BOOST_AUTO_TEST_CASE(test_minimal_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, true, true); + pinocchio::buildModels::humanoidRandom(model, true); pinocchio::Data data(model), data_ref(model); model.lowerPositionLimit.head<7>().fill(-1.); From 97cb06582950d649750749de91bd4b8266e292d6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 111/166] [EtienneAr feedback] Remove useless override of joint{Config,Velocity}From{Nv,Dof}Selector_impl and patch base FromDof ones --- .../pinocchio/multibody/joint/joint-mimic.hpp | 49 ------------------- .../multibody/joint/joint-model-base.hpp | 4 +- 2 files changed, 2 insertions(+), 51 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index b9b0b3d627..09ffe9ec4c 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -873,55 +873,6 @@ namespace pinocchio return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); } - // Const access - template - typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), idx_q(), 0); - } - - // Non-const access - template - typename SizeDepType::template SegmentReturn::Type - jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), idx_q(), 0); - } - - /* Acces to dedicated segment in robot config velocity space. */ - // Const access - template - typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), idx_v(), m_jmodel_ref.nv()); - } - - // Non-const access - template - typename SizeDepType::template SegmentReturn::Type - jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), idx_v(), m_jmodel_ref.nv()); - } - - // Const access - template - typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), idx_v(), 0); - } - - // Non-const access - template - typename SizeDepType::template SegmentReturn::Type - jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const - { - return SizeDepType::segment(a.derived(), idx_v(), 0); - } - /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ // Const access template diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index 18fd002ed2..e527f6a99c 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -354,7 +354,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), idx_v(), nv()); + return SizeDepType::segment(a.derived(), idx_v(), nj()); } // Non-const access @@ -369,7 +369,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), idx_v(), nv()); + return SizeDepType::segment(a.derived(), idx_v(), nj()); } // Const access From 86dcb6bf0b164cac4ebdd7198605c774687ae7d5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 112/166] [EtienneAr feedback] Fix selectConfigFromDof for JointModelTpl in the case of a mimic by using a visitor --- .../multibody/joint/joint-basic-visitors.hxx | 15 +++++++++++ .../multibody/joint/joint-generic.hpp | 25 +++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 1787a9bac4..1fc6cecd7f 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -193,6 +193,21 @@ namespace pinocchio PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I)); } + template + struct JointConfigFromDofSelectorVisitor + : fusion:: + JointUnaryVisitorBase, ReturnType> + { + typedef boost::fusion::vector ArgsType; + + template + static ReturnType algo(const JointModelBase & jmodel, InputType a) + { + auto vectorBlock = jmodel.jointConfigFromDofSelector(a); + return ReturnType(vectorBlock.nestedExpression(), 0, vectorBlock.size()); + } + }; + /** * @brief JointNvVisitor visitor */ diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index 7dd75683ef..c4d14bbf92 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -382,6 +382,31 @@ namespace pinocchio *this, data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); } + /* Acces to dedicated segment in robot config space. */ + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + { + typedef const Eigen::MatrixBase & InputType; + typedef typename SizeDepType::template SegmentReturn::ConstType ReturnType; + typedef JointConfigFromDofSelectorVisitor Visitor; + typename Visitor::ArgsType arg(a); + return Visitor::run(*this, arg); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + { + typedef Eigen::MatrixBase & InputType; + typedef typename SizeDepType::template SegmentReturn::Type ReturnType; + typedef JointConfigFromDofSelectorVisitor Visitor; + typename Visitor::ArgsType arg(a); + return Visitor::run(*this, arg); + } + std::string shortname() const { return ::pinocchio::shortname(*this); From fef5932124fc0e98e7c31b5322b87dbca4cc2e5c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 113/166] [EtienneAr feedback] Fix ConfigSelectorFromDof visitor --- .../multibody/joint/joint-basic-visitors.hxx | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 1fc6cecd7f..354f47c882 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -203,8 +203,18 @@ namespace pinocchio template static ReturnType algo(const JointModelBase & jmodel, InputType a) { + // Converting a VectorBlock of anysize (static or dynamic) to another vector block of anysize + // (static or dynamic) since there is no copy constructor. auto vectorBlock = jmodel.jointConfigFromDofSelector(a); - return ReturnType(vectorBlock.nestedExpression(), 0, vectorBlock.size()); + + // VectorBlock does not implemet such getter, hack the Eigen::Block base class to retreive + // such values. + const Index start = vectorBlock.startRow() + + vectorBlock.startCol(); // The other dimension is always 0 (for vectors) + const Index size = + vectorBlock.rows() * vectorBlock.cols(); // The other dimension is always 1 (for vectors) + + return ReturnType(vectorBlock.nestedExpression(), start, size); } }; From 813798f3fa55bf1d6d40b941edb9b24b1683e536 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 114/166] [EtienneAr feedback] Fix CRBA test for mimic, and remove mimic in world convention test --- unittest/crba.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 3e15402135..3092f931f2 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -150,20 +150,20 @@ void test_mimic_against_full_model( G(secondary_idxv, primary_idxv) = ratio; Eigen::VectorXd q_mimic = pinocchio::randomConfiguration(model_mimic); - Eigen::VectorXd v_mimic = pinocchio::randomConfiguration(model_mimic); - Eigen::VectorXd a_mimic = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v_mimic = Eigen::VectorXd::Random(model_mimic.nv); + Eigen::VectorXd a_mimic = Eigen::VectorXd::Random(model_mimic.nv); Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); - Eigen::VectorXd v_full = G.transpose() * v_mimic; - Eigen::VectorXd a_full = G.transpose() * a_mimic; + Eigen::VectorXd v_full = G * v_mimic; + Eigen::VectorXd a_full = G * a_mimic; for (int n = 1; n < model_full.njoints; n++) { const double joint_ratio = ((n == secondary_id) ? ratio : 1.0); const double joint_offset = ((n == secondary_id) ? offset : 0.0); - Eigen::VectorXd aaa = model_mimic.joints[n].jointConfigFromDofSelector(q_mimic); model_full.joints[n].jointConfigFromDofSelector(q_full) = - joint_ratio * aaa + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + joint_ratio * model_mimic.joints[n].jointConfigFromDofSelector(q_mimic) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); } // Run crba @@ -189,10 +189,16 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) test_mimic_against_full_model( humanoid_model, humanoid_model.getJointId("rleg4_joint"), humanoid_model.getJointId("rleg5_joint")); + + // Test for spaced parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg2_joint"), + humanoid_model.getJointId("rleg5_joint")); + // Test for parallel joint mimic test_mimic_against_full_model( - humanoid_model, humanoid_model.getJointId("rleg4_joint"), - humanoid_model.getJointId("lleg4_joint")); + humanoid_model, humanoid_model.getJointId("lleg4_joint"), + humanoid_model.getJointId("rleg4_joint")); } BOOST_AUTO_TEST_CASE(test_minimal_crba) @@ -236,7 +242,7 @@ BOOST_AUTO_TEST_CASE(test_minimal_crba) BOOST_AUTO_TEST_CASE(test_roto_inertia_effects) { pinocchio::Model model, model_ref; - pinocchio::buildModels::humanoidRandom(model, true, true); + pinocchio::buildModels::humanoidRandom(model, true); model_ref = model; BOOST_CHECK(model == model_ref); From a4ac37c0dd4ad42a1a24d92dc1066d0811ca6bd5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 115/166] [EtienneAr feedback] Fix G matric for mimic crba test --- unittest/crba.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 3092f931f2..4490d46de2 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -146,7 +146,9 @@ void test_mimic_against_full_model( // Prepare test data Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); - G.topLeftCorner(model_mimic.nv, model_mimic.nv).setIdentity(); + G.topLeftCorner(secondary_idxv, secondary_idxv).setIdentity(); + G.bottomRightCorner(model_mimic.nv - secondary_idxv, model_mimic.nv - secondary_idxv) + .setIdentity(); G(secondary_idxv, primary_idxv) = ratio; Eigen::VectorXd q_mimic = pinocchio::randomConfiguration(model_mimic); From 33282e5ed766668a01d2c20b2e86891fcaa90021 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 116/166] [EtienneAr feedback] Patch warnings in mimic crba test --- unittest/crba.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 4490d46de2..07e2d32896 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -127,7 +127,9 @@ BOOST_AUTO_TEST_CASE(test_crba) } void test_mimic_against_full_model( - const pinocchio::Model & model_full, int primary_id, int secondary_id) + const pinocchio::Model & model_full, + const pinocchio::JointIndex & primary_id, + const pinocchio::JointIndex & secondary_id) { // constants const int primary_idxq = model_full.joints[primary_id].idx_q(); From dd83d3f9a2dd7ac8e7891ddd9d078281bb9fc2fe Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 117/166] [Algorithm/crba] Fix world convention for mimic joints --- include/pinocchio/algorithm/crba.hxx | 37 ++++++++++++++++++++++------ unittest/crba.cpp | 7 ++++++ 2 files changed, 37 insertions(+), 7 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index ad2ec509b0..816bf9ec05 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -57,6 +57,24 @@ namespace pinocchio } }; + template class JointCollectionTpl> + static inline void mimic_patch_CrbaWorldConventionBackwardStep( + const JointModelBase> & jmodel, + const Model & model, + Data & data) + { + typedef typename Model::JointIndex JointIndex; + + const JointIndex & i = jmodel.id(); + typedef + typename SizeDepType::template ColsReturn::Type + ColsBlock; + ColsBlock J_cols = jmodel.jointJacCols(data.J); + // Joint Space Inertia Matrix + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); + } + template class JointCollectionTpl> struct CrbaWorldConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -70,11 +88,6 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { - assert( - (std::is_same>::value - == false) - && "WORLD convention not supported for mimic joints"); - typedef typename Model::JointIndex JointIndex; typedef typename SizeDepType::template ColsReturn::Type @@ -87,8 +100,11 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); // Joint Space Inertia Matrix - jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += - J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); + if ( + std::is_same>::value + == false) + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); const JointIndex & parent = model.parents[i]; data.oYcrb[parent] += data.oYcrb[i]; @@ -301,12 +317,19 @@ namespace pinocchio data.M.setZero(); data.Ag.setZero(); + JointIndex mimic = -1; typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { + if (boost::get(&model.joints[i])) + mimic = i; Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); } + if (mimic != -1) + mimic_patch_CrbaWorldConventionBackwardStep( + boost::get(model.joints[mimic]), model, data); + // Add the armature contribution data.M.diagonal() += model.armature; diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 07e2d32896..1ffa5aa3db 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -145,6 +145,7 @@ void test_mimic_against_full_model( model_full, primary_id, secondary_id, ratio, offset, model_mimic); pinocchio::Data data_full(model_full); pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_ref_mimic(model_mimic); // Prepare test data Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); @@ -157,6 +158,12 @@ void test_mimic_against_full_model( Eigen::VectorXd v_mimic = Eigen::VectorXd::Random(model_mimic.nv); Eigen::VectorXd a_mimic = Eigen::VectorXd::Random(model_mimic.nv); + // World vs local + pinocchio::crba(model_mimic, data_ref_mimic, q_mimic); + pinocchio::crba(model_mimic, data_mimic, q_mimic, pinocchio::Convention::WORLD); + + BOOST_CHECK(data_ref_mimic.M.isApprox(data_mimic.M)); + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); Eigen::VectorXd v_full = G * v_mimic; Eigen::VectorXd a_full = G * a_mimic; From 2ce8335783ab2ca34e9930fa31d843018c965625 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 118/166] fix not working for all cases (spaced and parallel) --- include/pinocchio/algorithm/crba.hxx | 44 ++++++++++++------- include/pinocchio/multibody/sample-models.hxx | 16 +++---- 2 files changed, 35 insertions(+), 25 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 816bf9ec05..6515f45ba8 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -57,24 +57,33 @@ namespace pinocchio } }; + /// \brief Patch to the crba algorithm for joint mimic (in local convention) + template + static inline void mimic_patch_CrbaWorldConventionBackwardStep( + const JointModelBase &, const Model &, Data &) + { + } + + /// \note For the case of a joint j mimicking a joint i + /// the resulting inertia of both joints is + /// M = Mii + 2* Mij + Mjj + /// But in this current implementation only M = Mii + Mij + Mjj is computed + /// So an extra Mij is added here. template class JointCollectionTpl> static inline void mimic_patch_CrbaWorldConventionBackwardStep( const JointModelBase> & jmodel, const Model & model, Data & data) { - typedef typename Model::JointIndex JointIndex; + typedef JointModelMimicTpl JointModel; + const JointIndex secondary_id = jmodel.id(); + const JointIndex primary_id = jmodel.derived().jmodel().id(); - const JointIndex & i = jmodel.id(); - typedef - typename SizeDepType::template ColsReturn::Type - ColsBlock; - ColsBlock J_cols = jmodel.jointJacCols(data.J); - // Joint Space Inertia Matrix - jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += - J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); + jmodel.jointVelRows(data.M) + .middleCols(jmodel.idx_v(), data.nvSubtree[primary_id]) + .noalias() += jmodel.jointVelCols(data.J).transpose() + * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[primary_id]); } - template class JointCollectionTpl> struct CrbaWorldConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -99,12 +108,17 @@ namespace pinocchio ColsBlock J_cols = jmodel.jointJacCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + if(std::is_same>::value) + { + ColsBlock J_colsPrim = jmodel.jointVelCols(data.J); + // Joint Space Inertia Matrix + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + J_colsPrim.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); + } // Joint Space Inertia Matrix - if ( - std::is_same>::value - == false) jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); + mimic_patch_CrbaWorldConventionBackwardStep(jmodel, model, data); const JointIndex & parent = model.parents[i]; data.oYcrb[parent] += data.oYcrb[i]; @@ -326,10 +340,6 @@ namespace pinocchio Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); } - if (mimic != -1) - mimic_patch_CrbaWorldConventionBackwardStep( - boost::get(model.joints[mimic]), model, data); - // Add the armature contribution data.M.diagonal() += model.armature; diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 4fac645a23..560c7f3d3d 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -83,35 +83,35 @@ namespace pinocchio const std::string & root_joint_name = model.names[root_joint_idx]; joint_id = addJointAndBody( model, typename JC::JointModelRX(), root_joint_name, pre + "shoulder1", Mroot); - model.inertias[joint_id] = Ijoint; + // model.inertias[joint_id] = Ijoint; const JointIndex root_joint_id = joint_id; joint_id = addJointAndBody( model, typename JC::JointModelRY(), model.names[joint_id], pre + "shoulder2", Id4); - model.inertias[joint_id] = Ijoint; + // model.inertias[joint_id] = Ijoint; joint_id = addJointAndBody( model, typename JC::JointModelRZ(), model.names[joint_id], pre + "shoulder3", Id4); - model.inertias[joint_id] = Iarm; + // model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "upperarm_body", joint_id); joint_id = addJointAndBody( model, typename JC::JointModelRY(), model.names[joint_id], pre + "elbow", Marm); - model.inertias[joint_id] = Iarm; + // model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "lowerarm_body", joint_id); model.addBodyFrame(pre + "elbow_body", joint_id); joint_id = addJointAndBody( model, typename JC::JointModelRX(), model.names[joint_id], pre + "wrist1", Marm); - model.inertias[joint_id] = Ijoint; + // model.inertias[joint_id] = Ijoint; if (mimic) { // Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); // Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); - Scalar multiplier = 2.5; - Scalar offset = 0.75; + Scalar multiplier = 1; + Scalar offset = 0; joint_id = addJointAndBody( model, typename JC::JointModelMimic( @@ -124,7 +124,7 @@ namespace pinocchio model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4); } - model.inertias[joint_id] = Iarm; + // model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "effector_body", joint_id); const int nq = mimic ? 5 : 6; From 7c429018203579e144c4bffbc101f08bffb09ded Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 119/166] [algo/crba] Fix for crba world (works for spaced and parallel) --- include/pinocchio/algorithm/crba.hxx | 29 ++++++++----------- include/pinocchio/multibody/sample-models.hxx | 4 +-- 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 6515f45ba8..2b238035d9 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -79,11 +79,16 @@ namespace pinocchio const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); - jmodel.jointVelRows(data.M) - .middleCols(jmodel.idx_v(), data.nvSubtree[primary_id]) - .noalias() += jmodel.jointVelCols(data.J).transpose() - * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[primary_id]); + if (secondary_id <= primary_id + data.nvSubtree[primary_id]) + for (JointIndex i = primary_id; i < secondary_id; i++) + { + jmodel.jointVelRows(data.M) + .middleCols(model.joints[i].idx_v(), jmodel.derived().jmodel().nv()) + .noalias() += model.joints.at(i).jointJacCols(data.J).transpose() + * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); + } } + template class JointCollectionTpl> struct CrbaWorldConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -108,18 +113,11 @@ namespace pinocchio ColsBlock J_cols = jmodel.jointJacCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); - if(std::is_same>::value) - { - ColsBlock J_colsPrim = jmodel.jointVelCols(data.J); - // Joint Space Inertia Matrix - jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += - J_colsPrim.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - } // Joint Space Inertia Matrix - jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += - J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - mimic_patch_CrbaWorldConventionBackwardStep(jmodel, model, data); + jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); + mimic_patch_CrbaWorldConventionBackwardStep(jmodel, model, data); const JointIndex & parent = model.parents[i]; data.oYcrb[parent] += data.oYcrb[i]; } @@ -331,12 +329,9 @@ namespace pinocchio data.M.setZero(); data.Ag.setZero(); - JointIndex mimic = -1; typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { - if (boost::get(&model.joints[i])) - mimic = i; Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); } diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index 560c7f3d3d..a016d7d833 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -110,8 +110,8 @@ namespace pinocchio // Scalar multiplier = JC::JointModelRX::ConfigVector_t::Random(1)(0); // Scalar offset = JC::JointModelRX::ConfigVector_t::Random(1)(0); - Scalar multiplier = 1; - Scalar offset = 0; + Scalar multiplier = 2.5; + Scalar offset = 0.75; joint_id = addJointAndBody( model, typename JC::JointModelMimic( From 6491e4a4105e1410d402a8eae41e157a1883fc23 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 120/166] Fix crba world (small issue with parallel joint at the root where the two subtree meet) --- include/pinocchio/algorithm/crba.hxx | 31 ++++++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 2b238035d9..b807f1b798 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -9,6 +9,7 @@ #include "pinocchio/spatial/act-on-set.hpp" #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/model.hpp" /// @cond DEV @@ -79,14 +80,40 @@ namespace pinocchio const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); - if (secondary_id <= primary_id + data.nvSubtree[primary_id]) + JointIndex ancestor_prim, ancestor_sec; + JointIndex j_id = + findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); + + // Same branch of the tree + if (j_id == primary_id) for (JointIndex i = primary_id; i < secondary_id; i++) { jmodel.jointVelRows(data.M) - .middleCols(model.joints[i].idx_v(), jmodel.derived().jmodel().nv()) + .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) .noalias() += model.joints.at(i).jointJacCols(data.J).transpose() * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); } + else + { + for (JointIndex j = ancestor_sec; j < model.supports[secondary_id].size() - 1; j++) + { + j_id = model.supports[secondary_id].at(j); + jmodel.jointVelRows(data.M) + .middleCols(model.joints[j_id].idx_v(), model.joints[j_id].nv()) + .noalias() += + data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()).transpose() + * model.joints.at(j_id).jointJacCols(data.J); + } + for (JointIndex j = ancestor_prim + 1; j < model.supports[primary_id].size(); j++) + { + j_id = model.supports[primary_id].at(j); + jmodel.jointVelRows(data.M) + .middleCols(model.joints[j_id].idx_v(), model.joints[j_id].nv()) + .noalias() += + data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()).transpose() + * model.joints.at(j_id).jointJacCols(data.J); + } + } } template class JointCollectionTpl> From df5949b710f91a8b29abc427747134cf834661f1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 121/166] Fix for world convention --- include/pinocchio/algorithm/crba.hxx | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index b807f1b798..08b0795b96 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -95,7 +95,7 @@ namespace pinocchio } else { - for (JointIndex j = ancestor_sec; j < model.supports[secondary_id].size() - 1; j++) + for (JointIndex j = ancestor_sec + 1; j < model.supports[secondary_id].size() - 1; j++) { j_id = model.supports[secondary_id].at(j); jmodel.jointVelRows(data.M) @@ -107,11 +107,10 @@ namespace pinocchio for (JointIndex j = ancestor_prim + 1; j < model.supports[primary_id].size(); j++) { j_id = model.supports[primary_id].at(j); - jmodel.jointVelRows(data.M) - .middleCols(model.joints[j_id].idx_v(), model.joints[j_id].nv()) - .noalias() += - data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()).transpose() - * model.joints.at(j_id).jointJacCols(data.J); + jmodel.jointVelCols(data.M) + .middleRows(model.joints[j_id].idx_v(), model.joints[j_id].nv()) + .noalias() -= model.joints.at(j_id).jointJacCols(data.J).transpose() + * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); } } } From 121a201e30093f9867fbc98ce3c4ba4f661c7d27 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 122/166] Make centroidal algorithm compatible with mimic and add tests --- include/pinocchio/algorithm/centroidal.hxx | 2 + unittest/centroidal.cpp | 98 ++++++++++++++++++++++ unittest/crba.cpp | 12 +-- 3 files changed, 106 insertions(+), 6 deletions(-) diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index ed888cbd3f..19b38916dc 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -141,6 +141,7 @@ namespace pinocchio ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + data.oYcrb[parent] += data.oYcrb[i]; } @@ -173,6 +174,7 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); diff --git a/unittest/centroidal.cpp b/unittest/centroidal.cpp index b5bab82481..6494864950 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -90,6 +90,104 @@ BOOST_AUTO_TEST_CASE(test_ccrba) BOOST_CHECK(data.Ag.isApprox(Ag_ref, 1e-12)); } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const pinocchio::JointIndex & primary_id, + const pinocchio::JointIndex & secondary_id) +{ + // constants + const int primary_idxq = model_full.joints[primary_id].idx_q(); + const int primary_idxv = model_full.joints[primary_id].idx_v(); + const int secondary_idxq = model_full.joints[secondary_id].idx_q(); + const int secondary_idxv = model_full.joints[secondary_id].idx_v(); + const double ratio = 2.5; + const double offset = 0.75; + + // Build mimic model + pinocchio::Model model_mimic; + pinocchio::transformJointIntoMimic( + model_full, primary_id, secondary_id, ratio, offset, model_mimic); + pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_ref_mimic(model_mimic); + pinocchio::Data data_full(model_full); + + // Prepare test data + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); + G.topLeftCorner(secondary_idxv, secondary_idxv).setIdentity(); + G.bottomRightCorner(model_mimic.nv - secondary_idxv, model_mimic.nv - secondary_idxv) + .setIdentity(); + G(secondary_idxv, primary_idxv) = ratio; + + Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v = Eigen::VectorXd::Ones(model_mimic.nv); + + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); + Eigen::VectorXd v_full = G * v; + + for (int n = 1; n < model_full.njoints; n++) + { + const double joint_ratio = ((n == secondary_id) ? ratio : 1.0); + const double joint_offset = ((n == secondary_id) ? offset : 0.0); + model_full.joints[n].jointConfigFromDofSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigFromDofSelector(q) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } + + model_mimic.lowerPositionLimit.head<3>().fill(-1.); + model_mimic.upperPositionLimit.head<3>().fill(1.); + + pinocchio::crba(model_mimic, data_ref_mimic, q, pinocchio::Convention::LOCAL); + data_ref_mimic.M.triangularView() = + data_ref_mimic.M.transpose().triangularView(); + + data_ref_mimic.Ycrb[0] = data_ref_mimic.liMi[1].act(data_ref_mimic.Ycrb[1]); + pinocchio::Data data_ref_other(model_mimic); + pinocchio::crba(model_mimic, data_ref_other, q, pinocchio::Convention::WORLD); + data_ref_other.M.triangularView() = + data_ref_other.M.transpose().triangularView(); + + pinocchio::SE3 cMo(pinocchio::SE3::Matrix3::Identity(), -data_ref_mimic.Ycrb[0].lever()); + BOOST_CHECK(data_ref_mimic.Ycrb[0].isApprox(data_ref_other.oYcrb[0])); + + ccrba(model_mimic, data_mimic, q, v); + ccrba(model_full, data_full, q_full, v_full); + BOOST_CHECK(data_mimic.com[0].isApprox(data_full.com[0], 1e-12)); + BOOST_CHECK(data_mimic.oYcrb[0].matrix().isApprox(data_full.oYcrb[0].matrix(), 1e-12)); + + BOOST_CHECK(data_mimic.com[0].isApprox(-cMo.translation(), 1e-12)); + BOOST_CHECK(data_mimic.oYcrb[0].matrix().isApprox(data_ref_mimic.Ycrb[0].matrix(), 1e-12)); + + pinocchio::Inertia Ig_ref(cMo.act(data_mimic.oYcrb[0])); + BOOST_CHECK(data_mimic.Ig.matrix().isApprox(Ig_ref.matrix(), 1e-12)); + + pinocchio::SE3 oM1(data_ref_other.liMi[1]); + pinocchio::SE3 cM1(cMo * oM1); + pinocchio::Data::Matrix6x Ag_ref( + cM1.inverse().toActionMatrix().transpose() * data_ref_other.M.topRows<6>()); + BOOST_CHECK(data_mimic.Ag.isApprox(Ag_ref, 1e-12)); +} + +BOOST_AUTO_TEST_CASE(test_ccrba_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model); + + // Test for direct parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg3_joint"), + humanoid_model.getJointId("rleg4_joint")); + + // Test for spaced parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg4_joint")); + + // // Test for parallel joint mimic + // test_mimic_against_full_model( + // humanoid_model, humanoid_model.getJointId("lleg4_joint"), + // humanoid_model.getJointId("rleg4_joint")); +} + BOOST_AUTO_TEST_CASE(test_centroidal_mapping) { pinocchio::Model model; diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 1ffa5aa3db..672926bf42 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -159,7 +159,7 @@ void test_mimic_against_full_model( Eigen::VectorXd a_mimic = Eigen::VectorXd::Random(model_mimic.nv); // World vs local - pinocchio::crba(model_mimic, data_ref_mimic, q_mimic); + // pinocchio::crba(model_mimic, data_ref_mimic, q_mimic); pinocchio::crba(model_mimic, data_mimic, q_mimic, pinocchio::Convention::WORLD); BOOST_CHECK(data_ref_mimic.M.isApprox(data_mimic.M)); @@ -177,8 +177,8 @@ void test_mimic_against_full_model( + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); } - // Run crba - pinocchio::crba(model_mimic, data_mimic, q_mimic); + // // // Run crba + // // pinocchio::crba(model_mimic, data_mimic, q_mimic); pinocchio::crba(model_full, data_full, q_full); // Compute other half of matrix @@ -203,10 +203,10 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) // Test for spaced parent/child joint mimic test_mimic_against_full_model( - humanoid_model, humanoid_model.getJointId("rleg2_joint"), - humanoid_model.getJointId("rleg5_joint")); + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg4_joint")); - // Test for parallel joint mimic + // // Test for parallel joint mimic test_mimic_against_full_model( humanoid_model, humanoid_model.getJointId("lleg4_joint"), humanoid_model.getJointId("rleg4_joint")); From 80e9f401988ad4835e49262cc3e918ee3041545e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 123/166] Add mimic tests to rnea --- unittest/rnea.cpp | 92 +++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 85 insertions(+), 7 deletions(-) diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index ba7c688511..a916e6fc06 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE(test_rnea) using namespace pinocchio; pinocchio::Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -79,7 +79,7 @@ BOOST_AUTO_TEST_CASE(test_nle_vs_rnea) using namespace pinocchio; pinocchio::Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -136,7 +136,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_with_fext) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -179,7 +179,7 @@ BOOST_AUTO_TEST_CASE(test_rnea_with_armature) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model, true); model.armature = VectorXd::Random(model.nv) + VectorXd::Ones(model.nv); model.lowerPositionLimit.head<3>().fill(-1.); @@ -209,7 +209,7 @@ BOOST_AUTO_TEST_CASE(test_compute_gravity) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -239,7 +239,7 @@ BOOST_AUTO_TEST_CASE(test_compute_static_torque) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -285,7 +285,7 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis) const double prec = Eigen::NumTraits::dummy_precision(); Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model, true); model.lowerPositionLimit.head<3>().fill(-1.); model.upperPositionLimit.head<3>().fill(1.); @@ -344,4 +344,82 @@ BOOST_AUTO_TEST_CASE(test_compute_coriolis) } } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const pinocchio::JointIndex & primary_id, + const pinocchio::JointIndex & secondary_id) +{ + // constants + const int primary_idxq = model_full.joints[primary_id].idx_q(); + const int primary_idxv = model_full.joints[primary_id].idx_v(); + const int secondary_idxq = model_full.joints[secondary_id].idx_q(); + const int secondary_idxv = model_full.joints[secondary_id].idx_v(); + const double ratio = 2.5; + const double offset = 0.75; + + // Build mimic model + pinocchio::Model model_mimic; + pinocchio::transformJointIntoMimic( + model_full, primary_id, secondary_id, ratio, offset, model_mimic); + pinocchio::Data data_nle_mimic(model_mimic); + pinocchio::Data data_rnea_mimic(model_mimic); + pinocchio::Data data_full(model_full); + + // Prepare test data + Eigen::MatrixXd G = Eigen::MatrixXd::Zero(model_full.nv, model_mimic.nv); + G.topLeftCorner(secondary_idxv, secondary_idxv).setIdentity(); + G.bottomRightCorner(model_mimic.nv - secondary_idxv, model_mimic.nv - secondary_idxv) + .setIdentity(); + G(secondary_idxv, primary_idxv) = ratio; + + Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic); + Eigen::VectorXd v = Eigen::VectorXd::Random(model_mimic.nv); + Eigen::VectorXd a = Eigen::VectorXd::Random(model_mimic.nv); + + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); + Eigen::VectorXd v_full = G * v; + Eigen::VectorXd a_full = G * v; + + for (int n = 1; n < model_full.njoints; n++) + { + const double joint_ratio = ((n == secondary_id) ? ratio : 1.0); + const double joint_offset = ((n == secondary_id) ? offset : 0.0); + model_full.joints[n].jointConfigFromDofSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigFromDofSelector(q) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } + + pinocchio::crba(model_full, data_full, q_full, pinocchio::Convention::WORLD); + data_full.M.triangularView() = + data_full.M.transpose().triangularView(); + const Eigen::VectorXd nle = pinocchio::nonLinearEffects(model_full, data_full, q_full, v_full); + + // // Use equation of motion to compute tau from a_reduced + Eigen::VectorXd tau_ref = G.transpose() * data_full.M * G * a + (G.transpose() * nle); + + pinocchio::rnea(model_mimic, data_rnea_mimic, q, v, a); + BOOST_CHECK(tau_ref.isApprox(data_rnea_mimic.tau)); +} + +BOOST_AUTO_TEST_CASE(test_rnea_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model); + + // Test for direct parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg2_joint")); + + // Test for spaced parent/child joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("rleg1_joint"), + humanoid_model.getJointId("rleg4_joint")); + + // // Test for parallel joint mimic + test_mimic_against_full_model( + humanoid_model, humanoid_model.getJointId("lleg4_joint"), + humanoid_model.getJointId("rleg4_joint")); +} + BOOST_AUTO_TEST_SUITE_END() From c95789be0cfb56d1aed77651e7484fca5b58672b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 124/166] Add security --- include/pinocchio/algorithm/crba.hxx | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 08b0795b96..04925a1fa0 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -80,6 +80,8 @@ namespace pinocchio const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); + assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); + JointIndex ancestor_prim, ancestor_sec; JointIndex j_id = findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); From 35c26d7d240fd645c716d524f9863b367dc0e476 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 125/166] [EtienneAr feedback] Fix getRelativePlacement and add unit test --- include/pinocchio/algorithm/kinematics.hxx | 24 ++++++---- include/pinocchio/algorithm/model.hpp | 4 +- unittest/kinematics.cpp | 51 ++++++++++++++++++++++ 3 files changed, 68 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index fa61d1b49c..5ba623c990 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -5,6 +5,7 @@ #ifndef __pinocchio_kinematics_hxx__ #define __pinocchio_kinematics_hxx__ +#include "model.hpp" #include "pinocchio/multibody/visitor.hpp" #include "pinocchio/algorithm/check.hpp" @@ -297,19 +298,24 @@ namespace pinocchio result.setIdentity(); // Traverse the kinematic chain from "tip" to "root" - JointIndex i = child_id; - while (i > parent_id) + size_t common_ancestor_parent, common_ancestor_child; + findCommonAncestor(model, parent_id, child_id, common_ancestor_parent, common_ancestor_child); + + // Traverse the kinematic chain backward to the common ancestor + for (size_t i = model.supports[child_id].size() - 1; i > common_ancestor_child; i--) { - result = data.liMi[i].act(result); - i = model.parents[i]; + const JointIndex j = model.supports[child_id][(size_t)i]; + result = data.liMi[j].act(result); + } + // Then forward to the "parent" + for (size_t i = common_ancestor_parent + 1; i <= model.supports[parent_id].size() - 1; i++) + { + const JointIndex j = model.supports[parent_id][i]; + result = data.liMi[j].actInv(result); } - - // The parent have been overshot, meaning it is in a different sub kinematic chain as the - // child - assert(i == parent_id && "Joints are not direct ancestor / descendants. Unsupported case."); // Inverse the result if necessary - if (child_id == jointIdRef) + if (child_id == jointIdRef && jointIdRef != jointIdTarget) { result = result.inverse(); } diff --git a/include/pinocchio/algorithm/model.hpp b/include/pinocchio/algorithm/model.hpp index 95823d67e2..a6f593578c 100644 --- a/include/pinocchio/algorithm/model.hpp +++ b/include/pinocchio/algorithm/model.hpp @@ -232,8 +232,8 @@ namespace pinocchio * \param[in] model the input model. * \param[in] joint1_id index of the first joint. * \param[in] joint2_id index of the second joint. - * \param[out] index_ancestor_in_support1 index of the ancestor within model.support[joint1_id]. - * \param[out] index_ancestor_in_support2 index of the ancestor within model.support[joint2_id]. + * \param[out] index_ancestor_in_support1 index of the ancestor within model.supports[joint1_id]. + * \param[out] index_ancestor_in_support2 index of the ancestor within model.supports[joint2_id]. * */ template class JointCollectionTpl> diff --git a/unittest/kinematics.cpp b/unittest/kinematics.cpp index afde400324..6cd7d49e37 100644 --- a/unittest/kinematics.cpp +++ b/unittest/kinematics.cpp @@ -81,6 +81,57 @@ BOOST_AUTO_TEST_CASE(test_kinematics_first) } } +BOOST_AUTO_TEST_CASE(test_getRelativePlacement) +{ + using namespace Eigen; + using namespace pinocchio; + + Model model; + buildModels::humanoid(model); + Data data(model); + + model.lowerPositionLimit.head<3>().fill(-1.); + model.upperPositionLimit.head<3>().fill(1.); + forwardKinematics(model, data, randomConfiguration(model)); + + const std::vector test_joints{ + 0, 1, model.getJointId("rleg_elbow_joint"), model.getJointId("lleg_elbow_joint"), + model.njoints - 1}; + + for (const JointIndex i : test_joints) + { + for (const JointIndex j : test_joints) + { + SE3 placement_world = getRelativePlacement(model, data, i, j, Convention::WORLD); + SE3 placement_local = getRelativePlacement(model, data, i, j, Convention::LOCAL); + + // Both convention should match + BOOST_CHECK(placement_world.isApprox(placement_local)); + + // Relative placement to itself is identity + if (i == j) + { + BOOST_CHECK(placement_world.isIdentity()); + BOOST_CHECK(placement_local.isIdentity()); + } + + // Relative placement to world + if (i == 0) + { + BOOST_CHECK(placement_world.isApprox(data.oMi[j])); + BOOST_CHECK(placement_local.isApprox(data.oMi[j])); + } + + // Relative placement from world + if (j == 0) + { + BOOST_CHECK(placement_world.isApprox(data.oMi[i].inverse())); + BOOST_CHECK(placement_local.isApprox(data.oMi[i].inverse())); + } + } + } +} + BOOST_AUTO_TEST_CASE(test_kinematics_second) { using namespace Eigen; From cffacd804cfb230e5fc8e2b2cec4dff88ea6d676 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 126/166] Add security --- .../algorithm/centroidal-derivatives.hxx | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 4047ca92e5..33f412d72e 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -58,6 +58,11 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; Motion & ov = data.ov[i]; @@ -78,7 +83,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityFromNvSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -149,6 +154,7 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { + typedef typename Model::JointIndex JointIndex; const JointIndex & i = jmodel.id(); @@ -168,7 +174,7 @@ namespace pinocchio ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); // tau - jmodel.jointVelocityFromDofSelector(data.tau).noalias() = + jmodel.jointVelocityFromNvSelector(data.tau).noalias() = J_cols.transpose() * data.of[i].toVector(); // dtau/da similar to data.M @@ -369,6 +375,11 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; From aec853ae9a5e1e902aefe7d1b91ee498f6bfc552 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 127/166] [EtienneAr feedback] Fix ForceOperator for generic motion subspace and mimic and prevent any malloc --- .../joint-motion-subspace-generic.hpp | 36 ++++++++++++++----- .../pinocchio/multibody/joint/joint-mimic.hpp | 36 +++++++------------ 2 files changed, 39 insertions(+), 33 deletions(-) diff --git a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp index 462203a687..9906d404a8 100644 --- a/include/pinocchio/multibody/joint-motion-subspace-generic.hpp +++ b/include/pinocchio/multibody/joint-motion-subspace-generic.hpp @@ -38,10 +38,27 @@ namespace pinocchio typedef Eigen::Matrix ReturnType; }; - template - struct MotionAlgebraAction, MotionDerived> + template + struct MotionAlgebraAction, MotionDerived> { - typedef Eigen::Matrix ReturnType; + typedef Eigen::Matrix ReturnType; + }; + + template + struct ConstraintForceOp, ForceDerived> + { + typedef + typename traits>::DenseBase DenseBase; + typedef Eigen::Matrix ReturnType; + }; + + template + struct ConstraintForceSetOp, ForceSet> + { + typedef + typename traits>::DenseBase DenseBase; + typedef + typename MatrixMatrixProduct, ForceSet>::type ReturnType; }; template @@ -119,17 +136,18 @@ namespace pinocchio { } - template - JointForce operator*(const ForceDense & f) const + template + typename ConstraintForceOp::ReturnType + operator*(const ForceDense & f) const { return (ref.S.transpose() * f.toVector()).eval(); } - template - typename Eigen::Matrix - operator*(const Eigen::MatrixBase & F) + template + typename ConstraintForceSetOp::ReturnType + operator*(const Eigen::MatrixBase & F) { - return ref.S.transpose() * F; + return ref.S.transpose() * F.derived(); } }; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 09ffe9ec4c..89c2416f31 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -70,35 +70,23 @@ namespace pinocchio template struct ConstraintForceOp, ForceDerived> { - typedef typename traits>:: - RefJointMotionSubspace::Scalar Scalar; - typedef Eigen::Matrix<_Scalar, Eigen::Dynamic, Eigen::Dynamic, _Options, _MaxDim, _MaxDim> - OriginalReturnType; - - typedef typename ScalarMatrixProduct::type IdealReturnType; - typedef Eigen::Matrix< - Scalar, - IdealReturnType::RowsAtCompileTime, - IdealReturnType::ColsAtCompileTime, - traits< - ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace::Options> - ReturnType; + typedef + typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace + RefJointMotionSubspace; + typedef + typename ConstraintForceOp::ReturnType RefReturnType; + typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType; }; template struct ConstraintForceSetOp, ForceSet> { - typedef ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim> MotionSubspace; - typedef typename traits::RefJointMotionSubspace::Scalar Scalar; - - typedef Eigen::Matrix< - Scalar, - Eigen::Dynamic, - Eigen::Dynamic, - traits::RefJointMotionSubspace::Options | Eigen::RowMajor, - _MaxDim, - _MaxDim> - ReturnType; + typedef + typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace + RefJointMotionSubspace; + typedef + typename ConstraintForceSetOp::ReturnType RefReturnType; + typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType; }; template From 303a1bca24611cc3b288bd42ab2bda5c167d4cd0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 128/166] [EtienneAr feedback] Patch previous fix on ForceOperator for motion subspace tpl --- include/pinocchio/multibody/joint/joint-mimic.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 89c2416f31..7a396d3c9f 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -174,8 +174,8 @@ namespace pinocchio } template - typename ConstraintForceOp::ReturnType - operator*(const ForceDense & f) const + // typename ConstraintForceOp::ReturnType + JointForce operator*(const ForceDense & f) const { return ref.m_scaling_factor * (ref.m_constraint.transpose() * f); } From 3681413295b43d31f877c4c7d5190491dbe92050 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 129/166] Fix some errrors for com computation --- include/pinocchio/algorithm/center-of-mass.hxx | 11 ++++++----- include/pinocchio/multibody/sample-models.hxx | 12 ++++++------ 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index e09ed85a5c..d4ba06dbe4 100644 --- a/include/pinocchio/algorithm/center-of-mass.hxx +++ b/include/pinocchio/algorithm/center-of-mass.hxx @@ -256,12 +256,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - Jcom_.fill(0); ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); - for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) + for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) { jmodel.jointVelCols(Jcom_).col(col_id) += data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR) @@ -319,6 +318,7 @@ namespace pinocchio } // Backward step + data.Jcom.setZero(); typedef JacobianCenterOfMassBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -366,12 +366,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - Jcom_.fill(0); - + ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); - for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id) + for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) { jmodel.jointVelCols(Jcom_).col(col_id) += Jcols.col(col_id).template segment<3>(Motion::LINEAR) @@ -446,6 +445,7 @@ namespace pinocchio } // Backward step + data.Jcom.setZero(); typedef JacobianCenterOfMassBackwardStep Pass2; for (Eigen::DenseIndex k = (Eigen::DenseIndex)subtree.size() - 1; k >= 0; --k) @@ -476,6 +476,7 @@ namespace pinocchio Jcom_subtree.middleCols(idx_v, nv_subtree) *= mass_inv_subtree; // Second backward step + typedef JacobianSubtreeCenterOfMassBackwardStep< Scalar, Options, JointCollectionTpl, Matrix3xLike> Pass3; diff --git a/include/pinocchio/multibody/sample-models.hxx b/include/pinocchio/multibody/sample-models.hxx index a016d7d833..4fac645a23 100644 --- a/include/pinocchio/multibody/sample-models.hxx +++ b/include/pinocchio/multibody/sample-models.hxx @@ -83,27 +83,27 @@ namespace pinocchio const std::string & root_joint_name = model.names[root_joint_idx]; joint_id = addJointAndBody( model, typename JC::JointModelRX(), root_joint_name, pre + "shoulder1", Mroot); - // model.inertias[joint_id] = Ijoint; + model.inertias[joint_id] = Ijoint; const JointIndex root_joint_id = joint_id; joint_id = addJointAndBody( model, typename JC::JointModelRY(), model.names[joint_id], pre + "shoulder2", Id4); - // model.inertias[joint_id] = Ijoint; + model.inertias[joint_id] = Ijoint; joint_id = addJointAndBody( model, typename JC::JointModelRZ(), model.names[joint_id], pre + "shoulder3", Id4); - // model.inertias[joint_id] = Iarm; + model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "upperarm_body", joint_id); joint_id = addJointAndBody( model, typename JC::JointModelRY(), model.names[joint_id], pre + "elbow", Marm); - // model.inertias[joint_id] = Iarm; + model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "lowerarm_body", joint_id); model.addBodyFrame(pre + "elbow_body", joint_id); joint_id = addJointAndBody( model, typename JC::JointModelRX(), model.names[joint_id], pre + "wrist1", Marm); - // model.inertias[joint_id] = Ijoint; + model.inertias[joint_id] = Ijoint; if (mimic) { @@ -124,7 +124,7 @@ namespace pinocchio model, typename JC::JointModelRY(), model.names[joint_id], pre + "wrist2", Id4); } - // model.inertias[joint_id] = Iarm; + model.inertias[joint_id] = Iarm; model.addBodyFrame(pre + "effector_body", joint_id); const int nq = mimic ? 5 : 6; From e524e42aa98946ba571329186f8d96bdbd0ace2d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 130/166] [Rnea] Patch Algorithms for mimic --- include/pinocchio/algorithm/rnea.hxx | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index d7d34809d7..297521a096 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -389,7 +389,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - jmodel.jointVelocityFromDofSelector(g) = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityFromDofSelector(g) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); } @@ -425,6 +425,7 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } + data.g.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -467,6 +468,7 @@ namespace pinocchio data.f[i] -= fext[i]; } + data.g.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -589,13 +591,17 @@ namespace pinocchio ColsBlock J_cols = jmodel.jointJacCols(data.J); ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); + motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); jmodel.jointVelCols(data.dFdv).noalias() += data.B[i] * J_cols; jmodel.jointVelRows(data.C).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + std::cout + << data.parents_fromRow[(JointIndex)jmodel.idx_v()] << " " + << data.parents_fromRow[(JointIndex)data.parents_fromRow[(JointIndex)jmodel.idx_v()]] + << std::endl; for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) jmodel.jointVelRows(data.C).col(j).noalias() += Ag_cols.transpose() * data.dJ.col(j); @@ -643,6 +649,8 @@ namespace pinocchio } data.C.setZero(); + data.dFdv.setZero(); + data.Ag.setZero(); typedef CoriolisMatrixBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { From 9d823612acc3d54694e730601fb7e6afcb965c5f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 131/166] remove mimic joints from test --- unittest/python/bindings_joints.py | 2 +- unittest/python/bindings_model.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index 1a27e92abf..69be539214 100644 --- a/unittest/python/bindings_joints.py +++ b/unittest/python/bindings_joints.py @@ -7,7 +7,7 @@ class TestJointsAlgo(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) qmax = np.full((self.model.nq, 1), np.pi) self.q = pin.randomConfiguration(self.model, -qmax, qmax) diff --git a/unittest/python/bindings_model.py b/unittest/python/bindings_model.py index 25d257f0ea..912596e752 100644 --- a/unittest/python/bindings_model.py +++ b/unittest/python/bindings_model.py @@ -8,7 +8,7 @@ class TestModel(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) def test_empty_model_sizes(self): model = pin.Model() From 9f2a1e3255f0914e8ea911b18b1028261f7caeab Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 132/166] [unittest/rnea] Add more test --- unittest/rnea.cpp | 43 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index a916e6fc06..1877994641 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -399,6 +399,49 @@ void test_mimic_against_full_model( pinocchio::rnea(model_mimic, data_rnea_mimic, q, v, a); BOOST_CHECK(tau_ref.isApprox(data_rnea_mimic.tau)); + + // NLE + pinocchio::Data data_nle(model_mimic); + pinocchio::Data data_ref_nle(model_mimic); + Eigen::VectorXd tau_ref_nle = + pinocchio::rnea(model_mimic, data_ref_nle, q, v, Eigen::VectorXd::Zero(model_mimic.nv)); + Eigen::VectorXd tau_nle = pinocchio::nonLinearEffects(model_mimic, data_nle, q, v); + BOOST_CHECK(tau_nle.isApprox(tau_nle)); + + // Generalized Gravity + pinocchio::Data data_ref_gg(model_mimic); + pinocchio::Data data_gg(model_mimic); + Eigen::VectorXd tau_ref_gg = pinocchio::rnea( + model_mimic, data_ref_gg, q, Eigen::VectorXd::Zero(model_mimic.nv), + Eigen::VectorXd::Zero(model_mimic.nv)); + Eigen::VectorXd tau_gg = pinocchio::computeGeneralizedGravity(model_mimic, data_gg, q); + BOOST_CHECK(tau_gg.isApprox(tau_ref_gg)); + + // Static Torque + typedef PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::Force) ForceVector; + ForceVector fext((size_t)model_mimic.njoints); + for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) + (*it).setRandom(); + + pinocchio::Data data_ref_st(model_mimic); + pinocchio::Data data_st(model_mimic); + pinocchio::rnea( + model_mimic, data_ref_st, q, Eigen::VectorXd::Zero(model_mimic.nv), + Eigen::VectorXd::Zero(model_mimic.nv), fext); + Eigen::VectorXd tau_st = pinocchio::computeStaticTorque(model_mimic, data_gg, q, fext); + BOOST_CHECK(tau_st.isApprox(data_ref_gg.tau)); + + // Compute Coriolis + pinocchio::Data data_ref_cc(model_mimic); + pinocchio::Data data_cc(model_mimic); + Eigen::MatrixXd C = pinocchio::computeCoriolisMatrix(model_mimic, data_cc, q, v); + Eigen::MatrixXd tau_ref_cc = + pinocchio::rnea(model_mimic, data_ref_cc, q, v, Eigen::VectorXd::Zero(model_mimic.nv)) + - pinocchio::rnea( + model_mimic, data_ref_cc, q, Eigen::VectorXd::Zero(model_mimic.nv), + Eigen::VectorXd::Zero(model_mimic.nv)); + + BOOST_CHECK(tau_ref_cc.isApprox(C * v)); } BOOST_AUTO_TEST_CASE(test_rnea_mimic) From ec4db7cb90241898ceb6bb4e604f83992936e2be Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 133/166] Fix tests and remove modification for Coriolis Matrix --- .../pinocchio/algorithm/center-of-mass.hxx | 2 +- .../algorithm/centroidal-derivatives.hxx | 4 +- include/pinocchio/algorithm/centroidal.hxx | 1 + include/pinocchio/algorithm/rnea.hxx | 47 +++++++++++-------- unittest/python/bindings_rnea.py | 2 +- unittest/rnea.cpp | 14 +----- 6 files changed, 34 insertions(+), 36 deletions(-) diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index d4ba06dbe4..d67be398b8 100644 --- a/include/pinocchio/algorithm/center-of-mass.hxx +++ b/include/pinocchio/algorithm/center-of-mass.hxx @@ -366,7 +366,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - + ColBlock Jcols = jmodel.jointJacCols(data.J); Jcols = data.oMi[i].act(jdata.S()); diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 33f412d72e..35e68ef917 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -154,7 +154,7 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { - + typedef typename Model::JointIndex JointIndex; const JointIndex & i = jmodel.id(); @@ -375,7 +375,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - assert( + assert( (std::is_same>::value == false) && "Algorithm not supported for mimic joints"); diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 19b38916dc..1786cf33f8 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -222,6 +222,7 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 297521a096..ea3b138aea 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -469,6 +469,7 @@ namespace pinocchio } data.g.setZero(); + data.tau.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -573,6 +574,12 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { + + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, @@ -582,7 +589,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nj(), 6); + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6); typedef typename SizeDepType::template ColsReturn::Type @@ -591,25 +598,23 @@ namespace pinocchio ColsBlock J_cols = jmodel.jointJacCols(data.J); ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); + motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); jmodel.jointVelCols(data.dFdv).noalias() += data.B[i] * J_cols; - jmodel.jointVelRows(data.C).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); - std::cout - << data.parents_fromRow[(JointIndex)jmodel.idx_v()] << " " - << data.parents_fromRow[(JointIndex)data.parents_fromRow[(JointIndex)jmodel.idx_v()]] - << std::endl; + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - jmodel.jointVelRows(data.C).col(j).noalias() += Ag_cols.transpose() * data.dJ.col(j); + data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = + Ag_cols.transpose() * data.dJ.col(j); - Mat_tmp.topRows(jmodel.nj()).noalias() = J_cols.transpose() * data.B[i]; + Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - jmodel.jointVelRows(data.C).col(j).noalias() += Mat_tmp * data.J.col(j); + data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() += + Mat_tmp * data.J.col(j); if (parent > 0) { @@ -648,9 +653,6 @@ namespace pinocchio typename Pass1::ArgsType(model, data, q.derived(), v.derived())); } - data.C.setZero(); - data.dFdv.setZero(); - data.Ag.setZero(); typedef CoriolisMatrixBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -673,6 +675,12 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { + + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, @@ -682,7 +690,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nj(), 6); + typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6); typedef typename SizeDepType::template ColsReturn::Type @@ -696,18 +704,19 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dFdv_cols); dFdv_cols.noalias() += data.B[i] * J_cols; - jmodel.jointVelRows(data.C).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - jmodel.jointVelRows(data.C).col(j).noalias() = Ag_cols.transpose() * data.dJ.col(j); + data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = + Ag_cols.transpose() * data.dJ.col(j); - Mat_tmp.topRows(jmodel.nj()).noalias() = J_cols.transpose() * data.B[i]; + Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i]; for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0; j = data.parents_fromRow[(JointIndex)j]) - jmodel.jointVelRows(data.C).col(j) += Mat_tmp * data.J.col(j); + data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j) += Mat_tmp * data.J.col(j); if (parent > 0) data.B[parent] += data.B[i]; diff --git a/unittest/python/bindings_rnea.py b/unittest/python/bindings_rnea.py index a6c9111707..a3e4f611b6 100644 --- a/unittest/python/bindings_rnea.py +++ b/unittest/python/bindings_rnea.py @@ -7,7 +7,7 @@ class TestRNEA(TestCase): def setUp(self): - self.model = pin.buildSampleModelHumanoidRandom(True, True) + self.model = pin.buildSampleModelHumanoidRandom(True) self.data = self.model.createData() qmax = np.full((self.model.nq, 1), np.pi) diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 1877994641..b3b484e597 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -429,19 +429,7 @@ void test_mimic_against_full_model( model_mimic, data_ref_st, q, Eigen::VectorXd::Zero(model_mimic.nv), Eigen::VectorXd::Zero(model_mimic.nv), fext); Eigen::VectorXd tau_st = pinocchio::computeStaticTorque(model_mimic, data_gg, q, fext); - BOOST_CHECK(tau_st.isApprox(data_ref_gg.tau)); - - // Compute Coriolis - pinocchio::Data data_ref_cc(model_mimic); - pinocchio::Data data_cc(model_mimic); - Eigen::MatrixXd C = pinocchio::computeCoriolisMatrix(model_mimic, data_cc, q, v); - Eigen::MatrixXd tau_ref_cc = - pinocchio::rnea(model_mimic, data_ref_cc, q, v, Eigen::VectorXd::Zero(model_mimic.nv)) - - pinocchio::rnea( - model_mimic, data_ref_cc, q, Eigen::VectorXd::Zero(model_mimic.nv), - Eigen::VectorXd::Zero(model_mimic.nv)); - - BOOST_CHECK(tau_ref_cc.isApprox(C * v)); + BOOST_CHECK(tau_st.isApprox(data_ref_st.tau)); } BOOST_AUTO_TEST_CASE(test_rnea_mimic) From f099ac71401604d69e11b7727b180b93e28c240e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 134/166] [rnea] Remove useless setZero --- include/pinocchio/algorithm/rnea.hxx | 1 - 1 file changed, 1 deletion(-) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index ea3b138aea..a5511da0e8 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -468,7 +468,6 @@ namespace pinocchio data.f[i] -= fext[i]; } - data.g.setZero(); data.tau.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) From 4f8db67fea76e95718af87be3c72a69c78d1f659 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 135/166] [Urdf] Rebase on new pinocchio release --- bindings/python/parsers/urdf/model.cpp | 96 ++++++++++--------- bindings/python/pinocchio/shortcuts.py | 14 ++- examples/build-reduced-model.py | 2 +- examples/geometry-models.py | 2 +- include/pinocchio/parsers/urdf.hpp | 12 +++ include/pinocchio/parsers/urdf/model.hxx | 9 +- .../python/bindings_geometry_model_urdf.py | 18 ++-- 7 files changed, 95 insertions(+), 58 deletions(-) diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index e9bfcabbbb..e89daaefb0 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -20,19 +20,21 @@ namespace pinocchio #ifdef PINOCCHIO_WITH_URDFDOM - Model buildModelFromUrdf(const bp::object & filename, const bool mimic=false) + Model buildModelFromUrdf(const bp::object & filename, const bool mimic = false) { Model model; pinocchio::urdf::buildModel(path(filename), model, mimic); return model; } - Model & buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic=false) + Model & + buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic = false) { return pinocchio::urdf::buildModel(path(filename), model, mimic); } - Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, const bool mimic=false) + Model buildModelFromUrdf( + const bp::object & filename, const JointModel & root_joint, const bool mimic = false) { Model model; pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); @@ -43,15 +45,18 @@ namespace pinocchio const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name, - const bool mimic=false) + const bool mimic = false) { Model model; pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); return model; } - Model & - buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, Model & model, const bool mimic=false) + Model & buildModelFromUrdf( + const bp::object & filename, + const JointModel & root_joint, + Model & model, + const bool mimic = false) { return pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); } @@ -60,13 +65,14 @@ namespace pinocchio const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name, - Model & model, + Model & model, const bool mimic = false) { return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); } - Model buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, const bool mimic=false) + Model buildModelFromXML( + const std::string & xml_stream, const JointModel & root_joint, const bool mimic = false) { Model model; pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); @@ -76,7 +82,7 @@ namespace pinocchio Model buildModelFromXML( const std::string & xml_stream, const JointModel & root_joint, - const std::string & root_joint_name, + const std::string & root_joint_name, const bool mimic = false) { Model model; @@ -84,8 +90,11 @@ namespace pinocchio return model; } - Model & - buildModelFromXML(const std::string & xml_stream, const JointModel & root_joint, Model & model, const bool mimic = false) + Model & buildModelFromXML( + const std::string & xml_stream, + const JointModel & root_joint, + Model & model, + const bool mimic = false) { pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); return model; @@ -95,21 +104,22 @@ namespace pinocchio const std::string & xml_stream, const JointModel & root_joint, const std::string & root_joint_name, - Model & model, + Model & model, const bool mimic = false) { pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); return model; } - Model buildModelFromXML(const std::string & xml_stream, const bool mimic=true) + Model buildModelFromXML(const std::string & xml_stream, const bool mimic = true) { Model model; pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); return model; } - Model & buildModelFromXML(const std::string & xml_stream, Model & model, const bool mimic=true) + Model & + buildModelFromXML(const std::string & xml_stream, Model & model, const bool mimic = true) { pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); return model; @@ -126,37 +136,31 @@ namespace pinocchio "buildModelFromUrdf", static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint"), + bp::args("urdf_filename", "root_joint", "mimic"), "Parse the URDF file given in input and return a pinocchio Model starting with the " "given root joint."); bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "root_joint_name"), + bp::args("urdf_filename", "root_joint", "root_joint_name", "mimic"), "Parse the URDF file given in input and return a pinocchio Model starting with the " "given root joint with its specified name."); bp::def( "buildModelFromUrdf", - static_cast( + static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "root_joint_name"), - "Parse the URDF file given in input and return a pinocchio Model starting with the " - "given root joint with its specified name."); - - bp::def( - "buildModelFromUrdf", - static_cast(pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename"), + bp::args("urdf_filename", "mimic"), "Parse the URDF file given in input and return a pinocchio Model."); bp::def( "buildModelFromUrdf", static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "model"), + bp::args("urdf_filename", "model", "mimic"), "Append to a given model a URDF structure given by its filename.", bp::return_internal_reference<2>()); @@ -164,7 +168,7 @@ namespace pinocchio "buildModelFromUrdf", static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "model"), + bp::args("urdf_filename", "root_joint", "model", "mimic"), "Append to a given model a URDF structure given by its filename and the root joint.\n" "Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons," "it is treated as operational frame and not as a joint of the model.", @@ -173,8 +177,8 @@ namespace pinocchio bp::def( "buildModelFromUrdf", static_cast(pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "root_joint_name", "model"), + Model &, const bool)>(pinocchio::python::buildModelFromUrdf), + bp::args("urdf_filename", "root_joint", "root_joint_name", "model", "mimic"), "Append to a given model a URDF structure given by its filename and the root joint with " "its specified name.\n" "Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons," @@ -185,23 +189,28 @@ namespace pinocchio "buildModelFromXML", static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint"), + bp::args("urdf_xml_stream", "root_joint", "mimic"), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " "the given root joint."); bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "root_joint_name"), + bp::args( + "urdf_xml_stream", "root_joint", + "root_joint_name" + "mimic"), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " "the given root joint with its specified name."); bp::def( "buildModelFromXML", - static_cast( + static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "root_joint_name"), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "mimic"), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " "the given root joint with its specified name."); @@ -209,7 +218,7 @@ namespace pinocchio "buildModelFromXML", static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "model"), + bp::args("urdf_xml_stream", "root_joint", "model", "mimic"), "Parse the URDF XML stream given in input and append it to the input model with the " "given interfacing joint.", bp::return_internal_reference<3>()); @@ -217,8 +226,8 @@ namespace pinocchio bp::def( "buildModelFromXML", static_cast(pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model"), + Model &, const bool)>(pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model", "mimic"), "Parse the URDF XML stream given in input and append it to the input model with the " "given interfacing joint with its specified name.", bp::return_internal_reference<3>()); @@ -226,23 +235,24 @@ namespace pinocchio bp::def( "buildModelFromXML", static_cast(pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model"), + Model &, const bool)>(pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "model", "mimic"), "Parse the URDF XML stream given in input and append it to the input model with the " "given interfacing joint with its specified name.", bp::return_internal_reference<3>()); bp::def( "buildModelFromXML", - static_cast(pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream"), + static_cast( + pinocchio::python::buildModelFromXML), + bp::args("urdf_xml_stream", "mimic"), "Parse the URDF XML stream given in input and return a pinocchio Model."); bp::def( "buildModelFromXML", static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "model"), + bp::args("urdf_xml_stream", "model", "mimic"), "Parse the URDF XML stream given in input and append it to the input model.", bp::return_internal_reference<2>()); #endif diff --git a/bindings/python/pinocchio/shortcuts.py b/bindings/python/pinocchio/shortcuts.py index 0346619572..85d101bac2 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -35,7 +35,14 @@ def buildModelsFromUrdf( Remark: In the URDF format, a joint of type fixed can be defined. For efficiency reasons, it is treated as operational frame and not as a joint of the model. """ # Handle the switch from old to new api - arg_keys = ["package_dirs", "root_joint", "verbose", "meshLoader", "geometry_types", "mimic"] + arg_keys = [ + "package_dirs", + "root_joint", + "verbose", + "meshLoader", + "geometry_types", + "mimic", + ] if len(args) >= 3: if isinstance(args[2], str): arg_keys = [ @@ -45,7 +52,7 @@ def buildModelsFromUrdf( "verbose", "meshLoader", "geometry_types", - "mimic" + "mimic", ] for key, arg in zip(arg_keys, args): @@ -59,14 +66,13 @@ def buildModelsFromUrdf( def _buildModelsFromUrdf( filename, - mimic=False, package_dirs=None, root_joint=None, root_joint_name=None, verbose=False, meshLoader=None, geometry_types=None, - mimic=False + mimic=False, ) -> Tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]: if geometry_types is None: geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL] diff --git a/examples/build-reduced-model.py b/examples/build-reduced-model.py index 2916fdf27e..d61ad4da36 100644 --- a/examples/build-reduced-model.py +++ b/examples/build-reduced-model.py @@ -78,7 +78,7 @@ # reference_configuration is optional: if not provided, neutral configuration used # you can even mix joint names and joint ids mixed_jointsToLockIDs = [jointsToLockIDs[0], "wrist_2_joint", "wrist_3_joint"] -robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, True, mesh_dir) +robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir, mimic=True) reduced_robot = robot.buildReducedRobot( list_of_joints_to_lock=mixed_jointsToLockIDs, reference_configuration=initialJointConfig, diff --git a/examples/geometry-models.py b/examples/geometry-models.py index 046f27a2ba..7544003005 100644 --- a/examples/geometry-models.py +++ b/examples/geometry-models.py @@ -14,7 +14,7 @@ # Load the urdf model model, collision_model, visual_model = pinocchio.buildModelsFromUrdf( - urdf_model_path, False, mesh_dir + urdf_model_path, mesh_dir, mimic=False ) print("model name: " + model.name) diff --git a/include/pinocchio/parsers/urdf.hpp b/include/pinocchio/parsers/urdf.hpp index 942fa3085f..c3dfebfb99 100644 --- a/include/pinocchio/parsers/urdf.hpp +++ b/include/pinocchio/parsers/urdf.hpp @@ -33,6 +33,7 @@ namespace pinocchio /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] rootJointName Name of the rootJoint. /// \param[in] verbose Print parsing info. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. /// @@ -42,6 +43,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic = false, const bool verbose = false); /// @@ -50,6 +52,7 @@ namespace pinocchio /// /// \param[in] filename The URDF complete file path. /// \param[in] rootJoint The joint at the root of the model tree. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -66,6 +69,7 @@ namespace pinocchio /// \brief Build the model from a URDF file with a fixed joint as root of the model tree. /// /// \param[in] filename The URDF complete file path. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -83,6 +87,7 @@ namespace pinocchio /// /// \param[in] urdfTree the tree build from the URDF /// \param[in] rootJoint The joint at the root of the model tree. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -102,6 +107,7 @@ namespace pinocchio /// inside the model given as reference argument. /// /// \param[in] urdfTree the tree build from the URDF + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] rootJointName Name of the rootJoint. /// \param[in] verbose Print parsing info. @@ -116,12 +122,14 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic = false, const bool verbose = false); /// /// \brief Build the model from a URDF model /// /// \param[in] urdfTree the tree build from the URDF + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -142,6 +150,7 @@ namespace pinocchio /// \param[in] xml_stream stream containing the URDF model. /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] rootJointName Name of the rootJoint. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. @@ -154,6 +163,7 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, + const bool mimic = false, const bool verbose = false); /// @@ -163,6 +173,7 @@ namespace pinocchio /// \param[in] xml_stream stream containing the URDF model. /// \param[in] rootJoint The joint at the root of the model tree. /// \param[in] verbose Print parsing info. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. /// @@ -180,6 +191,7 @@ namespace pinocchio /// \brief Build the model from an XML stream /// /// \param[in] xml_stream stream containing the URDF model. + /// \param[in] mimic Parsing joints as mimic or not. /// \param[in] verbose Print parsing info. /// \param[out] model Reference model where to put the parsed information. /// \return Return the reference on argument model for convenience. diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index af00a86b47..5b6dd49992 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -654,9 +654,10 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic, const bool verbose) { - return buildModel(filename, rootJoint, "root_joint", model, verbose); + return buildModel(filename, rootJoint, "root_joint", model, mimic, verbose); } template class JointCollectionTpl> @@ -699,9 +700,10 @@ namespace pinocchio const std::string & xmlStream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic, const bool verbose) { - return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, verbose); + return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, mimic, verbose); } template class JointCollectionTpl> @@ -745,9 +747,10 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, + const bool mimic, const bool verbose) { - return buildModel(urdfTree, rootJoint, "root_joint", model, verbose); + return buildModel(urdfTree, rootJoint, "root_joint", model, mimic, verbose); } template class JointCollectionTpl> diff --git a/unittest/python/bindings_geometry_model_urdf.py b/unittest/python/bindings_geometry_model_urdf.py index dc1ca82d21..f3180d4f06 100644 --- a/unittest/python/bindings_geometry_model_urdf.py +++ b/unittest/python/bindings_geometry_model_urdf.py @@ -23,7 +23,9 @@ def test_load(self): self.model_dir / "romeo_description/meshes/V1/collision/LHipPitch.dae" ) - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), True) + model = pin.buildModelFromUrdf( + self.model_path, pin.JointModelFreeFlyer(), mimic=True + ) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -36,7 +38,9 @@ def test_load(self): def test_self_load(self): hint_list = [self.mesh_path] - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), True) + model = pin.buildModelFromUrdf( + self.model_path, pin.JointModelFreeFlyer(), mimic=True + ) collision_model_ref = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -82,7 +86,9 @@ def test_multi_load(self): self.model_dir / "romeo_description/meshes/V1/visual/LHipPitch.dae" ) - model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), True) + model = pin.buildModelFromUrdf( + self.model_path, pin.JointModelFreeFlyer(), mimic=True + ) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list @@ -101,7 +107,7 @@ def test_multi_load(self): ) model_2, collision_model_2, visual_model_2 = pin.buildModelsFromUrdf( - self.model_path, True, hint_list, pin.JointModelFreeFlyer() + self.model_path, hint_list, pin.JointModelFreeFlyer(), mimic=True ) self.assertEqual(model, model_2) @@ -118,10 +124,10 @@ def test_multi_load(self): model_c, collision_model_c = pin.buildModelsFromUrdf( self.model_path, - True, hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.COLLISION, + mimic=True, ) self.assertEqual(model, model_c) @@ -133,10 +139,10 @@ def test_multi_load(self): model_v, visual_model_v = pin.buildModelsFromUrdf( self.model_path, - True, hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.VISUAL, + mimic=True, ) self.assertEqual(model, model_v) From e7d28cdbbd98bbadee80bcce6d5a28d48c267ba7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 136/166] Add assert to non supported algorithms --- .../pinocchio/algorithm/compute-all-terms.hxx | 5 +++++ .../pinocchio/algorithm/rnea-derivatives.hxx | 20 +++++++++++++++++++ .../rnea-second-order-derivatives.hxx | 9 +++++++++ 3 files changed, 34 insertions(+) diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index c4c32bba54..da517dbacb 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -103,6 +103,11 @@ namespace pinocchio const Model & model, Data & data) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename SizeDepType::template ColsReturn::Type diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index 11376912d9..134049f213 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -37,6 +37,11 @@ namespace pinocchio Data & data, const Eigen::MatrixBase & q) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -93,6 +98,11 @@ namespace pinocchio typename Data::VectorXs & g, const Eigen::MatrixBase & gravity_partial_dq) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV, @@ -268,6 +278,11 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -382,6 +397,11 @@ namespace pinocchio const Eigen::MatrixBase & rnea_partial_dv, const Eigen::MatrixBase & rnea_partial_da) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Matrix6x Matrix6x; diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index ef6b606b0a..99ac2dfccf 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -47,6 +47,11 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; typedef typename Data::Inertia Inertia; @@ -159,6 +164,10 @@ namespace pinocchio const Tensor3 & dtau_dqdv, const Tensor3 & dtau_dadq) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); typedef typename Data::Motion Motion; typedef typename Data::Force Force; typedef typename Data::Inertia Inertia; From 34819636defd0c00a0e6a7c370ddcd2dcbff0803 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 137/166] [algo/crba] Fix local convention for crba --- include/pinocchio/algorithm/crba.hxx | 58 +++++++++++++++++++++++----- unittest/crba.cpp | 4 +- 2 files changed, 50 insertions(+), 12 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 04925a1fa0..72e86c1277 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -218,22 +218,60 @@ namespace pinocchio Data & data) { typedef JointModelMimicTpl JointModel; + typedef typename Eigen::Matrix + MotionSubspace; const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); - const SE3 jMi = - getRelativePlacement(model, data, primary_id, secondary_id, Convention::LOCAL); - Data::Matrix6x::ColsBlockXpr iF = jmodel.jointVelCols(data.Fcrb[secondary_id]); + assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); + + JointIndex ancestor_prim, ancestor_sec; + JointIndex j_id = + findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); + Eigen::Matrix jF( 6, jmodel.nj()); - forceSet::se3Action(jMi, iF, jF); + Data::Matrix6x::ColsBlockXpr iF = jmodel.jointVelCols(data.Fcrb[secondary_id]); - typedef typename Eigen::Matrix - MotionSubspace; - jmodel.jointVelBlock(data.M).noalias() += - GetMotionSubspaceTplNoMalloc::run(data.joints[primary_id]) - .transpose() - * jF; + JointIndex start = ancestor_sec; + if (j_id != primary_id) + start += 1; + for (JointIndex j = start; j < model.supports[secondary_id].size() - 1; j++) + { + j_id = model.supports[secondary_id].at(j); + + const SE3 jMi = getRelativePlacement(model, data, j_id, secondary_id, Convention::LOCAL); + forceSet::se3Action(jMi, iF, jF); + + jmodel.jointVelRows(data.M) + .middleCols(model.joints[j_id].idx_v(), model.joints[j_id].nv()) + .noalias() += + GetMotionSubspaceTplNoMalloc::run(data.joints[j_id]) + .transpose() + * jF; + } + for (JointIndex j = ancestor_prim + 1; j < model.supports[primary_id].size(); j++) + { + j_id = model.supports[primary_id].at(j); + + const SE3 jMi = getRelativePlacement(model, data, j_id, secondary_id, Convention::LOCAL); + forceSet::se3Action(jMi, iF, jF); + + jmodel.jointVelCols(data.M) + .middleRows(model.joints[j_id].idx_v(), model.joints[j_id].nv()) + .noalias() -= + jF.transpose() + * GetMotionSubspaceTplNoMalloc::run(data.joints[j_id]); + } + + if (model.parents[secondary_id] != primary_id) + { + const SE3 jMi = + getRelativePlacement(model, data, primary_id, secondary_id, Convention::LOCAL); + forceSet::se3Action( + jMi, data.Fcrb[secondary_id].col(jmodel.idx_v()), + data.Fcrb[primary_id].col(jmodel.idx_v())); + } } template class JointCollectionTpl> diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 672926bf42..f96492458b 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -159,8 +159,8 @@ void test_mimic_against_full_model( Eigen::VectorXd a_mimic = Eigen::VectorXd::Random(model_mimic.nv); // World vs local - // pinocchio::crba(model_mimic, data_ref_mimic, q_mimic); - pinocchio::crba(model_mimic, data_mimic, q_mimic, pinocchio::Convention::WORLD); + pinocchio::crba(model_mimic, data_ref_mimic, q_mimic, pinocchio::Convention::WORLD); + pinocchio::crba(model_mimic, data_mimic, q_mimic, pinocchio::Convention::LOCAL); BOOST_CHECK(data_ref_mimic.M.isApprox(data_mimic.M)); From 39a7590b0da17c7f07d0a5a71755e455400e3a40 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 138/166] [EtienneAr feedback] Comment local fix for crba and don't use getRelativePlacement to remove redundant computations --- include/pinocchio/algorithm/crba.hxx | 70 +++++++++++++++------------- 1 file changed, 37 insertions(+), 33 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 72e86c1277..996b989bcd 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -206,11 +206,10 @@ namespace pinocchio } }; - /// \note For the case of a joint j mimicking a joint i - /// the resulting inertia of both joints is - /// M = Mii + 2* Mij + Mjj - /// But in this current implementation only M = Mii + Mij + Mjj is computed - /// So an extra Mij is added here. + /// \note For each joint the crba computation is done only for the sub cols/rows + /// from idx_v to joint.nvSubtree. In the case of the joint j=(i+n) mimicking the joint i, + /// the joints i+[1..n-1] will have idx_v greater than the joint j. This patch compute + /// this "left out" part of the M matrix. template class JointCollectionTpl> static inline void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase> & jmodel, @@ -218,58 +217,63 @@ namespace pinocchio Data & data) { typedef JointModelMimicTpl JointModel; + typedef typename Eigen::Matrix< + Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel::MaxNJ> + SpatialForcesX; + typedef typename Data::Matrix6x::ColsBlockXpr SpatialForcesBlock; typedef typename Eigen::Matrix MotionSubspace; + typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; + const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); JointIndex ancestor_prim, ancestor_sec; - JointIndex j_id = - findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); + findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); - Eigen::Matrix jF( - 6, jmodel.nj()); - Data::Matrix6x::ColsBlockXpr iF = jmodel.jointVelCols(data.Fcrb[secondary_id]); + SpatialForcesBlock jF = jmodel.jointVelCols(data.Fcrb[secondary_id]); // Mimic joint forces + SpatialForcesX iF(6, jmodel.nj()); // Current joint forces + SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint - JointIndex start = ancestor_sec; - if (j_id != primary_id) - start += 1; - for (JointIndex j = start; j < model.supports[secondary_id].size() - 1; j++) + // Traverse the kinematic tree backward from mimicking (secondary) joint parent to common + // ancestor + for (int k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) { - j_id = model.supports[secondary_id].at(j); + const JointIndex i = model.supports[secondary_id].at(k); + const JointIndex ui = + model.supports[secondary_id].at(k + 1); // Child link to compute placement + iMj = data.liMi[ui].act(iMj); + + // Skip the common ancestor if it's not the primary id + // as this computation would be canceled by the next loop forward + if (k == ancestor_sec && i != primary_id) + continue; - const SE3 jMi = getRelativePlacement(model, data, j_id, secondary_id, Convention::LOCAL); - forceSet::se3Action(jMi, iF, jF); + forceSet::se3Action(iMj, jF, iF); jmodel.jointVelRows(data.M) - .middleCols(model.joints[j_id].idx_v(), model.joints[j_id].nv()) - .noalias() += - GetMotionSubspaceTplNoMalloc::run(data.joints[j_id]) - .transpose() - * jF; + .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() += GetSNoMalloc::run(data.joints[i]).transpose() * iF; } - for (JointIndex j = ancestor_prim + 1; j < model.supports[primary_id].size(); j++) + // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint + for (int k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { - j_id = model.supports[primary_id].at(j); + const JointIndex i = model.supports[primary_id].at(k); + iMj = data.liMi[i].actInv(iMj); - const SE3 jMi = getRelativePlacement(model, data, j_id, secondary_id, Convention::LOCAL); - forceSet::se3Action(jMi, iF, jF); + forceSet::se3Action(iMj, jF, iF); jmodel.jointVelCols(data.M) - .middleRows(model.joints[j_id].idx_v(), model.joints[j_id].nv()) - .noalias() -= - jF.transpose() - * GetMotionSubspaceTplNoMalloc::run(data.joints[j_id]); + .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() -= iF.transpose() * GetSNoMalloc::run(data.joints[i]); } if (model.parents[secondary_id] != primary_id) { - const SE3 jMi = - getRelativePlacement(model, data, primary_id, secondary_id, Convention::LOCAL); forceSet::se3Action( - jMi, data.Fcrb[secondary_id].col(jmodel.idx_v()), + iMj, data.Fcrb[secondary_id].col(jmodel.idx_v()), data.Fcrb[primary_id].col(jmodel.idx_v())); } } From 18309bed63a9411db8e20b2d4b2f3bb4fcec1c41 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 139/166] [EtienneAr feedback] Add comments to world fix for crba and factorize code to prevent duplication --- include/pinocchio/algorithm/crba.hxx | 65 +++++++++++++--------------- 1 file changed, 29 insertions(+), 36 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 996b989bcd..dc000bdf13 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -65,11 +65,10 @@ namespace pinocchio { } - /// \note For the case of a joint j mimicking a joint i - /// the resulting inertia of both joints is - /// M = Mii + 2* Mij + Mjj - /// But in this current implementation only M = Mii + Mij + Mjj is computed - /// So an extra Mij is added here. + /// \note For each joint the crba computation is done only for the sub cols/rows + /// from idx_v to joint.nvSubtree. In the case of the joint j=(i+n) mimicking the joint i, + /// the joints i+[1..n-1] will have idx_v greater than the joint j. This patch compute + /// this "left out" part of the M matrix. template class JointCollectionTpl> static inline void mimic_patch_CrbaWorldConventionBackwardStep( const JointModelBase> & jmodel, @@ -83,37 +82,32 @@ namespace pinocchio assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); JointIndex ancestor_prim, ancestor_sec; - JointIndex j_id = - findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); + findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); - // Same branch of the tree - if (j_id == primary_id) - for (JointIndex i = primary_id; i < secondary_id; i++) - { - jmodel.jointVelRows(data.M) - .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) - .noalias() += model.joints.at(i).jointJacCols(data.J).transpose() - * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); - } - else + // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor + for (int k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) { - for (JointIndex j = ancestor_sec + 1; j < model.supports[secondary_id].size() - 1; j++) - { - j_id = model.supports[secondary_id].at(j); - jmodel.jointVelRows(data.M) - .middleCols(model.joints[j_id].idx_v(), model.joints[j_id].nv()) - .noalias() += - data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()).transpose() - * model.joints.at(j_id).jointJacCols(data.J); - } - for (JointIndex j = ancestor_prim + 1; j < model.supports[primary_id].size(); j++) - { - j_id = model.supports[primary_id].at(j); - jmodel.jointVelCols(data.M) - .middleRows(model.joints[j_id].idx_v(), model.joints[j_id].nv()) - .noalias() -= model.joints.at(j_id).jointJacCols(data.J).transpose() - * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); - } + const JointIndex i = model.supports[secondary_id].at(k); + + // Skip the common ancestor if it's not the primary id + // as this computation would be canceled by the next loop forward + if (k == ancestor_sec && i != primary_id) + continue; + + jmodel.jointVelRows(data.M) + .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() += + data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()).transpose() + * model.joints.at(i).jointJacCols(data.J); + } + // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint + for (int k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) + { + const JointIndex i = model.supports[primary_id].at(k); + jmodel.jointVelCols(data.M) + .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) + .noalias() -= model.joints.at(i).jointJacCols(data.J).transpose() + * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); } } @@ -237,8 +231,7 @@ namespace pinocchio SpatialForcesX iF(6, jmodel.nj()); // Current joint forces SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint - // Traverse the kinematic tree backward from mimicking (secondary) joint parent to common - // ancestor + // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor for (int k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) { const JointIndex i = model.supports[secondary_id].at(k); From 14060fde1e592e5b385aad39afba28ca98485617 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 30 Sep 2024 17:33:30 +0200 Subject: [PATCH 140/166] Update examples --- examples/gepetto-viewer.py | 2 +- examples/robot-wrapper-viewer.py | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/examples/gepetto-viewer.py b/examples/gepetto-viewer.py index cdc4628082..2ba488e66f 100644 --- a/examples/gepetto-viewer.py +++ b/examples/gepetto-viewer.py @@ -17,7 +17,7 @@ urdf_model_path = model_path / "talos_data/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() ) viz = GepettoVisualizer(model, collision_model, visual_model) diff --git a/examples/robot-wrapper-viewer.py b/examples/robot-wrapper-viewer.py index 6a9e9550d6..5388af21de 100644 --- a/examples/robot-wrapper-viewer.py +++ b/examples/robot-wrapper-viewer.py @@ -34,9 +34,7 @@ urdf_filename = "talos_reduced.urdf" urdf_model_path = model_path / "talos_data/robots" / urdf_filename -robot = RobotWrapper.BuildFromURDF( - urdf_model_path, True, mesh_dir, pin.JointModelFreeFlyer() -) +robot = RobotWrapper.BuildFromURDF(urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()) # alias model = robot.model From 741d1a144e93f88e601a8831bb9e2bb74703594e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:20:53 +0200 Subject: [PATCH 141/166] [EtienneAr Feedback] Fix LOCAL enum ambiguity in getRelativePlacement --- include/pinocchio/algorithm/kinematics.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/kinematics.hpp b/include/pinocchio/algorithm/kinematics.hpp index 4193e697fc..bc2f856230 100644 --- a/include/pinocchio/algorithm/kinematics.hpp +++ b/include/pinocchio/algorithm/kinematics.hpp @@ -123,7 +123,7 @@ namespace pinocchio const DataTpl & data, const JointIndex jointIdRef, const JointIndex jointIdTarget, - const Convention convention = LOCAL); + const Convention convention = Convention::LOCAL); /** * @brief Returns the spatial velocity of the joint expressed in the desired reference frame. From b05103e7a5af824b8b176a904c9d40e47559985a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:21:38 +0200 Subject: [PATCH 142/166] [EtienneAr Feedback]Fix Scalar / int confusion for model index --- include/pinocchio/algorithm/model.hxx | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 1e7c272b5d..d50613666f 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -852,12 +852,12 @@ namespace pinocchio input_model.joints.at(index_secondary), output_model.joints.at(index_primary), scaling, offset); - Scalar old_nq = input_model.joints.at(index_secondary).nq(); - Scalar old_nv = input_model.joints.at(index_secondary).nv(); + int old_nq = input_model.joints.at(index_secondary).nq(); + int old_nv = input_model.joints.at(index_secondary).nv(); output_model.nq = input_model.nq - old_nq; output_model.nv = input_model.nv - old_nv; - Scalar nq = output_model.nq; - Scalar nv = output_model.nv; + int nq = output_model.nq; + int nv = output_model.nv; // Resize limits output_model.effortLimit.resize(nv); @@ -898,8 +898,8 @@ namespace pinocchio } // Move indexes and limits - Scalar idx_q = output_model.idx_qs[index_secondary]; - Scalar idx_v = output_model.idx_vs[index_secondary]; + int idx_q = output_model.idx_qs[index_secondary]; + int idx_v = output_model.idx_vs[index_secondary]; for (JointIndex joint_id = index_secondary; joint_id < (JointIndex)input_model.njoints; ++joint_id) { From fd54278dcbfd26eb8a9f40f11a92795e8754ab12 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:22:11 +0200 Subject: [PATCH 143/166] [EtienneAr Feedback] Fix explicit ConfigVectorAffineTransformVisitor template parameters --- include/pinocchio/multibody/joint/joint-basic-visitors.hxx | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 354f47c882..61ba13f82c 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -1019,7 +1019,10 @@ namespace pinocchio const Scalar & offset, const Eigen::MatrixBase & qOut) { - boost::apply_visitor(ConfigVectorAffineTransformVisitor(qIn, scaling, offset, qOut), jmodel); + boost::apply_visitor( + ConfigVectorAffineTransformVisitor( + qIn, scaling, offset, qOut), + jmodel); } /// @endcond From 4706838c24a2f592a904a5d792bb9766e6ac92b3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:53:07 +0200 Subject: [PATCH 144/166] [EtienneAr Feedback] Fix Scalar / int confusion for joint mimic index --- include/pinocchio/multibody/joint/joint-mimic.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 7a396d3c9f..98039cc604 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -380,8 +380,8 @@ namespace pinocchio // JointDataMimicTpl(const RefJointDataVariant & jdata, // const Scalar & scaling, - // const Scalar & nq, - // const Scalar & nv) + // const int & nq, + // const int & nv) // : m_jdata_ref(jdata) // , m_scaling(scaling) // , S(m_jdata_ref.S(),scaling) @@ -390,7 +390,7 @@ namespace pinocchio // } JointDataMimicTpl( - const RefJointData & jdata, const Scalar & scaling, const Scalar & nq, const Scalar & nv) + const RefJointData & jdata, const Scalar & scaling, const int & nq, const int & nv) : m_jdata_ref(checkMimic(jdata.derived())) , m_scaling(scaling) , S(m_jdata_ref.S(), scaling) From 6d3fb86a96cc9d2cc985e5427c246e70ba826224 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 3 Oct 2024 18:12:57 +0200 Subject: [PATCH 145/166] [bindings/urdf] Rebasing --- bindings/python/parsers/urdf/model.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index e89daaefb0..27d62666ab 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -27,8 +27,7 @@ namespace pinocchio return model; } - Model & - buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic = false) + Model & buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic = false) { return pinocchio::urdf::buildModel(path(filename), model, mimic); } From dfe746f390b3c50ad68739356d303d7d045dd0e4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 3 Oct 2024 18:13:42 +0200 Subject: [PATCH 146/166] Make code compile with CppAd --- include/pinocchio/algorithm/check.hxx | 2 +- include/pinocchio/algorithm/crba.hxx | 52 +++++++++++++------ include/pinocchio/algorithm/jacobian.hxx | 10 ++-- include/pinocchio/multibody/data.hxx | 14 +++-- .../joint/joint-common-operations.hpp | 12 +++-- .../pinocchio/multibody/joint/joint-mimic.hpp | 3 +- unittest/cppad/joints.cpp | 25 ++++----- 7 files changed, 74 insertions(+), 44 deletions(-) diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 94783bf975..f297b94591 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -160,7 +160,7 @@ namespace pinocchio for (JointIndex j = 1; int(j) < model.njoints; ++j) { - if (boost::get(&model.joints[j])) + if (boost::get>(&model.joints[j])) continue; JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index dc000bdf13..9b9e6ff8c0 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -59,9 +59,16 @@ namespace pinocchio }; /// \brief Patch to the crba algorithm for joint mimic (in local convention) - template + template< + typename JointModel, + typename Scalar, + int Options, + template + class JointCollectionTpl> static inline void mimic_patch_CrbaWorldConventionBackwardStep( - const JointModelBase &, const Model &, Data &) + const JointModelBase &, + const ModelTpl &, + DataTpl &) { } @@ -72,20 +79,19 @@ namespace pinocchio template class JointCollectionTpl> static inline void mimic_patch_CrbaWorldConventionBackwardStep( const JointModelBase> & jmodel, - const Model & model, - Data & data) + const ModelTpl & model, + DataTpl & data) { - typedef JointModelMimicTpl JointModel; const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); - JointIndex ancestor_prim, ancestor_sec; + size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor - for (int k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) + for (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) { const JointIndex i = model.supports[secondary_id].at(k); @@ -101,7 +107,7 @@ namespace pinocchio * model.joints.at(i).jointJacCols(data.J); } // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint - for (int k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) + for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { const JointIndex i = model.supports[primary_id].at(k); jmodel.jointVelCols(data.M) @@ -177,10 +183,17 @@ namespace pinocchio } }; - /// \brief Patch to the crba algorithm for joint mimic (in local convention) - template + // /// \brief Patch to the crba algorithm for joint mimic (in local convention) + template< + typename JointModel, + typename Scalar, + int Options, + template + class JointCollectionTpl> static inline void mimic_patch_CrbaLocalConventionBackwardStep( - const JointModelBase &, const Model &, Data &) + const JointModelBase &, + const ModelTpl &, + DataTpl &) { } @@ -207,8 +220,8 @@ namespace pinocchio template class JointCollectionTpl> static inline void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase> & jmodel, - const Model & model, - Data & data) + const ModelTpl & model, + DataTpl & data) { typedef JointModelMimicTpl JointModel; typedef typename Eigen::Matrix< @@ -224,7 +237,7 @@ namespace pinocchio assert(secondary_id > primary_id && "Mimicking joint id is before the primary."); - JointIndex ancestor_prim, ancestor_sec; + size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); SpatialForcesBlock jF = jmodel.jointVelCols(data.Fcrb[secondary_id]); // Mimic joint forces @@ -232,7 +245,7 @@ namespace pinocchio SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor - for (int k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) + for (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) { const JointIndex i = model.supports[secondary_id].at(k); const JointIndex ui = @@ -251,7 +264,7 @@ namespace pinocchio .noalias() += GetSNoMalloc::run(data.joints[i]).transpose() * iF; } // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint - for (int k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) + for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { const JointIndex i = model.supports[primary_id].at(k); iMj = data.liMi[i].actInv(iMj); @@ -322,6 +335,13 @@ namespace pinocchio mimic_patch_CrbaLocalConventionBackwardStep(jmodel, model, data); } + + /// \brief Patch to the crba algorithm for joint mimic (in local convention) + template + static inline void mimic_patch_CrbaLocalConventionBackwardStep( + const JointModelBase &, const Model &, Data &) + { + } }; template< diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index a211254856..69855d49e6 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -202,8 +202,12 @@ namespace pinocchio Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); ======= Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); +<<<<<<< HEAD Jout_.fill(0); >>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) +======= + Jout_.setZero(); +>>>>>>> 4cc92137 (Make code compile with CppAd) typedef typename ModelTpl::JointIndex JointIndex; typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; @@ -387,7 +391,7 @@ namespace pinocchio data.iMf[jointId].setIdentity(); Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); - J_.fill(0); + J_.setZero(); typedef JointJacobianForwardStep< Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> @@ -395,12 +399,8 @@ namespace pinocchio for (JointIndex i = jointId; i > 0; i = model.parents[i]) { Pass::run( -<<<<<<< HEAD model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); -======= - model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J_)); ->>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) } } diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 80f2eb28dd..644f596306 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -202,10 +202,14 @@ namespace pinocchio const Index & parent = model.parents[(Index)i]; lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); - Scalar nv_; - - if (boost::get(&model.joints[(Index)lastChild[(Index)i]])) - nv_ = boost::get(model.joints[(Index)lastChild[(Index)i]]).jmodel().nv(); + int nv_; + + if (boost::get>( + &model.joints[(Index)lastChild[(Index)i]])) + nv_ = boost::get>( + model.joints[(Index)lastChild[(Index)i]]) + .jmodel() + .nv(); else nv_ = nv(model.joints[(Index)lastChild[(Index)i]]); @@ -222,7 +226,7 @@ namespace pinocchio for (Index joint = 1; joint < (Index)(model.njoints); joint++) { - if (boost::get(&model.joints[joint])) + if (boost::get>(&model.joints[joint])) continue; // Mimic joints should not override mimicked joint fromRow values const Index & parent = model.parents[joint]; const int nvj = model.joints[joint].nv(); diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index d10215d171..ac6922b877 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -7,6 +7,7 @@ #include "pinocchio/macros.hpp" #include "pinocchio/math/matrix.hpp" +#include "pinocchio/math/fwd.hpp" #include #include @@ -95,10 +96,13 @@ namespace pinocchio const Scalar & offset, const Eigen::MatrixBase & qOut) { - assert( - fabs(scaling - 1.0) < Eigen::NumTraits::dummy_precision() - && fabs(offset) < Eigen::NumTraits::dummy_precision() - && "No ConfigVectorAffineTransform specialized for this joint type"); + if ( + math::fabs(static_cast(scaling - Scalar(1))) + < Eigen::NumTraits::dummy_precision() + && math::fabs(offset) < Eigen::NumTraits::dummy_precision()) + throw std::invalid_argument( + "No ConfigVectorAffineTransform specialized for this joint type"); + PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = qIn; } }; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 98039cc604..53aeaa25c8 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -806,7 +806,8 @@ namespace pinocchio typedef typename CastType::type ReturnType; ReturnType res( - m_jmodel_ref.template cast(), (NewScalar)m_scaling, (NewScalar)m_offset); + m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), + ScalarCast::cast(m_offset)); res.setIndexes(id(), idx_q(), idx_v(), idx_j()); return res; } diff --git a/unittest/cppad/joints.cpp b/unittest/cppad/joints.cpp index 023067f249..3ab7e3ec33 100644 --- a/unittest/cppad/joints.cpp +++ b/unittest/cppad/joints.cpp @@ -44,7 +44,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) typedef JointCollection::JointDataRX JointDataRX; JointModelRX jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); JointDataRX jdata(jmodel.createData()); JointModelRXAD jmodel_ad = jmodel.cast(); @@ -108,7 +108,7 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -118,7 +118,7 @@ struct TestADOnJoints { typedef pinocchio::JointModelHelicalTpl JointModel; JointModel jmodel(Scalar(0.4)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -129,7 +129,7 @@ struct TestADOnJoints typedef pinocchio::JointModelUniversalTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::UnitX(), Vector3::UnitY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -140,7 +140,7 @@ struct TestADOnJoints typedef pinocchio::JointModelHelicalUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -151,7 +151,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -162,7 +162,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -173,7 +173,7 @@ struct TestADOnJoints typedef pinocchio::JointModelPrismaticUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -184,14 +184,15 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteTpl JointModelRX; typedef pinocchio::JointModelTpl JointModel; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } // TODO: implement it - template - void operator()(const pinocchio::JointModelMimic & /*jmodel*/) const + template class JointCollection> + void operator()( + const pinocchio::JointModelMimicTpl & /*jmodel*/) const { /* do nothing */ } @@ -204,7 +205,7 @@ struct TestADOnJoints typedef pinocchio::JointModelCompositeTpl JointModel; JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } From d18fea3d3dcd1a5cd7dd5389351e37f05787e4e6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:41:23 +0200 Subject: [PATCH 147/166] [algo/crba] remove double function --- include/pinocchio/algorithm/crba.hxx | 7 ------- 1 file changed, 7 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 9b9e6ff8c0..b858e42763 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -335,13 +335,6 @@ namespace pinocchio mimic_patch_CrbaLocalConventionBackwardStep(jmodel, model, data); } - - /// \brief Patch to the crba algorithm for joint mimic (in local convention) - template - static inline void mimic_patch_CrbaLocalConventionBackwardStep( - const JointModelBase &, const Model &, Data &) - { - } }; template< From 13fdd10c64a6bde124fe87cb7e2f3b122f887f55 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:41:49 +0200 Subject: [PATCH 148/166] [unittest/kinematics] Cast to avoid errors --- unittest/kinematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittest/kinematics.cpp b/unittest/kinematics.cpp index 6cd7d49e37..02d3864e0b 100644 --- a/unittest/kinematics.cpp +++ b/unittest/kinematics.cpp @@ -96,7 +96,7 @@ BOOST_AUTO_TEST_CASE(test_getRelativePlacement) const std::vector test_joints{ 0, 1, model.getJointId("rleg_elbow_joint"), model.getJointId("lleg_elbow_joint"), - model.njoints - 1}; + (JointIndex)(model.njoints - 1)}; for (const JointIndex i : test_joints) { From ea9fdea56c91920902e476004a82eb1013604281 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:45:54 +0200 Subject: [PATCH 149/166] [multibody/geometry] Force scalar to be double --- .../pinocchio/multibody/geometry-object.hpp | 4 ++-- include/pinocchio/multibody/geometry.hpp | 20 +++++++++++++------ 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/include/pinocchio/multibody/geometry-object.hpp b/include/pinocchio/multibody/geometry-object.hpp index 03776d8e90..004fe6ee90 100644 --- a/include/pinocchio/multibody/geometry-object.hpp +++ b/include/pinocchio/multibody/geometry-object.hpp @@ -77,10 +77,10 @@ namespace pinocchio template<> struct traits { - typedef context::Scalar Scalar; + typedef double Scalar; enum { - Options = context::Options + Options = 0 }; }; diff --git a/include/pinocchio/multibody/geometry.hpp b/include/pinocchio/multibody/geometry.hpp index 4458548d3e..f2a0ecdd1e 100644 --- a/include/pinocchio/multibody/geometry.hpp +++ b/include/pinocchio/multibody/geometry.hpp @@ -44,7 +44,11 @@ namespace pinocchio template<> struct traits { - typedef context::Scalar Scalar; + typedef double Scalar; + enum + { + Options = 0 + }; }; struct GeometryModel @@ -53,10 +57,10 @@ namespace pinocchio { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef context::Scalar Scalar; + typedef typename traits::Scalar Scalar; enum { - Options = context::Options + Options = traits::Options }; typedef SE3Tpl SE3; @@ -227,7 +231,11 @@ namespace pinocchio template<> struct traits { - typedef context::Scalar Scalar; + typedef double Scalar; + enum + { + Options = 0 + }; }; struct GeometryData @@ -236,10 +244,10 @@ namespace pinocchio { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef context::Scalar Scalar; + typedef typename traits::Scalar Scalar; enum { - Options = context::Options + Options = traits::Options }; typedef SE3Tpl SE3; From c9fc664cae5500984eb4d3ceb7e3ccae04866f12 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:46:18 +0200 Subject: [PATCH 150/166] [multibody/op] Only do comparison when scalar=double --- .../multibody/joint/joint-common-operations.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index ac6922b877..9267bb4141 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -96,12 +96,12 @@ namespace pinocchio const Scalar & offset, const Eigen::MatrixBase & qOut) { - if ( - math::fabs(static_cast(scaling - Scalar(1))) - < Eigen::NumTraits::dummy_precision() - && math::fabs(offset) < Eigen::NumTraits::dummy_precision()) - throw std::invalid_argument( - "No ConfigVectorAffineTransform specialized for this joint type"); + assert( + check_expression_if_real( + math::fabs(scaling - 1.0) < Eigen::NumTraits::dummy_precision()) + && check_expression_if_real( + math::fabs(offset) < Eigen::NumTraits::dummy_precision()) + && "No ConfigVectorAffineTransform specialized for this joint type"); PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() = qIn; } From ef226741a570654413d2ee7f1ffd1c882f363e0f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 16:35:32 +0200 Subject: [PATCH 151/166] Remove compilation warnings --- include/pinocchio/algorithm/kinematics.hxx | 4 ++-- include/pinocchio/algorithm/model.hxx | 3 ++- include/pinocchio/multibody/joint/fwd.hpp | 3 --- 3 files changed, 4 insertions(+), 6 deletions(-) diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index 5ba623c990..925d52ab80 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -287,8 +287,8 @@ namespace pinocchio const Convention convention) { assert(model.check(data) && "data is not consistent with model."); - assert(jointIdRef >= 0 && jointIdRef < model.njoints && "invalid joint id"); - assert(jointIdTarget >= 0 && jointIdTarget < model.njoints && "invalid joint id"); + assert(jointIdRef >= 0 && jointIdRef < (JointIndex)model.njoints && "invalid joint id"); + assert(jointIdTarget >= 0 && jointIdTarget < (JointIndex)model.njoints && "invalid joint id"); switch (convention) { case Convention::LOCAL: { diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index d50613666f..1c280bacce 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -157,7 +157,8 @@ namespace pinocchio static typename std::enable_if< !std::is_same>::value, JointModel>::type - updateMimicIds(const JointModel & jmodel, const Model & old_model, const Model & new_model) + updateMimicIds( + const JointModel & jmodel, const Model & /*old_model*/, const Model & /*new_model*/) { JointModel res(jmodel); return res; diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index ebc395c102..a4d29db3ef 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -173,9 +173,6 @@ namespace pinocchio struct JointModelTpl; typedef JointModelTpl JointModel; - typedef JointModelTpl - JointModelMimicable; - template< typename Scalar, int Options = context::Options, From cc6dc3e2a5eaf1569f736fdff581c242daf47539 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 17:49:56 +0200 Subject: [PATCH 152/166] Fix crba and some tests for casadi --- include/pinocchio/algorithm/crba.hxx | 14 ++++---- unittest/casadi/algorithms.cpp | 2 +- unittest/casadi/joints.cpp | 49 ++++++++++++++-------------- 3 files changed, 33 insertions(+), 32 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index b858e42763..dadd13ba9c 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -227,10 +227,11 @@ namespace pinocchio typedef typename Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel::MaxNJ> SpatialForcesX; - typedef typename Data::Matrix6x::ColsBlockXpr SpatialForcesBlock; + typedef JointDataTpl JointData; typedef typename Eigen::Matrix MotionSubspace; - typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; + typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; + typedef SE3Tpl SE3; const JointIndex secondary_id = jmodel.id(); const JointIndex primary_id = jmodel.derived().jmodel().id(); @@ -240,9 +241,8 @@ namespace pinocchio size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); - SpatialForcesBlock jF = jmodel.jointVelCols(data.Fcrb[secondary_id]); // Mimic joint forces - SpatialForcesX iF(6, jmodel.nj()); // Current joint forces - SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint + SpatialForcesX iF(6, jmodel.nj()); // Current joint forces + SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint // Traverse the tree backward from parent of mimicking (secondary) joint to common ancestor for (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) @@ -257,7 +257,7 @@ namespace pinocchio if (k == ancestor_sec && i != primary_id) continue; - forceSet::se3Action(iMj, jF, iF); + forceSet::se3Action(iMj, jmodel.jointVelCols(data.Fcrb[secondary_id]), iF); jmodel.jointVelRows(data.M) .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) @@ -269,7 +269,7 @@ namespace pinocchio const JointIndex i = model.supports[primary_id].at(k); iMj = data.liMi[i].actInv(iMj); - forceSet::se3Action(iMj, jF, iF); + forceSet::se3Action(iMj, jmodel.jointVelCols(data.Fcrb[secondary_id]), iF); jmodel.jointVelCols(data.M) .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) diff --git a/unittest/casadi/algorithms.cpp b/unittest/casadi/algorithms.cpp index 1cc9d5efb2..cde719fce7 100644 --- a/unittest/casadi/algorithms.cpp +++ b/unittest/casadi/algorithms.cpp @@ -55,7 +55,7 @@ BOOST_AUTO_TEST_CASE(test_jacobian) Model::Index joint_id = model.existJointName("rarm2") ? model.getJointId("rarm2") : (Model::Index)(model.njoints - 1); - Data::Matrix6x jacobian_local(6, model.nv), jacobian_world(6, model.nv); + Data::Matrix6x jacobian_local(6, model.nj), jacobian_world(6, model.nj); jacobian_local.setZero(); jacobian_world.setZero(); diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 81c904d1ad..5430a9e85e 100644 --- a/unittest/casadi/joints.cpp +++ b/unittest/casadi/joints.cpp @@ -40,7 +40,7 @@ BOOST_AUTO_TEST_CASE(test_jointRX_motion_space) typedef JointCollection::JointDataRX JointDataRX; JointModelRX jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); JointDataRX jdata(jmodel.createData()); JointModelRXAD jmodel_ad = jmodel.cast(); @@ -152,7 +152,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -172,7 +172,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -192,7 +192,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -212,7 +212,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -232,7 +232,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -254,7 +254,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } @@ -264,18 +264,18 @@ struct init> } }; -template -struct init> +template class JointCollection> +struct init> { - typedef pinocchio::JointModelMimic JointModel; + typedef pinocchio::JointModelMimicTpl JointModel; static JointModel run() { - JointModel_ jmodel_ref = init::run(); + typedef pinocchio::JointModelRevoluteTpl JointModelRX; + JointModelRX jmodel_ref = init::run(); + jmodel_ref.setIndexes(0, 0, 0, 0); JointModel jmodel(jmodel_ref, 1., 0.); - - return jmodel; } static std::string name() @@ -290,7 +290,7 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel_ jmodel = init::run(); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -299,7 +299,7 @@ struct TestADOnJoints { typedef pinocchio::JointModelHelicalTpl JointModel; JointModel jmodel(Scalar(0.4)); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -310,7 +310,7 @@ struct TestADOnJoints typedef pinocchio::JointModelUniversalTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::UnitX(), Vector3::UnitY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -321,7 +321,7 @@ struct TestADOnJoints typedef pinocchio::JointModelHelicalUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -332,7 +332,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -343,7 +343,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -354,7 +354,7 @@ struct TestADOnJoints typedef pinocchio::JointModelPrismaticUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -365,7 +365,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteTpl JointModelRX; typedef pinocchio::JointModelTpl JointModel; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } @@ -378,14 +378,15 @@ struct TestADOnJoints typedef pinocchio::JointModelCompositeTpl JointModel; JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0); + jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } // TODO: get the nq and nv quantity from LieGroups - template - static void test(const pinocchio::JointModelMimic & /*jmodel*/) + template class JointCollection> + static void + test(const pinocchio::JointModelMimicTpl & /*jmodel*/) { /* do nothing */ } From ac94d1c13c33e650f03461500d9080800ad5b883 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 6 Oct 2024 20:21:32 +0200 Subject: [PATCH 153/166] [EtienneAr Feedback] Fix Scalar forced to double for GeometryObjects --- include/pinocchio/algorithm/geometry.hxx | 4 +++- include/pinocchio/algorithm/model.hxx | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/geometry.hxx b/include/pinocchio/algorithm/geometry.hxx index 89093d8ea7..232c57aa6b 100644 --- a/include/pinocchio/algorithm/geometry.hxx +++ b/include/pinocchio/algorithm/geometry.hxx @@ -45,7 +45,9 @@ namespace pinocchio { const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint; if (joint_id > 0) - geom_data.oMg[i] = (data.oMi[joint_id] * geom_model.geometryObjects[i].placement); + geom_data.oMg[i] = + (static_cast(data.oMi[joint_id]) + * geom_model.geometryObjects[i].placement); else geom_data.oMg[i] = geom_model.geometryObjects[i].placement; } diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 1c280bacce..d904c763a7 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -129,7 +129,7 @@ namespace pinocchio { go.parentFrame = parentFrame; } - go.placement = pframe.placement * pfMAB * go.placement; + go.placement = static_cast(pframe.placement * pfMAB) * go.placement; geomModel.addGeometryObject(go); } } @@ -789,7 +789,7 @@ namespace pinocchio const std::string & parent_joint_name = input_model.names[joint_id_in_input_model]; JointIndex reduced_joint_id = (JointIndex)-1; - typedef typename Model::SE3 SE3; + typedef typename GeometryObject::SE3 SE3; SE3 relative_placement = SE3::Identity(); if (reduced_model.existJointName(parent_joint_name)) { @@ -799,7 +799,7 @@ namespace pinocchio { const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name); reduced_joint_id = reduced_model.frames[reduced_frame_id].parentJoint; - relative_placement = reduced_model.frames[reduced_frame_id].placement; + relative_placement = static_cast(reduced_model.frames[reduced_frame_id].placement); } GeometryObject reduced_geom(geom); From 237b26e7fd0dc7c390fc4dc06b701eb61acd4172 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 11:54:10 +0200 Subject: [PATCH 154/166] [EtienneAr Feedback] Fix casadi test for joint mimic --- unittest/casadi/joints.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 5430a9e85e..5b657d7766 100644 --- a/unittest/casadi/joints.cpp +++ b/unittest/casadi/joints.cpp @@ -273,9 +273,10 @@ struct init> { typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModelRX jmodel_ref = init::run(); - jmodel_ref.setIndexes(0, 0, 0, 0); JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); + return jmodel; } static std::string name() From 4cf3bf7f187ce22d07bfec86114f8865325c5282 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 10:05:36 +0200 Subject: [PATCH 155/166] [serialization] Fix template errors --- examples/meshcat-viewer.py | 2 +- include/pinocchio/serialization/joints-model.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/meshcat-viewer.py b/examples/meshcat-viewer.py index 94e7f279f6..60c375e496 100644 --- a/examples/meshcat-viewer.py +++ b/examples/meshcat-viewer.py @@ -21,7 +21,7 @@ urdf_model_path = model_path / "solo_description/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() ) # Start a new MeshCat server and client. diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index c0379e5e9c..1f5e354832 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -108,7 +108,7 @@ namespace boost ar & make_nvp("i_v", i_v); ar & make_nvp("i_j", i_j); - SetJointIndexes(joint.derived()).run(i_id, i_q, i_v, i_j); + SetJointIndexes>(joint.derived()).run(i_id, i_q, i_v, i_j); } template From a0dc57b776a1bc2ddcb8231238fbbf17397e118e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 13:46:10 +0200 Subject: [PATCH 156/166] Fix examples --- examples/collisions.py | 2 +- examples/meshcat-viewer-dae.py | 2 +- examples/simulation-contact-dynamics.py | 2 +- examples/static-contact-dynamics.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/collisions.py b/examples/collisions.py index d323f1fc1e..1578efa4d9 100644 --- a/examples/collisions.py +++ b/examples/collisions.py @@ -10,7 +10,7 @@ urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename # Load model -model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer(), False) +model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer()) # Load collision geometries geom_model = pin.buildGeomFromUrdf( diff --git a/examples/meshcat-viewer-dae.py b/examples/meshcat-viewer-dae.py index 2e96509ac3..e80c08e4ff 100644 --- a/examples/meshcat-viewer-dae.py +++ b/examples/meshcat-viewer-dae.py @@ -19,7 +19,7 @@ urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() ) # Start a new MeshCat server and client. diff --git a/examples/simulation-contact-dynamics.py b/examples/simulation-contact-dynamics.py index 6dbdf5008d..5194abacee 100644 --- a/examples/simulation-contact-dynamics.py +++ b/examples/simulation-contact-dynamics.py @@ -18,7 +18,7 @@ srdf_full_path = model_path / "talos_data/srdf" / srdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() ) # Start a new MeshCat server and client. diff --git a/examples/static-contact-dynamics.py b/examples/static-contact-dynamics.py index f512ff70b7..e6757d5596 100644 --- a/examples/static-contact-dynamics.py +++ b/examples/static-contact-dynamics.py @@ -60,7 +60,7 @@ urdf_model_path = model_path / "solo_description/robots" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, False, mesh_dir, pin.JointModelFreeFlyer() + urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() ) data = model.createData() From a87b80d5dca36cf71f5814cf431d3b7c5826877e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 14:06:05 +0200 Subject: [PATCH 157/166] Fix Serialization --- include/pinocchio/serialization/joints-model.hpp | 2 +- unittest/serialization.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 1f5e354832..4855587523 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -108,7 +108,7 @@ namespace boost ar & make_nvp("i_v", i_v); ar & make_nvp("i_j", i_j); - SetJointIndexes>(joint.derived()).run(i_id, i_q, i_v, i_j); + SetJointIndexes(joint.derived()).run(i_id, i_q, i_v, i_j); } template diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index e181d8faff..68dafa72e1 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -408,6 +408,7 @@ struct init> JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); + jmodel.setIndexes(0, 0, 0, 0); return jmodel; } From 86822ee0d3b38d0a6bb251c952470af0dab3498a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 17:13:59 +0200 Subject: [PATCH 158/166] Fix examples --- examples/collision-with-point-clouds.py | 2 +- examples/collisions.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/collision-with-point-clouds.py b/examples/collision-with-point-clouds.py index ff024d89c1..5a933f18de 100644 --- a/examples/collision-with-point-clouds.py +++ b/examples/collision-with-point-clouds.py @@ -22,7 +22,7 @@ urdf_model_path = model_path / "panda_description/urdf" / urdf_filename model, collision_model, visual_model = pin.buildModelsFromUrdf( - urdf_model_path, False, mesh_dir + urdf_model_path, package_dirs=mesh_dir ) # Add point clouds diff --git a/examples/collisions.py b/examples/collisions.py index 1578efa4d9..d323f1fc1e 100644 --- a/examples/collisions.py +++ b/examples/collisions.py @@ -10,7 +10,7 @@ urdf_model_path = model_path / "romeo_description/urdf" / urdf_filename # Load model -model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer()) +model = pin.buildModelFromUrdf(urdf_model_path, pin.JointModelFreeFlyer(), False) # Load collision geometries geom_model = pin.buildGeomFromUrdf( From 26ee5163b39dfc825cd4a1a449ca7b08e1b0ea01 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 8 Oct 2024 09:26:17 +0200 Subject: [PATCH 159/166] [test/rnea] Try fix for windows --- unittest/rnea.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index b3b484e597..6daddb47cb 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -17,6 +17,7 @@ #include "pinocchio/algorithm/center-of-mass.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/crba.hpp" +#include "pinocchio/algorithm/model.hpp" #include "pinocchio/algorithm/centroidal.hpp" #include "pinocchio/multibody/sample-models.hpp" #include "pinocchio/utils/timer.hpp" From ccb5ade0c4265ff4383c48852d0f73ad6d2dec09 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 13:57:08 +0200 Subject: [PATCH 160/166] [EtienneAr Feedback] Swap import order --- unittest/rnea.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 6daddb47cb..961d6a1873 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -9,15 +9,15 @@ * */ -#include "pinocchio/spatial/fwd.hpp" #include "pinocchio/multibody/model.hpp" #include "pinocchio/multibody/data.hpp" +#include "pinocchio/algorithm/model.hpp" +#include "pinocchio/spatial/fwd.hpp" #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/algorithm/jacobian.hpp" #include "pinocchio/algorithm/center-of-mass.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" #include "pinocchio/algorithm/crba.hpp" -#include "pinocchio/algorithm/model.hpp" #include "pinocchio/algorithm/centroidal.hpp" #include "pinocchio/multibody/sample-models.hpp" #include "pinocchio/utils/timer.hpp" From 1219043a4b22e184565b356360b24027d6d41d63 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 16:00:54 +0200 Subject: [PATCH 161/166] [EtienneAr Feedback] Fix PINOCCHIO_EXPLICIT_INSTANTIATION DECLARATION to DEFINITION for transformJointIntoMimic --- src/algorithm/model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithm/model.cpp b/src/algorithm/model.cpp index 44b37ac98d..fc3f5cbd81 100644 --- a/src/algorithm/model.cpp +++ b/src/algorithm/model.cpp @@ -81,7 +81,7 @@ namespace pinocchio context::Model &, std::vector> &); - template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void + template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void transformJointIntoMimic( const context::Model &, const JointIndex &, From 0406098f6dc206adbf537a891a3e9ae98f16fe85 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 16:24:27 +0200 Subject: [PATCH 162/166] [EtienneAr Feedback] Update changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6abe0d8f97..5de7deac8d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Allow use of `pathlib.Path | str` for paths in python bindings ([#2431](https://github.com/stack-of-tasks/pinocchio/pull/2431)) - Add Pseudo inertia and Log-cholesky parametrization ([#2296](https://github.com/stack-of-tasks/pinocchio/pull/2296)) - Add Pixi support ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459)) +- Extend support for mimic joint: rnea, crba, forward kinematics, centroidal, jacobians and frames ([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441)) +- Add idx_j, nj members in joint models, with their respective column/row/block selectors. ([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441)) ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) From 765059d1ccc1201a17a67a54f1d716e16e9f3e28 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 16:35:53 +0200 Subject: [PATCH 163/166] [EtienneAr Feedback] Add explicit to single argument constructors --- include/pinocchio/multibody/joint/joint-mimic.hpp | 2 +- include/pinocchio/serialization/joints-model.hpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 53aeaa25c8..da977040e8 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -168,7 +168,7 @@ namespace pinocchio struct TransposeConst { const ScaledJointMotionSubspaceTpl & ref; - TransposeConst(const ScaledJointMotionSubspaceTpl & ref) + explicit TransposeConst(const ScaledJointMotionSubspaceTpl & ref) : ref(ref) { } diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 4855587523..45fec54d59 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -71,7 +71,7 @@ namespace boost Derived & joint; public: - SetJointIndexes(Derived & joint) + explicit SetJointIndexes(Derived & joint) : joint(joint) {}; void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) @@ -86,7 +86,8 @@ namespace boost pinocchio::JointModelMimicTpl & joint; public: - SetJointIndexes(pinocchio::JointModelMimicTpl & joint) + explicit SetJointIndexes( + pinocchio::JointModelMimicTpl & joint) : joint(joint) {}; void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) From 7b9a81b639534e048c1ccee7cec39212e9ba8f7c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 17:26:24 +0200 Subject: [PATCH 164/166] Applied pre-commit --- include/pinocchio/algorithm/crba.hxx | 6 ++---- include/pinocchio/multibody/joint/joint-basic-visitors.hpp | 3 +-- include/pinocchio/multibody/joint/joint-basic-visitors.hxx | 3 +-- include/pinocchio/multibody/joint/joint-mimic.hpp | 3 +-- include/pinocchio/serialization/joints-data.hpp | 6 ++---- include/pinocchio/serialization/joints-model.hpp | 3 +-- unittest/joint-motion-subspace.cpp | 3 +-- 7 files changed, 9 insertions(+), 18 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index dadd13ba9c..203b0a0468 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -63,8 +63,7 @@ namespace pinocchio typename JointModel, typename Scalar, int Options, - template - class JointCollectionTpl> + template class JointCollectionTpl> static inline void mimic_patch_CrbaWorldConventionBackwardStep( const JointModelBase &, const ModelTpl &, @@ -188,8 +187,7 @@ namespace pinocchio typename JointModel, typename Scalar, int Options, - template - class JointCollectionTpl> + template class JointCollectionTpl> static inline void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase &, const ModelTpl &, diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index 7cb2d00418..a70833836b 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -482,8 +482,7 @@ namespace pinocchio template< typename Scalar, int Options, - template - class JointCollectionTpl, + template class JointCollectionTpl, typename ConfigVectorIn, typename ConfigVectorOut> void configVectorAffineTransform( diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 61ba13f82c..3fef515c54 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -1008,8 +1008,7 @@ namespace pinocchio template< typename Scalar, int Options, - template - class JointCollectionTpl, + template class JointCollectionTpl, typename ConfigVectorIn, typename ConfigVectorOut> void configVectorAffineTransform( diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index da977040e8..3ad4f17f6c 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -622,8 +622,7 @@ namespace pinocchio typename NewScalar, typename Scalar, int Options, - template - class JointCollectionTpl> + template class JointCollectionTpl> struct CastType> { typedef JointModelMimicTpl type; diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 9cf657a6d4..240629a1f5 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -66,8 +66,7 @@ namespace boost class Archive, typename Scalar, int Options, - template - class JointCollection> + template class JointCollection> void serialize( Archive & ar, pinocchio::JointDataBase> & @@ -264,8 +263,7 @@ namespace boost class Archive, typename Scalar, int Options, - template - typename JointCollection> + template typename JointCollection> void serialize( Archive & ar, pinocchio::JointDataMimicTpl & joint, diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 45fec54d59..76e452d701 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -325,8 +325,7 @@ namespace boost class Archive, typename Scalar, int Options, - template - class JointCollectionTpl> + template class JointCollectionTpl> void serialize( Archive & ar, pinocchio::JointModelMimicTpl & joint, diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index 1a1a55b625..6c1c899d51 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -93,8 +93,7 @@ void test_nv_against_jmodel( template< typename Scalar, int Options, - template - class JointCollection, + template class JointCollection, typename ConstraintDerived> void test_nv_against_jmodel( const JointModelMimicTpl & jmodel, From df43e4aae4b8590d003332de5ce1693472bc6039 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 18:17:21 +0200 Subject: [PATCH 165/166] Fix init problem --- include/pinocchio/serialization/joints-model.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 76e452d701..4791ff444d 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -83,17 +83,17 @@ namespace boost template class JointCollectionTpl> class SetJointIndexes> { - pinocchio::JointModelMimicTpl & joint; + pinocchio::JointModelMimicTpl & joint_m; public: explicit SetJointIndexes( pinocchio::JointModelMimicTpl & joint) - : joint(joint) {}; + : joint_m(joint) {}; void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) { - joint.setIndexes(i_id, 0, 0, i_j); - joint.setMimicIndexes(0, i_q, i_v, 0); + joint_m.setIndexes(i_id, 0, 0, i_j); + joint_m.setMimicIndexes(0, i_q, i_v, 0); } }; From bd466b31c996088efd6687e01ea7550a7584e949 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 13 Nov 2024 11:27:49 +0100 Subject: [PATCH 166/166] [algo/jacobian] Fix rebasing --- include/pinocchio/algorithm/jacobian.hxx | 86 ++++++++++-------------- unittest/joint-jacobian.cpp | 7 +- 2 files changed, 37 insertions(+), 56 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 69855d49e6..d86e73488e 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -56,17 +56,8 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; -<<<<<<< HEAD Matrix6xLike & J_ = J.const_cast_derived(); - jmodel.jointCols(J_) = data.oMi[i].act(jdata.S()); -======= - Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); -<<<<<<< HEAD - jmodel.jointVelCols(J_) = data.oMi[i].act(jdata.S()); ->>>>>>> a55d3bf7 ([EtienneAr feedback] Split jointCols jointRows and jointBLock for full and reduced system) -======= jmodel.jointJacCols(J_) = data.oMi[i].act(jdata.S()); ->>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) } }; @@ -158,6 +149,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); + Jout_.setZero(); typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; @@ -198,24 +190,16 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); -<<<<<<< HEAD Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); -======= - Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout); -<<<<<<< HEAD - Jout_.fill(0); ->>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) -======= Jout_.setZero(); ->>>>>>> 4cc92137 (Make code compile with CppAd) - typedef typename ModelTpl::JointIndex JointIndex; typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; typedef typename Matrix6xLikeOut::ColXpr ColXprOut; typedef MotionRef MotionOut; + const int colRefIn = nj(model.joints[joint_id]) + idx_j(model.joints[joint_id]) - 1; switch (rf) { case WORLD: { @@ -354,17 +338,8 @@ namespace pinocchio data.liMi[i] = model.jointPlacements[i] * jdata.M(); data.iMf[parent] = data.liMi[i] * data.iMf[i]; -<<<<<<< HEAD Matrix6xLike & J_ = J.const_cast_derived(); - jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); -======= - Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); -<<<<<<< HEAD - jmodel.jointVelCols(J_) = data.iMf[i].actInv(jdata.S()); ->>>>>>> a55d3bf7 ([EtienneAr feedback] Split jointCols jointRows and jointBLock for full and reduced system) -======= jmodel.jointVelCols(J_) += data.iMf[i].actInv(jdata.S()); ->>>>>>> 6b22ea5e ([EtienneAr feedback] Fix joint jacobians computation for mimic) } }; @@ -389,10 +364,8 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; data.iMf[jointId].setIdentity(); - - Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J); + Matrix6xLike & J_ = J.const_cast_derived(); J_.setZero(); - typedef JointJacobianForwardStep< Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> Pass; @@ -400,7 +373,7 @@ namespace pinocchio { Pass::run( model.joints[i], data.joints[i], - typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived())); + typename Pass::ArgsType(model, data, q.derived(), J_.const_cast_derived())); } } @@ -530,6 +503,7 @@ namespace pinocchio && "jointId is larger than the number of joints contained in the model"); Matrix6xLike & dJ = dJ_.const_cast_derived(); + dJ.setZero(); ::pinocchio::details::translateJointJacobian(model, data, jointId, rf, data.dJ, dJ); // Add contribution for LOCAL and LOCAL_WORLD_ALIGNED @@ -538,38 +512,48 @@ namespace pinocchio case LOCAL: { const SE3 & oMjoint = data.oMi[jointId]; const Motion & v_joint = data.v[jointId]; - const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1; - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) { - typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; - typedef const MotionRef MotionIn; + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; + typedef const MotionRef MotionIn; - typedef typename Matrix6xLike::ColXpr ColXprOut; - typedef MotionRef MotionOut; - MotionIn v_in(data.J.col(j)); - MotionOut v_out(dJ.col(j)); + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(col_in)); + MotionOut v_out(dJ.col(col_out)); - v_out -= v_joint.cross(oMjoint.actInv(v_in)); + v_out -= v_joint.cross(oMjoint.actInv(v_in)); + } } break; } case LOCAL_WORLD_ALIGNED: { const Motion & ov_joint = data.ov[jointId]; const SE3 & oMjoint = data.oMi[jointId]; - const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1; - for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j]) + for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) { - typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; - typedef const MotionRef MotionIn; + for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + { + const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + + typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; + typedef const MotionRef MotionIn; - typedef typename Matrix6xLike::ColXpr ColXprOut; - typedef MotionRef MotionOut; - MotionIn v_in(data.J.col(j)); - MotionOut v_out(dJ.col(j)); + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(col_in)); + MotionOut v_out(dJ.col(col_out)); - v_out.linear() -= - Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation())) - .cross(v_in.angular()); + v_out.linear() -= + Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation())) + .cross(v_in.angular()); + } } break; } diff --git a/unittest/joint-jacobian.cpp b/unittest/joint-jacobian.cpp index 913760a731..77ea9f2073 100644 --- a/unittest/joint-jacobian.cpp +++ b/unittest/joint-jacobian.cpp @@ -31,7 +31,7 @@ BOOST_AUTO_TEST_CASE(test_jacobian) using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, true, true); + pinocchio::buildModels::humanoidRandom(model); pinocchio::Data data(model); VectorXd q = VectorXd::Zero(model.nq); @@ -80,7 +80,6 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) pinocchio::buildModels::humanoidRandom(model, true, true); pinocchio::Data data(model); pinocchio::Data data_ref(model); - VectorXd q = randomConfiguration( model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)); VectorXd v = VectorXd::Random(model.nv); @@ -103,7 +102,6 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) getJointJacobian(model, data, idx, WORLD, J); BOOST_CHECK(J.isApprox(getJointJacobian(model, data, idx, WORLD))); getJointJacobianTimeVariation(model, data, idx, WORLD, dJ); - Motion v_idx(J * v); BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx]))); @@ -168,7 +166,6 @@ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(alpha))); } - // compare to finite differencies : LOCAL { Data data_ref(model), data_ref_plus(model); @@ -234,7 +231,7 @@ BOOST_AUTO_TEST_CASE(test_timings) using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, true, true); + pinocchio::buildModels::humanoidRandom(model); pinocchio::Data data(model); long flag = BOOST_BINARY(1111);