From e2ad7ca2f43b3bfb1aca990b827cef344a61b9f3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 001/310] 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 7a372a63c30ea2ad9ce682149d71934753e75d78 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 002/310] 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 03061bd19dc3054cdc3d2d76f5d91fd2c7d68d13 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 003/310] [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 97cfa0f091dea5d3e10fd9af7523ad5e1b41f7f1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 004/310] [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 45a9ef69d668e33752b10fd76f8e370b0f394267 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 005/310] [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 18097b7e112ec2adab460bcad0a672758c9ba31a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 006/310] [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 5b46b69959fb537b7f4d7a43643550d60bf3fae1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 007/310] [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 684a0e11843d36d1670a949071339dda4da0cd35 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 008/310] [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 0abb250f52d86e4f0fa669ce1b4e09b867bbaba1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 009/310] [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 b9687800faf27fd854bd9e179b39972d15851b26 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 010/310] [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 67875e0773b43d9e07374d920413da45a458468c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 011/310] [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 4ee413ff5fadbe9c2a9a6f622bdf3c052d118845 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 012/310] [Urdf] Add possibility to use mimic or not --- bindings/python/parsers/urdf/model.cpp | 93 +++++++++++++++++--------- bindings/python/pinocchio/shortcuts.py | 13 ++-- examples/overview-urdf.py | 2 +- unittest/python/bindings_urdf.py | 16 ++--- 4 files changed, 78 insertions(+), 46 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 949794a2e6..5a1f7694c4 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -26,6 +26,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. @@ -35,7 +36,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 = [ @@ -45,6 +46,7 @@ def buildModelsFromUrdf( "verbose", "meshLoader", "geometry_types", + "mimic" ] for key, arg in zip(arg_keys, args): @@ -64,16 +66,17 @@ def _buildModelsFromUrdf( verbose=False, meshLoader=None, geometry_types=None, -) -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]: + 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 4546d4b16bdde3505995e5158f7ec6e80c91568a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 013/310] [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 9ee8c21c3e0ea4810969c855b85f87c8f2b29d19 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 014/310] [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 d6507965fc6af2d109471bf4e4d690c21cdd0149 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 015/310] [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 f54bd120b51bc17e6ddab3a1948ad6d558906f72 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 016/310] 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 5a1f7694c4..fcb7b05a05 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -60,6 +60,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 bacdcf420e69f6d54c38b3acfd40b58f88b1775b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 017/310] 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 d03061dce795eff9d771c01b7c5856a5b67d5123 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 018/310] 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 bd1dd9493e4230ce6f312328d9e4d331cefcf35d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 019/310] [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 15243d31922083613dbf697ff0b6a3022e5fe88f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 020/310] [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 704c929e7a23efcce597ac9c245b31770049c25b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 021/310] [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 d1c839485145a1ab3b5bcf0ced07f81ea6b11a85 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 022/310] [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 976c344cf6f163e78d381074cd230e2c5974f6a5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 023/310] [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 34ec1de1ee84713cadcca5077842500d29d701fc Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 024/310] [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 1cf70c78c9c8442a8da839963a0d26a40f4af5d1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 025/310] [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 7f7d9f24878d80537d113a57a3b671a9c8c89557 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 026/310] [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 4c1973cbfaab19f5b5c0dc10f02f5eea8f625d1d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 027/310] [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 9c0dda93c5267064df944fe0177b3e00d498c88d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 028/310] [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 4215449abf538fbbb35258e3b4e4170e178be40d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 029/310] [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 5148321c4549c35bed0fa3874dc3a154383481ec Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 030/310] [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 d19794858e77b3e796cdc476d8f0e1d5efccb7c3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 031/310] [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 fe5dd3077fef02301c43542be07ff28649542cb1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 032/310] 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 c258e46c1861151a4ada0c2a2f3cb14a6664f853 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 033/310] [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 a728fa34f441353418077f768717b8a935d1a70f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 034/310] [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 4df32c8c427465df6c59bb583fc9e2109d2de48b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 035/310] [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 ea434f839d83996459c4aca92f370e4e6e3bde97 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 036/310] [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 abe3cbfc15b381b52b81a0ccc5ec58c87fd662e5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 037/310] 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 cc8ef1f78aee07e7dddf81468ae5b703b322053a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 038/310] [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 0e203bac3e7d30c55501e6a81d8e4879b5052ef5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 039/310] [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 a708990658b1039ed7f8d39061e86e0cca40bb15 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 040/310] [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 bf62387b2376352e8b3a60093382b9a7d67aaf6e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 041/310] [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 afd4e1f4eb90f25a5ca7345dbdf601170aa475ea Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 042/310] [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 425fc0fa427af935c5844ee2751cfd802c1a63cf Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 043/310] [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 d0282a4837510b7466dfffb1b0c195907ea0d056 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 044/310] [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 319065515ba54afeb0ff60dea4910543a2ef8558 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 045/310] [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 c3a09d9eed7a0720dab77a78eabcc460bceab4a0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 046/310] [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 5682dddc25cf9dbdbad7f7f6c7b3ccdc90d9f1ed Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 047/310] [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 99e9953fc3936a81ed98be634552a499ff51573d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 048/310] [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 ecb907e278d2b5522a2445a1b7d2c2d8c97ef68b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 049/310] [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 85b7dd4c234fd51d8f6b9419e509df13d18a8e19 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 050/310] [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 4f12d96f7839de8b40028500c6a2d88cca8d223c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 051/310] [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/310] 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 e8ef6b576ea13c60687c80cbf7c9e224fd93a20f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 053/310] 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 0926cb109f4f5b16d0e50191dfcd8a71d898eb33 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 054/310] 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 b99cf2ad11b96f23a029981f5e70bcf388ae17ae Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 055/310] [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 8c7c1c0417df2e65d38745025da2124203bc937a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 056/310] [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 dc589eecc42c700064464b16ccb2dc9e1431105e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 057/310] [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 1c95b2ca25f4120f5f179a866eda73aa9ad0e8a3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 058/310] [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 82b3059e8c5e63c61f03b1c0140fbf86fb08cf58 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 059/310] [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 9848688f454805eb32231092221709ecd5cc133a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 060/310] [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 7887a53edeb494670148759d6d4d1133fa394c28 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 061/310] [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 f95b04501ece8ad256c735a79fac2087b3af6422 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 062/310] [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 53628232641399de1b6ec2d7f0a96e3bc683ddda Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 063/310] [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 602bbae5751ca4a3b3823377a07e70b0fd4390de Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 064/310] [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 ceb115f9ed82417e1adf47849cd05f1a61540cf7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 065/310] [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 094eff8bc95c29b31a61bbbd54651901fd11cab6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 066/310] [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 4f61e828b3a07869fe61051efda9871a4fee263c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 067/310] [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 f176fc36aa9a0886d0d940cde2034097da92ca23 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 068/310] [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 7ae06c9c27046a3304cc22e2eefb5dadb8fa8681 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 069/310] [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 3148c10ecd3c42157d0ab1832cc40b30f6ca9b45 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 070/310] [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 e4f1e22250cd73244939903a811a96e1dea3c694 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 071/310] [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 f4a78164d4cd5a13560713a63427094095229527 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 072/310] [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 c9d0bb445551f18fd2e4e95c6be67797726b3879 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 073/310] [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 be97cd0bd8ed86b73d3c06675c8d64887e59b038 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 074/310] {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 b7563bc71b9482e5b3b137b1d80511e95abd4775 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 075/310] [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 46886aa6d8e262eb430bd00a04d3f4928603a570 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 076/310] [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 30d8c0d29d27a40b1b2681ca01a5353b4228d229 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 077/310] [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 3679c505c48de4d1105fa22d8a73d06169f47ade Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 078/310] [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 f4f0a3e7a87b1799c3e148546ee4f0a5fd1831ce Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 079/310] [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 5671c7f57aa2154585801d2afca3f8c8f225e5d3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 080/310] [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 5a939bb2ad2810bfe63e915d14666775c051ba35 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 081/310] [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 dd5a9d88b4a9b15d6ded8fa6ba600ee4fa395c48 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 082/310] [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 8b5aa7753d8647fd6090698c7989b5bf88c47de4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 083/310] [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 f2144c761de652bec934f9c7e2d870d8cdb7a090 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 084/310] [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 5fcaf9ba38b8c6fee93df9d080235064e2e75c2a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 085/310] [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 2f8e76f62c0a7fc08e6357659fa6275f05e84527 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 086/310] [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 3e76128038ec3811ef870b44ff9c26c4d42b808d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 087/310] [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 b16c5ccbcce2910bf4109e9cea88c1edac4bca3e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 088/310] [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 06e64cac8b5f451af9e5d4f9b20497753304ab57 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 089/310] [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 ac93681ee7dae8108e34afd83048f5f1ab1da034 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 090/310] [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 adbb5d9e375dade105294cb89576d7e3da94e218 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 091/310] [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 104d1184e4e4de9e4f29b938e98aa813ee568c82 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 092/310] [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 ec994a303cf7256a97c6f967b66a02bf1f63d34c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 093/310] [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 f293d7de42a74e74de43ce6fbf1ed5fa3885767d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 094/310] [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 ff7361f840c20442d24f5d6bc59e8a3cee4d260f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 095/310] [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 01eedd78d7980d9ffef9293107403d9556a72e94 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 096/310] [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 e94a1145e22a4cb0958134dcfc6c733dfe2bb8b1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 097/310] [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 6cfc3da5ee76625b52e3e38e40aa73da2d0ce4bf Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 098/310] [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 5ef85cc0af4df98647eddf7321ee08b8b02bcc43 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 099/310] [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 170f847a26c63cf35008489ed4de83eddc4d3e25 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 100/310] [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 392b96cf6dfad6dc7538699d492cfce727211b14 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 101/310] [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 384ea6a60d1247fc64a9655421e5bab017d36371 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 102/310] [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 0cff961de058aeca29a332c354e7732883dfcbe5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 103/310] [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 2507ddbcd9f6dbcec381287e3c843997b2339045 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 104/310] [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 703c9a08e369afc0f1266f14934771b8f07ebc1d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 105/310] 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 80aeb4b70a7ed29ec40b68344c88d7cff5b89bd5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 106/310] 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 2df67e3a0b8c63ffaa0c0ec80777fcf12cff144f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 107/310] [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 26be96e1db7f70d49f34da2ce09f8ec729a9f034 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 108/310] [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 94dddcf75df5e22a83bcd2253c5a857fdc5409bd Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 109/310] [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 759c0e6438e40c320c26b87657e267bf37ba90be Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 110/310] [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 b4df55f27b284d74dbc9daf758170d4d197be0e8 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 111/310] [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 d3ba80223f95f39ddf55c3dfdce047e7d98b56c4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 112/310] [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 546265722945a3b12c17fbd09798e629c03f9360 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 113/310] [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 09e05371f7b8e10654d951bd8ece55ed76e10539 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 114/310] [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 ed7a8f1aeb4327cfe37afea4215b3c233799fba2 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 115/310] [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 fe768abc7cf6197ae256c5daf647b77415bd9fde Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 116/310] [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 5523548bab12eccbba6a7d2bd8a0520ca6c5a712 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 117/310] [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 36406a91fe365fa4cb97a2727ddd62e6a791dd12 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 118/310] 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 be02d76f54c3ebdc5285ce9126f6946588e2f5a7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 119/310] [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 efeab670ba53d55d6c23f7da649755de1c6b702e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 120/310] 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 59d7dc7d308989ba3df9a32d7eeda4a579b33187 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 121/310] 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 3baef4cccd0bdfdabfdfcdfae55a5bd7a5056067 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 122/310] 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 42e4f659be6f15944a7ce4998a519060733a6deb Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 123/310] 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 1169ae925fd7b5a57150d03c8ee70bbb4ddc5c7b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 124/310] 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 4de03e5a23f060098418681347e5d14b4b0112b4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 125/310] [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 f46e8d55750e62032eba646afa29a07e1135e4c6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 126/310] 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 f710c40fe997ce4a836553a50b348a7ad480210c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 127/310] [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 8a63fe6608047ff745db7e20886b6f0ac858b648 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 128/310] [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 daa1fe4f1dae2008849cfdd48ba99aa09b667ba3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 129/310] 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 ed1fbc9f621c36a966b348b5c436e21076193e26 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 130/310] [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 f363dc0d4f6c4366aaf0d07b1aef666d409829e9 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 131/310] 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 83dc18821ee365180cd351ab06b0dd11a98ff444 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 132/310] [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 9f8adbc8900df34e404a3299cd742d77f829add0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 133/310] 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 f9d1adb178fedfdc5cea6c53319709ebf324170c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 134/310] [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 8b5e681f3463c2a565f69e3a3aeff1056ac72e32 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 135/310] [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 fcb7b05a05..2b60fd9faf 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -36,7 +36,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 = [ @@ -46,7 +53,7 @@ def buildModelsFromUrdf( "verbose", "meshLoader", "geometry_types", - "mimic" + "mimic", ] for key, arg in zip(arg_keys, args): @@ -60,14 +67,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 37ae077f56a24b4a55523292e2b55d7edf01e658 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 136/310] 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 4279a408dbb8478bcb43ba400cd31fde34649945 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 137/310] [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 baa0e76cfd2c527680a2bd3d60bb24c786920128 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 138/310] [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 71d81687b7a5a5e4912d867a4a446bdce3ddb379 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 29 Sep 2024 14:31:57 +0200 Subject: [PATCH 139/310] [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 f5860b2ac65b9651788435639d08ff1dd6468c0c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 30 Sep 2024 17:33:30 +0200 Subject: [PATCH 140/310] 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 6b0e0af44e0888f581529c0be10e8e9f8f6c181d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:20:53 +0200 Subject: [PATCH 141/310] [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 d15925b3168417a34c61b1d7d41dff0e3bfb10f4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:21:38 +0200 Subject: [PATCH 142/310] [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 47e8662c20cad3aa71f903bec15d914760eb78ff Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:22:11 +0200 Subject: [PATCH 143/310] [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 875ca2dd41505cbdac6ad11622f410386ccb22a4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 1 Oct 2024 13:53:07 +0200 Subject: [PATCH 144/310] [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 43abc66160d193dfbf66043ca6e144aaef088e73 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 3 Oct 2024 18:12:57 +0200 Subject: [PATCH 145/310] [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 f9b3e577a4cdad43f058c7a6e1ce6f1f870b4680 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 3 Oct 2024 18:13:42 +0200 Subject: [PATCH 146/310] 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 e2fa463bf2777271c7307b9eed1f4c8c2f0de848 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:41:23 +0200 Subject: [PATCH 147/310] [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 8e7374b1a561ae9d6e8294410917f520fffb4cc6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:41:49 +0200 Subject: [PATCH 148/310] [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 b268f936bd667ecc2fa47fbddb7a3da4c90305c0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:45:54 +0200 Subject: [PATCH 149/310] [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 2d72a4f39ed57ca4b63aeff30599cc5177617be3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 15:46:18 +0200 Subject: [PATCH 150/310] [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 331261bc61dfcb6bf3838ee72b45dd74c0d4024a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 16:35:32 +0200 Subject: [PATCH 151/310] 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 3d1f010683ea8d7947435fc039123b5480ca89e5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 4 Oct 2024 17:49:56 +0200 Subject: [PATCH 152/310] 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 5a89c28b4dea6c239ad95b0b75fa9e550eb617ce Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 6 Oct 2024 20:21:32 +0200 Subject: [PATCH 153/310] [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 4dc28f2a6232c4ce19c4d18087bdf90e0ebcc802 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 11:54:10 +0200 Subject: [PATCH 154/310] [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 ec881ffbdfae02f553be482d0268f1c3ddb94e6b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 10:05:36 +0200 Subject: [PATCH 155/310] [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 c224e775df3865d80690ba0b2ad8b1a8be30a65f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 13:46:10 +0200 Subject: [PATCH 156/310] 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 947e2e58af0c6d1ba5837a6cf68568e365de2a57 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 14:06:05 +0200 Subject: [PATCH 157/310] 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 18e1f4582a3306186daa74101cc098e193ddbd4d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 7 Oct 2024 17:13:59 +0200 Subject: [PATCH 158/310] 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 78de13b129a59d88b70ece9bdf4835d4be93981e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 8 Oct 2024 09:26:17 +0200 Subject: [PATCH 159/310] [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 87a8077090f62409b211e1aeb368de56986e8f83 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 13:57:08 +0200 Subject: [PATCH 160/310] [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 0271d4621873e363a37aa7c83677439f5fadeac1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 16:00:54 +0200 Subject: [PATCH 161/310] [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 828ebf0af52456ddd6d51e9d2e1a5f6ab738d5ae Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 16:24:27 +0200 Subject: [PATCH 162/310] [EtienneAr Feedback] Update changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a49bd546f3..02e0574c0a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -48,6 +48,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 d9ad2a41b1c6759f1bd7405bac99a47df0c9fe62 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 16:35:53 +0200 Subject: [PATCH 163/310] [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 aa23cc9bd21bbb0c6c555b0d7ecf11bd088bdb61 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 17:26:24 +0200 Subject: [PATCH 164/310] 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 5b4d2823db72f1f4365acb42c930ee5237f02861 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 11 Oct 2024 18:17:21 +0200 Subject: [PATCH 165/310] 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 c8ae31663e3a4bf47800fe7d1bd0a3fe47fed8b7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 13 Nov 2024 11:27:49 +0100 Subject: [PATCH 166/310] [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); From d04a675b37d16b900d2c99e58ba835d8da022aab Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Wed, 15 Jan 2025 12:54:27 +0100 Subject: [PATCH 167/310] python: Fix linting --- bindings/python/pinocchio/shortcuts.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/pinocchio/shortcuts.py b/bindings/python/pinocchio/shortcuts.py index 2b60fd9faf..2fa33b9b3f 100644 --- a/bindings/python/pinocchio/shortcuts.py +++ b/bindings/python/pinocchio/shortcuts.py @@ -74,7 +74,7 @@ def _buildModelsFromUrdf( meshLoader=None, geometry_types=None, mimic=False, -) -> Tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]: +) -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]: if geometry_types is None: geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL] From ae3098dc4f56040fab39c759f76e90c8561b6e94 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:51:17 +0100 Subject: [PATCH 168/310] [kinematics] Add documentation to getRelativePlacement --- include/pinocchio/algorithm/kinematics.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/kinematics.hpp b/include/pinocchio/algorithm/kinematics.hpp index bc2f856230..e92086927c 100644 --- a/include/pinocchio/algorithm/kinematics.hpp +++ b/include/pinocchio/algorithm/kinematics.hpp @@ -104,18 +104,20 @@ namespace pinocchio /** * @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. + * structure. LOCAL convention should only be used when aba and crba algorithms are called in + * LOCAL convention as well. * * @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 + * @param[in] convention Convention to use (computation 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. * + * \note WORLD convention complexity is in O(1) and LOCAL is in O(n). */ template class JointCollectionTpl> SE3Tpl getRelativePlacement( From d7a0da547ba562256aaba269de084851dc9de9da Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:51:44 +0100 Subject: [PATCH 169/310] [algo/model] Change error message to be more precise --- include/pinocchio/algorithm/model.hxx | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index d904c763a7..4e98d24771 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -837,12 +837,12 @@ namespace pinocchio { PINOCCHIO_CHECK_INPUT_ARGUMENT( index_primary <= (size_t)input_model.njoints, - "index of primary is greater than the total of joints"); + "index_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"); + "index_primary is greater than the total of joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT( - index_primary < index_secondary, "index of primary is greater than secondary"); + index_primary < index_secondary, "index_primary is greater than sindex_secondary"); typedef ModelTpl Model; typedef typename Model::JointModel JointModel; From cfbcb357dbe7874dda9b1801eed11428c97f5bf5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:52:13 +0100 Subject: [PATCH 170/310] [algo/model] Access directly to variables --- include/pinocchio/algorithm/model.hxx | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 4e98d24771..b192753439 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -850,11 +850,10 @@ namespace pinocchio output_model = input_model; output_model.joints[index_secondary] = JointModelMimic( - input_model.joints.at(index_secondary), output_model.joints.at(index_primary), scaling, - offset); + input_model.joints[index_secondary], output_model.joints[index_primary], scaling, offset); - int old_nq = input_model.joints.at(index_secondary).nq(); - int old_nv = input_model.joints.at(index_secondary).nv(); + int old_nq = input_model.joints[index_secondary].nq(); + int old_nv = input_model.joints[index_secondary].nv(); output_model.nq = input_model.nq - old_nq; output_model.nv = input_model.nv - old_nv; int nq = output_model.nq; From a7f18ce4074a355fb6a38576c9a68526838b745e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:52:35 +0100 Subject: [PATCH 171/310] [algo/model] Remove casting --- include/pinocchio/algorithm/model.hxx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index b192753439..96caa64992 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -129,7 +129,7 @@ namespace pinocchio { go.parentFrame = parentFrame; } - go.placement = static_cast(pframe.placement * pfMAB) * go.placement; + go.placement = (pframe.placement * pfMAB) * go.placement; geomModel.addGeometryObject(go); } } @@ -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 = static_cast(reduced_model.frames[reduced_frame_id].placement); + relative_placement = reduced_model.frames[reduced_frame_id].placement; } GeometryObject reduced_geom(geom); From c29d601d0f9826a888e6a056e0749c65413b9d8b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:52:46 +0100 Subject: [PATCH 172/310] [algo/model] Simplify function --- include/pinocchio/algorithm/model.hxx | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 96caa64992..5c516398f0 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -160,8 +160,7 @@ namespace pinocchio updateMimicIds( const JointModel & jmodel, const Model & /*old_model*/, const Model & /*new_model*/) { - JointModel res(jmodel); - return res; + return jmodel; } template From 995dfee997a4afac34396677e6d2c2231848f5a4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:53:06 +0100 Subject: [PATCH 173/310] [algo/jacobian] Remove useless line --- include/pinocchio/algorithm/jacobian.hxx | 1 - 1 file changed, 1 deletion(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index d86e73488e..b388707db4 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -199,7 +199,6 @@ namespace pinocchio 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: { From 103499a4b7bbebfca7fc48669e6b147768b0bcbe Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:53:22 +0100 Subject: [PATCH 174/310] [algo/jacobian] Remove useless cast --- include/pinocchio/algorithm/jacobian.hxx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index b388707db4..720302f2a4 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -372,7 +372,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_)); } } From e117132b1980c2f1f252c0775b4ef9dd3f741e3c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:53:56 +0100 Subject: [PATCH 175/310] [algo/kinematics] Access directly variables --- include/pinocchio/algorithm/crba.hxx | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 203b0a0468..e3bdc91b0f 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -111,7 +111,7 @@ namespace pinocchio 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() + .noalias() -= model.joints[i].jointJacCols(data.J).transpose() * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); } } @@ -245,9 +245,9 @@ namespace pinocchio // 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--) { - const JointIndex i = model.supports[secondary_id].at(k); + const JointIndex i = model.supports[secondary_id][k]; const JointIndex ui = - model.supports[secondary_id].at(k + 1); // Child link to compute placement + model.supports[secondary_id][k + 1]; // Child link to compute placement iMj = data.liMi[ui].act(iMj); // Skip the common ancestor if it's not the primary id @@ -264,7 +264,7 @@ namespace pinocchio // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { - const JointIndex i = model.supports[primary_id].at(k); + const JointIndex i = model.supports[primary_id][k]; iMj = data.liMi[i].actInv(iMj); forceSet::se3Action(iMj, jmodel.jointVelCols(data.Fcrb[secondary_id]), iF); From 5aa1149a8ea1bfc203e044b05e3e150fc7e05472 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 10:54:07 +0100 Subject: [PATCH 176/310] [algo/kinematics] Remove inline --- include/pinocchio/algorithm/crba.hxx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index e3bdc91b0f..dd823d2d5a 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -76,7 +76,7 @@ namespace pinocchio /// 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( + static void mimic_patch_CrbaWorldConventionBackwardStep( const JointModelBase> & jmodel, const ModelTpl & model, DataTpl & data) @@ -188,7 +188,7 @@ namespace pinocchio typename Scalar, int Options, template class JointCollectionTpl> - static inline void mimic_patch_CrbaLocalConventionBackwardStep( + static void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase &, const ModelTpl &, DataTpl &) From d2e887a0ae4c3653ccfa0a91a7f240a01838d630 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 11:00:34 +0100 Subject: [PATCH 177/310] [algo/crba] access variables directly --- include/pinocchio/algorithm/crba.hxx | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index dd823d2d5a..5498b74a8c 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -92,7 +92,7 @@ namespace pinocchio // 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--) { - const JointIndex i = model.supports[secondary_id].at(k); + const JointIndex i = model.supports[secondary_id][k]; // Skip the common ancestor if it's not the primary id // as this computation would be canceled by the next loop forward @@ -103,12 +103,12 @@ namespace pinocchio .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); + * model.joints[i].jointJacCols(data.J); } // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { - const JointIndex i = model.supports[primary_id].at(k); + const JointIndex i = model.supports[primary_id][k]; jmodel.jointVelCols(data.M) .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) .noalias() -= model.joints[i].jointJacCols(data.J).transpose() From 1864a263757a12918ffb65502be6b0d82c21715b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 11:02:17 +0100 Subject: [PATCH 178/310] [algo/crba] Remove inline --- include/pinocchio/algorithm/crba.hxx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 5498b74a8c..f30d3d979a 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -64,7 +64,7 @@ namespace pinocchio typename Scalar, int Options, template class JointCollectionTpl> - static inline void mimic_patch_CrbaWorldConventionBackwardStep( + static void mimic_patch_CrbaWorldConventionBackwardStep( const JointModelBase &, const ModelTpl &, DataTpl &) @@ -216,7 +216,7 @@ namespace pinocchio /// 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( + static void mimic_patch_CrbaLocalConventionBackwardStep( const JointModelBase> & jmodel, const ModelTpl & model, DataTpl & data) From 064c2a537f88c9be0b73b9185015735943e74e2e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 16 Jan 2025 13:10:30 +0100 Subject: [PATCH 179/310] Change naming convention --- .../pinocchio/algorithm/aba-derivatives.hxx | 56 ++++---- include/pinocchio/algorithm/aba.hxx | 44 +++--- .../algorithm/center-of-mass-derivatives.hxx | 2 +- .../pinocchio/algorithm/center-of-mass.hxx | 8 +- .../algorithm/centroidal-derivatives.hxx | 38 ++--- include/pinocchio/algorithm/centroidal.hxx | 12 +- include/pinocchio/algorithm/check.hxx | 1 + .../pinocchio/algorithm/compute-all-terms.hxx | 16 +-- .../constrained-dynamics-derivatives.hxx | 28 ++-- .../algorithm/constrained-dynamics.hxx | 48 +++---- include/pinocchio/algorithm/crba.hxx | 28 ++-- include/pinocchio/algorithm/delassus.hxx | 18 +-- include/pinocchio/algorithm/energy.hxx | 2 +- include/pinocchio/algorithm/geometry.hxx | 2 +- .../impulse-dynamics-derivatives.hxx | 12 +- include/pinocchio/algorithm/jacobian.hxx | 12 +- .../algorithm/kinematics-derivatives.hxx | 42 +++--- include/pinocchio/algorithm/kinematics.hxx | 2 +- include/pinocchio/algorithm/model.hxx | 132 +++++++++--------- include/pinocchio/algorithm/pv.hxx | 42 +++--- include/pinocchio/algorithm/regressor.hxx | 2 +- .../pinocchio/algorithm/rnea-derivatives.hxx | 44 +++--- .../rnea-second-order-derivatives.hxx | 10 +- include/pinocchio/algorithm/rnea.hxx | 30 ++-- .../multibody/joint/joint-basic-visitors.hxx | 6 +- .../multibody/joint/joint-composite.hpp | 48 +++---- .../multibody/joint/joint-generic.hpp | 8 +- .../pinocchio/multibody/joint/joint-mimic.hpp | 14 +- .../multibody/joint/joint-model-base.hpp | 120 ++++++++-------- .../multibody/joint/joint-translation.hpp | 6 +- .../multibody/liegroup/liegroup-algo.hxx | 68 ++++----- include/pinocchio/multibody/model.hxx | 18 +-- unittest/centroidal.cpp | 4 +- unittest/crba.cpp | 4 +- unittest/joint-composite.cpp | 32 ++--- unittest/model.cpp | 100 ++++++------- unittest/rnea.cpp | 4 +- 37 files changed, 532 insertions(+), 531 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 1d2198a740..1e9c60e3aa 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.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); J_cols = data.oMi[i].act(jdata.S()); } }; @@ -119,14 +119,14 @@ namespace pinocchio Force & fi = data.of[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - const ColBlock J_cols = jmodel.jointJacCols(data.J); + const ColBlock J_cols = jmodel.jointExtendedModelCols(data.J); - jmodel.jointVelocityFromNvSelector(data.u).noalias() -= J_cols.transpose() * fi.toVector(); + jmodel.jointVelocitySelector(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.jointVelocityFromNvSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -137,7 +137,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv_.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -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.jointVelocityFromNvSelector(data.u); + + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -204,14 +204,14 @@ namespace pinocchio typename Data::Motion & oa_gf = data.oa_gf[i]; typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); oa_gf += data.oa_gf[parent]; - jmodel.jointVelocityFromNvSelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocityFromNvSelector(data.u) + jmodel.jointVelocitySelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * oa_gf.toVector(); - oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocityFromNvSelector(data.ddq); + oa_gf.toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); oa = oa_gf + model.gravity; of = data.oYcrb[i] * oa_gf + ov.cross(data.oh[i]); @@ -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.jointJacCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointCols(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.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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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); // 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.jointJacCols(data.J); + const ColBlock J_cols = jmodel.jointExtendedModelCols(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.jointVelCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointCols(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.jointJacCols(data.J); + const ColsBlock J_cols = jmodel.jointExtendedModelCols(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.jointJacCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); - ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); - ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointCols(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 fc75b85cd1..2ef64adca2 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.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); ov = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -165,17 +165,17 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); Force & fi = data.of[i]; - jmodel.jointVelocityFromDofSelector(data.u).noalias() -= Jcols.transpose() * fi.toVector(); + jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.u); + + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -214,17 +214,17 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock J_cols = jmodel.jointJacCols(data.J); + ColBlock J_cols = jmodel.jointExtendedModelCols(data.J); const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; data.oa_gf[i] += data.oa_gf[parent]; // does take into account the gravity field - jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocityFromDofSelector(data.u) + jmodel.jointVelocityExtendedModelSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityExtendedModelSelector(data.u) - jdata.UDinv().transpose() * data.oa_gf[i].toVector(); data.oa_gf[i].toVector().noalias() += - J_cols * jmodel.jointVelocityFromDofSelector(data.ddq); + J_cols * jmodel.jointVelocityExtendedModelSelector(data.ddq); // Handle consistent output data.oa[i] = data.oa_gf[i] + model.gravity; @@ -424,15 +424,15 @@ namespace pinocchio const JointIndex parent = model.parents[i]; typename Inertia::Matrix6 & Ia = data.Yaba[i]; - jmodel.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocityFromDofSelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.u); + + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -462,10 +462,10 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); - jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocityFromDofSelector(data.u) + jmodel.jointVelocityExtendedModelSelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocityExtendedModelSelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); - data.a_gf[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.ddq); data.a[i] = data.a_gf[i]; data.a[i].linear().noalias() += data.oMi[i].rotation().transpose() * model.gravity.linear(); @@ -637,7 +637,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); J_cols = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); @@ -675,13 +675,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); jdata.U().noalias() = Ia * J_cols; jdata.StU().noalias() = J_cols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocityFromDofSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocityExtendedModelSelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -690,7 +690,7 @@ namespace pinocchio const int nv_children = data.nvSubtree[i] - jmodel.nv(); if (nv_children > 0) { - ColsBlock SDinv_cols = jmodel.jointVelCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) @@ -749,13 +749,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - const ColsBlock J_cols = jmodel.jointJacCols(data.J); + const ColsBlock J_cols = jmodel.jointExtendedModelCols(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.jointVelCols(data.SDinv); + ColsBlock SDinv_cols = jmodel.jointCols(data.SDinv); SDinv_cols.noalias() = J_cols * jdata.Dinv(); Minv.block(jmodel.idx_v(), jmodel.idx_v() + jmodel.nv(), jmodel.nv(), nv_children) .noalias() = @@ -805,7 +805,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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 be9d6a0a81..6bd999b8f6 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.jointVelCols(dvcom_dq); + dvcom_dqi = jmodel.jointCols(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 d67be398b8..a3f2d2b70c 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.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); Jcols = data.oMi[i].act(jdata.S()); for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) { - jmodel.jointVelCols(Jcom_).col(col_id) += + jmodel.jointCols(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)); } @@ -367,12 +367,12 @@ namespace pinocchio Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom); - ColBlock Jcols = jmodel.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); Jcols = data.oMi[i].act(jdata.S()); for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) { - jmodel.jointVelCols(Jcom_).col(col_id) += + jmodel.jointCols(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 35e68ef917..68d59928cb 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -83,7 +83,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; data.a[i] = - jdata.S() * jmodel.jointVelocityFromNvSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -99,11 +99,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(ov, J_cols, dJ_cols); @@ -164,17 +164,17 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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); // tau - jmodel.jointVelocityFromNvSelector(data.tau).noalias() = + jmodel.jointVelocitySelector(data.tau).noalias() = J_cols.transpose() * data.of[i].toVector(); // dtau/da similar to data.M @@ -386,10 +386,10 @@ namespace pinocchio typename Data::Motion & vtmp = data.v[0]; typename Data::Matrix6x & Ftmp = data.Fcrb[0]; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); + ColsBlock Ftmp_cols = jmodel.jointCols(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 1786cf33f8..9332ab18d7 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.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); data.oYcrb[parent] += data.oYcrb[i]; @@ -267,10 +267,10 @@ namespace pinocchio const Inertia & Y = data.oYcrb[i]; const typename Inertia::Matrix6 & doYcrb = data.doYcrb[i]; - ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.oYcrb[parent] += Y; @@ -278,11 +278,11 @@ namespace pinocchio data.doYcrb[parent] += doYcrb; // Calc Ag - ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); motionSet::inertiaAction(Y, J_cols, Ag_cols); // Calc dAg = Ivx + vxI - ColsBlock dAg_cols = jmodel.jointVelCols(data.dAg); + ColsBlock dAg_cols = jmodel.jointCols(data.dAg); dAg_cols.noalias() = doYcrb * J_cols; motionSet::inertiaAction(Y, dJ_cols, dAg_cols); } diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index f297b94591..766ab5a685 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -160,6 +160,7 @@ namespace pinocchio for (JointIndex j = 1; int(j) < model.njoints; ++j) { + // do not check mimic joint, because nv = 0, but it has a subtree if (boost::get>(&model.joints[j])) continue; JointIndex c = (JointIndex)data.lastChild[j]; diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index da517dbacb..e6cf7f4eb8 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.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(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()); @@ -116,10 +116,10 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock dAg_cols = jmodel.jointCols(data.dAg); // Calc Ag = Y * S motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); @@ -129,10 +129,10 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dAg_cols); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ - jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = + jmodel.jointRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - jmodel.jointVelocityFromDofSelector(data.nle) += jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityExtendedModelSelector(data.nle) += jdata.S().transpose() * data.f[i]; data.oYcrb[parent] += data.oYcrb[i]; data.doYcrb[parent] += data.doYcrb[i]; diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index 695bad7328..c00aae6073 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.jointJacCols(data.J); - ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); if (ContactMode) { const Motion & ov = data.ov[i]; - ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); - ColsBlock dVdq_cols = jmodel.jointVelCols(data.dVdq); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); motionSet::motionAction(ov, J_cols, dJ_cols); // TODO: make more efficient @@ -79,10 +79,10 @@ 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.jointVelCols(data.dAdv); + ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); const typename Data::TangentVectorType & a = data.ddq; data.a[i] = - jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + jdata.S() * jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(dimpulse); + odv = J_cols * jmodel.jointVelocityExtendedModelSelector(dimpulse); if (parent > 0) odv += odvparent; motionSet::motionAction(odvparent, J_cols, dAdq_cols); @@ -142,11 +142,11 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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); typename Data::RowMatrixXs & dtau_dq = data.dtau_dq; @@ -185,8 +185,8 @@ namespace pinocchio if (ContactMode) { - ColsBlock dAdv_cols = jmodel.jointVelCols(data.dAdv); - ColsBlock dFdv_cols = jmodel.jointVelCols(data.dFdv); + ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); + ColsBlock dFdv_cols = jmodel.jointCols(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 f88ea81777..18647c7f8c 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.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(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,17 +162,17 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock dFda_cols = jmodel.jointVelCols(data.dFda); - const ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock dFda_cols = jmodel.jointCols(data.dFda); + const ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); - jmodel.jointVelRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = + jmodel.jointRows(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]; if (ContactMode) { - jmodel.jointVelocityFromNvSelector(data.nle).noalias() = + jmodel.jointVelocitySelector(data.nle).noalias() = J_cols.transpose() * data.of[i].toVector(); data.of[parent] += data.of[i]; } @@ -562,7 +562,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); data.ov[i] = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -618,20 +618,20 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; fi_augmented += fi; - jmodel.jointVelocityFromNvSelector(data.u).noalias() = - jmodel.jointVelocityFromNvSelector(tau) - Jcols.transpose() * fi_augmented.toVector(); + jmodel.jointVelocitySelector(data.u).noalias() = + jmodel.jointVelocitySelector(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.jointVelocityFromNvSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocitySelector(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.jointVelocityFromNvSelector(data.u); + Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); data.oYaba[parent] += Ia; data.of_augmented[parent] += fi_augmented; } @@ -680,19 +680,19 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; fi_augmented += fi; - jmodel.jointVelocityFromNvSelector(data.u).noalias() = - jmodel.jointVelocityFromNvSelector(tau) - Jcols.transpose() * fi_augmented.toVector(); + jmodel.jointVelocitySelector(data.u).noalias() = + jmodel.jointVelocitySelector(tau) - Jcols.transpose() * fi_augmented.toVector(); if (parent > 0) { fi_augmented.toVector().noalias() += - Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocityFromNvSelector(data.u); + Ia * data.oa[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); data.of_augmented[parent] += fi_augmented; } } @@ -719,7 +719,7 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; @@ -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.jointVelocityFromNvSelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocityFromNvSelector(data.u) + jmodel.jointVelocitySelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.oa_augmented[i].toVector(); - data.oa_augmented[i].toVector() += Jcols * jmodel.jointVelocityFromNvSelector(data.ddq); + data.oa_augmented[i].toVector() += Jcols * jmodel.jointVelocitySelector(data.ddq); } }; @@ -1090,8 +1090,8 @@ namespace pinocchio data.of[parent] += data.of[i]; // subtract the bias forces from the torque to get Mv_dot_free - jmodel.jointVelocityFromNvSelector(data.tau).noalias() -= - jmodel.jointJacCols(data.J).transpose() * (data.of[i].toVector()); + jmodel.jointVelocitySelector(data.tau).noalias() -= + jmodel.jointExtendedModelCols(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.jointVelocityFromNvSelector(data.u); + + jmodel.jointExtendedModelCols(data.J) * jmodel.jointVelocitySelector(data.u); data.of_augmented[i].toVector().setZero(); } } @@ -1158,8 +1158,8 @@ namespace pinocchio const JointModel & jmodel = model.joints[i]; data.of_augmented[parent] += data.of_augmented[i]; - jmodel.jointVelocityFromNvSelector(data.tau).noalias() = - -jmodel.jointJacCols(data.J).transpose() * (data.of_augmented[i].toVector()); + jmodel.jointVelocitySelector(data.tau).noalias() = + -jmodel.jointExtendedModelCols(data.J).transpose() * (data.of_augmented[i].toVector()); } } diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index f30d3d979a..a3e97aa813 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -52,7 +52,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); } @@ -99,19 +99,19 @@ namespace pinocchio if (k == ancestor_sec && i != primary_id) continue; - jmodel.jointVelRows(data.M) + jmodel.jointRows(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[i].jointJacCols(data.J); + * model.joints[i].jointExtendedModelCols(data.J); } // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) { const JointIndex i = model.supports[primary_id][k]; - jmodel.jointVelCols(data.M) + jmodel.jointCols(data.M) .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) - .noalias() -= model.joints[i].jointJacCols(data.J).transpose() + .noalias() -= model.joints[i].jointExtendedModelCols(data.J).transpose() * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); } } @@ -136,12 +136,12 @@ namespace pinocchio const JointIndex & i = jmodel.id(); // Centroidal momentum map - ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); - ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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() += + jmodel.jointRows(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); @@ -255,9 +255,9 @@ namespace pinocchio if (k == ancestor_sec && i != primary_id) continue; - forceSet::se3Action(iMj, jmodel.jointVelCols(data.Fcrb[secondary_id]), iF); + forceSet::se3Action(iMj, jmodel.jointCols(data.Fcrb[secondary_id]), iF); - jmodel.jointVelRows(data.M) + jmodel.jointRows(data.M) .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) .noalias() += GetSNoMalloc::run(data.joints[i]).transpose() * iF; } @@ -267,9 +267,9 @@ namespace pinocchio const JointIndex i = model.supports[primary_id][k]; iMj = data.liMi[i].actInv(iMj); - forceSet::se3Action(iMj, jmodel.jointVelCols(data.Fcrb[secondary_id]), iF); + forceSet::se3Action(iMj, jmodel.jointCols(data.Fcrb[secondary_id]), iF); - jmodel.jointVelCols(data.M) + jmodel.jointCols(data.M) .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) .noalias() -= iF.transpose() * GetSNoMalloc::run(data.joints[i]); } @@ -313,10 +313,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.jointVelCols(data.Fcrb[i]) += data.Ycrb[i] * jdata.S(); + jmodel.jointCols(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() += + jmodel.jointRows(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]; diff --git a/include/pinocchio/algorithm/delassus.hxx b/include/pinocchio/algorithm/delassus.hxx index 5b07052aec..a5b922de4b 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.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(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,13 +81,13 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); @@ -437,13 +437,13 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointJacCols(data.J); + ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocityFromNvSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); @@ -588,7 +588,7 @@ namespace pinocchio { // When propagating 6D constraints scratch_pad_vector.noalias() = - data.extended_motion_propagator[i] * model.joints[i].jointJacCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointExtendedModelCols(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].jointJacCols(data.J); + * model.joints[i].jointExtendedModelCols(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].jointJacCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointExtendedModelCols(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].jointJacCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointExtendedModelCols(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/energy.hxx b/include/pinocchio/algorithm/energy.hxx index b321be13ca..77693a1e79 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -29,7 +29,7 @@ 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() + data.kinetic_energy += (jmodel.jointVelocityExtendedModelSelector(model.armature).array() * jdata.joint_v().array().square()) .sum(); } diff --git a/include/pinocchio/algorithm/geometry.hxx b/include/pinocchio/algorithm/geometry.hxx index 232c57aa6b..a1d45642c0 100644 --- a/include/pinocchio/algorithm/geometry.hxx +++ b/include/pinocchio/algorithm/geometry.hxx @@ -46,7 +46,7 @@ namespace pinocchio const Model::JointIndex joint_id = geom_model.geometryObjects[i].parentJoint; if (joint_id > 0) geom_data.oMg[i] = - (static_cast(data.oMi[joint_id]) + (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/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index 18cbe4b9d3..cef4dff8f0 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.jointJacCols(data.J); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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.jointVelCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(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.jointVelCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(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.jointJacCols(data.J); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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.jointVelCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); switch (rf) { @@ -200,7 +200,7 @@ namespace pinocchio } // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(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 720302f2a4..8599ac3ea8 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -57,7 +57,7 @@ namespace pinocchio data.oMi[i] = data.liMi[i]; Matrix6xLike & J_ = J.const_cast_derived(); - jmodel.jointJacCols(J_) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(J_) = data.oMi[i].act(jdata.S()); } }; @@ -113,7 +113,7 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; const JointIndex & i = jmodel.id(); - jmodel.jointJacCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); } }; } // namespace impl @@ -338,7 +338,7 @@ namespace pinocchio data.iMf[parent] = data.liMi[i] * data.iMf[i]; Matrix6xLike & J_ = J.const_cast_derived(); - jmodel.jointVelCols(J_) += data.iMf[i].actInv(jdata.S()); + jmodel.jointCols(J_) += data.iMf[i].actInv(jdata.S()); } }; @@ -431,7 +431,7 @@ namespace pinocchio oMi = data.liMi[i]; } - jmodel.jointJacCols(data.J) = oMi.act(jdata.S()); + jmodel.jointExtendedModelCols(data.J) = oMi.act(jdata.S()); // Spatial velocity of joint i expressed in the global frame o data.ov[i] = oMi.act(vJ); @@ -439,8 +439,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointJacCols(data.dJ); - ColsBlock Jcols = jmodel.jointJacCols(data.J); + ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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 a44324b3ae..fc69b22515 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -76,15 +76,15 @@ namespace pinocchio if (parent > 0) vi += data.liMi[i].actInv(data.v[parent]); - ai = jdata.S() * jmodel.jointVelocityFromDofSelector(a) + jdata.c() + (vi ^ jdata.v()); + ai = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (vi ^ jdata.v()); if (parent > 0) ai += data.liMi[i].actInv(data.a[parent]); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointJacCols(data.dJ); - ColsBlock Jcols = jmodel.jointJacCols(data.J); + ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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.jointJacCols(data.J); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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.jointVelCols(v_partial_dv_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(v_partial_dv_); switch (rf) { case WORLD: @@ -208,7 +208,7 @@ namespace pinocchio } // dvec/dq - ColsBlockOut1 v_partial_dq_cols = jmodel.jointVelCols(v_partial_dq_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(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.jointJacCols(data.dJ); - ColsBlock Jcols = jmodel.jointJacCols(data.J); + ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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.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_); + 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_); // dacc/da switch (rf) @@ -588,7 +588,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock Jcols = jmodel.jointJacCols(data.J); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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.jointVelCols(v_partial_dq_); - ColsBlockOut2 v_partial_dv_cols = jmodel.jointVelCols(v_partial_dv_); + ColsBlockOut1 v_partial_dq_cols = jmodel.jointCols(v_partial_dq_); + ColsBlockOut2 v_partial_dv_cols = jmodel.jointCols(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.jointJacCols(data.dJ); - ColsBlock Jcols = jmodel.jointJacCols(data.J); + ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock Jcols = jmodel.jointExtendedModelCols(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.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_); + 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_); const int nv = jmodel.nv(); Eigen::Matrix a_spatial_partial_da_cols(6, nv); diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index 925d52ab80..144f780321 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -234,7 +234,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.jointVelocityExtendedModelSelector(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 5c516398f0..adbc831aba 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.jointVelocityFromNvSelector( + jmodel_in.jointVelocitySelector( modelAB .effortLimit), // Need to select the vector base on the origin idxs, so use jmodel_in. - jmodel_in.jointVelocityFromNvSelector(modelAB.velocityLimit), - jmodel_in.jointConfigFromNqSelector(modelAB.lowerPositionLimit), - jmodel_in.jointConfigFromNqSelector(modelAB.upperPositionLimit), - jmodel_in.jointVelocityFromNvSelector(modelAB.friction), - jmodel_in.jointVelocityFromNvSelector(modelAB.damping)); + jmodel_in.jointVelocitySelector(modelAB.velocityLimit), + jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit), + jmodel_in.jointConfigSelector(modelAB.upperPositionLimit), + jmodel_in.jointVelocitySelector(modelAB.friction), + jmodel_in.jointVelocitySelector(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.jointVelocityFromNvSelector(model.rotorInertia) = - jmodel_in.jointVelocityFromNvSelector(modelAB.rotorInertia); - jmodel_out.jointVelocityFromNvSelector(model.rotorGearRatio) = - jmodel_in.jointVelocityFromNvSelector(modelAB.rotorGearRatio); + jmodel_out.jointVelocitySelector(model.rotorInertia) = + jmodel_in.jointVelocitySelector(modelAB.rotorInertia); + jmodel_out.jointVelocitySelector(model.rotorGearRatio) = + jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio); // Add all frames whose parent is this joint. for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) @@ -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.jointConfigFromNqSelector(config_vector) = - joint_modelA.jointConfigFromNqSelector(config_vectorA); + joint_model.jointConfigSelector(config_vector) = + joint_modelA.jointConfigSelector(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.jointConfigFromNqSelector(config_vector) = - joint_modelB.jointConfigFromNqSelector(config_vectorB); + joint_model.jointConfigSelector(config_vector) = + joint_modelB.jointConfigSelector(config_vectorB); } } @@ -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.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.jointVelocityFromNvSelector(input_model.friction), - joint_input_model.jointVelocityFromNvSelector(input_model.damping)); + 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.jointVelocitySelector(input_model.friction), + joint_input_model.jointVelocitySelector(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.jointVelocityFromNvSelector(reduced_model.rotorInertia) = - joint_input_model.jointVelocityFromNvSelector(input_model.rotorInertia); - jmodel_out.jointVelocityFromNvSelector(reduced_model.rotorGearRatio) = - joint_input_model.jointVelocityFromNvSelector(input_model.rotorGearRatio); + 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); } } @@ -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.jointConfigFromNqSelector(reduced_config_vector) = - input_joint_model.jointConfigFromNqSelector(input_config_vector); + reduced_joint_model.jointConfigSelector(reduced_config_vector) = + input_joint_model.jointConfigSelector(input_config_vector); } reduced_model.referenceConfigurations.insert( @@ -874,26 +874,26 @@ namespace pinocchio { 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); + jmodel_output.jointVelocitySelector(output_model.effortLimit) = + jmodel_input.jointVelocitySelector(input_model.effortLimit); + jmodel_output.jointVelocitySelector(output_model.velocityLimit) = + jmodel_input.jointVelocitySelector(input_model.velocityLimit); + + jmodel_output.jointConfigSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocitySelector(output_model.armature) = + jmodel_input.jointVelocitySelector(input_model.armature); + jmodel_output.jointVelocitySelector(output_model.rotorInertia) = + jmodel_input.jointVelocitySelector(input_model.rotorInertia); + jmodel_output.jointVelocitySelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocitySelector(input_model.rotorGearRatio); + jmodel_output.jointVelocitySelector(output_model.friction) = + jmodel_input.jointVelocitySelector(input_model.friction); + jmodel_output.jointVelocitySelector(output_model.damping) = + jmodel_input.jointVelocitySelector(input_model.damping); } // Move indexes and limits @@ -915,26 +915,26 @@ namespace pinocchio idx_v += jmodel_output.nv(); if (joint_id != index_secondary) { - 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); + jmodel_output.jointVelocitySelector(output_model.effortLimit) = + jmodel_input.jointVelocitySelector(input_model.effortLimit); + jmodel_output.jointVelocitySelector(output_model.velocityLimit) = + jmodel_input.jointVelocitySelector(input_model.velocityLimit); + + jmodel_output.jointConfigSelector(output_model.lowerPositionLimit) = + jmodel_input.jointConfigSelector(input_model.lowerPositionLimit); + jmodel_output.jointConfigSelector(output_model.upperPositionLimit) = + jmodel_input.jointConfigSelector(input_model.upperPositionLimit); + + jmodel_output.jointVelocitySelector(output_model.armature) = + jmodel_input.jointVelocitySelector(input_model.armature); + jmodel_output.jointVelocitySelector(output_model.rotorInertia) = + jmodel_input.jointVelocitySelector(input_model.rotorInertia); + jmodel_output.jointVelocitySelector(output_model.rotorGearRatio) = + jmodel_input.jointVelocitySelector(input_model.rotorGearRatio); + jmodel_output.jointVelocitySelector(output_model.friction) = + jmodel_input.jointVelocitySelector(input_model.friction); + jmodel_output.jointVelocitySelector(output_model.damping) = + jmodel_input.jointVelocitySelector(input_model.damping); } } } diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx index 9ffcd16b9a..a19bb12a96 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.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocityFromNvSelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocitySelector(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.jointVelocityFromDofSelector(data.u); + + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocityFromDofSelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia, parent > 0); Force & pa = data.f[i]; if (parent > 0) { pa.toVector() += Ia * data.a_bias[i].toVector() - + jdata.UDinv() * jmodel.jointVelocityFromDofSelector(data.u); + + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -257,14 +257,14 @@ 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() = + jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.tau))); + + (data.KAS[i].col(ind2).dot(jmodel.jointVelocityExtendedModelSelector(data.tau))); } } @@ -272,11 +272,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.jointVelocityFromDofSelector(data.tau).noalias() = + jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() = (jdata.Dinv() - * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocityFromDofSelector(data.u))) + * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocityExtendedModelSelector(data.u))) .eval(); - const Motion a_bf = jdata.S() * jmodel.jointVelocityFromDofSelector(data.tau); + const Motion a_bf = jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.tau); const Motion a_bf_motion = a_bf + data.a_bias[i]; for (int ind = 0; ind < data.constraints_supported_dim[i]; ind++) { @@ -311,10 +311,10 @@ namespace pinocchio const JointIndex parent = model.parents[i]; // 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); + jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.tau); + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.tau); if (parent > 0) { @@ -345,11 +345,11 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a[i] = data.liMi[i].actInv(data.a[parent]) + data.a_bias[i]; - jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocityFromDofSelector(data.u)) + jmodel.jointVelocityExtendedModelSelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocityExtendedModelSelector(data.u)) - jdata.UDinv().transpose() * data.a[i].toVector(); - data.a[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.ddq); } }; @@ -375,18 +375,18 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a[i] = data.liMi[i].actInv(data.a[parent]) + data.a_bias[i]; - jmodel.jointVelocityFromDofSelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocityFromDofSelector(data.u)) + jmodel.jointVelocityExtendedModelSelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.ddq).noalias() -= + jmodel.jointVelocityExtendedModelSelector(data.ddq).noalias() -= data.lambdaA[i][j] * jdata.Dinv() * (data.KAS[i].col(j)); } - data.a[i] += jdata.S() * jmodel.jointVelocityFromDofSelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.ddq); } }; diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index 5c85c42537..b4c55fc512 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.jointVelocityFromDofSelector(a); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(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 134049f213..2dec07841a 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -64,8 +64,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointJacCols(data.J); - ColsBlock dAdq_cols = jmodel.jointVelCols(data.dAdq); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(minus_gravity, J_cols, dAdq_cols); } @@ -118,10 +118,10 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); motionSet::inertiaAction(data.oYcrb[i], dAdq_cols, dFdq_cols); @@ -138,7 +138,7 @@ 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() = + jmodel.jointVelocityExtendedModelSelector(g).noalias() = J_cols.transpose() * data.of[i].toVector(); if (parent > 0) { @@ -307,7 +307,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.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -324,11 +324,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); + ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); + ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(ov, J_cols, dJ_cols); @@ -411,21 +411,21 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; Matrix6x & dYtJ = data.Fcrb[0]; - 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); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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); MatrixType1 & rnea_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType1, rnea_partial_dq); MatrixType2 & rnea_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType2, rnea_partial_dv); MatrixType3 & rnea_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da); // tau - jmodel.jointVelocityFromDofSelector(data.tau).noalias() = + jmodel.jointVelocityExtendedModelSelector(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 99ac2dfccf..353770e7ae 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -82,12 +82,12 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; ColsBlock J_cols = - jmodel.jointJacCols(data.J); // data.J has all the phi (in world frame) stacked in columns + jmodel.jointExtendedModelCols(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 + jmodel.jointCols(data.psid); // psid_cols is the psi_dot in world frame ColsBlock psidd_cols = - 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 + jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(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 @@ -99,7 +99,7 @@ namespace pinocchio 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()); + (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocityExtendedModelSelector(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 a5511da0e8..e0e177d420 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.jointVelocityFromDofSelector(a); + data.a_gf[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.tau) += jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityExtendedModelSelector(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.jointVelocityFromDofSelector(data.nle) += jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityExtendedModelSelector(data.nle) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); } @@ -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.jointVelocityExtendedModelSelector(g) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); } @@ -535,11 +535,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointJacCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(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.jointJacCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); motionSet::motionAction(data.ov[i], J_cols, dJ_cols); data.B[i] = data.oYcrb[i].variation(Scalar(0.5) * data.ov[i]); @@ -593,12 +593,12 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); - ColsBlock J_cols = jmodel.jointJacCols(data.J); - ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointVelCols(data.dFdv)); - jmodel.jointVelCols(data.dFdv).noalias() += data.B[i] * J_cols; + motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointCols(data.dFdv)); + jmodel.jointCols(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]); @@ -694,11 +694,11 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointJacCols(data.dJ); - ColsBlock J_cols = jmodel.jointJacCols(data.J); - ColsBlock Ag_cols = jmodel.jointVelCols(data.Ag); + ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock Ag_cols = jmodel.jointCols(data.Ag); typename Data::Matrix6x & dFdv = data.Fcrb[0]; - ColsBlock dFdv_cols = jmodel.jointVelCols(dFdv); + ColsBlock dFdv_cols = jmodel.jointCols(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-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 3fef515c54..634d927527 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -194,9 +194,9 @@ namespace pinocchio } template - struct JointConfigFromDofSelectorVisitor + struct jointConfigExtendedModelSelectorVisitor : fusion:: - JointUnaryVisitorBase, ReturnType> + JointUnaryVisitorBase, ReturnType> { typedef boost::fusion::vector ArgsType; @@ -205,7 +205,7 @@ namespace pinocchio { // 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); + auto vectorBlock = jmodel.jointConfigExtendedModelSelector(a); // VectorBlock does not implemet such getter, hack the Eigen::Block base class to retreive // such values. diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index b94d43a1f6..a1699490a6 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -479,154 +479,154 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromDofSelector(const Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigFromDofSelector(Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromNqSelector(const Eigen::MatrixBase & a) const + jointConfigSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigFromNqSelector(Eigen::MatrixBase & a) const + jointConfigSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromDofSelector(const Eigen::MatrixBase & a) const + jointVelocityExtendedModelSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityFromDofSelector(Eigen::MatrixBase & a) const + jointVelocityExtendedModelSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromNvSelector(const Eigen::MatrixBase & a) const + jointVelocitySelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityFromNvSelector(Eigen::MatrixBase & a) const + jointVelocitySelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template ColsReturn::ConstType - jointVelCols(const Eigen::MatrixBase & A) const + jointCols(const Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } template typename SizeDepType::template ColsReturn::ConstType - jointJacCols(const Eigen::MatrixBase & A) const + jointExtendedModelCols(const Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } template typename SizeDepType::template ColsReturn::Type - jointVelCols(Eigen::MatrixBase & A) const + jointCols(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } template typename SizeDepType::template ColsReturn::Type - jointJacCols(Eigen::MatrixBase & A) const + jointExtendedModelCols(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const + jointConfigSelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const + jointConfigSelector_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 + jointConfigExtendedModelSelector_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 + jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const + jointVelocityExtendedModelSelector_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 + jointVelocityExtendedModelSelector_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 + jointVelocitySelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const + jointVelocitySelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template ColsReturn::ConstType - jointVelCols_impl(const Eigen::MatrixBase & A) const + jointCols_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 + jointExtendedModelCols_impl(const Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } template typename SizeDepType::template ColsReturn::Type - jointVelCols_impl(Eigen::MatrixBase & A) const + jointCols_impl(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } template typename SizeDepType::template ColsReturn::Type - jointJacCols_impl(Eigen::MatrixBase & A) const + jointExtendedModelCols_impl(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_j, nj()); } diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index c4d14bbf92..b10ffbf8b3 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -386,11 +386,11 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const { typedef const Eigen::MatrixBase & InputType; typedef typename SizeDepType::template SegmentReturn::ConstType ReturnType; - typedef JointConfigFromDofSelectorVisitor Visitor; + typedef jointConfigExtendedModelSelectorVisitor Visitor; typename Visitor::ArgsType arg(a); return Visitor::run(*this, arg); } @@ -398,11 +398,11 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const { typedef Eigen::MatrixBase & InputType; typedef typename SizeDepType::template SegmentReturn::Type ReturnType; - typedef JointConfigFromDofSelectorVisitor Visitor; + typedef jointConfigExtendedModelSelectorVisitor Visitor; typename Visitor::ArgsType arg(a); return Visitor::run(*this, arg); } diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 3ad4f17f6c..702028e874 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -848,7 +848,7 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); } @@ -856,7 +856,7 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); } @@ -865,7 +865,7 @@ namespace pinocchio // Const access template typename SizeDepType::template ColsReturn::ConstType - jointVelCols_impl(const Eigen::MatrixBase & A) const + jointCols_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); } @@ -873,7 +873,7 @@ namespace pinocchio // Non-const access template typename SizeDepType::template ColsReturn::Type - jointVelCols_impl(Eigen::MatrixBase & A) const + jointCols_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); } @@ -890,7 +890,7 @@ namespace pinocchio // Non-const access template typename SizeDepType::template RowsReturn::Type - jointVelRows_impl(Eigen::MatrixBase & A) const + jointRows_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_v(), m_jmodel_ref.nv()); } @@ -900,7 +900,7 @@ namespace pinocchio // // Const access template typename SizeDepType::template BlockReturn::ConstType - jointVelBlock_impl(const Eigen::MatrixBase & Mat) const + jointBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block( Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); @@ -909,7 +909,7 @@ namespace pinocchio // Non-const access template typename SizeDepType::template BlockReturn::Type - jointVelBlock_impl(Eigen::MatrixBase & Mat) const + jointBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block( Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index e527f6a99c..b737740ecb 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -284,14 +284,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromDofSelector(const Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector(const Eigen::MatrixBase & a) const { - return derived().jointConfigFromDofSelector_impl(a); + return derived().jointConfigExtendedModelSelector_impl(a); } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromDofSelector_impl(const Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_q(), nq()); } @@ -299,14 +299,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigFromDofSelector(Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector(Eigen::MatrixBase & a) const { - return derived().jointConfigFromDofSelector_impl(a); + return derived().jointConfigExtendedModelSelector_impl(a); } template typename SizeDepType::template SegmentReturn::Type - jointConfigFromDofSelector_impl(Eigen::MatrixBase & a) const + jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a, idx_q(), nq()); } @@ -314,14 +314,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromNqSelector(const Eigen::MatrixBase & a) const + jointConfigSelector(const Eigen::MatrixBase & a) const { - return derived().jointConfigFromNqSelector_impl(a); + return derived().jointConfigSelector_impl(a); } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigFromNqSelector_impl(const Eigen::MatrixBase & a) const + jointConfigSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_q(), nq()); } @@ -329,14 +329,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigFromNqSelector(Eigen::MatrixBase & a) const + jointConfigSelector(Eigen::MatrixBase & a) const { - return derived().jointConfigFromNqSelector_impl(a); + return derived().jointConfigSelector_impl(a); } template typename SizeDepType::template SegmentReturn::Type - jointConfigFromNqSelector_impl(Eigen::MatrixBase & a) const + jointConfigSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a, idx_q(), nq()); } @@ -345,14 +345,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromDofSelector(const Eigen::MatrixBase & a) const + jointVelocityExtendedModelSelector(const Eigen::MatrixBase & a) const { - return derived().jointVelocityFromDofSelector_impl(a.derived()); + return derived().jointVelocityExtendedModelSelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromDofSelector_impl(const Eigen::MatrixBase & a) const + jointVelocityExtendedModelSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nj()); } @@ -360,14 +360,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointVelocityFromDofSelector(Eigen::MatrixBase & a) const + jointVelocityExtendedModelSelector(Eigen::MatrixBase & a) const { - return derived().jointVelocityFromDofSelector_impl(a.derived()); + return derived().jointVelocityExtendedModelSelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityFromDofSelector_impl(Eigen::MatrixBase & a) const + jointVelocityExtendedModelSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nj()); } @@ -375,14 +375,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromNvSelector(const Eigen::MatrixBase & a) const + jointVelocitySelector(const Eigen::MatrixBase & a) const { - return derived().jointVelocityFromNvSelector_impl(a.derived()); + return derived().jointVelocitySelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityFromNvSelector_impl(const Eigen::MatrixBase & a) const + jointVelocitySelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nv()); } @@ -390,14 +390,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointVelocityFromNvSelector(Eigen::MatrixBase & a) const + jointVelocitySelector(Eigen::MatrixBase & a) const { - return derived().jointVelocityFromNvSelector_impl(a.derived()); + return derived().jointVelocitySelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityFromNvSelector_impl(Eigen::MatrixBase & a) const + jointVelocitySelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nv()); } @@ -406,28 +406,28 @@ namespace pinocchio // Const access template typename SizeDepType::template ColsReturn::ConstType - jointVelCols(const Eigen::MatrixBase & A) const + jointCols(const Eigen::MatrixBase & A) const { - return derived().jointVelCols_impl(A.derived()); + return derived().jointCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::ConstType - jointJacCols(const Eigen::MatrixBase & A) const + jointExtendedModelCols(const Eigen::MatrixBase & A) const { - return derived().jointJacCols_impl(A.derived()); + return derived().jointExtendedModelCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::ConstType - jointVelCols_impl(const Eigen::MatrixBase & A) const + jointCols_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 + jointExtendedModelCols_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } @@ -436,28 +436,28 @@ namespace pinocchio // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?) template typename SizeDepType::template ColsReturn::Type - jointVelCols(Eigen::MatrixBase & A) const + jointCols(Eigen::MatrixBase & A) const { - return derived().jointVelCols_impl(A.derived()); + return derived().jointCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::Type - jointJacCols(Eigen::MatrixBase & A) const + jointExtendedModelCols(Eigen::MatrixBase & A) const { - return derived().jointJacCols_impl(A.derived()); + return derived().jointExtendedModelCols_impl(A.derived()); } template typename SizeDepType::template ColsReturn::Type - jointVelCols_impl(Eigen::MatrixBase & A) const + jointCols_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 + jointExtendedModelCols_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleCols(A.derived(), idx_j(), nj()); } @@ -466,28 +466,28 @@ namespace pinocchio // Const access template typename SizeDepType::template RowsReturn::ConstType - jointVelRows(const Eigen::MatrixBase & A) const + jointRows(const Eigen::MatrixBase & A) const { - return derived().jointVelRows_impl(A.derived()); + return derived().jointRows_impl(A.derived()); } template typename SizeDepType::template RowsReturn::ConstType - jointJacRows(const Eigen::MatrixBase & A) const + jointExtendedModelRows(const Eigen::MatrixBase & A) const { - return derived().jointJacRows_impl(A.derived()); + return derived().jointExtendedModelRows_impl(A.derived()); } template typename SizeDepType::template RowsReturn::ConstType - jointVelRows_impl(const Eigen::MatrixBase & A) const + jointRows_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } template typename SizeDepType::template RowsReturn::ConstType - jointJacRows_impl(const Eigen::MatrixBase & A) const + jointExtendedModelRows_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } @@ -495,28 +495,28 @@ namespace pinocchio // Non-const access template typename SizeDepType::template RowsReturn::Type - jointVelRows(Eigen::MatrixBase & A) const + jointRows(Eigen::MatrixBase & A) const { - return derived().jointVelRows_impl(A.derived()); + return derived().jointRows_impl(A.derived()); } template typename SizeDepType::template RowsReturn::Type - jointJacRows(Eigen::MatrixBase & A) const + jointExtendedModelRows(Eigen::MatrixBase & A) const { - return derived().jointJacRows_impl(A.derived()); + return derived().jointExtendedModelRows_impl(A.derived()); } template typename SizeDepType::template RowsReturn::Type - jointVelRows_impl(Eigen::MatrixBase & A) const + jointRows_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_v(), nv()); } template typename SizeDepType::template RowsReturn::Type - jointJacRows_impl(Eigen::MatrixBase & A) const + jointExtendedModelRows_impl(Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), idx_j(), nj()); } @@ -526,28 +526,28 @@ namespace pinocchio // Const access template typename SizeDepType::template BlockReturn::ConstType - jointVelBlock(const Eigen::MatrixBase & Mat) const + jointBlock(const Eigen::MatrixBase & Mat) const { - return derived().jointVelBlock_impl(Mat.derived()); + return derived().jointBlock_impl(Mat.derived()); } template typename SizeDepType::template BlockReturn::ConstType - jointJacBlock(const Eigen::MatrixBase & Mat) const + jointExtendedModelBlock(const Eigen::MatrixBase & Mat) const { - return derived().jointJacBlock_impl(Mat.derived()); + return derived().jointExtendedModelBlock_impl(Mat.derived()); } template typename SizeDepType::template BlockReturn::ConstType - jointVelBlock_impl(const Eigen::MatrixBase & Mat) const + jointBlock_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 + jointExtendedModelBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } @@ -555,28 +555,28 @@ namespace pinocchio // Non-const access template typename SizeDepType::template BlockReturn::Type - jointVelBlock(Eigen::MatrixBase & Mat) const + jointBlock(Eigen::MatrixBase & Mat) const { - return derived().jointVelBlock_impl(Mat.derived()); + return derived().jointBlock_impl(Mat.derived()); } template typename SizeDepType::template BlockReturn::Type - jointJacBlock(Eigen::MatrixBase & Mat) const + jointExtendedModelBlock(Eigen::MatrixBase & Mat) const { - return derived().jointJacBlock_impl(Mat.derived()); + return derived().jointExtendedModelBlock_impl(Mat.derived()); } template typename SizeDepType::template BlockReturn::Type - jointVelBlock_impl(Eigen::MatrixBase & Mat) const + jointBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_v(), idx_v(), nv(), nv()); } template typename SizeDepType::template BlockReturn::Type - jointJacBlock_impl(Eigen::MatrixBase & Mat) const + jointExtendedModelBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); } diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index f4702ec097..041607916d 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -601,7 +601,7 @@ namespace pinocchio template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - data.joint_q = this->jointConfigFromDofSelector(qs); + data.joint_q = this->jointConfigExtendedModelSelector(qs); data.M.translation() = data.joint_q; } @@ -610,7 +610,7 @@ namespace pinocchio calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase & vs) const { - data.joint_v = this->jointVelocityFromDofSelector(vs); + data.joint_v = this->jointVelocityExtendedModelSelector(vs); data.v.linear() = data.joint_v; } @@ -622,7 +622,7 @@ namespace pinocchio { calc(data, qs.derived()); - data.joint_v = this->jointVelocityFromDofSelector(vs); + data.joint_v = this->jointVelocityExtendedModelSelector(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 d5dfd2b6d2..3e5717583b 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -197,9 +197,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.integrate( - jmodel.jointConfigFromNqSelector(q.derived()), - jmodel.jointVelocityFromNvSelector(v.derived()), - jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); + jmodel.jointConfigSelector(q.derived()), + jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -244,9 +244,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrate( - jmodel.jointConfigFromNqSelector(q.derived()), - jmodel.jointVelocityFromNvSelector(v.derived()), - jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); + jmodel.jointConfigSelector(q.derived()), + jmodel.jointVelocitySelector(v.derived()), + jmodel.jointExtendedModelBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); } }; @@ -300,9 +300,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigFromNqSelector(q.derived()), - jmodel.jointVelocityFromNvSelector(v.derived()), jmodel.jointJacRows(mat_in.derived()), - jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); + jmodel.jointConfigSelector(q.derived()), + jmodel.jointVelocitySelector(v.derived()), jmodel.jointExtendedModelRows(mat_in.derived()), + jmodel.jointExtendedModelRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); } }; @@ -349,9 +349,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigFromNqSelector(q.derived()), - jmodel.jointVelocityFromNvSelector(v.derived()), - jmodel.jointJacRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); + jmodel.jointConfigSelector(q.derived()), + jmodel.jointVelocitySelector(v.derived()), + jmodel.jointExtendedModelRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; @@ -394,9 +394,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dDifference( - jmodel.jointConfigFromNqSelector(q0.derived()), - jmodel.jointConfigFromNqSelector(q1.derived()), - jmodel.jointJacBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); + jmodel.jointConfigSelector(q0.derived()), + jmodel.jointConfigSelector(q1.derived()), + jmodel.jointExtendedModelBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); } }; @@ -441,9 +441,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.interpolate( - jmodel.jointConfigFromNqSelector(q0.derived()), - jmodel.jointConfigFromNqSelector(q1.derived()), u, - jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); + jmodel.jointConfigSelector(q0.derived()), + jmodel.jointConfigSelector(q1.derived()), u, + jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -482,9 +482,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.difference( - jmodel.jointConfigFromNqSelector(q0.derived()), - jmodel.jointConfigFromNqSelector(q1.derived()), - jmodel.jointVelocityFromNvSelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); + jmodel.jointConfigSelector(q0.derived()), + jmodel.jointConfigSelector(q1.derived()), + jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); } }; @@ -527,8 +527,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.jointConfigSelector(q0.derived()), + jmodel.jointConfigSelector(q1.derived())); } }; @@ -561,8 +561,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.jointConfigSelector(q0.derived()), + jmodel.jointConfigSelector(q1.derived())); } }; @@ -601,9 +601,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.randomConfiguration( - jmodel.jointConfigFromNqSelector(lowerLimits.derived()), - jmodel.jointConfigFromNqSelector(upperLimits.derived()), - jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, q))); + jmodel.jointConfigSelector(lowerLimits.derived()), + jmodel.jointConfigSelector(upperLimits.derived()), + jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, q))); } }; @@ -632,7 +632,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.normalize( - jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); + jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); } }; @@ -663,7 +663,7 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - res &= lgo.isNormalized(jmodel.jointConfigFromNqSelector(q.derived()), prec); + res &= lgo.isNormalized(jmodel.jointConfigSelector(q.derived()), prec); } }; @@ -699,8 +699,8 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; isSame &= lgo.isSameConfiguration( - jmodel.jointConfigFromNqSelector(q1.derived()), - jmodel.jointConfigFromNqSelector(q2.derived()), prec); + jmodel.jointConfigSelector(q1.derived()), + jmodel.jointConfigSelector(q2.derived()), prec); } }; @@ -729,7 +729,7 @@ namespace pinocchio typedef typename Visitor::LieGroupMap LieGroupMap; typename LieGroupMap::template operation::type lgo; - jmodel.jointConfigFromNqSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, neutral_elt)) = + jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, neutral_elt)) = lgo.neutral(); } }; @@ -764,7 +764,7 @@ namespace pinocchio typedef typename LieGroupMap::template operation::type LieGroup; LieGroup lgo; lgo.integrateCoeffWiseJacobian( - jmodel.jointConfigFromNqSelector(q.derived()), + jmodel.jointConfigSelector(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 243c53f303..bd809a815b 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.jointVelocityFromNvSelector(effortLimit) = max_effort; + jmodel.jointVelocitySelector(effortLimit) = max_effort; velocityLimit.conservativeResize(nv); - jmodel.jointVelocityFromNvSelector(velocityLimit) = max_velocity; + jmodel.jointVelocitySelector(velocityLimit) = max_velocity; lowerPositionLimit.conservativeResize(nq); - jmodel.jointConfigFromNqSelector(lowerPositionLimit) = min_config; + jmodel.jointConfigSelector(lowerPositionLimit) = min_config; upperPositionLimit.conservativeResize(nq); - jmodel.jointConfigFromNqSelector(upperPositionLimit) = max_config; + jmodel.jointConfigSelector(upperPositionLimit) = max_config; armature.conservativeResize(nv); - jmodel.jointVelocityFromNvSelector(armature).setZero(); + jmodel.jointVelocitySelector(armature).setZero(); rotorInertia.conservativeResize(nv); - jmodel.jointVelocityFromNvSelector(rotorInertia).setZero(); + jmodel.jointVelocitySelector(rotorInertia).setZero(); rotorGearRatio.conservativeResize(nv); - jmodel.jointVelocityFromNvSelector(rotorGearRatio).setOnes(); + jmodel.jointVelocitySelector(rotorGearRatio).setOnes(); friction.conservativeResize(nv); - jmodel.jointVelocityFromNvSelector(friction) = joint_friction; + jmodel.jointVelocitySelector(friction) = joint_friction; damping.conservativeResize(nv); - jmodel.jointVelocityFromNvSelector(damping) = joint_damping; + jmodel.jointVelocitySelector(damping) = joint_damping; // Init and add joint index to its parent subtrees. subtrees.push_back(IndexVector(1)); diff --git a/unittest/centroidal.cpp b/unittest/centroidal.cpp index 6494864950..157c8feab5 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -128,8 +128,8 @@ void test_mimic_against_full_model( { 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) + model_full.joints[n].jointConfigExtendedModelSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q) + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); } diff --git a/unittest/crba.cpp b/unittest/crba.cpp index f96492458b..5a7f3a5adb 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -172,8 +172,8 @@ void test_mimic_against_full_model( { 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_mimic) + model_full.joints[n].jointConfigExtendedModelSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q_mimic) + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); } diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 6440a87741..f001e214ca 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -143,33 +143,33 @@ void test_joint_methods( const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m)); BOOST_CHECK( - jmodel.jointConfigFromDofSelector(vec) == jmodel_composite.jointConfigFromDofSelector(vec)); + jmodel.jointConfigExtendedModelSelector(vec) == jmodel_composite.jointConfigExtendedModelSelector(vec)); BOOST_CHECK( - jmodel.jointConfigFromDofSelector(vec_const) - == jmodel_composite.jointConfigFromDofSelector(vec_const)); + jmodel.jointConfigExtendedModelSelector(vec_const) + == jmodel_composite.jointConfigExtendedModelSelector(vec_const)); BOOST_CHECK( - jmodel.jointConfigFromNqSelector(vec) == jmodel_composite.jointConfigFromNqSelector(vec)); + jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec)); BOOST_CHECK( - jmodel.jointConfigFromNqSelector(vec_const) - == jmodel_composite.jointConfigFromNqSelector(vec_const)); + jmodel.jointConfigSelector(vec_const) + == jmodel_composite.jointConfigSelector(vec_const)); BOOST_CHECK( - jmodel.jointVelocityFromNvSelector(vec) == jmodel_composite.jointVelocityFromNvSelector(vec)); + jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec)); BOOST_CHECK( - jmodel.jointVelocityFromNvSelector(vec_const) - == jmodel_composite.jointVelocityFromNvSelector(vec_const)); + jmodel.jointVelocitySelector(vec_const) + == jmodel_composite.jointVelocitySelector(vec_const)); BOOST_CHECK( - jmodel.jointVelocityFromDofSelector(vec) == jmodel_composite.jointVelocityFromDofSelector(vec)); + jmodel.jointVelocityExtendedModelSelector(vec) == jmodel_composite.jointVelocityExtendedModelSelector(vec)); BOOST_CHECK( - jmodel.jointVelocityFromDofSelector(vec_const) - == jmodel_composite.jointVelocityFromDofSelector(vec_const)); + jmodel.jointVelocityExtendedModelSelector(vec_const) + == jmodel_composite.jointVelocityExtendedModelSelector(vec_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)); + BOOST_CHECK(jmodel.jointCols(mat) == jmodel_composite.jointCols(mat)); + BOOST_CHECK(jmodel.jointCols(mat_const) == jmodel_composite.jointCols(mat_const)); + BOOST_CHECK(jmodel.jointExtendedModelCols(mat) == jmodel_composite.jointExtendedModelCols(mat)); + BOOST_CHECK(jmodel.jointExtendedModelCols(mat_const) == jmodel_composite.jointExtendedModelCols(mat_const)); } struct TestJointComposite diff --git a/unittest/model.cpp b/unittest/model.cpp index 8e7c11e982..c791091f60 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -250,11 +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.jointConfigFromNqSelector(humanoid_config->second) - == joint_model1.jointConfigFromNqSelector(config_vector)); + joint_model_humanoid.jointConfigSelector(humanoid_config->second) + == joint_model1.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_humanoid.jointConfigFromDofSelector(humanoid_config->second) - == joint_model1.jointConfigFromDofSelector(config_vector)); + joint_model_humanoid.jointConfigExtendedModelSelector(humanoid_config->second) + == joint_model1.jointConfigExtendedModelSelector(config_vector)); // std::cerr<<"humanoid "<second) - == joint_model1.jointConfigFromNqSelector(config_vector)); + joint_model_manipulator.jointConfigSelector(manipulator_config->second) + == joint_model1.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_manipulator.jointConfigFromDofSelector(manipulator_config->second) - == joint_model1.jointConfigFromDofSelector(config_vector)); + joint_model_manipulator.jointConfigExtendedModelSelector(manipulator_config->second) + == joint_model1.jointConfigExtendedModelSelector(config_vector)); // std::cerr<<"manipulator "<second) - == joint_model.jointConfigFromNqSelector(config_vector)); + joint_model_humanoid.jointConfigSelector(humanoid_config->second) + == joint_model.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_humanoid.jointConfigFromDofSelector(humanoid_config->second) - == joint_model.jointConfigFromDofSelector(config_vector)); + joint_model_humanoid.jointConfigExtendedModelSelector(humanoid_config->second) + == joint_model.jointConfigExtendedModelSelector(config_vector)); // std::cerr<<"humanoid "<second) - == joint_model.jointConfigFromNqSelector(config_vector)); + joint_model_manipulator.jointConfigSelector(manipulator_config->second) + == joint_model.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_manipulator.jointConfigFromDofSelector(manipulator_config->second) - == joint_model.jointConfigFromDofSelector(config_vector)); + joint_model_manipulator.jointConfigExtendedModelSelector(manipulator_config->second) + == joint_model.jointConfigExtendedModelSelector(config_vector)); // std::cerr<<"manipulator "< Date: Thu, 16 Jan 2025 13:25:56 +0100 Subject: [PATCH 180/310] Added assert for non supported algorithms and apply precommit --- .../pinocchio/algorithm/aba-derivatives.hxx | 9 ++++- include/pinocchio/algorithm/aba.hxx | 31 +++++++++++--- .../algorithm/center-of-mass-derivatives.hxx | 5 +++ .../pinocchio/algorithm/compute-all-terms.hxx | 5 +++ .../constrained-dynamics-derivatives.hxx | 9 ++++- .../algorithm/constrained-dynamics.hxx | 5 +++ include/pinocchio/algorithm/energy.hxx | 5 +++ include/pinocchio/algorithm/geometry.hxx | 4 +- .../impulse-dynamics-derivatives.hxx | 6 +++ include/pinocchio/algorithm/jacobian.hxx | 3 +- .../algorithm/kinematics-derivatives.hxx | 8 +++- include/pinocchio/algorithm/kinematics.hxx | 4 +- include/pinocchio/algorithm/pv.hxx | 8 ++-- include/pinocchio/algorithm/regressor.hxx | 5 +++ .../pinocchio/algorithm/rnea-derivatives.hxx | 4 +- .../rnea-second-order-derivatives.hxx | 14 +++---- .../multibody/joint/joint-basic-visitors.hxx | 5 ++- .../multibody/joint/joint-composite.hpp | 3 +- .../multibody/joint/joint-model-base.hpp | 6 +-- .../multibody/liegroup/liegroup-algo.hxx | 40 ++++++++----------- unittest/joint-composite.cpp | 21 +++++----- 21 files changed, 127 insertions(+), 73 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 1e9c60e3aa..7c6465a8de 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -43,6 +43,11 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; const JointIndex i = jmodel.id(); @@ -162,8 +167,8 @@ namespace pinocchio { Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); - fi.toVector().noalias() += Ia * data.oa_gf[i].toVector() - + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); + fi.toVector().noalias() += + Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(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 2ef64adca2..77f2a8ba5b 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -106,6 +106,11 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -169,7 +174,8 @@ namespace pinocchio Force & fi = data.of[i]; - jmodel.jointVelocityExtendedModelSelector(data.u).noalias() -= Jcols.transpose() * fi.toVector(); + jmodel.jointVelocityExtendedModelSelector(data.u).noalias() -= + Jcols.transpose() * fi.toVector(); jdata.U().noalias() = Ia * Jcols; jdata.StU().noalias() = Jcols.transpose() * jdata.U(); @@ -184,8 +190,9 @@ namespace pinocchio { Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); - fi.toVector().noalias() += Ia * data.oa_gf[i].toVector() - + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); + fi.toVector().noalias() += + Ia * data.oa_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -379,6 +386,11 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; const JointIndex i = jmodel.id(); @@ -426,13 +438,15 @@ namespace pinocchio jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocityExtendedModelSelector(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.jointVelocityExtendedModelSelector(data.u); + pa.toVector().noalias() += + Ia * data.a_gf[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -621,6 +635,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; const JointIndex & i = jmodel.id(); diff --git a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx index 6bd999b8f6..456e66c628 100644 --- a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx +++ b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx @@ -35,6 +35,11 @@ namespace pinocchio Data & data, const Eigen::MatrixBase & vcom_partial_dq) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index e6cf7f4eb8..8a1541a01c 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -41,6 +41,11 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { + 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/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index c00aae6073..22f2ee9a71 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -41,6 +41,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 Data::Motion Motion; @@ -81,8 +86,8 @@ namespace pinocchio Motion & oa_gf = data.oa_gf[i]; ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); const typename Data::TangentVectorType & a = data.ddq; - data.a[i] = - jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(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]); diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index 18647c7f8c..a4cd794a2b 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -92,6 +92,11 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { + 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::Force Force; diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx index 77693a1e79..ff83bf8cfe 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -27,6 +27,11 @@ namespace pinocchio const Model & model, Data & data) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + const JointIndex & i = jmodel.id(); data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); data.kinetic_energy += (jmodel.jointVelocityExtendedModelSelector(model.armature).array() diff --git a/include/pinocchio/algorithm/geometry.hxx b/include/pinocchio/algorithm/geometry.hxx index a1d45642c0..89093d8ea7 100644 --- a/include/pinocchio/algorithm/geometry.hxx +++ b/include/pinocchio/algorithm/geometry.hxx @@ -45,9 +45,7 @@ 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] = (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/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index cef4dff8f0..3bd0d8d8ef 100644 --- a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx @@ -54,6 +54,11 @@ namespace pinocchio const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & v_partial_dv) { + assert( + (std::is_same>::value + == false) + && "Algorithm not supported for mimic joints"); + typedef typename Model::JointIndex JointIndex; typedef typename Data::SE3 SE3; typedef typename Data::Motion Motion; @@ -160,6 +165,7 @@ namespace pinocchio const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & v_partial_dv) { + typedef typename Model::JointIndex JointIndex; typedef typename Data::SE3 SE3; typedef typename Data::Motion Motion; diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 8599ac3ea8..c1c3cf3468 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -371,8 +371,7 @@ namespace pinocchio for (JointIndex i = jointId; i > 0; i = model.parents[i]) { Pass::run( - model.joints[i], data.joints[i], - typename Pass::ArgsType(model, data, q.derived(), J_)); + model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J_)); } } diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index fc69b22515..11caa540dd 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -51,6 +51,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::SE3 SE3; typedef typename Data::Motion Motion; @@ -76,7 +81,8 @@ namespace pinocchio if (parent > 0) vi += data.liMi[i].actInv(data.v[parent]); - ai = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (vi ^ jdata.v()); + ai = + jdata.S() * jmodel.jointVelocityExtendedModelSelector(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 144f780321..22ef547c7f 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -233,8 +233,8 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - data.a[i] = - jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + + (data.v[i] ^ jdata.v()); data.a[i] += data.liMi[i].actInv(data.a[parent]); } }; diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx index a19bb12a96..d9a1bb92fe 100644 --- a/include/pinocchio/algorithm/pv.hxx +++ b/include/pinocchio/algorithm/pv.hxx @@ -191,8 +191,9 @@ namespace pinocchio if (parent > 0) { - pa.toVector().noalias() += Ia * data.a_bias[i].toVector() - + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); + pa.toVector().noalias() += + Ia * data.a_bias[i].toVector() + + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -311,7 +312,8 @@ namespace pinocchio const JointIndex parent = model.parents[i]; // Abusing otherwise unused data.tau below. - jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() = jdata.S().transpose() * data.f[i]; + jmodel.jointVelocityExtendedModelSelector(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.jointVelocityExtendedModelSelector(data.tau); diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index b4c55fc512..bfb4cad5b2 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -363,6 +363,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; const JointIndex i = jmodel.id(); diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index 2dec07841a..c3ce2df0b0 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -306,8 +306,8 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - data.a[i] = - jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); + data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index 353770e7ae..0bc56c1828 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -81,13 +81,13 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = - jmodel.jointExtendedModelCols(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 + ColsBlock J_cols = jmodel.jointExtendedModelCols( + 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 ColsBlock psidd_cols = jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); // This here is phi_dot in world frame + ColsBlock dJ_cols = + jmodel.jointExtendedModelCols(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 @@ -98,8 +98,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.jointVelocityExtendedModelSelector(a) + jdata.c()); + oa += (ov ^ vJ) + + data.oMi[i].act(jdata.S() * jmodel.jointVelocityExtendedModelSelector(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/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 634d927527..91770cdebc 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -195,8 +195,9 @@ namespace pinocchio template struct jointConfigExtendedModelSelectorVisitor - : fusion:: - JointUnaryVisitorBase, ReturnType> + : fusion::JointUnaryVisitorBase< + jointConfigExtendedModelSelectorVisitor, + ReturnType> { typedef boost::fusion::vector ArgsType; diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index a1699490a6..e48e43be9e 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -542,8 +542,7 @@ namespace pinocchio return A.middleCols(Base::i_j, nj()); } template - typename SizeDepType::template ColsReturn::Type - jointCols(Eigen::MatrixBase & A) const + typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const { return A.middleCols(Base::i_v, nv()); } diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index b737740ecb..fc46d57d65 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -435,8 +435,7 @@ namespace pinocchio // Non-const access // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?) template - typename SizeDepType::template ColsReturn::Type - jointCols(Eigen::MatrixBase & A) const + typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const { return derived().jointCols_impl(A.derived()); } @@ -494,8 +493,7 @@ namespace pinocchio // Non-const access template - typename SizeDepType::template RowsReturn::Type - jointRows(Eigen::MatrixBase & A) const + typename SizeDepType::template RowsReturn::Type jointRows(Eigen::MatrixBase & A) const { return derived().jointRows_impl(A.derived()); } diff --git a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx index 3e5717583b..adfa560e89 100644 --- a/include/pinocchio/multibody/liegroup/liegroup-algo.hxx +++ b/include/pinocchio/multibody/liegroup/liegroup-algo.hxx @@ -197,8 +197,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.integrate( - jmodel.jointConfigSelector(q.derived()), - jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -244,9 +243,9 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrate( - jmodel.jointConfigSelector(q.derived()), - jmodel.jointVelocitySelector(v.derived()), - jmodel.jointExtendedModelBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, op); + jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointExtendedModelBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg, + op); } }; @@ -300,9 +299,10 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigSelector(q.derived()), - jmodel.jointVelocitySelector(v.derived()), jmodel.jointExtendedModelRows(mat_in.derived()), - jmodel.jointExtendedModelRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), arg); + jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), + jmodel.jointExtendedModelRows(mat_in.derived()), + jmodel.jointExtendedModelRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixOutType, mat_out)), + arg); } }; @@ -349,8 +349,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dIntegrateTransport( - jmodel.jointConfigSelector(q.derived()), - jmodel.jointVelocitySelector(v.derived()), + jmodel.jointConfigSelector(q.derived()), jmodel.jointVelocitySelector(v.derived()), jmodel.jointExtendedModelRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType, mat)), arg); } }; @@ -394,8 +393,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.dDifference( - jmodel.jointConfigSelector(q0.derived()), - jmodel.jointConfigSelector(q1.derived()), + jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), jmodel.jointExtendedModelBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, mat)), arg); } }; @@ -441,8 +439,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.interpolate( - jmodel.jointConfigSelector(q0.derived()), - jmodel.jointConfigSelector(q1.derived()), u, + jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), u, jmodel.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, result))); } }; @@ -482,8 +479,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; lgo.difference( - jmodel.jointConfigSelector(q0.derived()), - jmodel.jointConfigSelector(q1.derived()), + jmodel.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived()), jmodel.jointVelocitySelector(PINOCCHIO_EIGEN_CONST_CAST(TangentVectorOut, result))); } }; @@ -527,8 +523,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.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived())); } }; @@ -561,8 +556,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.jointConfigSelector(q0.derived()), jmodel.jointConfigSelector(q1.derived())); } }; @@ -631,8 +625,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.jointConfigSelector(PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType, qout))); } }; @@ -699,8 +692,7 @@ namespace pinocchio typename LieGroupMap::template operation::type lgo; isSame &= lgo.isSameConfiguration( - jmodel.jointConfigSelector(q1.derived()), - jmodel.jointConfigSelector(q2.derived()), prec); + jmodel.jointConfigSelector(q1.derived()), jmodel.jointConfigSelector(q2.derived()), prec); } }; diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index f001e214ca..94d9c48e0c 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -143,25 +143,23 @@ void test_joint_methods( const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m)); BOOST_CHECK( - jmodel.jointConfigExtendedModelSelector(vec) == jmodel_composite.jointConfigExtendedModelSelector(vec)); + jmodel.jointConfigExtendedModelSelector(vec) + == jmodel_composite.jointConfigExtendedModelSelector(vec)); BOOST_CHECK( jmodel.jointConfigExtendedModelSelector(vec_const) == jmodel_composite.jointConfigExtendedModelSelector(vec_const)); + BOOST_CHECK(jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec)); BOOST_CHECK( - jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec)); - BOOST_CHECK( - jmodel.jointConfigSelector(vec_const) - == jmodel_composite.jointConfigSelector(vec_const)); + jmodel.jointConfigSelector(vec_const) == jmodel_composite.jointConfigSelector(vec_const)); + BOOST_CHECK(jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec)); BOOST_CHECK( - jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec)); - BOOST_CHECK( - jmodel.jointVelocitySelector(vec_const) - == jmodel_composite.jointVelocitySelector(vec_const)); + jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const)); BOOST_CHECK( - jmodel.jointVelocityExtendedModelSelector(vec) == jmodel_composite.jointVelocityExtendedModelSelector(vec)); + jmodel.jointVelocityExtendedModelSelector(vec) + == jmodel_composite.jointVelocityExtendedModelSelector(vec)); BOOST_CHECK( jmodel.jointVelocityExtendedModelSelector(vec_const) == jmodel_composite.jointVelocityExtendedModelSelector(vec_const)); @@ -169,7 +167,8 @@ void test_joint_methods( BOOST_CHECK(jmodel.jointCols(mat) == jmodel_composite.jointCols(mat)); BOOST_CHECK(jmodel.jointCols(mat_const) == jmodel_composite.jointCols(mat_const)); BOOST_CHECK(jmodel.jointExtendedModelCols(mat) == jmodel_composite.jointExtendedModelCols(mat)); - BOOST_CHECK(jmodel.jointExtendedModelCols(mat_const) == jmodel_composite.jointExtendedModelCols(mat_const)); + BOOST_CHECK( + jmodel.jointExtendedModelCols(mat_const) == jmodel_composite.jointExtendedModelCols(mat_const)); } struct TestJointComposite From 0abc4f6fd62e0b69ad9cc135124bdb2128214f6e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 20 Jan 2025 11:15:20 +0100 Subject: [PATCH 181/310] [EtienneAr feedback] Fix -Wreorder for urdf parsing of mimic joints / and mimiced -> mimicked for naming consistency --- include/pinocchio/parsers/urdf/model.hxx | 36 ++++++++++++------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 5b6dd49992..981b0c72dc 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -115,7 +115,7 @@ namespace pinocchio virtual void addMimicInfo( const std::string & mimic_name, - const std::string & mimiced_name, + const std::string & mimicked_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset, @@ -147,10 +147,10 @@ namespace pinocchio typedef _Scalar Scalar; typedef Eigen::Matrix Vector3; - Vector3 axis; - std::string mimiced_name; + std::string mimicked_name; Scalar multiplier; Scalar offset; + Vector3 axis; // Use the JointType from UrdfVisitorBaseTpl typedef typename UrdfVisitorBaseTpl<_Scalar, Options>::JointType JointType; @@ -161,12 +161,12 @@ namespace pinocchio } MimicInfo( - std::string _mimiced_name, + std::string _mimicked_name, Scalar _multiplier, Scalar _offset, Vector3 _axis, JointType _jointType) - : mimiced_name(_mimiced_name) + : mimicked_name(_mimicked_name) , multiplier(_multiplier) , offset(_offset) , axis(_axis) @@ -465,13 +465,13 @@ namespace pinocchio void addMimicInfo( const std::string & mimic_name, - const std::string & mimiced_name, + const std::string & mimicked_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset, JointType jointType) { - MimicInfo mimic_info(mimiced_name, multiplier, offset, axis, jointType); + MimicInfo mimic_info(mimicked_name, multiplier, offset, axis, jointType); this->mimicInfo_map.insert(std::make_pair(mimic_name, mimic_info)); } @@ -485,7 +485,7 @@ namespace pinocchio typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, typename JointCollection::JointModelRZ, typename JointCollection::JointModelRevoluteUnaligned>( - mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, + mimic_name, mimic_info.mimicked_name, mimic_info.axis, mimic_info.multiplier, mimic_info.offset); break; case Base::PRISMATIC: @@ -493,7 +493,7 @@ namespace pinocchio typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, typename JointCollection::JointModelPZ, typename JointCollection::JointModelPrismaticUnaligned>( - mimic_name, mimic_info.mimiced_name, mimic_info.axis, mimic_info.multiplier, + mimic_name, mimic_info.mimicked_name, mimic_info.axis, mimic_info.multiplier, mimic_info.offset); break; @@ -505,35 +505,35 @@ namespace pinocchio template void createMimicJoint( const std::string & mimic_name, - const std::string & mimiced_name, + const std::string & mimicked_name, const Vector3 & axis, const Scalar multiplier, const Scalar offset) { - auto mimiced_joint = model.joints[getJointId(mimiced_name)]; + auto mimicked_joint = model.joints[getJointId(mimicked_name)]; 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); + mimic_joint = typename JointCollection::JointModelMimic( + TypeX(), mimicked_joint, multiplier, offset); break; case AXIS_Y: - mimic_joint = - typename JointCollection::JointModelMimic(TypeY(), mimiced_joint, multiplier, offset); + mimic_joint = typename JointCollection::JointModelMimic( + TypeY(), mimicked_joint, multiplier, offset); break; case AXIS_Z: - mimic_joint = - typename JointCollection::JointModelMimic(TypeZ(), mimiced_joint, multiplier, offset); + mimic_joint = typename JointCollection::JointModelMimic( + TypeZ(), mimicked_joint, multiplier, offset); break; case AXIS_UNALIGNED: mimic_joint = typename JointCollection::JointModelMimic( - TypeUnaligned(), mimiced_joint, multiplier, offset); + TypeUnaligned(), mimicked_joint, multiplier, offset); break; default: From 8fdf846e3d6040f6c1943aec47b4708c83d5633c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 20 Jan 2025 11:46:49 +0100 Subject: [PATCH 182/310] [EtienneAr feedback] Simplify getRelativePlacement by doing two backward passes --- include/pinocchio/algorithm/kinematics.hxx | 35 +++++++++------------- 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index 22ef547c7f..6a13a9bb50 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -292,34 +292,27 @@ namespace pinocchio 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(); + SE3Tpl ancestorMref(1); // Initialize to Identity + SE3Tpl ancestorMtarget(1); // Initialize to Identity - // Traverse the kinematic chain from "tip" to "root" - size_t common_ancestor_parent, common_ancestor_child; - findCommonAncestor(model, parent_id, child_id, common_ancestor_parent, common_ancestor_child); + size_t ancestor_ref, ancestor_target; + findCommonAncestor(model, jointIdRef, jointIdTarget, ancestor_ref, ancestor_target); - // Traverse the kinematic chain backward to the common ancestor - for (size_t i = model.supports[child_id].size() - 1; i > common_ancestor_child; i--) + // Traverse the kinematic chain backward from Ref to the common ancestor + for (size_t i = model.supports[jointIdRef].size() - 1; i > ancestor_ref; 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); + const JointIndex j = model.supports[jointIdRef][(size_t)i]; + ancestorMref = data.liMi[j].act(ancestorMref); } - // Inverse the result if necessary - if (child_id == jointIdRef && jointIdRef != jointIdTarget) + // Traverse the kinematic chain backward from Target to the common ancestor + for (size_t i = model.supports[jointIdTarget].size() - 1; i > ancestor_target; i--) { - result = result.inverse(); + const JointIndex j = model.supports[jointIdTarget][(size_t)i]; + ancestorMtarget = data.liMi[j].act(ancestorMtarget); } - return result; + + return ancestorMref.actInv(ancestorMtarget); } case Convention::WORLD: return data.oMi[jointIdRef].actInv(data.oMi[jointIdTarget]); From 96bd68fda47ba30fa1cfddc1d4e501e9dbe3e824 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 20 Jan 2025 14:20:50 +0100 Subject: [PATCH 183/310] [EtienneAr feedback] Remove unused scaling in JointDataMimic --- .../pinocchio/multibody/joint/joint-mimic.hpp | 34 ++----------------- .../pinocchio/serialization/joints-data.hpp | 1 - 2 files changed, 3 insertions(+), 32 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 702028e874..5e56e049e9 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -366,8 +366,7 @@ namespace pinocchio typedef typename RefJointData::JointDataVariant RefJointDataVariant; JointDataMimicTpl() - : m_scaling((Scalar)0) - , S((Scalar)0) + : S((Scalar)0) { joint_q.resize(0, 1); joint_q_transformed.resize(0, 1); @@ -375,24 +374,9 @@ namespace pinocchio joint_v_transformed.resize(0, 1); } - // JointDataMimicTpl(const JointDataMimicTpl & other) - // { *this = other; } - - // JointDataMimicTpl(const RefJointDataVariant & jdata, - // const Scalar & scaling, - // const int & nq, - // const int & nv) - // : m_jdata_ref(jdata) - // , m_scaling(scaling) - // , S(m_jdata_ref.S(),scaling) - // { - - // } - JointDataMimicTpl( 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) { joint_q.resize(nq, 1); @@ -404,20 +388,18 @@ namespace pinocchio JointDataMimicTpl & operator=(const JointDataMimicTpl & other) { m_jdata_ref = other.m_jdata_ref; - m_scaling = other.m_scaling; joint_q = other.joint_q; joint_q_transformed = other.joint_q_transformed; joint_v = other.joint_v; joint_v_transformed = other.joint_v_transformed; - S = Constraint_t(m_jdata_ref.S(), other.m_scaling); + S = Constraint_t(other.S); 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 && joint_q == other.joint_q + return Base::isEqual(other) && m_jdata_ref == other.m_jdata_ref && joint_q == other.joint_q && joint_q_transformed == other.joint_q_transformed && joint_v == other.joint_v && joint_v_transformed == other.joint_v_transformed; } @@ -544,15 +526,6 @@ namespace pinocchio return m_jdata_ref; } - const Scalar & scaling() const - { - return m_scaling; - } - Scalar & scaling() - { - return m_scaling; - } - ConfigVector_t & joint_q_accessor() { return joint_q; @@ -591,7 +564,6 @@ namespace pinocchio protected: RefJointData m_jdata_ref; - Scalar m_scaling; /// \brief original configuration vector ConfigVector_t joint_q; diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 240629a1f5..5ec27b75d0 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -276,7 +276,6 @@ namespace boost ar & make_nvp("m_v_transform", joint.v_transformed()); ar & make_nvp("jdata", joint.jdata()); - ar & make_nvp("scaling", joint.scaling()); } } // namespace serialization From f6a28ee21f924315352260d57fac3f2d7331061a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 20 Jan 2025 14:54:31 +0100 Subject: [PATCH 184/310] [algorithms] Use throw instead of assert --- .../pinocchio/algorithm/aba-derivatives.hxx | 6 ++--- include/pinocchio/algorithm/aba.hxx | 12 ++++----- .../algorithm/centroidal-derivatives.hxx | 13 +++++----- .../pinocchio/algorithm/compute-all-terms.hxx | 13 +++++----- .../constrained-dynamics-derivatives.hxx | 6 ++--- .../algorithm/constrained-dynamics.hxx | 7 +++-- include/pinocchio/algorithm/crba.hxx | 10 ++++--- include/pinocchio/algorithm/energy.hxx | 7 +++-- .../impulse-dynamics-derivatives.hxx | 6 ++--- .../algorithm/kinematics-derivatives.hxx | 6 ++--- include/pinocchio/algorithm/regressor.hxx | 7 +++-- .../pinocchio/algorithm/rnea-derivatives.hxx | 26 +++++++++---------- .../rnea-second-order-derivatives.hxx | 13 +++++----- include/pinocchio/algorithm/rnea.hxx | 12 ++++----- 14 files changed, 69 insertions(+), 75 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 7c6465a8de..0379a0e72c 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -43,10 +43,10 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index 77f2a8ba5b..e24b4d4cd5 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -106,10 +106,10 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -386,10 +386,10 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 68d59928cb..7818e980eb 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -58,11 +58,10 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); - + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; Motion & ov = data.ov[i]; @@ -375,10 +374,10 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 8a1541a01c..62cacad5ef 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -41,11 +41,10 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); - + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename SizeDepType::template ColsReturn::Type @@ -108,10 +107,10 @@ namespace pinocchio const Model & model, Data & data) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index 22f2ee9a71..60a350bd1c 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -41,10 +41,10 @@ namespace pinocchio const Model & model, Data & data) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index a4cd794a2b..518a09bf97 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -92,11 +92,10 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); - + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; typedef typename Data::Force Force; diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index a3e97aa813..abbce93527 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -83,8 +83,9 @@ 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."); + PINOCCHIO_THROW( + secondary_id > primary_id, std::invalid_argument, + std::string("Mimicking joint id is before the primary in the tree")); size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); @@ -234,8 +235,9 @@ 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."); - + PINOCCHIO_THROW( + secondary_id > primary_id, std::invalid_argument, + std::string("Mimicking joint id is before the primary in the tree")); size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx index ff83bf8cfe..f0a1379000 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -27,11 +27,10 @@ namespace pinocchio const Model & model, Data & data) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); - + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); const JointIndex & i = jmodel.id(); data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); data.kinetic_energy += (jmodel.jointVelocityExtendedModelSelector(model.armature).array() diff --git a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index 3bd0d8d8ef..032539da71 100644 --- a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx @@ -54,10 +54,10 @@ namespace pinocchio const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & v_partial_dv) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::SE3 SE3; diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index 11caa540dd..85b09b8721 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -51,10 +51,10 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::SE3 SE3; diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index bfb4cad5b2..1bb76e7cde 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -363,10 +363,9 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - assert( - (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + PINOCCHIO_THROW( + (std::is_same>::value == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index c3ce2df0b0..6352377bb2 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -37,11 +37,10 @@ namespace pinocchio Data & data, const Eigen::MatrixBase & q) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); - + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -98,10 +97,10 @@ namespace pinocchio typename Data::VectorXs & g, const Eigen::MatrixBase & gravity_partial_dq) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< @@ -278,11 +277,10 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); - + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -397,10 +395,10 @@ namespace pinocchio const Eigen::MatrixBase & rnea_partial_dv, const Eigen::MatrixBase & rnea_partial_da) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("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 0bc56c1828..84192cdbba 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -47,10 +47,9 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - assert( - (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + PINOCCHIO_THROW( + (std::is_same>::value == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -164,10 +163,10 @@ namespace pinocchio const Tensor3 & dtau_dqdv, const Tensor3 & dtau_dadq) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Data::Motion Motion; typedef typename Data::Force Force; typedef typename Data::Inertia Inertia; diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index e0e177d420..0d77633da1 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -574,10 +574,10 @@ namespace pinocchio static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< @@ -675,10 +675,10 @@ namespace pinocchio static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { - assert( + PINOCCHIO_THROW( (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); + == false), + std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef Eigen::Matrix< From 6258e8b6c8265768fc25be70eaefd10ac5abe3bd Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 23 Jan 2025 10:29:17 +0100 Subject: [PATCH 185/310] [parser/urdf] Correct urdf parser --- include/pinocchio/parsers/urdf/model.hxx | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 981b0c72dc..e547a12e3b 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -511,8 +511,9 @@ namespace pinocchio const Scalar offset) { auto mimicked_joint = model.joints[getJointId(mimicked_name)]; - auto mimic_joint = model.joints[getJointId(mimic_name)]; - + auto &mimic_joint = model.joints[getJointId(mimic_name)]; + int id = mimic_joint.id(); + int idx_j = mimic_joint.idx_j(); CartesianAxis axisType = extractCartesianAxis(axis); switch (axisType) { @@ -540,6 +541,7 @@ namespace pinocchio PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); break; } + mimic_joint.setIndexes(id, mimicked_joint.idx_q(), mimicked_joint.idx_v(), idx_j); } private: From 9f8fa73b2b716caea47b722a2f5086caa0003fee Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 23 Jan 2025 10:34:42 +0100 Subject: [PATCH 186/310] Apply pre-commit --- include/pinocchio/algorithm/regressor.hxx | 3 ++- include/pinocchio/algorithm/rnea-second-order-derivatives.hxx | 3 ++- include/pinocchio/parsers/urdf/model.hxx | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index 1bb76e7cde..e398125cd4 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -364,7 +364,8 @@ namespace pinocchio const Eigen::MatrixBase & a) { PINOCCHIO_THROW( - (std::is_same>::value == false), + (std::is_same>::value + == false), std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index 84192cdbba..d8ac68b294 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -48,7 +48,8 @@ namespace pinocchio const Eigen::MatrixBase & a) { PINOCCHIO_THROW( - (std::is_same>::value == false), + (std::is_same>::value + == false), std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index e547a12e3b..52df6fd7f2 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -511,7 +511,7 @@ namespace pinocchio const Scalar offset) { auto mimicked_joint = model.joints[getJointId(mimicked_name)]; - auto &mimic_joint = model.joints[getJointId(mimic_name)]; + auto & mimic_joint = model.joints[getJointId(mimic_name)]; int id = mimic_joint.id(); int idx_j = mimic_joint.idx_j(); CartesianAxis axisType = extractCartesianAxis(axis); From 09735a758fecc555cbcc65de0e249e763a91e2f0 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 24 Jan 2025 11:25:53 +0100 Subject: [PATCH 187/310] Renaming nj and idx_j into nvExtended and idx_vExtended to be conform to the new convention --- .../pinocchio/algorithm/center-of-mass.hxx | 4 +- include/pinocchio/algorithm/check.hxx | 6 +- .../algorithm/constrained-dynamics.hxx | 18 ++-- include/pinocchio/algorithm/crba.hxx | 6 +- include/pinocchio/algorithm/jacobian.hxx | 22 ++--- include/pinocchio/algorithm/model.hxx | 4 +- .../bindings/python/multibody/model.hpp | 6 +- include/pinocchio/multibody/data.hpp | 4 +- include/pinocchio/multibody/data.hxx | 6 +- .../multibody/joint/joint-basic-visitors.hpp | 6 +- .../multibody/joint/joint-basic-visitors.hxx | 38 ++++----- .../multibody/joint/joint-composite.hpp | 84 +++++++++---------- .../multibody/joint/joint-free-flyer.hpp | 6 +- .../multibody/joint/joint-generic.hpp | 14 ++-- .../joint/joint-helical-unaligned.hpp | 6 +- .../multibody/joint/joint-helical.hpp | 6 +- .../pinocchio/multibody/joint/joint-mimic.hpp | 30 +++---- .../multibody/joint/joint-model-base.hpp | 80 +++++++++--------- .../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 +- include/pinocchio/multibody/model.hpp | 12 +-- include/pinocchio/multibody/model.hxx | 26 +++--- include/pinocchio/parsers/urdf/model.hxx | 4 +- .../pinocchio/serialization/joints-model.hpp | 24 +++--- include/pinocchio/serialization/model.hpp | 6 +- unittest/casadi/algorithms.cpp | 2 +- unittest/joint-composite.cpp | 2 +- unittest/joint-generic.cpp | 2 +- unittest/joint-jacobian.cpp | 2 +- unittest/joint-mimic.cpp | 2 +- unittest/model.cpp | 2 +- 40 files changed, 248 insertions(+), 248 deletions(-) diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index a3f2d2b70c..4c865eaa6e 100644 --- a/include/pinocchio/algorithm/center-of-mass.hxx +++ b/include/pinocchio/algorithm/center-of-mass.hxx @@ -260,7 +260,7 @@ namespace pinocchio ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); Jcols = data.oMi[i].act(jdata.S()); - for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) + for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nvExtended(); ++col_id) { jmodel.jointCols(Jcom_).col(col_id) += data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR) @@ -370,7 +370,7 @@ namespace pinocchio ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); Jcols = data.oMi[i].act(jdata.S()); - for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nj(); ++col_id) + for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nvExtended(); ++col_id) { jmodel.jointCols(Jcom_).col(col_id) += Jcols.col(col_id).template segment<3>(Motion::LINEAR) diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index 766ab5a685..a9ea4c67a7 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -124,7 +124,7 @@ 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.nj); + CHECK_DATA(data.J.cols() == model.nvExtended); CHECK_DATA(data.Jcom.cols() == model.nv); CHECK_DATA(data.torque_residual.size() == model.nv); CHECK_DATA(data.dq_after.size() == model.nv); @@ -154,8 +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()); + CHECK_DATA(model.nvExtendeds[joint_id] == jmodel.nvExtended()); + CHECK_DATA(model.idx_vExtendeds[joint_id] == jmodel.idx_vExtended()); } for (JointIndex j = 1; int(j) < model.njoints; ++j) diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index 518a09bf97..01ede92913 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -119,7 +119,7 @@ namespace pinocchio if (parent > 0) ov += data.ov[parent]; - jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); oinertias = data.oMi[i].act(model.inertias[i]); data.oYcrb[i] = data.oinertias[i]; if (ContactMode) @@ -167,7 +167,7 @@ namespace pinocchio const JointIndex parent = model.parents[i]; ColsBlock dFda_cols = jmodel.jointCols(data.dFda); - const ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + const ColsBlock J_cols = jmodel.jointCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); jmodel.jointRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = @@ -566,7 +566,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); data.ov[i] = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -622,7 +622,7 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColBlock Jcols = jmodel.jointCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; @@ -684,7 +684,7 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColBlock Jcols = jmodel.jointCols(data.J); Force & fi_augmented = data.of_augmented[i]; const Force & fi = data.of[i]; @@ -723,7 +723,7 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColBlock Jcols = jmodel.jointCols(data.J); const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; @@ -1095,7 +1095,7 @@ namespace pinocchio // subtract the bias forces from the torque to get Mv_dot_free jmodel.jointVelocitySelector(data.tau).noalias() -= - jmodel.jointExtendedModelCols(data.J).transpose() * (data.of[i].toVector()); + jmodel.jointCols(data.J).transpose() * (data.of[i].toVector()); data.of_augmented[i].toVector().setZero(); } @@ -1119,7 +1119,7 @@ namespace pinocchio const JointModel & jmodel = model.joints[i]; data.oa_augmented[i].toVector().noalias() = data.oa_augmented[model.parents[i]].toVector() - + jmodel.jointExtendedModelCols(data.J) * jmodel.jointVelocitySelector(data.u); + + jmodel.jointCols(data.J) * jmodel.jointVelocitySelector(data.u); data.of_augmented[i].toVector().setZero(); } } @@ -1163,7 +1163,7 @@ namespace pinocchio data.of_augmented[parent] += data.of_augmented[i]; jmodel.jointVelocitySelector(data.tau).noalias() = - -jmodel.jointExtendedModelCols(data.J).transpose() * (data.of_augmented[i].toVector()); + -jmodel.jointCols(data.J).transpose() * (data.of_augmented[i].toVector()); } } diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index abbce93527..991a4270a1 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -224,10 +224,10 @@ namespace pinocchio { typedef JointModelMimicTpl JointModel; typedef typename Eigen::Matrix< - Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel::MaxNJ> + Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel::MaxNVExtended> SpatialForcesX; typedef JointDataTpl JointData; - typedef typename Eigen::Matrix + typedef typename Eigen::Matrix MotionSubspace; typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; typedef SE3Tpl SE3; @@ -241,7 +241,7 @@ namespace pinocchio size_t ancestor_prim, ancestor_sec; findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); - SpatialForcesX iF(6, jmodel.nj()); // Current joint forces + SpatialForcesX iF(6, jmodel.nvExtended()); // 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 diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index c1c3cf3468..632df7a827 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -185,7 +185,7 @@ 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.nj); + PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), model.nvExtended); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); @@ -204,9 +204,9 @@ namespace pinocchio case WORLD: { for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) { - const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_in = idx_vExtended(model.joints[j]) + i; const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; MotionIn v_in(Jin.col(col_in)); @@ -220,9 +220,9 @@ namespace pinocchio case LOCAL_WORLD_ALIGNED: { for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) { - const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_in = idx_vExtended(model.joints[j]) + i; const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; MotionIn v_in(Jin.col(col_in)); @@ -237,9 +237,9 @@ namespace pinocchio case LOCAL: { for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) { - for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) { - const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_in = idx_vExtended(model.joints[j]) + i; const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; MotionIn v_in(Jin.col(col_in)); @@ -512,9 +512,9 @@ namespace pinocchio const Motion & v_joint = data.v[jointId]; for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) { - for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) { - const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_in = idx_vExtended(model.joints[j]) + i; const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; @@ -535,9 +535,9 @@ namespace pinocchio const SE3 & oMjoint = data.oMi[jointId]; for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) { - for (int i = nj(model.joints[j]) - 1; i >= 0; i--) + for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) { - const Eigen::DenseIndex col_in = idx_j(model.joints[j]) + i; + const Eigen::DenseIndex col_in = idx_vExtended(model.joints[j]) + i; const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index adbc831aba..2e10010b02 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -178,7 +178,7 @@ namespace pinocchio const JointIndex mimicked_new_id = new_model.getJointId(mimicked_name); 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()); + new_model.joints[mimicked_new_id].idx_v(), new_model.joints[mimicked_new_id].idx_vExtended()); return res; } @@ -904,7 +904,7 @@ namespace pinocchio { 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()); + jmodel_output.setIndexes(jmodel_input.id(), idx_q, idx_v, jmodel_input.idx_vExtended()); output_model.idx_qs[joint_id] = jmodel_output.idx_q(); output_model.nqs[joint_id] = jmodel_output.nq(); diff --git a/include/pinocchio/bindings/python/multibody/model.hpp b/include/pinocchio/bindings/python/multibody/model.hpp index e3fac073c7..0825b0a334 100644 --- a/include/pinocchio/bindings/python/multibody/model.hpp +++ b/include/pinocchio/bindings/python/multibody/model.hpp @@ -71,7 +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("nvExtended", &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.") @@ -91,8 +91,8 @@ 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.") - .add_property("njs", &Model::njs, "Dimension of the *i*th joint jacobian subspace.") + "idx_vExtendeds", &Model::idx_vExtendeds, "Starting index of the *i*th joint in the jacobian space.") + .add_property("nvExtendeds", &Model::nvExtendeds, "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*, " diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index d19468315c..2e64fb4e80 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -354,8 +354,8 @@ namespace pinocchio /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and /// expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} /// \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = - /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} - /// S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian + /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nvExtended}} + /// S_{\text{nvExtended}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian /// of a precise joint, you need to call pinocchio::getJointJacobian Matrix6x J; diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 644f596306..804d4d1cfd 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.nj)) - , dJ(Matrix6x::Zero(6, model.nj)) - , ddJ(Matrix6x::Zero(6, model.nj)) + , J(Matrix6x::Zero(6, model.nvExtended)) + , dJ(Matrix6x::Zero(6, model.nvExtended)) + , ddJ(Matrix6x::Zero(6, model.nvExtended)) , psid(Matrix6x::Zero(6, model.nv)) , psidd(Matrix6x::Zero(6, model.nv)) , dVdq(Matrix6x::Zero(6, model.nv)) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index a70833836b..092b77c0da 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -153,7 +153,7 @@ namespace pinocchio * @return The dimension of joint jacobian space */ template class JointCollectionTpl> - inline int nj(const JointModelTpl & jmodel); + inline int nvExtended(const JointModelTpl & jmodel); /** * @brief Visit a JointModelTpl through JointConfigurationLimitVisitor @@ -213,7 +213,7 @@ namespace pinocchio * joint jacobian space degree */ template class JointCollectionTpl> - inline int idx_j(const JointModelTpl & jmodel); + inline int idx_vExtended(const JointModelTpl & jmodel); /** * @brief Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the @@ -247,7 +247,7 @@ namespace pinocchio JointIndex id, int q, int v, - int j); + int vExtended); /** * @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 91770cdebc..5ab0b517fd 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -270,25 +270,25 @@ namespace pinocchio /** * @brief JointNjVisitor visitor */ - struct JointNjVisitor : boost::static_visitor + struct JointNvExtendedVisitor : boost::static_visitor { template int operator()(const JointModelBase & jmodel) const { - return jmodel.nj(); + return jmodel.nvExtended(); } template class JointCollectionTpl> static int run(const JointModelTpl & jmodel) { - return boost::apply_visitor(JointNjVisitor(), jmodel); + return boost::apply_visitor(JointNvExtendedVisitor(), jmodel); } }; template class JointCollectionTpl> - inline int nj(const JointModelTpl & jmodel) + inline int nvExtended(const JointModelTpl & jmodel) { - return JointNjVisitor::run(jmodel); + return JointNvExtendedVisitor::run(jmodel); } /** @@ -394,25 +394,25 @@ namespace pinocchio /** * @brief JointIdxjVisitor visitor */ - struct JointIdxJVisitor : boost::static_visitor + struct JointIdxVExtendedVisitor : boost::static_visitor { template int operator()(const JointModelBase & jmodel) const { - return jmodel.idx_j(); + return jmodel.idx_vExtended(); } template class JointCollectionTpl> static int run(const JointModelTpl & jmodel) { - return boost::apply_visitor(JointIdxJVisitor(), jmodel); + return boost::apply_visitor(JointIdxVExtendedVisitor(), jmodel); } }; template class JointCollectionTpl> - inline int idx_j(const JointModelTpl & jmodel) + inline int idx_vExtended(const JointModelTpl & jmodel) { - return JointIdxJVisitor::run(jmodel); + return JointIdxVExtendedVisitor::run(jmodel); } /** @@ -447,20 +447,20 @@ namespace pinocchio JointIndex id; int q; int v; - int j; + int vExtended; - JointSetIndexesVisitor(JointIndex id, int q, int v, int j) + JointSetIndexesVisitor(JointIndex id, int q, int v, int vExtended) : id(id) , q(q) , v(v) - , j(j) + , vExtended(vExtended) { } template void operator()(JointModelBase & jmodel) const { - jmodel.setIndexes(id, q, v, j); + jmodel.setIndexes(id, q, v, vExtended); } template class JointCollectionTpl> @@ -469,17 +469,17 @@ namespace pinocchio JointIndex id, int q, int v, - int j) + int vExtended) { - return boost::apply_visitor(JointSetIndexesVisitor(id, q, v, j), jmodel); + return boost::apply_visitor(JointSetIndexesVisitor(id, q, v, vExtended), jmodel); } }; 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 vExtended) { - return JointSetIndexesVisitor::run(jmodel, id, q, v, j); + return JointSetIndexesVisitor::run(jmodel, id, q, v, vExtended); } /** @@ -965,7 +965,7 @@ namespace pinocchio typename boost::disable_if_c::value, JointModel>::type operator()(const T & value) const { - assert(false && "Type not supported in new variant"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Type not supported in new variant"); return value; } }; diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index e48e43be9e..ff4b821420 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -29,7 +29,7 @@ namespace pinocchio Options = _Options, NQ = Eigen::Dynamic, NV = Eigen::Dynamic, - NJ = Eigen::Dynamic + NVExtended = Eigen::Dynamic }; typedef JointCollectionTpl JointCollection; @@ -196,10 +196,10 @@ namespace pinocchio typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; - using Base::nj; + using Base::nvExtended; using Base::nq; using Base::nv; using Base::setIndexes; @@ -210,7 +210,7 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) - , m_nj(0) + , m_nvExtended(0) , njoints(0) { } @@ -221,17 +221,17 @@ namespace pinocchio , jointPlacements() , m_nq(0) , m_nv(0) - , m_nj(0) + , m_nvExtended(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_idx_vExtended.reserve(size); m_nqs.reserve(size); m_nvs.reserve(size); - m_njs.reserve(size); + m_nvExtendeds.reserve(size); } /// @@ -247,13 +247,13 @@ namespace pinocchio , jointPlacements(1, placement) , m_nq(jmodel.nq()) , m_nv(jmodel.nv()) - , m_nj(jmodel.nj()) + , m_nvExtended(jmodel.nvExtended()) , 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()) + , m_idx_vExtended(1, 0) + , m_nvExtendeds(1, jmodel.nvExtended()) , njoints(1) { } @@ -269,13 +269,13 @@ namespace pinocchio , jointPlacements(other.jointPlacements) , m_nq(other.m_nq) , m_nv(other.m_nv) - , m_nj(other.m_nj) + , m_nvExtended(other.m_nvExtended) , 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) + , m_idx_vExtended(other.m_idx_vExtended) + , m_nvExtendeds(other.m_nvExtendeds) , njoints(other.njoints) { } @@ -297,7 +297,7 @@ namespace pinocchio m_nq += jmodel.nq(); m_nv += jmodel.nv(); - m_nj += jmodel.nj(); + m_nvExtended += jmodel.nvExtended(); updateJointIndexes(); njoints++; @@ -383,17 +383,17 @@ namespace pinocchio { return m_nq; } - int nj_impl() const + int nvExtended_impl() const { - return m_nj; + return m_nvExtended; } /** * @brief Update the indexes of subjoints in the stack */ - void setIndexes_impl(JointIndex id, int q, int v, int j) + void setIndexes_impl(JointIndex id, int q, int v, int vExtended) { - Base::setIndexes_impl(id, q, v, j); + Base::setIndexes_impl(id, q, v, vExtended); updateJointIndexes(); } @@ -411,13 +411,13 @@ namespace pinocchio Base::operator=(other); m_nq = other.m_nq; m_nv = other.m_nv; - m_nj = other.m_nj; + m_nvExtended = other.m_nvExtended; m_idx_q = other.m_idx_q; m_idx_v = other.m_idx_v; - m_idx_j = other.m_idx_j; + m_idx_vExtended = other.m_idx_vExtended; m_nqs = other.m_nqs; m_nvs = other.m_nvs; - m_njs = other.m_njs; + m_nvExtendeds = other.m_nvExtendeds; joints = other.joints; jointPlacements = other.jointPlacements; njoints = other.njoints; @@ -430,13 +430,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(nvExtended(), other.nvExtended()) && 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_idx_vExtended, other.m_idx_vExtended) && 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(m_nvExtendeds, other.m_nvExtendeds) && internal::comparison_eq(joints, other.joints) && internal::comparison_eq(jointPlacements, other.jointPlacements) && internal::comparison_eq(njoints, other.njoints); @@ -448,16 +448,16 @@ namespace pinocchio { typedef JointModelCompositeTpl ReturnType; ReturnType res((size_t)njoints); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); res.m_nq = m_nq; res.m_nv = m_nv; - res.m_nj = m_nj; + res.m_nvExtended = m_nvExtended; res.m_idx_q = m_idx_q; res.m_idx_v = m_idx_v; - res.m_idx_j = m_idx_j; + res.m_idx_vExtended = m_idx_vExtended; res.m_nqs = m_nqs; res.m_nvs = m_nvs; - res.m_njs = m_njs; + res.m_nvExtendeds = m_nvExtendeds; res.njoints = njoints; res.joints.resize(joints.size()); @@ -539,7 +539,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointExtendedModelCols(const Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_j, nj()); + return A.middleCols(Base::i_vExtended, nvExtended()); } template typename SizeDepType::template ColsReturn::Type jointCols(Eigen::MatrixBase & A) const @@ -550,7 +550,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type jointExtendedModelCols(Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_j, nj()); + return A.middleCols(Base::i_vExtended, nvExtended()); } template @@ -615,7 +615,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointExtendedModelCols_impl(const Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_j, nj()); + return A.middleCols(Base::i_vExtended, nvExtended()); } template typename SizeDepType::template ColsReturn::Type @@ -627,7 +627,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type jointExtendedModelCols_impl(Eigen::MatrixBase & A) const { - return A.middleCols(Base::i_j, nj()); + return A.middleCols(Base::i_vExtended, nvExtended()); } protected: @@ -642,14 +642,14 @@ namespace pinocchio { int idx_q = this->idx_q(); int idx_v = this->idx_v(); - int idx_j = this->idx_j(); + int idx_vExtended = this->idx_vExtended(); m_idx_q.resize(joints.size()); m_idx_v.resize(joints.size()); - m_idx_j.resize(joints.size()); + m_idx_vExtended.resize(joints.size()); m_nqs.resize(joints.size()); m_nvs.resize(joints.size()); - m_njs.resize(joints.size()); + m_nvExtendeds.resize(joints.size()); for (size_t i = 0; i < joints.size(); ++i) { @@ -657,19 +657,19 @@ namespace pinocchio m_idx_q[i] = idx_q; m_idx_v[i] = idx_v; - m_idx_j[i] = idx_j; - ::pinocchio::setIndexes(joint, i, idx_q, idx_v, idx_j); + m_idx_vExtended[i] = idx_vExtended; + ::pinocchio::setIndexes(joint, i, idx_q, idx_v, idx_vExtended); m_nqs[i] = ::pinocchio::nq(joint); m_nvs[i] = ::pinocchio::nv(joint); - m_njs[i] = ::pinocchio::nj(joint); + m_nvExtendeds[i] = ::pinocchio::nvExtended(joint); idx_q += m_nqs[i]; idx_v += m_nvs[i]; - idx_j += m_njs[i]; + idx_vExtended += m_nvExtendeds[i]; } } /// \brief Dimensions of the config and tangent space of the composite joint. - int m_nq, m_nv, m_nj; + int m_nq, m_nv, m_nvExtended; /// Keep information of both the dimension and the position of the joints in the composition. @@ -682,9 +682,9 @@ namespace pinocchio /// \brief Dimension of the segment in the tangent vector std::vector m_nvs; /// \brief Index in the jacobian matrix - std::vector m_idx_j; + std::vector m_idx_vExtended; /// \brief Dimension of the segment in the jacobian matrix - std::vector m_njs; + std::vector m_nvExtendeds; 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 5d3ca7b47b..1a44fef729 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -169,7 +169,7 @@ namespace pinocchio { NQ = 7, NV = 6, - NJ = 6 + NVExtended = 6 }; typedef _Scalar Scalar; enum @@ -290,7 +290,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -406,7 +406,7 @@ namespace pinocchio { typedef JointModelFreeFlyerTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index b10ffbf8b3..2ef91fea6f 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -30,7 +30,7 @@ namespace pinocchio Options = _Options, NQ = Eigen::Dynamic, // Dynamic because unknown at compile time NV = Eigen::Dynamic, - NJ = Eigen::Dynamic + NVExtended = Eigen::Dynamic }; typedef _Scalar Scalar; @@ -424,9 +424,9 @@ namespace pinocchio { return ::pinocchio::nv(*this); } - int nj_impl() const + int nvExtended_impl() const { - return ::pinocchio::nj(*this); + return ::pinocchio::nvExtended(*this); } int idx_q_impl() const @@ -437,9 +437,9 @@ namespace pinocchio { return ::pinocchio::idx_v(*this); } - int idx_j_impl() const + int idx_vExtended_impl() const { - return ::pinocchio::idx_j(*this); + return ::pinocchio::idx_vExtended(*this); } JointIndex id_impl() const @@ -447,9 +447,9 @@ namespace pinocchio return ::pinocchio::id(*this); } - void setIndexes(JointIndex id, int nq, int nv, int nj) + void setIndexes(JointIndex id, int nq, int nv, int nvExtended) { - ::pinocchio::setIndexes(*this, id, nq, nv, nj); + ::pinocchio::setIndexes(*this, id, nq, nv, nvExtended); } /// \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 55d2989e89..93100b7743 100644 --- a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp @@ -525,7 +525,7 @@ namespace pinocchio { NQ = 1, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -639,7 +639,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -762,7 +762,7 @@ namespace pinocchio { typedef JointModelHelicalUnalignedTpl ReturnType; ReturnType res(axis.template cast(), ScalarCast::cast(m_pitch)); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index 94ea970661..ed3bae345c 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -736,7 +736,7 @@ namespace pinocchio { NQ = 1, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -838,7 +838,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -951,7 +951,7 @@ namespace pinocchio { typedef JointModelHelicalTpl ReturnType; ReturnType res(ScalarCast::cast(m_pitch)); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 5e56e049e9..52f5294b84 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -312,14 +312,14 @@ namespace pinocchio Options = _Options, NQ = Eigen::Dynamic, NV = Eigen::Dynamic, - NJ = Eigen::Dynamic, - MaxNJ = 6 + NVExtended = Eigen::Dynamic, + MaxNVExtended = 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; @@ -611,7 +611,7 @@ namespace pinocchio PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); enum { - MaxNJ = traits::MaxNJ + MaxNVExtended = traits::MaxNVExtended }; typedef JointCollectionTpl JointCollection; @@ -623,10 +623,10 @@ namespace pinocchio typedef InertiaTpl Inertia; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; - using Base::nj; + using Base::nvExtended; using Base::nq; using Base::nv; using Base::setIndexes; @@ -654,11 +654,11 @@ namespace pinocchio { assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); - assert(jmodel_mimicking.nj() == jmodel_mimicked.nj()); + assert(jmodel_mimicking.nvExtended() == jmodel_mimicked.nvExtended()); setMimicIndexes( jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(), - jmodel_mimicked.idx_j()); + jmodel_mimicked.idx_vExtended()); } Base & base() @@ -678,26 +678,26 @@ namespace pinocchio { return 0; } - inline int nj_impl() const + inline int nvExtended_impl() const { - return m_jmodel_ref.nj(); + return m_jmodel_ref.nvExtended(); } - void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int j) + void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended) { 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; + Base::i_vExtended = vExtended; } // 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) + void setMimicIndexes(JointIndex id, int q, int v, int vExtended) { // 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); + m_jmodel_ref.setIndexes(id, 0, 0, vExtended); // idx_q, idx_v kept separately Base::i_q = q; Base::i_v = v; @@ -779,7 +779,7 @@ namespace pinocchio ReturnType res( m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), ScalarCast::cast(m_offset)); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index fc46d57d65..619709120e 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -31,7 +31,7 @@ Options = traits::Options, \ NQ = traits::NQ, \ NV = traits::NV, \ - NJ = traits::NJ \ + NVExtended = traits::NVExtended \ }; \ typedef TYPENAME traits::ConfigVector_t ConfigVector_t; \ typedef TYPENAME traits::TangentVector_t TangentVector_t @@ -62,7 +62,7 @@ typedef JointModelBase Base; \ using Base::idx_q; \ using Base::idx_v; \ - using Base::idx_j + using Base::idx_vExtended #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \ template \ @@ -149,9 +149,9 @@ namespace pinocchio { return derived().nq_impl(); } - int nj() const + int nvExtended() const { - return derived().nj_impl(); + return derived().nvExtended_impl(); } // Default _impl methods are reimplemented by dynamic-size joints. @@ -163,9 +163,9 @@ namespace pinocchio { return NQ; } - int nj_impl() const + int nvExtended_impl() const { - return NJ; + return NVExtended; } int idx_q() const @@ -176,9 +176,9 @@ namespace pinocchio { return derived().idx_v_impl(); } - int idx_j() const + int idx_vExtended() const { - return derived().idx_j_impl(); + return derived().idx_vExtended_impl(); } JointIndex id() const { @@ -193,25 +193,25 @@ namespace pinocchio { return i_v; } - int idx_j_impl() const + int idx_vExtended_impl() const { - return i_j; + return i_vExtended; } JointIndex id_impl() const { return i_id; } - void setIndexes(JointIndex id, int q, int v, int j) + void setIndexes(JointIndex id, int q, int v, int vExtended) { - derived().setIndexes_impl(id, q, v, j); + derived().setIndexes_impl(id, q, v, vExtended); } - void setIndexes_impl(JointIndex id, int q, int v, int j) + void setIndexes_impl(JointIndex id, int q, int v, int vExtended) { i_id = id, i_q = q; i_v = v; - i_j = j; + i_vExtended = vExtended; } void disp(std::ostream & os) const @@ -221,10 +221,10 @@ namespace pinocchio << " index: " << id() << endl << " index q: " << idx_q() << endl << " index v: " << idx_v() << endl - << " index j: " << idx_j() << endl + << " index vExtended: " << idx_vExtended() << endl << " nq: " << nq() << endl << " nv: " << nv() << endl - << " nj: " << nj() << endl; + << " nvExtended: " << nvExtended() << endl; } friend std::ostream & operator<<(std::ostream & os, const JointModelBase & joint) @@ -277,7 +277,7 @@ 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_j(), idx_j()); + && internal::comparison_eq(other.idx_vExtended(), idx_vExtended()); } /* Acces to dedicated segment in robot config space. */ @@ -354,7 +354,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointVelocityExtendedModelSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), idx_v(), nj()); + return SizeDepType::segment(a.derived(), idx_v(), nvExtended()); } // Non-const access @@ -369,7 +369,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointVelocityExtendedModelSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), idx_v(), nj()); + return SizeDepType::segment(a.derived(), idx_v(), nvExtended()); } // Const access @@ -412,7 +412,7 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::ConstType + typename SizeDepType::template ColsReturn::ConstType jointExtendedModelCols(const Eigen::MatrixBase & A) const { return derived().jointExtendedModelCols_impl(A.derived()); @@ -426,10 +426,10 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::ConstType + typename SizeDepType::template ColsReturn::ConstType jointExtendedModelCols_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_j(), nj()); + return SizeDepType::middleCols(A.derived(), idx_vExtended(), nvExtended()); } // Non-const access @@ -441,7 +441,7 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::Type + typename SizeDepType::template ColsReturn::Type jointExtendedModelCols(Eigen::MatrixBase & A) const { return derived().jointExtendedModelCols_impl(A.derived()); @@ -455,10 +455,10 @@ namespace pinocchio } template - typename SizeDepType::template ColsReturn::Type + typename SizeDepType::template ColsReturn::Type jointExtendedModelCols_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_j(), nj()); + return SizeDepType::middleCols(A.derived(), idx_vExtended(), nvExtended()); } /* Acces to dedicated rows in a matrix.*/ @@ -471,7 +471,7 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::ConstType + typename SizeDepType::template RowsReturn::ConstType jointExtendedModelRows(const Eigen::MatrixBase & A) const { return derived().jointExtendedModelRows_impl(A.derived()); @@ -485,10 +485,10 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::ConstType + typename SizeDepType::template RowsReturn::ConstType jointExtendedModelRows_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_j(), nj()); + return SizeDepType::middleRows(A.derived(), idx_vExtended(), nvExtended()); } // Non-const access @@ -499,7 +499,7 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::Type + typename SizeDepType::template RowsReturn::Type jointExtendedModelRows(Eigen::MatrixBase & A) const { return derived().jointExtendedModelRows_impl(A.derived()); @@ -513,10 +513,10 @@ namespace pinocchio } template - typename SizeDepType::template RowsReturn::Type + typename SizeDepType::template RowsReturn::Type jointExtendedModelRows_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_j(), nj()); + return SizeDepType::middleRows(A.derived(), idx_vExtended(), nvExtended()); } /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the @@ -530,7 +530,7 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::ConstType + typename SizeDepType::template BlockReturn::ConstType jointExtendedModelBlock(const Eigen::MatrixBase & Mat) const { return derived().jointExtendedModelBlock_impl(Mat.derived()); @@ -544,10 +544,10 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::ConstType + typename SizeDepType::template BlockReturn::ConstType jointExtendedModelBlock_impl(const Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); + return SizeDepType::block(Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); } // Non-const access @@ -559,7 +559,7 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::Type + typename SizeDepType::template BlockReturn::Type jointExtendedModelBlock(Eigen::MatrixBase & Mat) const { return derived().jointExtendedModelBlock_impl(Mat.derived()); @@ -573,10 +573,10 @@ namespace pinocchio } template - typename SizeDepType::template BlockReturn::Type + typename SizeDepType::template BlockReturn::Type jointExtendedModelBlock_impl(Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_j(), idx_j(), nj(), nj()); + return SizeDepType::block(Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); } protected: @@ -587,7 +587,7 @@ namespace pinocchio : i_id(std::numeric_limits::max()) , i_q(-1) , i_v(-1) - , i_j(-1) + , i_vExtended(-1) { } @@ -609,7 +609,7 @@ namespace pinocchio i_id = clone.i_id; i_q = clone.i_q; i_v = clone.i_v; - i_j = clone.i_j; + i_vExtended = clone.i_vExtended; return *this; } @@ -617,7 +617,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. + int i_vExtended; // 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 96e7c56f3c..4e3d02b583 100644 --- a/include/pinocchio/multibody/joint/joint-planar.hpp +++ b/include/pinocchio/multibody/joint/joint-planar.hpp @@ -472,7 +472,7 @@ namespace pinocchio { NQ = 4, NV = 3, - NJ = 3 + NVExtended = 3 }; enum { @@ -567,7 +567,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -676,7 +676,7 @@ namespace pinocchio { typedef JointModelPlanarTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp index 614abb07b1..ad990df4c0 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp @@ -479,7 +479,7 @@ namespace pinocchio { NQ = 1, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -592,7 +592,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -698,7 +698,7 @@ namespace pinocchio { typedef JointModelPrismaticUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-prismatic.hpp b/include/pinocchio/multibody/joint/joint-prismatic.hpp index 08c33a63c8..27cfbde06a 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -571,7 +571,7 @@ namespace pinocchio { NQ = 1, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -674,7 +674,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -770,7 +770,7 @@ namespace pinocchio { typedef JointModelPrismaticTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp index eb039f9a3e..fb2741724a 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp @@ -504,7 +504,7 @@ namespace pinocchio { NQ = 1, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -618,7 +618,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -722,7 +722,7 @@ namespace pinocchio { typedef JointModelRevoluteUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); 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 8211eaa935..c4a465f597 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -25,7 +25,7 @@ namespace pinocchio { NQ = 2, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -142,7 +142,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -250,7 +250,7 @@ namespace pinocchio { typedef JointModelRevoluteUnboundedUnalignedTpl ReturnType; ReturnType res(axis.template cast()); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp index 619ae467c2..1e3d432896 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -24,7 +24,7 @@ namespace pinocchio { NQ = 2, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -128,7 +128,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -228,7 +228,7 @@ namespace pinocchio { typedef JointModelRevoluteUnboundedTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-revolute.hpp b/include/pinocchio/multibody/joint/joint-revolute.hpp index 7be27ad828..49340bd77d 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -660,7 +660,7 @@ namespace pinocchio { NQ = 1, NV = 1, - NJ = 1 + NVExtended = 1 }; typedef _Scalar Scalar; enum @@ -762,7 +762,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -866,7 +866,7 @@ namespace pinocchio { typedef JointModelRevoluteTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp index 63bf442979..51bfc1b1ca 100644 --- a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp @@ -280,7 +280,7 @@ namespace pinocchio { NQ = 3, NV = 3, - NJ = 3 + NVExtended = 3 }; typedef _Scalar Scalar; enum @@ -380,7 +380,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -504,7 +504,7 @@ namespace pinocchio { typedef JointModelSphericalZYXTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-spherical.hpp b/include/pinocchio/multibody/joint/joint-spherical.hpp index 9ce0600f8b..2d10082c6c 100644 --- a/include/pinocchio/multibody/joint/joint-spherical.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical.hpp @@ -383,7 +383,7 @@ namespace pinocchio { NQ = 4, NV = 3, - NJ = 3 + NVExtended = 3 }; typedef _Scalar Scalar; enum @@ -481,7 +481,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -604,7 +604,7 @@ namespace pinocchio { typedef JointModelSphericalTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index 041607916d..1ac10dbe96 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -480,7 +480,7 @@ namespace pinocchio { NQ = 3, NV = 3, - NJ = 3 + NVExtended = 3 }; typedef _Scalar Scalar; enum @@ -578,7 +578,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -661,7 +661,7 @@ namespace pinocchio { typedef JointModelTranslationTpl ReturnType; ReturnType res; - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/joint/joint-universal.hpp b/include/pinocchio/multibody/joint/joint-universal.hpp index 54e797a1ab..1f24b77193 100644 --- a/include/pinocchio/multibody/joint/joint-universal.hpp +++ b/include/pinocchio/multibody/joint/joint-universal.hpp @@ -314,7 +314,7 @@ namespace pinocchio { NQ = 2, NV = 2, - NJ = 2 + NVExtended = 2 }; typedef _Scalar Scalar; enum @@ -414,7 +414,7 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_j; + using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; using Base::setIndexes; @@ -604,7 +604,7 @@ namespace pinocchio { typedef JointModelUniversalTpl ReturnType; ReturnType res(axis1.template cast(), axis2.template cast()); - res.setIndexes(id(), idx_q(), idx_v(), idx_j()); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); return res; } diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index 1936847e2f..16cee1df8e 100644 --- a/include/pinocchio/multibody/model.hpp +++ b/include/pinocchio/multibody/model.hpp @@ -101,7 +101,7 @@ namespace pinocchio int nv; /// \brief Dimension of the jacobian space. - int nj; + int nvExtended; /// \brief Number of joints. int njoints; @@ -134,10 +134,10 @@ namespace pinocchio std::vector nvs; /// \brief Starting index of the *i*th joint in the jacobian space. - std::vector idx_js; + std::vector idx_vExtendeds; /// \brief Dimension of the *i*th joint jacobian subspace. - std::vector njs; + std::vector nvExtendeds; /// \brief Vector of parent joint indexes. The parent of joint *i*, denoted *li*, corresponds to /// li==parents[i]. @@ -209,7 +209,7 @@ namespace pinocchio ModelTpl() : nq(0) , nv(0) - , nj(0) + , nvExtended(0) , njoints(1) , nbodies(1) , nframes(0) @@ -220,8 +220,8 @@ namespace pinocchio , nqs(1, 0) , idx_vs(1, 0) , nvs(1, 0) - , idx_js(1, 0) - , njs(1, 0) + , idx_vExtendeds(1, 0) + , nvExtendeds(1, 0) , parents(1, 0) , children(1) , names(1) diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index bd809a815b..fd63a5968b 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) && (joint_model.nj() >= 0)); + assert((joint_model.nq() >= 0) && (joint_model.nv() >= 0) && (joint_model.nvExtended() >= 0)); assert(joint_model.nq() >= joint_model.nv()); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -97,18 +97,18 @@ namespace pinocchio joints.push_back(JointModel(joint_model.derived())); JointModel & jmodel = joints.back(); - jmodel.setIndexes(joint_id, nq, nv, nj); + jmodel.setIndexes(joint_id, nq, nv, nvExtended); 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(); + const int joint_nvExtended = jmodel.nvExtended(); + const int joint_idx_vExtended = jmodel.idx_vExtended(); assert(joint_idx_q >= 0); assert(joint_idx_v >= 0); - assert(joint_idx_j >= 0); + assert(joint_idx_vExtended >= 0); inertias.push_back(Inertia::Zero()); parents.push_back(parent); @@ -123,9 +123,9 @@ 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); + nvExtended += joint_nvExtended; + nvExtendeds.push_back(joint_nvExtended); + idx_vExtendeds.push_back(joint_idx_vExtended); effortLimit.conservativeResize(nv); jmodel.jointVelocitySelector(effortLimit) = max_effort; @@ -233,7 +233,7 @@ namespace pinocchio ReturnType res; res.nq = nq; res.nv = nv; - res.nj = nj; + res.nvExtended = nvExtended; res.njoints = njoints; res.nbodies = nbodies; res.nframes = nframes; @@ -249,8 +249,8 @@ namespace pinocchio res.nqs = nqs; res.idx_vs = idx_vs; res.nvs = nvs; - res.idx_js = idx_js; - res.njs = njs; + res.idx_vExtendeds = idx_vExtendeds; + res.nvExtendeds = nvExtendeds; // Eigen Vectors res.armature = armature.template cast(); res.friction = friction.template cast(); @@ -294,13 +294,13 @@ namespace pinocchio template class JointCollectionTpl> bool ModelTpl::operator==(const ModelTpl & other) const { - bool res = other.nq == nq && other.nv == nv && other.nj == nj && other.njoints == njoints + bool res = other.nq == nq && other.nv == nv && other.nvExtended == nvExtended && 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 - && other.idx_js == idx_js && other.njs == njs; + && other.idx_vExtendeds == idx_vExtendeds && other.nvExtendeds == nvExtendeds; if (other.referenceConfigurations.size() != referenceConfigurations.size()) return false; diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 52df6fd7f2..9662644479 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -513,7 +513,7 @@ namespace pinocchio auto mimicked_joint = model.joints[getJointId(mimicked_name)]; auto & mimic_joint = model.joints[getJointId(mimic_name)]; int id = mimic_joint.id(); - int idx_j = mimic_joint.idx_j(); + int idx_vExtended = mimic_joint.idx_vExtended(); CartesianAxis axisType = extractCartesianAxis(axis); switch (axisType) { @@ -541,7 +541,7 @@ namespace pinocchio PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type."); break; } - mimic_joint.setIndexes(id, mimicked_joint.idx_q(), mimicked_joint.idx_v(), idx_j); + mimic_joint.setIndexes(id, mimicked_joint.idx_q(), mimicked_joint.idx_v(), idx_vExtended); } private: diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 4791ff444d..4564eba7e0 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -18,13 +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_nvExtended", joint.m_nvExtended); 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("m_idx_vExtended", joint.m_idx_vExtended); + ar & make_nvp("m_nvExtendeds", joint.m_nvExtendeds); ar & make_nvp("njoints", joint.njoints); ar & make_nvp("joints", joint.joints); @@ -57,12 +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(), i_j = joint.idx_j(); + const int i_q = joint.idx_q(), i_v = joint.idx_v(), i_vExtended = joint.idx_vExtended(); 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); + ar & make_nvp("i_vExtended", i_vExtended); } template @@ -74,9 +74,9 @@ namespace boost explicit SetJointIndexes(Derived & joint) : joint(joint) {}; - void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) + void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_vExtended) { - joint.setIndexes(i_id, i_q, i_v, i_j); + joint.setIndexes(i_id, i_q, i_v, i_vExtended); } }; @@ -90,9 +90,9 @@ namespace boost pinocchio::JointModelMimicTpl & joint) : joint_m(joint) {}; - void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_j) + void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_vExtended) { - joint_m.setIndexes(i_id, 0, 0, i_j); + joint_m.setIndexes(i_id, 0, 0, i_vExtended); joint_m.setMimicIndexes(0, i_q, i_v, 0); } }; @@ -102,14 +102,14 @@ namespace boost load(Archive & ar, pinocchio::JointModelBase & joint, const unsigned int /*version*/) { pinocchio::JointIndex i_id; - int i_q, i_v, i_j; + int i_q, i_v, i_vExtended; 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); + ar & make_nvp("i_vExtended", i_vExtended); - SetJointIndexes(joint.derived()).run(i_id, i_q, i_v, i_j); + SetJointIndexes(joint.derived()).run(i_id, i_q, i_v, i_vExtended); } template diff --git a/include/pinocchio/serialization/model.hpp b/include/pinocchio/serialization/model.hpp index 65f82659ee..b07f4221ca 100644 --- a/include/pinocchio/serialization/model.hpp +++ b/include/pinocchio/serialization/model.hpp @@ -37,9 +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("nvExtended", model.nvExtended); + ar & make_nvp("nvExtendeds", model.nvExtendeds); + ar & make_nvp("idx_vExtendeds", model.idx_vExtendeds); ar & make_nvp("njoints", model.njoints); ar & make_nvp("nbodies", model.nbodies); ar & make_nvp("nframes", model.nframes); diff --git a/unittest/casadi/algorithms.cpp b/unittest/casadi/algorithms.cpp index cde719fce7..8f71e0e864 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.nj), jacobian_world(6, model.nj); + Data::Matrix6x jacobian_local(6, model.nvExtended), jacobian_world(6, model.nvExtended); jacobian_local.setZero(); jacobian_world.setZero(); diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 94d9c48e0c..368dea9c0c 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.idx_j()); + jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v(), jmodel.idx_vExtended()); typedef typename JointModel::ConfigVector_t ConfigVector_t; typedef typename JointModel::TangentVector_t TangentVector_t; diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 844f2a6376..4a0ae184ab 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -333,7 +333,7 @@ namespace pinocchio Options = _Options, NQ = Eigen::Dynamic, // Dynamic because unknown at compile time NV = Eigen::Dynamic, - NJ = Eigen::Dynamic + NVExtended = Eigen::Dynamic }; typedef _Scalar Scalar; diff --git a/unittest/joint-jacobian.cpp b/unittest/joint-jacobian.cpp index 77ea9f2073..94cca9e990 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, false, true); pinocchio::Data data(model); VectorXd q = VectorXd::Zero(model.nq); diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index d5cbe2c5ae..146d7a55c4 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(); diff --git a/unittest/model.cpp b/unittest/model.cpp index c791091f60..4f86535f17 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -897,7 +897,7 @@ BOOST_AUTO_TEST_CASE(test_has_transform_to_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_model.nvExtended == humanoid_mimic.nvExtended); BOOST_CHECK(humanoid_mimic.njoints == humanoid_model.njoints); for (int i = 1; i < humanoid_model.names.size(); i++) From 05925c1296d7c1f89167fc60e0037bd249095b62 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 24 Jan 2025 11:03:35 +0100 Subject: [PATCH 188/310] [EtienneAr feedback] Revert all changes in unsupported algorithms --- .../pinocchio/algorithm/aba-derivatives.hxx | 16 +++---- include/pinocchio/algorithm/aba.hxx | 45 +++++++++---------- .../algorithm/centroidal-derivatives.hxx | 9 ++-- .../pinocchio/algorithm/compute-all-terms.hxx | 13 +++--- .../constrained-dynamics-derivatives.hxx | 12 ++--- .../algorithm/constrained-dynamics.hxx | 2 +- include/pinocchio/algorithm/delassus.hxx | 14 +++--- include/pinocchio/algorithm/energy.hxx | 6 +-- .../impulse-dynamics-derivatives.hxx | 5 +-- .../algorithm/kinematics-derivatives.hxx | 19 ++++---- include/pinocchio/algorithm/pv.hxx | 44 +++++++++--------- include/pinocchio/algorithm/regressor.hxx | 2 +- .../pinocchio/algorithm/rnea-derivatives.hxx | 19 ++++---- .../rnea-second-order-derivatives.hxx | 12 +++-- include/pinocchio/algorithm/rnea.hxx | 4 +- 15 files changed, 103 insertions(+), 119 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 0379a0e72c..5d4a1a7309 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -79,7 +79,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); J_cols = data.oMi[i].act(jdata.S()); } }; @@ -124,7 +124,7 @@ namespace pinocchio Force & fi = data.of[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - const ColBlock J_cols = jmodel.jointExtendedModelCols(data.J); + const ColBlock J_cols = jmodel.jointCols(data.J); jmodel.jointVelocitySelector(data.u).noalias() -= J_cols.transpose() * fi.toVector(); @@ -209,7 +209,7 @@ namespace pinocchio typename Data::Motion & oa_gf = data.oa_gf[i]; typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); oa_gf += data.oa_gf[parent]; jmodel.jointVelocitySelector(data.ddq).noalias() = @@ -238,7 +238,7 @@ namespace pinocchio data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + 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); @@ -308,7 +308,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + 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); @@ -583,7 +583,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColBlock; - const ColBlock J_cols = jmodel.jointExtendedModelCols(data.J); + const ColBlock J_cols = jmodel.jointCols(data.J); MatrixType & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType, Minv); @@ -654,7 +654,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - const ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + const ColsBlock J_cols = jmodel.jointCols(data.J); // Already done in optimized::aba // oa = oa_gf + model.gravity; @@ -679,7 +679,7 @@ namespace pinocchio data.Fcrb[i].rightCols(model.nv - jmodel.idx_v()) += data.Fcrb[parent].rightCols(model.nv - jmodel.idx_v()); - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + 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); diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index e24b4d4cd5..9398e4a455 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -125,7 +125,7 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); ov = data.oMi[i].act(jdata.v()); if (parent > 0) @@ -170,18 +170,17 @@ namespace pinocchio typename Inertia::Matrix6 & Ia = data.oYaba[i]; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColBlock Jcols = jmodel.jointCols(data.J); Force & fi = data.of[i]; - jmodel.jointVelocityExtendedModelSelector(data.u).noalias() -= - Jcols.transpose() * fi.toVector(); + jmodel.jointVelocitySelector(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.jointVelocityExtendedModelSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -191,8 +190,7 @@ namespace pinocchio Ia.noalias() -= jdata.UDinv() * jdata.U().transpose(); fi.toVector().noalias() += - Ia * data.oa_gf[i].toVector() - + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); + Ia * data.oa_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); data.oYaba[parent] += Ia; data.of[parent] += fi; } @@ -221,17 +219,16 @@ namespace pinocchio typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; - ColBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColBlock J_cols = jmodel.jointCols(data.J); const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; data.oa_gf[i] += data.oa_gf[parent]; // does take into account the gravity field - jmodel.jointVelocityExtendedModelSelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocityExtendedModelSelector(data.u) + jmodel.jointVelocitySelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.oa_gf[i].toVector(); - data.oa_gf[i].toVector().noalias() += - J_cols * jmodel.jointVelocityExtendedModelSelector(data.ddq); + data.oa_gf[i].toVector().noalias() += J_cols * jmodel.jointVelocitySelector(data.ddq); // Handle consistent output data.oa[i] = data.oa_gf[i] + model.gravity; @@ -436,17 +433,15 @@ namespace pinocchio const JointIndex parent = model.parents[i]; typename Inertia::Matrix6 & Ia = data.Yaba[i]; - jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia, - parent > 0); + jdata.derived(), jmodel.jointVelocitySelector(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.jointVelocityExtendedModelSelector(data.u); + Ia * data.a_gf[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); data.Yaba[parent] += internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -476,10 +471,10 @@ namespace pinocchio const JointIndex parent = model.parents[i]; data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); - jmodel.jointVelocityExtendedModelSelector(data.ddq).noalias() = - jdata.Dinv() * jmodel.jointVelocityExtendedModelSelector(data.u) + jmodel.jointVelocitySelector(data.ddq).noalias() = + jdata.Dinv() * jmodel.jointVelocitySelector(data.u) - jdata.UDinv().transpose() * data.a_gf[i].toVector(); - data.a_gf[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.ddq); + data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); data.a[i] = data.a_gf[i]; data.a[i].linear().noalias() += data.oMi[i].rotation().transpose() * model.gravity.linear(); @@ -656,7 +651,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); J_cols = data.oMi[i].act(jdata.S()); data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); @@ -694,13 +689,13 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); jdata.U().noalias() = Ia * J_cols; jdata.StU().noalias() = J_cols.transpose() * jdata.U(); // Account for the rotor inertia contribution - jdata.StU().diagonal() += jmodel.jointVelocityExtendedModelSelector(model.armature); + jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); ::pinocchio::internal::PerformStYSInversion::run(jdata.StU(), jdata.Dinv()); jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); @@ -768,7 +763,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - const ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + const ColsBlock J_cols = jmodel.jointCols(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(); @@ -824,7 +819,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); if (parent > 0) { diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 7818e980eb..8ee3a3873e 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -98,8 +98,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + 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); @@ -153,7 +153,6 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { - typedef typename Model::JointIndex JointIndex; const JointIndex & i = jmodel.id(); @@ -163,7 +162,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + 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); @@ -385,7 +384,7 @@ namespace pinocchio typename Data::Motion & vtmp = data.v[0]; typename Data::Matrix6x & Ftmp = data.Fcrb[0]; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + 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); diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 62cacad5ef..0f11dd1613 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -74,10 +74,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.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); J_cols = data.oMi[i].act(jdata.S()); - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointCols(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()); @@ -120,8 +120,8 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + 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); @@ -133,10 +133,10 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dAg_cols); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ - jmodel.jointRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = + data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); - jmodel.jointVelocityExtendedModelSelector(data.nle) += jdata.S().transpose() * data.f[i]; + jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i]; data.oYcrb[parent] += data.oYcrb[i]; data.doYcrb[parent] += data.doYcrb[i]; @@ -169,7 +169,6 @@ 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 60a350bd1c..2467264011 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -55,13 +55,13 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); if (ContactMode) { const Motion & ov = data.ov[i]; - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + ColsBlock dJ_cols = jmodel.jointCols(data.dJ); ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); motionSet::motionAction(ov, J_cols, dJ_cols); @@ -86,8 +86,8 @@ namespace pinocchio Motion & oa_gf = data.oa_gf[i]; ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); const typename Data::TangentVectorType & a = data.ddq; - data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() - + (data.v[i] ^ jdata.v()); + data.a[i] = + jdata.S() * jmodel.jointVelocitySelector(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]); @@ -107,7 +107,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.jointVelocityExtendedModelSelector(dimpulse); + odv = J_cols * jmodel.jointVelocitySelector(dimpulse); if (parent > 0) odv += odvparent; motionSet::motionAction(odvparent, J_cols, dAdq_cols); @@ -147,7 +147,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + 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); diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index 01ede92913..4bd640fd0b 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -170,7 +170,7 @@ namespace pinocchio const ColsBlock J_cols = jmodel.jointCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, dFda_cols); - jmodel.jointRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() = + data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), 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/delassus.hxx b/include/pinocchio/algorithm/delassus.hxx index a5b922de4b..61c1efb033 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.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S()); + jmodel.jointCols(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.jointExtendedModelCols(data.J); + ColBlock Jcols = jmodel.jointCols(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.jointExtendedModelCols(data.J); + ColBlock Jcols = jmodel.jointCols(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].jointExtendedModelCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointCols(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].jointExtendedModelCols(data.J); + * model.joints[i].jointCols(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].jointExtendedModelCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointCols(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].jointExtendedModelCols(data.J); + data.extended_motion_propagator[i] * model.joints[i].jointCols(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/energy.hxx b/include/pinocchio/algorithm/energy.hxx index f0a1379000..9da96d9f35 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -33,9 +33,9 @@ namespace pinocchio std::invalid_argument, std::string("Algorithm not supported for mimic joints")); const JointIndex & i = jmodel.id(); data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); - data.kinetic_energy += (jmodel.jointVelocityExtendedModelSelector(model.armature).array() - * jdata.joint_v().array().square()) - .sum(); + data.kinetic_energy += + (jmodel.jointVelocitySelector(model.armature).array() * jdata.joint_v().array().square()) + .sum(); } }; diff --git a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index 032539da71..5283ec1f07 100644 --- a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx @@ -71,7 +71,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColsBlock Jcols = jmodel.jointCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -165,7 +165,6 @@ namespace pinocchio const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & v_partial_dv) { - typedef typename Model::JointIndex JointIndex; typedef typename Data::SE3 SE3; typedef typename Data::Motion Motion; @@ -181,7 +180,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColsBlock Jcols = jmodel.jointCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index 85b09b8721..47a7f0b09e 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -81,16 +81,15 @@ namespace pinocchio if (parent > 0) vi += data.liMi[i].actInv(data.v[parent]); - ai = - jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() + (vi ^ jdata.v()); + ai = jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (vi ^ jdata.v()); if (parent > 0) ai += data.liMi[i].actInv(data.a[parent]); typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ); - ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dJcols = jmodel.jointCols(data.dJ); + ColsBlock Jcols = jmodel.jointCols(data.J); Jcols = oMi.act(jdata.S()); ov = oMi.act(vi); // Spatial velocity of joint i expressed in the global frame o @@ -187,7 +186,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColsBlock Jcols = jmodel.jointCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -343,8 +342,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ); - ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dJcols = jmodel.jointCols(data.dJ); + ColsBlock Jcols = jmodel.jointCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -594,7 +593,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColsBlock Jcols = jmodel.jointCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; @@ -757,8 +756,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn< typename Data::Matrix6x>::ConstType ColsBlock; - ColsBlock dJcols = jmodel.jointExtendedModelCols(data.dJ); - ColsBlock Jcols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dJcols = jmodel.jointCols(data.dJ); + ColsBlock Jcols = jmodel.jointCols(data.J); typedef typename SizeDepType::template ColsReturn::Type ColsBlockOut1; diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx index d9a1bb92fe..eacc35523a 100644 --- a/include/pinocchio/algorithm/pv.hxx +++ b/include/pinocchio/algorithm/pv.hxx @@ -183,7 +183,7 @@ namespace pinocchio bias_and_force.toVector() -= data.Yaba[i] * data.a_bias[i].toVector(); - jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); @@ -192,8 +192,7 @@ namespace pinocchio if (parent > 0) { pa.toVector().noalias() += - Ia * data.a_bias[i].toVector() - + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); + Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -229,16 +228,16 @@ namespace pinocchio bias_and_force.toVector() -= data.Yaba[i] * data.a_bias[i].toVector(); - jmodel.jointVelocityExtendedModelSelector(data.u) -= jdata.S().transpose() * data.f[i]; + jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i]; jmodel.calc_aba( - jdata.derived(), jmodel.jointVelocityExtendedModelSelector(model.armature), Ia, parent > 0); + jdata.derived(), jmodel.jointVelocitySelector(model.armature), Ia, parent > 0); Force & pa = data.f[i]; if (parent > 0) { - pa.toVector() += Ia * data.a_bias[i].toVector() - + jdata.UDinv() * jmodel.jointVelocityExtendedModelSelector(data.u); + pa.toVector() += + Ia * data.a_bias[i].toVector() + jdata.UDinv() * jmodel.jointVelocitySelector(data.u); data.Yaba[parent] += impl::internal::SE3actOn::run(data.liMi[i], Ia); data.f[parent] += data.liMi[i].act(pa); } @@ -258,14 +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.jointVelocityExtendedModelSelector(data.tau).noalias() = - jdata.Dinv() * data.KAS[i].col(ind); + jmodel.jointVelocitySelector(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.jointVelocityExtendedModelSelector(data.tau))); + + (data.KAS[i].col(ind2).dot(jmodel.jointVelocitySelector(data.tau))); } } @@ -273,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.jointVelocityExtendedModelSelector(data.tau).noalias() = + jmodel.jointVelocitySelector(data.tau).noalias() = (jdata.Dinv() - * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocityExtendedModelSelector(data.u))) + * (jdata.S().transpose() * bias_and_force + jmodel.jointVelocitySelector(data.u))) .eval(); - const Motion a_bf = jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.tau); + const Motion a_bf = jdata.S() * jmodel.jointVelocitySelector(data.tau); const Motion a_bf_motion = a_bf + data.a_bias[i]; for (int ind = 0; ind < data.constraints_supported_dim[i]; ind++) { @@ -312,11 +310,9 @@ namespace pinocchio const JointIndex parent = model.parents[i]; // Abusing otherwise unused data.tau below. - jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() = - jdata.S().transpose() * data.f[i]; + 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.jointVelocityExtendedModelSelector(data.tau); + data.f[i].toVector().noalias() -= jdata.UDinv() * jmodel.jointVelocitySelector(data.tau); if (parent > 0) { @@ -347,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.jointVelocityExtendedModelSelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocityExtendedModelSelector(data.u)) + jmodel.jointVelocitySelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocitySelector(data.u)) - jdata.UDinv().transpose() * data.a[i].toVector(); - data.a[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); } }; @@ -377,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.jointVelocityExtendedModelSelector(data.ddq).noalias() = - jdata.Dinv() * (jmodel.jointVelocityExtendedModelSelector(data.u)) + jmodel.jointVelocitySelector(data.ddq).noalias() = + jdata.Dinv() * (jmodel.jointVelocitySelector(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.jointVelocityExtendedModelSelector(data.ddq).noalias() -= + jmodel.jointVelocitySelector(data.ddq).noalias() -= data.lambdaA[i][j] * jdata.Dinv() * (data.KAS[i].col(j)); } - data.a[i] += jdata.S() * jmodel.jointVelocityExtendedModelSelector(data.ddq); + data.a[i] += jdata.S() * jmodel.jointVelocitySelector(data.ddq); } }; diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index e398125cd4..4d0083d40c 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -382,7 +382,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.jointVelocityExtendedModelSelector(a); + data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(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 6352377bb2..172e705bfb 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -63,7 +63,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); J_cols = data.oMi[i].act(jdata.S()); motionSet::motionAction(minus_gravity, J_cols, dAdq_cols); @@ -117,7 +117,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + 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); @@ -137,8 +137,7 @@ namespace pinocchio gravity_partial_dq_.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() = Ag_cols.transpose() * data.dAdq.col(j); - jmodel.jointVelocityExtendedModelSelector(g).noalias() = - J_cols.transpose() * data.of[i].toVector(); + jmodel.jointVelocitySelector(g).noalias() = J_cols.transpose() * data.of[i].toVector(); if (parent > 0) { data.oYcrb[parent] += data.oYcrb[i]; @@ -304,8 +303,8 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() - + (data.v[i] ^ jdata.v()); + data.a[i] = + jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); if (parent > 0) { data.a[i] += data.liMi[i].actInv(data.a[parent]); @@ -322,8 +321,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); + 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); @@ -409,7 +408,7 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; Matrix6x & dYtJ = data.Fcrb[0]; - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + 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); @@ -423,7 +422,7 @@ namespace pinocchio MatrixType3 & rnea_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, rnea_partial_da); // tau - jmodel.jointVelocityExtendedModelSelector(data.tau).noalias() = + jmodel.jointVelocitySelector(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 d8ac68b294..30a0147f4d 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -81,13 +81,12 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock J_cols = jmodel.jointExtendedModelCols( - data.J); // data.J has all the phi (in world frame) stacked in columns + 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 ColsBlock psidd_cols = - jmodel.jointCols(data.psidd); // psidd_cols is the psi_dotdot in world frame - ColsBlock dJ_cols = - jmodel.jointExtendedModelCols(data.dJ); // This here is phi_dot in world frame + 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 J_cols.noalias() = data.oMi[i].act(jdata.S()); // J_cols is just the phi in world frame for a joint @@ -98,8 +97,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.jointVelocityExtendedModelSelector(a) + jdata.c()); + oa += (ov ^ vJ) + data.oMi[i].act(jdata.S() * jmodel.jointVelocitySelector(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 0d77633da1..389f059f79 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -694,8 +694,8 @@ namespace pinocchio typedef typename SizeDepType::template ColsReturn::Type ColsBlock; - ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ); - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); + ColsBlock dJ_cols = jmodel.jointCols(data.dJ); + ColsBlock J_cols = jmodel.jointCols(data.J); ColsBlock Ag_cols = jmodel.jointCols(data.Ag); typename Data::Matrix6x & dFdv = data.Fcrb[0]; ColsBlock dFdv_cols = jmodel.jointCols(dFdv); From d58f0102165976195a655b63ddfe314a840e0da2 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 27 Jan 2025 11:23:48 +0100 Subject: [PATCH 189/310] [Check] Define MimicChecker to check if model has mimic joint --- include/pinocchio/algorithm/check.hpp | 2 ++ include/pinocchio/algorithm/check.hxx | 16 ++++++++++++++++ unittest/algo-check.cpp | 5 +++++ 3 files changed, 23 insertions(+) diff --git a/include/pinocchio/algorithm/check.hpp b/include/pinocchio/algorithm/check.hpp index 041fa094d9..3144804682 100644 --- a/include/pinocchio/algorithm/check.hpp +++ b/include/pinocchio/algorithm/check.hpp @@ -47,6 +47,8 @@ namespace pinocchio /// Simple model checker, that assert that model.parents is indeed a tree. PINOCCHIO_DEFINE_ALGO_CHECKER(Parent); + /// Simple model checker, that assert that there is a mimic joint in the tree + PINOCCHIO_DEFINE_ALGO_CHECKER(Mimic); #if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) /// Checker having a list of Checker as input argument diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index a9ea4c67a7..efe4e094d2 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -54,6 +54,22 @@ namespace pinocchio return true; } + // Check if there is a mimic joint in the kinematic tree + template class JointCollectionTpl> + inline bool + MimicChecker::checkModel_impl(const ModelTpl & model) const + { + typedef ModelTpl Model; + typedef typename Model::JointIndex JointIndex; + + for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) + if (boost::get>(&model.joints[j])) + return false; + + return true; + } + + #if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) template template class JointCollectionTpl> diff --git a/unittest/algo-check.cpp b/unittest/algo-check.cpp index e85cdfed94..57ec490fd9 100644 --- a/unittest/algo-check.cpp +++ b/unittest/algo-check.cpp @@ -51,6 +51,11 @@ BOOST_AUTO_TEST_CASE(test_check) Y.inertia().data().fill(-1.); } BOOST_CHECK(!model.check(ABAChecker())); // some inertias are negative ... check fail. + + pinocchio::Model model_mimic; + buildModels::humanoidRandom(model_mimic, false, true); + BOOST_CHECK(!model_mimic.check(MimicChecker())); + } BOOST_AUTO_TEST_SUITE_END() From 0ecb5d17b288626d1d3356e7b9edbf0fec2cc389 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 27 Jan 2025 11:24:27 +0100 Subject: [PATCH 190/310] [Check] Add model checker to all non supported algorithms for model with mimic joints --- .../pinocchio/algorithm/aba-derivatives.hxx | 12 +++++---- include/pinocchio/algorithm/aba.hxx | 25 ++++++++----------- .../algorithm/center-of-mass-derivatives.hxx | 6 +---- .../algorithm/centroidal-derivatives.hxx | 11 ++------ include/pinocchio/algorithm/centroidal.hxx | 2 ++ include/pinocchio/algorithm/cholesky.hxx | 17 +++++++++++++ .../pinocchio/algorithm/compute-all-terms.hxx | 10 +------- .../constrained-dynamics-derivatives.hxx | 6 +---- .../algorithm/constrained-dynamics.hxx | 7 +++--- .../pinocchio/algorithm/contact-cholesky.hxx | 4 +++ .../pinocchio/algorithm/contact-dynamics.hxx | 6 +++++ .../pinocchio/algorithm/contact-jacobian.hxx | 2 ++ include/pinocchio/algorithm/delassus.hxx | 8 ++++++ include/pinocchio/algorithm/energy.hxx | 8 +++--- .../impulse-dynamics-derivatives.hxx | 6 +---- .../pinocchio/algorithm/impulse-dynamics.hxx | 1 + .../algorithm/kinematics-derivatives.hxx | 12 +++++---- include/pinocchio/algorithm/pv.hxx | 4 +++ include/pinocchio/algorithm/regressor.hxx | 12 +++++---- .../pinocchio/algorithm/rnea-derivatives.hxx | 22 +++------------- .../rnea-second-order-derivatives.hxx | 10 +------- include/pinocchio/algorithm/rnea.hxx | 14 ++--------- 22 files changed, 94 insertions(+), 111 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 5d4a1a7309..56011ca5b2 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -43,11 +43,6 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; const JointIndex i = jmodel.id(); @@ -408,6 +403,7 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename ModelTpl::JointIndex JointIndex; @@ -495,6 +491,8 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + typedef typename ModelTpl::JointIndex JointIndex; @@ -732,6 +730,8 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + typedef typename ModelTpl::JointIndex JointIndex; @@ -796,6 +796,8 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + typedef typename ModelTpl::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/aba.hxx b/include/pinocchio/algorithm/aba.hxx index 9398e4a455..f03c2e2205 100644 --- a/include/pinocchio/algorithm/aba.hxx +++ b/include/pinocchio/algorithm/aba.hxx @@ -106,11 +106,6 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -252,6 +247,7 @@ namespace pinocchio const Eigen::MatrixBase & tau) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT( q.size() == model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -315,6 +311,8 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( q.size() == model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -383,11 +381,6 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; const JointIndex i = jmodel.id(); @@ -498,6 +491,8 @@ namespace pinocchio const Eigen::MatrixBase & tau) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -563,6 +558,8 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -630,11 +627,6 @@ 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; const JointIndex & i = jmodel.id(); @@ -849,6 +841,8 @@ namespace pinocchio const Eigen::MatrixBase & q) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); @@ -886,6 +880,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename ModelTpl::JointIndex JointIndex; data.Minv.template triangularView().setZero(); diff --git a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx index 456e66c628..7394ead69f 100644 --- a/include/pinocchio/algorithm/center-of-mass-derivatives.hxx +++ b/include/pinocchio/algorithm/center-of-mass-derivatives.hxx @@ -35,11 +35,6 @@ namespace pinocchio Data & data, const Eigen::MatrixBase & vcom_partial_dq) { - assert( - (std::is_same>::value - == false) - && "Algorithm not supported for mimic joints"); - typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -81,6 +76,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(vcom_partial_dq.cols(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/centroidal-derivatives.hxx b/include/pinocchio/algorithm/centroidal-derivatives.hxx index 8ee3a3873e..d835a53ff7 100644 --- a/include/pinocchio/algorithm/centroidal-derivatives.hxx +++ b/include/pinocchio/algorithm/centroidal-derivatives.hxx @@ -58,10 +58,6 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; Motion & ov = data.ov[i]; @@ -284,6 +280,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -373,11 +370,6 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type ColsBlock; - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; @@ -434,6 +426,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(dhdot_da.rows(), 6); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 9332ab18d7..64c1a4feae 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -302,6 +302,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); @@ -364,6 +365,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); diff --git a/include/pinocchio/algorithm/cholesky.hxx b/include/pinocchio/algorithm/cholesky.hxx index 0edea8947c..6ab5e394cd 100644 --- a/include/pinocchio/algorithm/cholesky.hxx +++ b/include/pinocchio/algorithm/cholesky.hxx @@ -34,6 +34,7 @@ namespace pinocchio */ PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; @@ -91,6 +92,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -155,6 +158,8 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -215,6 +220,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -278,6 +285,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -342,6 +351,8 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(vin.size(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(vout.size(), model.nv); @@ -422,6 +433,8 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -479,6 +492,7 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat) assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); Mat & v_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, v); @@ -522,6 +536,8 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT(col < model.nv && col >= 0); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); @@ -567,6 +583,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Minv.cols(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); Mat & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(Mat, Minv); diff --git a/include/pinocchio/algorithm/compute-all-terms.hxx b/include/pinocchio/algorithm/compute-all-terms.hxx index 0f11dd1613..68a71bf184 100644 --- a/include/pinocchio/algorithm/compute-all-terms.hxx +++ b/include/pinocchio/algorithm/compute-all-terms.hxx @@ -41,10 +41,6 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename SizeDepType::template ColsReturn::Type @@ -107,11 +103,6 @@ namespace pinocchio const Model & model, Data & data) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; typedef typename SizeDepType::template ColsReturn::Type @@ -163,6 +154,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size"); diff --git a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx index 2467264011..1ce9fdae2e 100644 --- a/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics-derivatives.hxx @@ -41,11 +41,6 @@ namespace pinocchio const Model & model, Data & data) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -403,6 +398,7 @@ namespace pinocchio "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); // TODO: User should make sure the internal quantities are reset. data.dtau_dq.setZero(); diff --git a/include/pinocchio/algorithm/constrained-dynamics.hxx b/include/pinocchio/algorithm/constrained-dynamics.hxx index 4bd640fd0b..d20111e163 100644 --- a/include/pinocchio/algorithm/constrained-dynamics.hxx +++ b/include/pinocchio/algorithm/constrained-dynamics.hxx @@ -92,10 +92,6 @@ namespace pinocchio const Eigen::MatrixBase & q, const Eigen::MatrixBase & v) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; typedef typename Data::Force Force; @@ -213,6 +209,7 @@ namespace pinocchio typedef RigidConstraintDataTpl RigidConstraintData; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -760,6 +757,7 @@ namespace pinocchio ProximalSettingsTpl & settings) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -992,6 +990,7 @@ namespace pinocchio using namespace Eigen; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( diff --git a/include/pinocchio/algorithm/contact-cholesky.hxx b/include/pinocchio/algorithm/contact-cholesky.hxx index ece0b0081f..bdc95886ed 100644 --- a/include/pinocchio/algorithm/contact-cholesky.hxx +++ b/include/pinocchio/algorithm/contact-cholesky.hxx @@ -27,6 +27,8 @@ namespace pinocchio typedef RigidConstraintModelTpl RigidConstraintModel; typedef std::vector RigidConstraintModelVector; + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + nv = model.nv; num_contacts = (Eigen::DenseIndex)contact_models.size(); @@ -189,6 +191,8 @@ namespace pinocchio typedef RigidConstraintDataTpl RigidConstraintData; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( (Eigen::DenseIndex)contact_models.size() == num_contacts, "The number of contacts inside contact_models and the one during allocation do not match.\n" diff --git a/include/pinocchio/algorithm/contact-dynamics.hxx b/include/pinocchio/algorithm/contact-dynamics.hxx index fb918d6e12..3f5e49c5b4 100644 --- a/include/pinocchio/algorithm/contact-dynamics.hxx +++ b/include/pinocchio/algorithm/contact-dynamics.hxx @@ -99,6 +99,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(J.rows(), gamma.size()); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; @@ -181,6 +182,8 @@ namespace pinocchio const Scalar & inv_damping) { assert(model.check(data)); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( check_expression_if_real(inv_damping >= 0.), "mu must be positive."); @@ -222,6 +225,8 @@ namespace pinocchio const Eigen::MatrixBase & KKTMatrix_inv) { assert(model.check(data)); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.cols(), data.JMinvJt.cols() + model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(KKTMatrix_inv.rows(), data.JMinvJt.rows() + model.nv); @@ -291,6 +296,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(v_before.size(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(J.cols(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/contact-jacobian.hxx b/include/pinocchio/algorithm/contact-jacobian.hxx index c2c718a448..9dba83bdbf 100644 --- a/include/pinocchio/algorithm/contact-jacobian.hxx +++ b/include/pinocchio/algorithm/contact-jacobian.hxx @@ -24,6 +24,8 @@ namespace pinocchio const Eigen::MatrixBase & J_) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.rows(), constraint_model.size()); PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.cols(), model.nv); diff --git a/include/pinocchio/algorithm/delassus.hxx b/include/pinocchio/algorithm/delassus.hxx index 61c1efb033..a42354b698 100644 --- a/include/pinocchio/algorithm/delassus.hxx +++ b/include/pinocchio/algorithm/delassus.hxx @@ -125,6 +125,8 @@ namespace pinocchio const Scalar mu) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -290,6 +292,8 @@ namespace pinocchio DataTpl & data, const std::vector, Allocator> & contact_models) { + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + std::fill(data.constraints_supported_dim.begin(), data.constraints_supported_dim.end(), 0); for (std::size_t i = 0; i < contact_models.size(); ++i) { @@ -476,6 +480,8 @@ namespace pinocchio const Scalar mu) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( @@ -802,6 +808,8 @@ namespace pinocchio const bool scaled, const bool Pv) { + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + PINOCCHIO_CHECK_INPUT_ARGUMENT( check_expression_if_real(mu >= Eigen::NumTraits::dummy_precision()), "mu is too small."); diff --git a/include/pinocchio/algorithm/energy.hxx b/include/pinocchio/algorithm/energy.hxx index 9da96d9f35..209b387346 100644 --- a/include/pinocchio/algorithm/energy.hxx +++ b/include/pinocchio/algorithm/energy.hxx @@ -27,10 +27,6 @@ namespace pinocchio const Model & model, Data & data) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); const JointIndex & i = jmodel.id(); data.kinetic_energy += model.inertias[i].vtiv(data.v[i]); data.kinetic_energy += @@ -45,6 +41,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -66,7 +63,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); - ; + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -93,6 +90,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx index 5283ec1f07..0968ddd9db 100644 --- a/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics-derivatives.hxx @@ -54,11 +54,6 @@ namespace pinocchio const Eigen::MatrixBase & v_partial_dq, const Eigen::MatrixBase & v_partial_dv) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; typedef typename Data::SE3 SE3; typedef typename Data::Motion Motion; @@ -259,6 +254,7 @@ namespace pinocchio const Eigen::MatrixBase & impulse_partial_dv) { const Eigen::DenseIndex nc = data.contact_chol.constraintDim(); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT( contact_data.size() == contact_models.size(), diff --git a/include/pinocchio/algorithm/impulse-dynamics.hxx b/include/pinocchio/algorithm/impulse-dynamics.hxx index 6a32329de2..950d33adf7 100644 --- a/include/pinocchio/algorithm/impulse-dynamics.hxx +++ b/include/pinocchio/algorithm/impulse-dynamics.hxx @@ -32,6 +32,7 @@ namespace pinocchio const ProximalSettingsTpl & settings) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT( q.size() == model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_INPUT_ARGUMENT( diff --git a/include/pinocchio/algorithm/kinematics-derivatives.hxx b/include/pinocchio/algorithm/kinematics-derivatives.hxx index 47a7f0b09e..98d7604ab4 100644 --- a/include/pinocchio/algorithm/kinematics-derivatives.hxx +++ b/include/pinocchio/algorithm/kinematics-derivatives.hxx @@ -51,11 +51,6 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; typedef typename Data::SE3 SE3; typedef typename Data::Motion Motion; @@ -118,6 +113,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE( a.size(), model.nv, "The acceleration vector is not of right size"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -267,6 +263,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv); PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid."); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename Model::JointIndex JointIndex; @@ -495,6 +492,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv); PINOCCHIO_CHECK_INPUT_ARGUMENT((int)jointId < model.njoints, "The joint id is invalid."); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -674,6 +672,7 @@ namespace pinocchio rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef typename Data::SE3 SE3; typedef typename Data::Motion Motion; @@ -910,6 +909,7 @@ namespace pinocchio rf == LOCAL || rf == LOCAL_WORLD_ALIGNED, "The reference frame is not valid, expected LOCAL or LOCAL_WORLD_ALIGNED"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); const SE3 oMpoint = data.oMi[joint_id] * placement; const Motion spatial_velocity = oMpoint.actInv(data.ov[joint_id]); @@ -971,6 +971,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -1075,6 +1076,7 @@ namespace pinocchio Tensor & kinematic_hessian) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); assert( joint_id < model.joints.size() && joint_id > 0 && "joint_id is outside the valid index for a joint in model.joints"); diff --git a/include/pinocchio/algorithm/pv.hxx b/include/pinocchio/algorithm/pv.hxx index eacc35523a..00a492e991 100644 --- a/include/pinocchio/algorithm/pv.hxx +++ b/include/pinocchio/algorithm/pv.hxx @@ -28,6 +28,8 @@ namespace pinocchio const std::vector, Allocator> & contact_models) { + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); + // Allocate memory for the backward propagation of LA, KA and lA typedef typename Model::JointIndex JointIndex; typedef RigidConstraintDataTpl RigidConstraintData; @@ -410,6 +412,7 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( @@ -607,6 +610,7 @@ namespace pinocchio { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE( q.size(), model.nq, "The joint configuration vector is not of right size"); PINOCCHIO_CHECK_ARGUMENT_SIZE( diff --git a/include/pinocchio/algorithm/regressor.hxx b/include/pinocchio/algorithm/regressor.hxx index 4d0083d40c..7a2c6298ce 100644 --- a/include/pinocchio/algorithm/regressor.hxx +++ b/include/pinocchio/algorithm/regressor.hxx @@ -29,6 +29,7 @@ namespace pinocchio const Eigen::MatrixBase & kinematic_regressor) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.rows(), 6); PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.cols(), 6 * (model.njoints - 1)); @@ -144,6 +145,7 @@ namespace pinocchio const Eigen::MatrixBase & q) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); typedef ModelTpl Model; @@ -296,6 +298,7 @@ namespace pinocchio JointIndex joint_id) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_UNUSED_VARIABLE(model); @@ -311,6 +314,7 @@ namespace pinocchio FrameIndex frame_id) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::Frame Frame; @@ -363,11 +367,6 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; const JointIndex i = jmodel.id(); @@ -437,6 +436,7 @@ namespace pinocchio const Eigen::MatrixBase & a) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv); @@ -483,6 +483,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); @@ -529,6 +530,7 @@ namespace pinocchio const Eigen::MatrixBase & q) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/rnea-derivatives.hxx b/include/pinocchio/algorithm/rnea-derivatives.hxx index 172e705bfb..494382a92a 100644 --- a/include/pinocchio/algorithm/rnea-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-derivatives.hxx @@ -37,10 +37,6 @@ namespace pinocchio Data & data, const Eigen::MatrixBase & q) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -97,11 +93,6 @@ namespace pinocchio typename Data::VectorXs & g, const Eigen::MatrixBase & gravity_partial_dq) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("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, @@ -163,6 +154,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(gravity_partial_dq.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(gravity_partial_dq.rows(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -210,6 +202,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE( fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; @@ -276,10 +269,6 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; @@ -394,11 +383,6 @@ namespace pinocchio const Eigen::MatrixBase & rnea_partial_dv, const Eigen::MatrixBase & rnea_partial_da) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; typedef typename Data::Matrix6x Matrix6x; @@ -511,6 +495,7 @@ namespace pinocchio isZero(model.gravity.angular()), "The gravity must be a pure force vector, no angular part"); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; @@ -591,6 +576,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.cols(), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(rnea_partial_da.rows(), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef DataTpl Data; diff --git a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx index 30a0147f4d..9bfdb33a09 100644 --- a/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx +++ b/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx @@ -47,11 +47,6 @@ namespace pinocchio const Eigen::MatrixBase & v, const Eigen::MatrixBase & a) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); - typedef typename Model::JointIndex JointIndex; typedef typename Data::Motion Motion; typedef typename Data::Inertia Inertia; @@ -162,10 +157,6 @@ namespace pinocchio const Tensor3 & dtau_dqdv, const Tensor3 & dtau_dadq) { - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("Algorithm not supported for mimic joints")); typedef typename Data::Motion Motion; typedef typename Data::Force Force; typedef typename Data::Inertia Inertia; @@ -440,6 +431,7 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(1), model.nv); PINOCCHIO_CHECK_ARGUMENT_SIZE(dtau_dadq.dimension(2), model.nv); assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 389f059f79..3125f6348b 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -573,12 +573,6 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { - - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("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, @@ -636,6 +630,7 @@ namespace pinocchio const Eigen::MatrixBase & v) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); @@ -674,12 +669,6 @@ namespace pinocchio template static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { - - PINOCCHIO_THROW( - (std::is_same>::value - == false), - std::invalid_argument, std::string("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, @@ -728,6 +717,7 @@ namespace pinocchio DataTpl & data) { assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); typedef DataTpl Data; typedef ModelTpl Model; From 5b9fd7b428c917581bf22c328da7e66e3a3c44ba Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 27 Jan 2025 15:35:04 +0100 Subject: [PATCH 191/310] [EtienneAr feedback] topic/mimic: nvSubtree_fromRow/parents_fromRow indexed by extended model, add condensed version of G matrix and fix jacobians --- include/pinocchio/algorithm/check.hxx | 15 +-- include/pinocchio/algorithm/jacobian.hxx | 107 +++++++----------- .../bindings/python/multibody/data.hpp | 3 + include/pinocchio/multibody/data.hpp | 11 +- include/pinocchio/multibody/data.hxx | 44 ++++--- include/pinocchio/serialization/data.hpp | 1 + 6 files changed, 91 insertions(+), 90 deletions(-) diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check.hxx index efe4e094d2..c6727596b5 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check.hxx @@ -69,7 +69,6 @@ namespace pinocchio return true; } - #if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) template template class JointCollectionTpl> @@ -159,8 +158,9 @@ namespace pinocchio CHECK_DATA((int)data.lastChild.size() == model.njoints); CHECK_DATA((int)data.nvSubtree.size() == model.njoints); - CHECK_DATA((int)data.parents_fromRow.size() == model.nv); - CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nv); + CHECK_DATA((int)data.parents_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.idx_v_extended_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nvExtended); for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) { @@ -192,10 +192,10 @@ namespace pinocchio for (JointIndex d = c + 1; (int)d < model.njoints; ++d) 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]); + CHECK_DATA( + data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)model.joints[j].idx_vExtended()]); + int row = model.joints[j].idx_vExtended(); const JointModel & jparent = model.joints[model.parents[j]]; if (row == 0) { @@ -203,7 +203,8 @@ namespace pinocchio } else { - CHECK_DATA(jparent.idx_v() + jparent.nv() - 1 == data.parents_fromRow[(size_t)row]); + CHECK_DATA( + jparent.idx_vExtended() + jparent.nvExtended() - 1 == data.parents_fromRow[(size_t)row]); } } diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 632df7a827..a8f3f0c703 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -199,54 +199,41 @@ namespace pinocchio typedef typename Matrix6xLikeOut::ColXpr ColXprOut; typedef MotionRef MotionOut; + const int colRef = + nvExtended(model.joints[joint_id]) + idx_vExtended(model.joints[joint_id]) - 1; switch (rf) { case WORLD: { - for (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.parents_fromRow[(size_t)jExtended]) { - for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) - { - const Eigen::DenseIndex col_in = idx_vExtended(model.joints[j]) + i; - const Eigen::DenseIndex col_out = idx_v(model.joints[j]) + i; + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - 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 (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.parents_fromRow[(size_t)jExtended]) { - for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) - { - const Eigen::DenseIndex col_in = idx_vExtended(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)); + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - 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 (JointIndex j = joint_id; j > 0; j = model.parents[(size_t)j]) + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.parents_fromRow[(size_t)jExtended]) { - for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) - { - const Eigen::DenseIndex col_in = idx_vExtended(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)); + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - v_out += placement.actInv(v_in); - } + v_out += placement.actInv(v_in); } break; } @@ -510,48 +497,42 @@ namespace pinocchio case LOCAL: { const SE3 & oMjoint = data.oMi[jointId]; const Motion & v_joint = data.v[jointId]; - for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) + const int colRef = + nvExtended(model.joints[jointId]) + idx_vExtended(model.joints[jointId]) - 1; + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.parents_fromRow[(size_t)jExtended]) { - for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) - { - const Eigen::DenseIndex col_in = idx_vExtended(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 Data::Matrix6x::ConstColXpr ConstColXprIn; - typedef const MotionRef MotionIn; + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(jExtended)); + MotionOut v_out(dJ.col(data.idx_v_extended_fromRow[jExtended])); - 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]; - for (JointIndex j = jointId; j > 0; j = model.parents[(size_t)j]) + const int colRef = + nvExtended(model.joints[jointId]) + idx_vExtended(model.joints[jointId]) - 1; + for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; + jExtended = data.parents_fromRow[(size_t)jExtended]) { - for (int i = nvExtended(model.joints[j]) - 1; i >= 0; i--) - { - const Eigen::DenseIndex col_in = idx_vExtended(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(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()); - } + typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn; + typedef const MotionRef MotionIn; + + typedef typename Matrix6xLike::ColXpr ColXprOut; + typedef MotionRef MotionOut; + MotionIn v_in(data.J.col(jExtended)); + MotionOut v_out(dJ.col(data.idx_v_extended_fromRow[jExtended])); + + v_out.linear() -= + Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation())) + .cross(v_in.angular()); } break; } diff --git a/include/pinocchio/bindings/python/multibody/data.hpp b/include/pinocchio/bindings/python/multibody/data.hpp index 9548a90a85..08eb44c98e 100644 --- a/include/pinocchio/bindings/python/multibody/data.hpp +++ b/include/pinocchio/bindings/python/multibody/data.hpp @@ -151,6 +151,9 @@ namespace pinocchio .ADD_DATA_PROPERTY(U, "Joint Inertia square root (upper triangle)") .ADD_DATA_PROPERTY(D, "Diagonal of UDUT inertia decomposition") .ADD_DATA_PROPERTY(parents_fromRow, "First previous non-zero row in M (used in Cholesky)") + .ADD_DATA_PROPERTY( + idx_v_extended_fromRow, "Extended model mapping of the joint rows " + "(idx_v_extended_fromRow[idx_v_extended] = idx_v)") .ADD_DATA_PROPERTY( nvSubtree_fromRow, "Subtree of the current row index (used in Cholesky)") .ADD_DATA_PROPERTY(J, "Jacobian of joint placement") diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index 2e64fb4e80..4fbb752d4f 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -324,6 +324,10 @@ namespace pinocchio /// \brief End index of the Joint motion subspace std::vector end_idx_v_fromRow; + /// \brief Extended model mapping of the joint rows (idx_v_extended_fromRow[idx_v_extended] = + /// idx_v) + std::vector idx_v_extended_fromRow; + /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a /// Cholesky Decomposition. MatrixXs U; @@ -354,9 +358,10 @@ namespace pinocchio /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and /// expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} /// \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = - /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nvExtended}} - /// S_{\text{nvExtended}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian - /// of a precise joint, you need to call pinocchio::getJointJacobian + /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & + /// ^{0}X_{\text{nvExtended}} S_{\text{nvExtended}} \end{bmatrix} \f$. This Jacobian has no + /// special meaning. To get the jacobian of a precise joint, you need to call + /// pinocchio::getJointJacobian Matrix6x J; /// \brief Derivative of the Jacobian with respect to the time. diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 804d4d1cfd..ead6b747c2 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -70,15 +70,16 @@ namespace pinocchio , Fcrb((std::size_t)model.njoints, Matrix6x::Zero(6, model.nv)) , lastChild((std::size_t)model.njoints, -1) , nvSubtree((std::size_t)model.njoints, -1) - , start_idx_v_fromRow((std::size_t)model.nv, -1) - , end_idx_v_fromRow((std::size_t)model.nv, -1) + , start_idx_v_fromRow((std::size_t)model.nvExtended, -1) + , end_idx_v_fromRow((std::size_t)model.nvExtended, -1) , U(MatrixXs::Identity(model.nv, model.nv)) , D(VectorXs::Zero(model.nv)) , Dinv(VectorXs::Zero(model.nv)) , tmp(VectorXs::Zero(model.nv)) - , parents_fromRow((std::size_t)model.nv, -1) + , parents_fromRow((std::size_t)model.nvExtended, -1) + , idx_v_extended_fromRow((std::size_t)model.nvExtended, -1) , supports_fromRow((std::size_t)model.nv) - , nvSubtree_fromRow((std::size_t)model.nv, -1) + , nvSubtree_fromRow((std::size_t)model.nvExtended, -1) , J(Matrix6x::Zero(6, model.nvExtended)) , dJ(Matrix6x::Zero(6, model.nvExtended)) , ddJ(Matrix6x::Zero(6, model.nvExtended)) @@ -226,28 +227,36 @@ 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(); + const int nvj = model.joints[joint].nv(); + const int nvExtended_j = model.joints[joint].nvExtended(); + const int idx_vExtended_j = model.joints[joint].idx_vExtended(); + assert(idx_vExtended_j >= 0 && idx_vExtended_j < model.nvExtended); assert(idx_vj >= 0 && idx_vj < model.nv); + if (parent > 0) - parents_fromRow[(Index)idx_vj] = - model.joints[parent].idx_v() + model.joints[parent].nv() - 1; + parents_fromRow[(Index)idx_vExtended_j] = + model.joints[parent].idx_vExtended() + model.joints[parent].nvExtended() - 1; else - parents_fromRow[(Index)idx_vj] = -1; - nvSubtree_fromRow[(Index)idx_vj] = nvSubtree[joint]; + parents_fromRow[(Index)idx_vExtended_j] = -1; + nvSubtree_fromRow[(Index)idx_vExtended_j] = nvSubtree[joint]; start_idx_v_fromRow[(size_t)idx_vj] = idx_vj; - end_idx_v_fromRow[(size_t)idx_vj] = idx_vj + nvj - 1; - for (int row = 1; row < nvj; ++row) + end_idx_v_fromRow[(size_t)idx_vj] = idx_vj + nvExtended_j - 1; + idx_v_extended_fromRow[(size_t)idx_vExtended_j] = idx_vj; + + for (int row = 1; row < nvExtended_j; ++row) { - parents_fromRow[(size_t)(idx_vj + row)] = idx_vj + row - 1; - nvSubtree_fromRow[(size_t)(idx_vj + row)] = nvSubtree[joint] - row; - start_idx_v_fromRow[(size_t)(idx_vj + row)] = start_idx_v_fromRow[(size_t)idx_vj]; - end_idx_v_fromRow[(size_t)(idx_vj + row)] = end_idx_v_fromRow[(size_t)idx_vj]; + parents_fromRow[(size_t)(idx_vExtended_j + row)] = idx_vExtended_j + row - 1; + nvSubtree_fromRow[(size_t)(idx_vExtended_j + row)] = nvSubtree[joint] - row; + start_idx_v_fromRow[(size_t)(idx_vExtended_j + row)] = + start_idx_v_fromRow[(size_t)idx_vExtended_j]; + end_idx_v_fromRow[(size_t)(idx_vExtended_j + row)] = + end_idx_v_fromRow[(size_t)idx_vExtended_j]; + idx_v_extended_fromRow[(size_t)(idx_vExtended_j + row)] = + idx_v_extended_fromRow[(size_t)idx_vExtended_j] + row; } } } @@ -309,6 +318,7 @@ namespace pinocchio && data1.end_idx_v_fromRow == data2.end_idx_v_fromRow && data1.U == data2.U && data1.D == data2.D && data1.Dinv == data2.Dinv && data1.parents_fromRow == data2.parents_fromRow + && data1.idx_v_extended_fromRow == data2.idx_v_extended_fromRow && data1.supports_fromRow == data2.supports_fromRow && data1.nvSubtree_fromRow == data2.nvSubtree_fromRow && data1.J == data2.J && data1.dJ == data2.dJ && data1.ddJ == data2.ddJ && data1.psid == data2.psid diff --git a/include/pinocchio/serialization/data.hpp b/include/pinocchio/serialization/data.hpp index daf0afb1f0..ff83e3ad62 100644 --- a/include/pinocchio/serialization/data.hpp +++ b/include/pinocchio/serialization/data.hpp @@ -89,6 +89,7 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar, data, D); PINOCCHIO_MAKE_DATA_NVP(ar, data, Dinv); PINOCCHIO_MAKE_DATA_NVP(ar, data, parents_fromRow); + PINOCCHIO_MAKE_DATA_NVP(ar, data, idx_v_extended_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, supports_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, nvSubtree_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, J); From 53f3ea59deafa44ece5de1c5c635f86a542cd186 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 27 Jan 2025 14:46:09 +0100 Subject: [PATCH 192/310] Remove mimicable collection variant and associated function --- include/pinocchio/bindings/python/context/generic.hpp | 3 --- include/pinocchio/multibody/joint/fwd.hpp | 4 ---- .../multibody/joint/joint-basic-visitors.hpp | 11 ----------- 3 files changed, 18 deletions(-) diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index 1eb7a151d3..cf2020aa8e 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -64,7 +64,6 @@ namespace pinocchio typedef DataTpl Data; typedef JointCollectionDefaultTpl JointCollectionDefault; - typedef JointCollectionMimicableTpl JointCollectionMimicable; // Joints typedef JointModelTpl JointModel; @@ -142,8 +141,6 @@ namespace pinocchio typedef JointModelMimicTpl JointModelMimic; typedef JointDataMimicTpl JointDataMimic; - typedef JointModelTpl JointModelMimicable; - // Algorithm typedef ProximalSettingsTpl ProximalSettings; typedef ContactCholeskyDecompositionTpl ContactCholeskyDecomposition; diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index a4d29db3ef..774111bb1e 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -134,10 +134,6 @@ namespace pinocchio struct JointCollectionDefaultTpl; typedef JointCollectionDefaultTpl JointCollectionDefault; - template - struct JointCollectionMimicableTpl; - typedef JointCollectionMimicableTpl JointCollectionMimicable; - 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 092b77c0da..1294f5cc56 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -452,17 +452,6 @@ 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); /** * @brief Apply the correct affine transform, on a joint configuration, depending on the joint From 72c11597597e3fe830ee803370d8728db7316e01 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 27 Jan 2025 14:46:52 +0100 Subject: [PATCH 193/310] [unittest] Revert changes to not pollute pr --- unittest/algo-check.cpp | 1 - unittest/crba.cpp | 8 ++++---- unittest/python/bindings_joints.py | 2 +- unittest/python/bindings_model.py | 2 +- unittest/python/bindings_rnea.py | 2 +- 5 files changed, 7 insertions(+), 8 deletions(-) diff --git a/unittest/algo-check.cpp b/unittest/algo-check.cpp index 57ec490fd9..a4797b1b70 100644 --- a/unittest/algo-check.cpp +++ b/unittest/algo-check.cpp @@ -55,7 +55,6 @@ BOOST_AUTO_TEST_CASE(test_check) pinocchio::Model model_mimic; buildModels::humanoidRandom(model_mimic, false, true); BOOST_CHECK(!model_mimic.check(MimicChecker())); - } BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 5a7f3a5adb..4563b4f2ed 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -65,7 +65,7 @@ BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, true); + pinocchio::buildModels::humanoidRandom(model); pinocchio::Data data(model); #ifdef NDEBUG @@ -215,7 +215,7 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) BOOST_AUTO_TEST_CASE(test_minimal_crba) { pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, true); + pinocchio::buildModels::humanoidRandom(model); pinocchio::Data data(model), data_ref(model); model.lowerPositionLimit.head<7>().fill(-1.); @@ -253,7 +253,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); + pinocchio::buildModels::humanoidRandom(model); model_ref = model; BOOST_CHECK(model == model_ref); @@ -281,7 +281,7 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) { using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, true, true); + pinocchio::buildModels::humanoidRandom(model, false, true); model.addJoint( size_t(model.njoints - 1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), diff --git a/unittest/python/bindings_joints.py b/unittest/python/bindings_joints.py index 69be539214..f82778be9c 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) + self.model = pin.buildSampleModelHumanoidRandom() 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 912596e752..e6b9474d7e 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) + self.model = pin.buildSampleModelHumanoidRandom() 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..88c54a0f8c 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) + self.model = pin.buildSampleModelHumanoidRandom() self.data = self.model.createData() qmax = np.full((self.model.nq, 1), np.pi) From f722b30c709750ed31fcb70ebdc9d7ab69cc0fbc Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 27 Jan 2025 14:47:07 +0100 Subject: [PATCH 194/310] Apply pre-commit --- include/pinocchio/algorithm/aba-derivatives.hxx | 3 --- include/pinocchio/algorithm/crba.hxx | 7 ++++--- include/pinocchio/algorithm/model.hxx | 3 ++- include/pinocchio/bindings/python/multibody/model.hpp | 8 +++++--- .../pinocchio/multibody/joint/joint-basic-visitors.hpp | 5 ++--- .../pinocchio/multibody/joint/joint-basic-visitors.hxx | 6 +++++- include/pinocchio/multibody/joint/joint-composite.hpp | 4 ++-- include/pinocchio/multibody/joint/joint-free-flyer.hpp | 2 +- .../pinocchio/multibody/joint/joint-helical-unaligned.hpp | 2 +- include/pinocchio/multibody/joint/joint-helical.hpp | 2 +- include/pinocchio/multibody/joint/joint-mimic.hpp | 4 ++-- include/pinocchio/multibody/joint/joint-model-base.hpp | 8 +++++--- include/pinocchio/multibody/joint/joint-planar.hpp | 2 +- .../multibody/joint/joint-prismatic-unaligned.hpp | 2 +- include/pinocchio/multibody/joint/joint-prismatic.hpp | 2 +- .../multibody/joint/joint-revolute-unaligned.hpp | 2 +- .../joint/joint-revolute-unbounded-unaligned.hpp | 2 +- .../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 +- include/pinocchio/multibody/model.hxx | 8 ++++---- 24 files changed, 45 insertions(+), 39 deletions(-) diff --git a/include/pinocchio/algorithm/aba-derivatives.hxx b/include/pinocchio/algorithm/aba-derivatives.hxx index 56011ca5b2..c50cb74eef 100644 --- a/include/pinocchio/algorithm/aba-derivatives.hxx +++ b/include/pinocchio/algorithm/aba-derivatives.hxx @@ -493,7 +493,6 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); assert(model.check(MimicChecker()) && "Function does not support mimic joints"); - typedef typename ModelTpl::JointIndex JointIndex; data.oa_gf[0] = -model.gravity; @@ -732,7 +731,6 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); assert(model.check(MimicChecker()) && "Function does not support mimic joints"); - typedef typename ModelTpl::JointIndex JointIndex; MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau); @@ -798,7 +796,6 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); assert(model.check(MimicChecker()) && "Function does not support mimic joints"); - typedef typename ModelTpl::JointIndex JointIndex; MatrixType3 & Minv_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixType3, aba_partial_dtau); diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 991a4270a1..f947160dbd 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -227,8 +227,9 @@ namespace pinocchio Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel::MaxNVExtended> SpatialForcesX; typedef JointDataTpl JointData; - typedef typename Eigen::Matrix - MotionSubspace; + typedef + typename Eigen::Matrix + MotionSubspace; typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; typedef SE3Tpl SE3; @@ -242,7 +243,7 @@ namespace pinocchio findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); SpatialForcesX iF(6, jmodel.nvExtended()); // Current joint forces - SE3 iMj = SE3::Identity(); // Transform from current joint to mimic joint + 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--) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 2e10010b02..8a4aa56896 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -178,7 +178,8 @@ namespace pinocchio const JointIndex mimicked_new_id = new_model.getJointId(mimicked_name); 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_vExtended()); + new_model.joints[mimicked_new_id].idx_v(), + new_model.joints[mimicked_new_id].idx_vExtended()); return res; } diff --git a/include/pinocchio/bindings/python/multibody/model.hpp b/include/pinocchio/bindings/python/multibody/model.hpp index 0825b0a334..d8e273c335 100644 --- a/include/pinocchio/bindings/python/multibody/model.hpp +++ b/include/pinocchio/bindings/python/multibody/model.hpp @@ -71,7 +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("nvExtended", &Model::nv, "Dimension of the jacobian matrix space.") + .add_property("nvExtended", &Model::nvExtended, "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.") @@ -91,8 +91,10 @@ 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_vExtendeds", &Model::idx_vExtendeds, "Starting index of the *i*th joint in the jacobian space.") - .add_property("nvExtendeds", &Model::nvExtendeds, "Dimension of the *i*th joint jacobian subspace.") + "idx_vExtendeds", &Model::idx_vExtendeds, + "Starting index of the *i*th joint in the jacobian space.") + .add_property( + "nvExtendeds", &Model::nvExtendeds, "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*, " diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index 1294f5cc56..a9fbec6a2b 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -236,8 +236,8 @@ 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 + * @param[in] vExtended 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 */ @@ -452,7 +452,6 @@ namespace pinocchio const JointDataTpl & jmodel_generic, const JointDataBase & jmodel); - /** * @brief Apply the correct affine transform, on a joint configuration, depending on the joint * type. diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 5ab0b517fd..cdb2cbd85a 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -477,7 +477,11 @@ namespace pinocchio template class JointCollectionTpl> inline void setIndexes( - JointModelTpl & jmodel, JointIndex id, int q, int v, int vExtended) + JointModelTpl & jmodel, + JointIndex id, + int q, + int v, + int vExtended) { return JointSetIndexesVisitor::run(jmodel, id, q, v, vExtended); } diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index ff4b821420..ddeb632523 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -196,12 +196,12 @@ namespace pinocchio typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; - using Base::nvExtended; + using Base::idx_vExtended; using Base::nq; using Base::nv; + using Base::nvExtended; using Base::setIndexes; /// \brief Default contructor diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 1a44fef729..232519a8ce 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -290,9 +290,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointDataDerived createData() const diff --git a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp index 93100b7743..2c1194a425 100644 --- a/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-helical-unaligned.hpp @@ -639,9 +639,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointModelHelicalUnalignedTpl() diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index ed3bae345c..a8c7ae837d 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -838,9 +838,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 52f5294b84..4e86d79493 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -623,12 +623,12 @@ namespace pinocchio typedef InertiaTpl Inertia; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; - using Base::nvExtended; + using Base::idx_vExtended; using Base::nq; using Base::nv; + using Base::nvExtended; using Base::setIndexes; JointModelMimicTpl() diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index 619709120e..441f4f6d7f 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -31,7 +31,7 @@ Options = traits::Options, \ NQ = traits::NQ, \ NV = traits::NV, \ - NVExtended = traits::NVExtended \ + NVExtended = traits::NVExtended \ }; \ typedef TYPENAME traits::ConfigVector_t ConfigVector_t; \ typedef TYPENAME traits::TangentVector_t TangentVector_t @@ -547,7 +547,8 @@ namespace pinocchio typename SizeDepType::template BlockReturn::ConstType jointExtendedModelBlock_impl(const Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); + return SizeDepType::block( + Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); } // Non-const access @@ -576,7 +577,8 @@ namespace pinocchio typename SizeDepType::template BlockReturn::Type jointExtendedModelBlock_impl(Eigen::MatrixBase & Mat) const { - return SizeDepType::block(Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); + return SizeDepType::block( + Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended()); } protected: diff --git a/include/pinocchio/multibody/joint/joint-planar.hpp b/include/pinocchio/multibody/joint/joint-planar.hpp index 4e3d02b583..5164f608f2 100644 --- a/include/pinocchio/multibody/joint/joint-planar.hpp +++ b/include/pinocchio/multibody/joint/joint-planar.hpp @@ -567,9 +567,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 ad990df4c0..9296725619 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic-unaligned.hpp @@ -592,9 +592,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 27cfbde06a..1bca694ce7 100644 --- a/include/pinocchio/multibody/joint/joint-prismatic.hpp +++ b/include/pinocchio/multibody/joint/joint-prismatic.hpp @@ -674,9 +674,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 fb2741724a..fc203cad09 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unaligned.hpp @@ -618,9 +618,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 c4a465f597..7cf88098b1 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp @@ -142,9 +142,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 1e3d432896..90ae6d7d8a 100644 --- a/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp @@ -128,9 +128,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 49340bd77d..f7195f9aed 100644 --- a/include/pinocchio/multibody/joint/joint-revolute.hpp +++ b/include/pinocchio/multibody/joint/joint-revolute.hpp @@ -762,9 +762,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 51bfc1b1ca..14eed470ec 100644 --- a/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp @@ -380,9 +380,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 2d10082c6c..44870f8476 100644 --- a/include/pinocchio/multibody/joint/joint-spherical.hpp +++ b/include/pinocchio/multibody/joint/joint-spherical.hpp @@ -481,9 +481,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 1ac10dbe96..4e3614099d 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -578,9 +578,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; 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 1f24b77193..75b7cbb485 100644 --- a/include/pinocchio/multibody/joint/joint-universal.hpp +++ b/include/pinocchio/multibody/joint/joint-universal.hpp @@ -414,9 +414,9 @@ namespace pinocchio typedef JointModelBase Base; using Base::id; - using Base::idx_vExtended; using Base::idx_q; using Base::idx_v; + using Base::idx_vExtended; using Base::setIndexes; JointModelUniversalTpl() diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index fd63a5968b..7b2c753604 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -294,10 +294,10 @@ namespace pinocchio template class JointCollectionTpl> bool ModelTpl::operator==(const ModelTpl & other) const { - bool res = other.nq == nq && other.nv == nv && other.nvExtended == nvExtended && 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; + bool res = other.nq == nq && other.nv == nv && other.nvExtended == nvExtended + && 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 && other.idx_vExtendeds == idx_vExtendeds && other.nvExtendeds == nvExtendeds; From 95019e4958e9a8b4a75617beb953b9feb4d20117 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 27 Jan 2025 16:07:10 +0100 Subject: [PATCH 195/310] [Check] Add Mimic Checker to checker list --- include/pinocchio/algorithm/default-check.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/default-check.hpp b/include/pinocchio/algorithm/default-check.hpp index 815336084c..4f0c56ee6f 100644 --- a/include/pinocchio/algorithm/default-check.hpp +++ b/include/pinocchio/algorithm/default-check.hpp @@ -12,9 +12,10 @@ namespace pinocchio { /// Default checker-list, used as the default argument in Model::check(). - inline AlgorithmCheckerList makeDefaultCheckerList() + inline AlgorithmCheckerList + makeDefaultCheckerList() { - return makeAlgoCheckerList(ParentChecker(), CRBAChecker(), ABAChecker()); + return makeAlgoCheckerList(ParentChecker(), CRBAChecker(), ABAChecker(), MimicChecker()); } #define DEFAULT_CHECKERS makeDefaultCheckerList() From 9329cc0a1754bbc68c8839a273fe74cc7a86c42f Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Mon, 27 Jan 2025 15:50:47 +0100 Subject: [PATCH 196/310] cppad: Deactivate algorithm geometry and model explicit template instantiation --- include/pinocchio/algorithm/geometry.txx | 4 ++-- include/pinocchio/context/casadi.hpp | 1 + include/pinocchio/context/cppad.hpp | 2 ++ include/pinocchio/context/cppadcg.hpp | 2 ++ src/algorithm/geometry.cpp | 4 ++-- 5 files changed, 9 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/algorithm/geometry.txx b/include/pinocchio/algorithm/geometry.txx index becf526b4b..99d14d0008 100644 --- a/include/pinocchio/algorithm/geometry.txx +++ b/include/pinocchio/algorithm/geometry.txx @@ -5,7 +5,7 @@ #ifndef __pinocchio_algorithm_geometry_txx__ #define __pinocchio_algorithm_geometry_txx__ -#ifndef PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#ifndef PINOCCHIO_SKIP_ALGORTIHM_GEOMETRY namespace pinocchio { @@ -27,6 +27,6 @@ namespace pinocchio } // namespace pinocchio -#endif // PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#endif // PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #endif // ifndef __pinocchio_algorithm_geometry_txx__ diff --git a/include/pinocchio/context/casadi.hpp b/include/pinocchio/context/casadi.hpp index 72104030cb..d26a338316 100644 --- a/include/pinocchio/context/casadi.hpp +++ b/include/pinocchio/context/casadi.hpp @@ -12,6 +12,7 @@ #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN #define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY #define PINOCCHIO_SKIP_ALGORITHM_MODEL +#define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS #define PINOCCHIO_SKIP_CASADI_UNSUPPORTED diff --git a/include/pinocchio/context/cppad.hpp b/include/pinocchio/context/cppad.hpp index ae72ef9a0c..92fe6e248e 100644 --- a/include/pinocchio/context/cppad.hpp +++ b/include/pinocchio/context/cppad.hpp @@ -13,6 +13,8 @@ #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_CHOLESKY #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN #define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY +#define PINOCCHIO_SKIP_ALGORITHM_MODEL +#define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS namespace pinocchio diff --git a/include/pinocchio/context/cppadcg.hpp b/include/pinocchio/context/cppadcg.hpp index 9507b83cc7..6fdce4692a 100644 --- a/include/pinocchio/context/cppadcg.hpp +++ b/include/pinocchio/context/cppadcg.hpp @@ -14,6 +14,8 @@ #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_CHOLESKY #define PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN #define PINOCCHIO_SKIP_ALGORITHM_CHOLESKY +#define PINOCCHIO_SKIP_ALGORITHM_MODEL +#define PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #define PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS namespace pinocchio diff --git a/src/algorithm/geometry.cpp b/src/algorithm/geometry.cpp index b37548592b..934537902c 100644 --- a/src/algorithm/geometry.cpp +++ b/src/algorithm/geometry.cpp @@ -4,7 +4,7 @@ #include "pinocchio/spatial/fwd.hpp" -#ifndef PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#ifndef PINOCCHIO_SKIP_ALGORITHM_GEOMETRY #include "pinocchio/algorithm/geometry.hpp" @@ -28,4 +28,4 @@ namespace pinocchio } // namespace pinocchio -#endif // PINOCCHIO_SKIP_CASADI_UNSUPPORTED +#endif // PINOCCHIO_SKIP_ALGORITHM_GEOMETRY From 10ce8445dd402ada98c8eaa4b3964f1afa57cb89 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Tue, 28 Jan 2025 17:12:51 +0100 Subject: [PATCH 197/310] algorithm: Split check into two headers to avoid include dependency hell in contact-cholesky.hxx --- include/pinocchio/algorithm/check-base.hpp | 36 +++++ include/pinocchio/algorithm/check-data.hpp | 29 ++++ .../algorithm/{check.hxx => check-data.hxx} | 92 +----------- include/pinocchio/algorithm/check-model.hpp | 57 ++++++++ include/pinocchio/algorithm/check-model.hxx | 84 +++++++++++ include/pinocchio/algorithm/check.hpp | 132 +----------------- .../pinocchio/algorithm/contact-cholesky.hxx | 6 +- 7 files changed, 223 insertions(+), 213 deletions(-) create mode 100644 include/pinocchio/algorithm/check-base.hpp create mode 100644 include/pinocchio/algorithm/check-data.hpp rename include/pinocchio/algorithm/{check.hxx => check-data.hxx} (62%) create mode 100644 include/pinocchio/algorithm/check-model.hpp create mode 100644 include/pinocchio/algorithm/check-model.hxx diff --git a/include/pinocchio/algorithm/check-base.hpp b/include/pinocchio/algorithm/check-base.hpp new file mode 100644 index 0000000000..a17e6b28e9 --- /dev/null +++ b/include/pinocchio/algorithm/check-base.hpp @@ -0,0 +1,36 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_base_hpp__ +#define __pinocchio_algorithm_check_base_hpp__ + +#include "pinocchio/multibody/fwd.hpp" + +namespace pinocchio +{ + + /// CRTP class describing the API of the checkers + template + struct AlgorithmCheckerBase + { + AlgorithmCheckerDerived & derived() + { + return *static_cast(this); + } + + const AlgorithmCheckerDerived & derived() const + { + return *static_cast(this); + } + + template class JointCollectionTpl> + bool checkModel(const ModelTpl & model) const + { + return derived().checkModel_impl(model); + } + }; + +} // namespace pinocchio + +#endif // __pinocchio_algorithm_check_base_hpp__ diff --git a/include/pinocchio/algorithm/check-data.hpp b/include/pinocchio/algorithm/check-data.hpp new file mode 100644 index 0000000000..5390066fb2 --- /dev/null +++ b/include/pinocchio/algorithm/check-data.hpp @@ -0,0 +1,29 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_data_hpp__ +#define __pinocchio_algorithm_check_data_hpp__ + +#include "pinocchio/multibody/fwd.hpp" + +namespace pinocchio +{ + + /// Check the validity of data wrt to model, in particular if model has been modified. + /// + /// \param[in] model reference model + /// \param[in] data corresponding data + /// + /// \returns True if data is valid wrt model. + template class JointCollectionTpl> + bool checkData( + const ModelTpl & model, + const DataTpl & data); + +} // namespace pinocchio + +/* --- Details -------------------------------------------------------------------- */ +#include "pinocchio/algorithm/check-data.hxx" + +#endif // __pinocchio_algorithm_check_data_hpp__ diff --git a/include/pinocchio/algorithm/check.hxx b/include/pinocchio/algorithm/check-data.hxx similarity index 62% rename from include/pinocchio/algorithm/check.hxx rename to include/pinocchio/algorithm/check-data.hxx index c6727596b5..2c6ac88678 100644 --- a/include/pinocchio/algorithm/check.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -1,93 +1,15 @@ // -// Copyright (c) 2016-2020 CNRS INRIA +// Copyright (c) 2025 INRIA // -#ifndef __pinocchio_check_hxx__ -#define __pinocchio_check_hxx__ +#ifndef __pinocchio_algorithm_check_data_hxx__ +#define __pinocchio_algorithm_check_data_hxx__ -#include -#include +#include "pinocchio/multibody/model.hpp" +#include "pinocchio/multibody/data.hpp" namespace pinocchio { - namespace internal - { - // Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm - // for all elements in a fusion list of AlgoCheckers. - template class JointCollectionTpl> - struct AlgoFusionChecker - { - typedef bool result_type; - typedef ModelTpl Model; - const Model & model; - - AlgoFusionChecker(const Model & model) - : model(model) - { - } - - inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const - { - return accumul; - } - - template - inline bool operator()(const bool & accumul, const AlgorithmCheckerBase & t) const - { - return accumul && t.checkModel(model); - } - }; - } // namespace internal - - // Check the validity of the kinematic tree defined by parents. - template class JointCollectionTpl> - inline bool - ParentChecker::checkModel_impl(const ModelTpl & model) const - { - typedef ModelTpl Model; - typedef typename Model::JointIndex JointIndex; - - for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) - if (model.parents[j] >= j) - return false; - - return true; - } - - // Check if there is a mimic joint in the kinematic tree - template class JointCollectionTpl> - inline bool - MimicChecker::checkModel_impl(const ModelTpl & model) const - { - typedef ModelTpl Model; - typedef typename Model::JointIndex JointIndex; - - for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) - if (boost::get>(&model.joints[j])) - return false; - - return true; - } - -#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) - template - template class JointCollectionTpl> - bool AlgorithmCheckerList:: - checkModel_impl(const ModelTpl & model) const - { - return boost::fusion::accumulate( - checkerList, true, internal::AlgoFusionChecker(model)); - } -#else - template - template class JointCollectionTpl> - bool AlgorithmCheckerList::checkModel_impl( - const ModelTpl & model) const - { - return boost::fusion::accumulate( - checkerList, true, internal::AlgoFusionChecker(model)); - } -#endif template class JointCollectionTpl> inline bool checkData( @@ -117,7 +39,7 @@ namespace pinocchio CHECK_DATA((int)data.Ycrb.size() == model.njoints); CHECK_DATA((int)data.Yaba.size() == model.njoints); CHECK_DATA((int)data.Fcrb.size() == model.njoints); - BOOST_FOREACH (const typename Data::Matrix6x & F, data.Fcrb) + for (const typename Data::Matrix6x & F : data.Fcrb) { CHECK_DATA(F.cols() == model.nv); } @@ -221,4 +143,4 @@ namespace pinocchio } // namespace pinocchio -#endif // ifndef __pinocchio_check_hxx__ +#endif // __pinocchio_algorithm_check_data_hxx__ diff --git a/include/pinocchio/algorithm/check-model.hpp b/include/pinocchio/algorithm/check-model.hpp new file mode 100644 index 0000000000..bc1dd430c1 --- /dev/null +++ b/include/pinocchio/algorithm/check-model.hpp @@ -0,0 +1,57 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_model_hpp__ +#define __pinocchio_algorithm_check_model_hpp__ + +#include "pinocchio/algorithm/check-base.hpp" + +#include +#include + +namespace pinocchio +{ + +#define PINOCCHIO_DEFINE_ALGO_CHECKER(NAME) \ + struct NAME##Checker : public AlgorithmCheckerBase \ + { \ + template class JointCollectionTpl> \ + bool checkModel_impl(const ModelTpl &) const; \ + } + + /// Simple model checker, that assert that model.parents is indeed a tree. + PINOCCHIO_DEFINE_ALGO_CHECKER(Parent); + /// Simple model checker, that assert that there is a mimic joint in the tree + PINOCCHIO_DEFINE_ALGO_CHECKER(Mimic); + + template + struct AlgorithmCheckerList : AlgorithmCheckerBase> + { + typedef typename boost::fusion::list ArgType; + + AlgorithmCheckerList(const ArgType & checkerList) + : checkerList(checkerList) + { + } + + // Calls model.check for each checker in the fusion::list. + // Each list element is supposed to implement the AlgorithmCheckerBase API. + template class JointCollectionTpl> + bool checkModel_impl(const ModelTpl & model) const; + + const ArgType & checkerList; + }; + + template + AlgorithmCheckerList makeAlgoCheckerList(const T &... args) + { + return AlgorithmCheckerList(boost::fusion::make_list(args...)); + } + +} // namespace pinocchio + +/* --- Details -------------------------------------------------------------------- */ +#include "pinocchio/algorithm/check-model.hxx" + +#endif // __pinocchio_algorithm_check_model_hpp__ diff --git a/include/pinocchio/algorithm/check-model.hxx b/include/pinocchio/algorithm/check-model.hxx new file mode 100644 index 0000000000..d957fe3e3a --- /dev/null +++ b/include/pinocchio/algorithm/check-model.hxx @@ -0,0 +1,84 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_check_model_hxx__ +#define __pinocchio_algorithm_check_model_hxx__ + +#include "pinocchio/multibody/model.hpp" + +#include + +namespace pinocchio +{ + namespace internal + { + // Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm + // for all elements in a fusion list of AlgoCheckers. + template class JointCollectionTpl> + struct AlgoFusionChecker + { + typedef bool result_type; + typedef ModelTpl Model; + const Model & model; + + AlgoFusionChecker(const Model & model) + : model(model) + { + } + + bool operator()(const bool & accumul, const boost::fusion::void_ &) const + { + return accumul; + } + + template + bool operator()(const bool & accumul, const AlgorithmCheckerBase & t) const + { + return accumul && t.checkModel(model); + } + }; + } // namespace internal + + // Check the validity of the kinematic tree defined by parents. + template class JointCollectionTpl> + inline bool + ParentChecker::checkModel_impl(const ModelTpl & model) const + { + typedef ModelTpl Model; + typedef typename Model::JointIndex JointIndex; + + for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) + if (model.parents[j] >= j) + return false; + + return true; + } + + // Check if there is a mimic joint in the kinematic tree + template class JointCollectionTpl> + inline bool + MimicChecker::checkModel_impl(const ModelTpl & model) const + { + typedef ModelTpl Model; + typedef typename Model::JointIndex JointIndex; + + for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) + if (boost::get>(&model.joints[j])) + return false; + + return true; + } + + template + template class JointCollectionTpl> + inline bool AlgorithmCheckerList::checkModel_impl( + const ModelTpl & model) const + { + return boost::fusion::accumulate( + checkerList, true, internal::AlgoFusionChecker(model)); + } + +} // namespace pinocchio + +#endif // __pinocchio_algorithm_check_model_hxx__ diff --git a/include/pinocchio/algorithm/check.hpp b/include/pinocchio/algorithm/check.hpp index 3144804682..841f91563f 100644 --- a/include/pinocchio/algorithm/check.hpp +++ b/include/pinocchio/algorithm/check.hpp @@ -2,131 +2,11 @@ // Copyright (c) 2016-2018 CNRS // -#ifndef __pinocchio_check_hpp__ -#define __pinocchio_check_hpp__ +#ifndef __pinocchio_algorithm_check_hpp__ +#define __pinocchio_algorithm_check_hpp__ -#include "pinocchio/multibody/model.hpp" -#include "pinocchio/multibody/data.hpp" -#include -#include +// This header is provided for backward compatibility +#include "pinocchio/algorithm/check-model.hpp" +#include "pinocchio/algorithm/check-data.hpp" -#ifndef PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE - #define PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE 5 -#endif - -namespace pinocchio -{ - - /// CRTP class describing the API of the checkers - template - struct AlgorithmCheckerBase - { - inline AlgorithmCheckerDerived & derived() - { - return *static_cast(this); - } - - inline const AlgorithmCheckerDerived & derived() const - { - return *static_cast(this); - } - - template class JointCollectionTpl> - inline bool checkModel(const ModelTpl & model) const - { - return derived().checkModel_impl(model); - } - }; - -#define PINOCCHIO_DEFINE_ALGO_CHECKER(NAME) \ - struct NAME##Checker : public AlgorithmCheckerBase \ - { \ - template class JointCollectionTpl> \ - inline bool checkModel_impl(const ModelTpl &) const; \ - } - - /// Simple model checker, that assert that model.parents is indeed a tree. - PINOCCHIO_DEFINE_ALGO_CHECKER(Parent); - /// Simple model checker, that assert that there is a mimic joint in the tree - PINOCCHIO_DEFINE_ALGO_CHECKER(Mimic); - -#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST) - /// Checker having a list of Checker as input argument - template - struct AlgorithmCheckerList - : AlgorithmCheckerBase< - AlgorithmCheckerList> - { - typedef typename boost::fusion::list - ArgType; - - AlgorithmCheckerList(const ArgType & checkerList) - : checkerList(checkerList) - { - } - - // Calls model.check for each checker in the fusion::list. - // Each list element is supposed to implement the AlgorithmCheckerBase API. - template class JointCollectionTpl> - bool checkModel_impl(const ModelTpl & model) const; - - const ArgType & checkerList; - }; - - #define MAKE_ALGO_CHECKER_LIST(z, n, _) \ - /**/ \ - template \ - AlgorithmCheckerList makeAlgoCheckerList( \ - BOOST_PP_ENUM_BINARY_PARAMS(BOOST_PP_INC(n), D, const & arg)) \ - { \ - return AlgorithmCheckerList( \ - boost::fusion::make_list(BOOST_PP_ENUM_PARAMS(BOOST_PP_INC(n), arg))); \ - } - - BOOST_PP_REPEAT(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE, MAKE_ALGO_CHECKER_LIST, BOOST_PP_EMPTY) -#else - template - struct AlgorithmCheckerList : AlgorithmCheckerBase> - { - typedef typename boost::fusion::list ArgType; - - AlgorithmCheckerList(const ArgType & checkerList) - : checkerList(checkerList) - { - } - - // Calls model.check for each checker in the fusion::list. - // Each list element is supposed to implement the AlgorithmCheckerBase API. - template class JointCollectionTpl> - bool checkModel_impl(const ModelTpl & model) const; - - const ArgType & checkerList; - }; - - template - AlgorithmCheckerList makeAlgoCheckerList(const T &... args) - { - return AlgorithmCheckerList(boost::fusion::make_list(args...)); - } - -#endif - - /// Check the validity of data wrt to model, in particular if model has been modified. - /// - /// \param[in] model reference model - /// \param[in] data corresponding data - /// - /// \returns True if data is valid wrt model. - template class JointCollectionTpl> - inline bool checkData( - const ModelTpl & model, - const DataTpl & data); - -} // namespace pinocchio - -/* --- Details -------------------------------------------------------------------- */ -#include "pinocchio/algorithm/check.hxx" - -#endif // ifndef __pinocchio_check_hpp__ +#endif // __pinocchio_algorithm_check_hpp__ diff --git a/include/pinocchio/algorithm/contact-cholesky.hxx b/include/pinocchio/algorithm/contact-cholesky.hxx index bdc95886ed..beccdb6062 100644 --- a/include/pinocchio/algorithm/contact-cholesky.hxx +++ b/include/pinocchio/algorithm/contact-cholesky.hxx @@ -5,7 +5,8 @@ #ifndef __pinocchio_algorithm_contact_cholesky_hxx__ #define __pinocchio_algorithm_contact_cholesky_hxx__ -#include "pinocchio/algorithm/check.hpp" +#include "pinocchio/algorithm/check-model.hpp" +#include "pinocchio/multibody/data.hpp" #include @@ -176,7 +177,8 @@ namespace pinocchio template< typename S1, int O1, - template class JointCollectionTpl, + template + class JointCollectionTpl, class ConstraintModelAllocator, class ConstraintDataAllocator, typename VectorLike> From 5b066c89f1a7c305c4c3955db80a6b180591e6ee Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 28 Jan 2025 17:56:58 +0100 Subject: [PATCH 198/310] [Cmake] Update sources file --- sources.cmake | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/sources.cmake b/sources.cmake index f8cd400628..9b884e5f40 100644 --- a/sources.cmake +++ b/sources.cmake @@ -17,8 +17,11 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal-derivatives.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal.hxx - ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check.hpp - ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check.hxx + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-base.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-data.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-data.hxx + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-model.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-model.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/cholesky.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/cholesky.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/compute-all-terms.hpp From f951f464a6f44d3926dc5ab3f73650a28af3ca14 Mon Sep 17 00:00:00 2001 From: Joris Vaillant Date: Tue, 28 Jan 2025 22:24:07 +0100 Subject: [PATCH 199/310] cmake: Add check.hpp --- sources.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/sources.cmake b/sources.cmake index 9b884e5f40..17e0cedac0 100644 --- a/sources.cmake +++ b/sources.cmake @@ -17,6 +17,7 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal-derivatives.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/centroidal.hxx + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-base.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-data.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/check-data.hxx From 5c40259539ff9ce5cec5e3eb7c5eee54455916c1 Mon Sep 17 00:00:00 2001 From: Antoine Bussy Date: Thu, 12 Dec 2024 14:36:14 +0100 Subject: [PATCH 200/310] [Unittest] Check consistency of cast models with a mimic joint. --- unittest/model.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/unittest/model.cpp b/unittest/model.cpp index 4f86535f17..cee8d150d3 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -950,4 +950,27 @@ BOOST_AUTO_TEST_CASE(test_has_transform_to_mimic) } } +BOOST_AUTO_TEST_CASE(test_cast_mimic) +{ + Model humanoid_model, humanoid_mimic; + buildModels::humanoid(humanoid_model); + Data data(humanoid_model); + BOOST_CHECK(humanoid_model.check(data)); + + 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); + data = Data(humanoid_mimic); + BOOST_CHECK(humanoid_mimic.check(data)); + + humanoid_mimic = humanoid_mimic.cast(); + data = Data(humanoid_mimic); + BOOST_CHECK(humanoid_mimic.check(data)); + + ModelTpl humanoid_mimic_f = humanoid_mimic.cast(); + DataTpl data_f(humanoid_mimic_f); + BOOST_CHECK(humanoid_mimic_f.check(data_f)); +} + BOOST_AUTO_TEST_SUITE_END() From dbd490b162719015b9089f1a27382131bc368c64 Mon Sep 17 00:00:00 2001 From: Antoine Bussy Date: Fri, 13 Dec 2024 09:54:00 +0100 Subject: [PATCH 201/310] [Unittest] Check specific indices when casting mimic joints. These are the specific checks that fail in checkData(). --- unittest/model.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/unittest/model.cpp b/unittest/model.cpp index cee8d150d3..e3247e0237 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -967,10 +967,16 @@ BOOST_AUTO_TEST_CASE(test_cast_mimic) humanoid_mimic = humanoid_mimic.cast(); data = Data(humanoid_mimic); BOOST_CHECK(humanoid_mimic.check(data)); + index_s = humanoid_mimic.getJointId("lleg_shoulder3_joint"); + BOOST_CHECK_EQUAL(humanoid_mimic.idx_qs[index_s], humanoid_mimic.joints[index_s].idx_q()); + BOOST_CHECK_EQUAL(humanoid_mimic.idx_vs[index_s], humanoid_mimic.joints[index_s].idx_v()); ModelTpl humanoid_mimic_f = humanoid_mimic.cast(); DataTpl data_f(humanoid_mimic_f); BOOST_CHECK(humanoid_mimic_f.check(data_f)); + index_s = humanoid_mimic_f.getJointId("lleg_shoulder3_joint"); + BOOST_CHECK_EQUAL(humanoid_mimic_f.idx_qs[index_s], humanoid_mimic_f.joints[index_s].idx_q()); + BOOST_CHECK_EQUAL(humanoid_mimic_f.idx_vs[index_s], humanoid_mimic_f.joints[index_s].idx_v()); } BOOST_AUTO_TEST_SUITE_END() From a7e70b091c383ec822c6dcb314dcb939bc078a4f Mon Sep 17 00:00:00 2001 From: Antoine Bussy Date: Thu, 26 Dec 2024 07:35:54 +0100 Subject: [PATCH 202/310] [Unittest] Add test of buildReducedModel with mimic joints. --- unittest/model.cpp | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/unittest/model.cpp b/unittest/model.cpp index e3247e0237..40d52231a0 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -979,4 +979,31 @@ BOOST_AUTO_TEST_CASE(test_cast_mimic) BOOST_CHECK_EQUAL(humanoid_mimic_f.idx_vs[index_s], humanoid_mimic_f.joints[index_s].idx_v()); } +BOOST_AUTO_TEST_CASE(test_build_reduced_model_mimic) +{ + typedef Model::JointCollection JC; + typedef Model::JointIndex JointIndex; + typedef JC::JointModelRX JointModelRX; + typedef JC::JointModelRUBX JointModelRUBX; + Model model; + + JointIndex ffidx = model.addJoint(0, JC::JointModelFreeFlyer(), SE3::Identity(), "root_joint"); + model.addJointFrame(ffidx); + JointIndex idx = model.addJoint(ffidx, JointModelRX(), SE3::Identity(), "joint1"); + model.addJointFrame(idx); + idx = model.addJoint(idx, JointModelRUBX(), SE3::Identity(), "joint2"); + model.addJointFrame(idx); + idx = model.addJoint(idx, JointModelRX(), SE3::Identity(), "joint3"); + model.addJointFrame(idx); + + auto const mimic = JointModelMimic(boost::get(model.joints[model.getJointId("joint3")].toVariant()), 1., 0.); + idx = model.addJoint(idx, mimic, SE3::Identity(), "joint4"); + model.addJointFrame(idx); + + // We lock a 2 DoF joint that is before the mimicked joint. + const std::vector joints_to_lock = {model.getJointId("joint2")}; + Model reduced_model; + BOOST_CHECK_NO_THROW(buildReducedModel(model, joints_to_lock, neutral(model), reduced_model)); +} + BOOST_AUTO_TEST_SUITE_END() From 67967feea6dbf8dbec0af57a8168c5df5ad0b79f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 30 Jan 2025 14:46:15 +0100 Subject: [PATCH 203/310] [urdf] Simplify mimic parsing --- .../pinocchio/algorithm/contact-cholesky.hxx | 3 +- include/pinocchio/parsers/urdf/model.hxx | 118 +++++++++--------- src/parsers/urdf/model.cpp | 5 - 3 files changed, 63 insertions(+), 63 deletions(-) diff --git a/include/pinocchio/algorithm/contact-cholesky.hxx b/include/pinocchio/algorithm/contact-cholesky.hxx index beccdb6062..e8834ea79f 100644 --- a/include/pinocchio/algorithm/contact-cholesky.hxx +++ b/include/pinocchio/algorithm/contact-cholesky.hxx @@ -177,8 +177,7 @@ namespace pinocchio template< typename S1, int O1, - template - class JointCollectionTpl, + template class JointCollectionTpl, class ConstraintModelAllocator, class ConstraintDataAllocator, typename VectorLike> diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 9662644479..a8641fbf51 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -121,9 +121,6 @@ namespace pinocchio const Scalar offset, JointType jointType) = 0; - virtual void convertMimicJoint( - const std::string & mimic_name, const MimicInfo & mimic_info) = 0; - UrdfVisitorBaseTpl() : log(NULL) { @@ -301,11 +298,30 @@ 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); + switch (this->mimicInfo_map[joint_name].jointType) + { + case Base::REVOLUTE: + joint_id = addMimicJoint< + typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, + typename JointCollection::JointModelRZ, + typename JointCollection::JointModelRevoluteUnaligned>( + frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, + friction, damping); + break; + case Base::PRISMATIC: + joint_id = addMimicJoint< + typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, + typename JointCollection::JointModelPZ, + typename JointCollection::JointModelPrismaticUnaligned>( + frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, + friction, damping); + break; + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT( + false, + "Cannot mimic this type. Only revolute, prismatic and helicoidal can be mimicked"); + break; + } break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); @@ -475,73 +491,63 @@ namespace pinocchio 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< - typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, - typename JointCollection::JointModelRZ, - typename JointCollection::JointModelRevoluteUnaligned>( - mimic_name, mimic_info.mimicked_name, mimic_info.axis, mimic_info.multiplier, - mimic_info.offset); - break; - case Base::PRISMATIC: - createMimicJoint< - typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, - typename JointCollection::JointModelPZ, - typename JointCollection::JointModelPrismaticUnaligned>( - mimic_name, mimic_info.mimicked_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 & mimicked_name, - const Vector3 & axis, - const Scalar multiplier, - const Scalar offset) + JointIndex addMimicJoint( + 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) { - auto mimicked_joint = model.joints[getJointId(mimicked_name)]; - auto & mimic_joint = model.joints[getJointId(mimic_name)]; - int id = mimic_joint.id(); - int idx_vExtended = mimic_joint.idx_vExtended(); - CartesianAxis axisType = extractCartesianAxis(axis); + const MimicInfo mimic_info = this->mimicInfo_map[joint_name]; + auto mimicked_joint = model.joints[getJointId(mimic_info.mimicked_name)]; + + CartesianAxis axisType = extractCartesianAxis(mimic_info.axis); switch (axisType) { case AXIS_X: - mimic_joint = typename JointCollection::JointModelMimic( - TypeX(), mimicked_joint, multiplier, offset); + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeX(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); break; - case AXIS_Y: - mimic_joint = typename JointCollection::JointModelMimic( - TypeY(), mimicked_joint, multiplier, offset); + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeY(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); break; case AXIS_Z: - mimic_joint = typename JointCollection::JointModelMimic( - TypeZ(), mimicked_joint, multiplier, offset); + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeZ(), mimicked_joint, mimic_info.multiplier, mimic_info.offset), + frame.placement * placement, joint_name, max_effort, max_velocity, min_config, + max_config, friction, damping); break; case AXIS_UNALIGNED: - mimic_joint = typename JointCollection::JointModelMimic( - TypeUnaligned(), mimicked_joint, multiplier, offset); + return model.addJoint( + frame.parentJoint, + typename JointCollection::JointModelMimic( + TypeUnaligned(), mimicked_joint, mimic_info.multiplier, mimic_info.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; } - mimic_joint.setIndexes(id, mimicked_joint.idx_q(), mimicked_joint.idx_v(), idx_vExtended); } private: diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index e247702c58..1bd46bc8f3 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -311,11 +311,6 @@ 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 f91de040ec76a27b233614ef0bda09af7f950912 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 30 Jan 2025 14:46:35 +0100 Subject: [PATCH 204/310] [Check] Remove mimic exception --- include/pinocchio/algorithm/check-data.hxx | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 2c6ac88678..50b42af57f 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -95,15 +95,17 @@ namespace pinocchio CHECK_DATA(model.nvExtendeds[joint_id] == jmodel.nvExtended()); CHECK_DATA(model.idx_vExtendeds[joint_id] == jmodel.idx_vExtended()); } - for (JointIndex j = 1; int(j) < model.njoints; ++j) { - // do not check mimic joint, because nv = 0, but it has a subtree - 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(); + int nv; + if (boost::get>(&model.joints[j])) + nv = boost::get>(model.joints[j]) + .jmodel() + .nv(); + else + nv = model.joints[j].nv(); for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant { CHECK_DATA(model.parents[d] >= j); From d9ca5369bc28431f9b98ff507b8ac7e99050920c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 30 Jan 2025 14:47:53 +0100 Subject: [PATCH 205/310] [Joint] Mimic is not allowed before its parent (throw) --- include/pinocchio/multibody/joint/joint-free-flyer.hpp | 2 +- include/pinocchio/multibody/joint/joint-mimic.hpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-free-flyer.hpp b/include/pinocchio/multibody/joint/joint-free-flyer.hpp index 232519a8ce..c94044c80a 100644 --- a/include/pinocchio/multibody/joint/joint-free-flyer.hpp +++ b/include/pinocchio/multibody/joint/joint-free-flyer.hpp @@ -191,7 +191,7 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; - typedef boost::mpl::true_ is_mimicable_t; + typedef boost::mpl::false_ is_mimicable_t; PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 4e86d79493..211ede568b 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -685,6 +685,9 @@ namespace pinocchio void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended) { + PINOCCHIO_THROW( + (id > m_jmodel_ref.id()), std::invalid_argument, + "Mimic joint index is lower than its directing joint. Should never happen"); Base::i_id = id; // When setting the indexes q and v should remain on the mimicked joint // Base::i_q = q; From 4dcac6eb00c3df7f5da8076f3d9a3962a475066f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 30 Jan 2025 14:48:20 +0100 Subject: [PATCH 206/310] [test/check] Add test for mimic data --- unittest/algo-check.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/unittest/algo-check.cpp b/unittest/algo-check.cpp index a4797b1b70..0114f008d6 100644 --- a/unittest/algo-check.cpp +++ b/unittest/algo-check.cpp @@ -54,7 +54,9 @@ BOOST_AUTO_TEST_CASE(test_check) pinocchio::Model model_mimic; buildModels::humanoidRandom(model_mimic, false, true); + pinocchio::Data data_mimic(model_mimic); BOOST_CHECK(!model_mimic.check(MimicChecker())); + BOOST_CHECK(model_mimic.check(data_mimic)); } BOOST_AUTO_TEST_SUITE_END() From 139f921b234c056025b2a4c2c6701432ed676f0b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 30 Jan 2025 14:48:47 +0100 Subject: [PATCH 207/310] [test] Modify to be conform to the new throw in mimic joint --- unittest/all-joints.cpp | 2 +- unittest/casadi/joints.cpp | 4 ++-- unittest/joint-mimic.cpp | 19 +++++++++---------- unittest/joint-motion-subspace.cpp | 5 ++--- unittest/serialization.cpp | 2 +- unittest/visitor.cpp | 2 +- 6 files changed, 16 insertions(+), 18 deletions(-) diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index de7bdee47a..cdde132bfd 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -119,7 +119,7 @@ struct init> JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); return jmodel; } }; diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 5b657d7766..38ad2614e4 100644 --- a/unittest/casadi/joints.cpp +++ b/unittest/casadi/joints.cpp @@ -275,7 +275,7 @@ struct init> JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); return jmodel; } @@ -291,7 +291,7 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel_ jmodel = init::run(); - jmodel.setIndexes(0, 0, 0, 0); + // jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 146d7a55c4..d1062c0010 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -93,7 +93,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -101,7 +101,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -109,7 +109,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); test_constraint_mimic(jmodel); } @@ -187,7 +187,7 @@ struct TestJointMimic void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); test_joint_mimic(jmodel); } @@ -195,7 +195,7 @@ struct TestJointMimic void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); test_joint_mimic( jmodel); @@ -204,7 +204,7 @@ struct TestJointMimic void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); test_joint_mimic( jmodel); @@ -287,15 +287,14 @@ BOOST_AUTO_TEST_CASE(test_joint_generic_cast) jmodel_ref.setIndexes(1, 2, 3, 3); JointModelMimic jmodel(jmodel_ref, 2., 1.); - jmodel.setIndexes(1, -1, -1, 3); + jmodel.setIndexes(2, -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(2, -2, -2, 3); - BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id()); + BOOST_CHECK(jmodel_generic.id() == jmodel.id()); } BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index 6c1c899d51..ab2e80430f 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -397,9 +397,8 @@ struct init> { typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModelRX jmodel_ref = init::run(); - JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); return jmodel; } @@ -441,7 +440,7 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModel jmodel = init::run(); - jmodel.setIndexes(0, 0, 0, 0); + // jmodel.setIndexes(0, 0, 0, 0); test_constraint_operations(jmodel); } diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 68dafa72e1..291b33ab5a 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -408,7 +408,7 @@ struct init> JointModelRX jmodel_ref = init::run(); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); return jmodel; } diff --git a/unittest/visitor.cpp b/unittest/visitor.cpp index 8b4db0d743..e03cd905f7 100644 --- a/unittest/visitor.cpp +++ b/unittest/visitor.cpp @@ -265,7 +265,7 @@ struct init> jmodel_ref.setIndexes(0, 0, 0, 0); JointModel jmodel(jmodel_ref, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(1, 0, 0, 0); return jmodel; } From eb514533a17dffbf679cf034fca94d980b48bf91 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 30 Jan 2025 14:49:25 +0100 Subject: [PATCH 208/310] [joint/translation] Go back to old api --- include/pinocchio/multibody/joint/joint-translation.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-translation.hpp b/include/pinocchio/multibody/joint/joint-translation.hpp index 4e3614099d..5520c5a0ab 100644 --- a/include/pinocchio/multibody/joint/joint-translation.hpp +++ b/include/pinocchio/multibody/joint/joint-translation.hpp @@ -601,7 +601,7 @@ namespace pinocchio template void calc(JointDataDerived & data, const typename Eigen::MatrixBase & qs) const { - data.joint_q = this->jointConfigExtendedModelSelector(qs); + data.joint_q = this->jointConfigSelector(qs); data.M.translation() = data.joint_q; } @@ -610,7 +610,7 @@ namespace pinocchio calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase & vs) const { - data.joint_v = this->jointVelocityExtendedModelSelector(vs); + data.joint_v = this->jointVelocitySelector(vs); data.v.linear() = data.joint_v; } @@ -622,7 +622,7 @@ namespace pinocchio { calc(data, qs.derived()); - data.joint_v = this->jointVelocityExtendedModelSelector(vs); + data.joint_v = this->jointVelocitySelector(vs); data.v.linear() = data.joint_v; } From 74df625fc90ed4d74a1c0f7bee8f8c2d791694db Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 31 Jan 2025 13:28:55 +0100 Subject: [PATCH 209/310] [Check] Remove MimicChecker from default list --- include/pinocchio/algorithm/default-check.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/default-check.hpp b/include/pinocchio/algorithm/default-check.hpp index 4f0c56ee6f..b6d4346674 100644 --- a/include/pinocchio/algorithm/default-check.hpp +++ b/include/pinocchio/algorithm/default-check.hpp @@ -12,10 +12,10 @@ namespace pinocchio { /// Default checker-list, used as the default argument in Model::check(). - inline AlgorithmCheckerList + inline AlgorithmCheckerList makeDefaultCheckerList() { - return makeAlgoCheckerList(ParentChecker(), CRBAChecker(), ABAChecker(), MimicChecker()); + return makeAlgoCheckerList(ParentChecker(), CRBAChecker(), ABAChecker()); } #define DEFAULT_CHECKERS makeDefaultCheckerList() From bcdd7d8ddf8b275782d2ef264fae0315f8054f82 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 31 Jan 2025 16:34:39 +0100 Subject: [PATCH 210/310] [Check] Remove mimic from default checker --- include/pinocchio/algorithm/default-check.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/default-check.hpp b/include/pinocchio/algorithm/default-check.hpp index b6d4346674..815336084c 100644 --- a/include/pinocchio/algorithm/default-check.hpp +++ b/include/pinocchio/algorithm/default-check.hpp @@ -12,8 +12,7 @@ namespace pinocchio { /// Default checker-list, used as the default argument in Model::check(). - inline AlgorithmCheckerList - makeDefaultCheckerList() + inline AlgorithmCheckerList makeDefaultCheckerList() { return makeAlgoCheckerList(ParentChecker(), CRBAChecker(), ABAChecker()); } From 8efd5b1f4fc1cc5b2a643746614128ab75ee6f2d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 31 Jan 2025 16:35:03 +0100 Subject: [PATCH 211/310] Remove dead code lines --- .../pinocchio/multibody/joint/joint-mimic.hpp | 5 -- .../pinocchio/serialization/joints-data.hpp | 8 -- unittest/serialization.cpp | 86 +------------------ 3 files changed, 3 insertions(+), 96 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 211ede568b..bfbe57e097 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -616,7 +616,6 @@ namespace pinocchio typedef JointCollectionTpl JointCollection; typedef JointModelTpl JointModel; - typedef typename JointModel::JointModelVariant JointModelVariant; typedef SE3Tpl SE3; typedef MotionTpl Motion; @@ -754,13 +753,10 @@ namespace pinocchio const Eigen::MatrixBase & I, 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() @@ -778,7 +774,6 @@ namespace pinocchio typename CastType::type cast() const { typedef typename CastType::type ReturnType; - ReturnType res( m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), ScalarCast::cast(m_offset)); diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 5ec27b75d0..1a4fc0e432 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -77,14 +77,6 @@ namespace boost ar & make_nvp("joint_v", joint_data.joint_v()); 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 diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 291b33ab5a..0445d1be00 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -476,35 +476,7 @@ 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(); - - // 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); + // Do nothing } template @@ -562,37 +534,7 @@ 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(); - - // 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); + // Do nothing } template @@ -665,29 +607,7 @@ 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::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); - // test(jdata_mimic); + // Do nothing } template From 93c2303bc14332cfbb264dd99e7eb7e98c72374e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 31 Jan 2025 16:35:31 +0100 Subject: [PATCH 212/310] [serialize] Fix serialize for mimic --- include/pinocchio/serialization/joints-model.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index 4564eba7e0..f6fa94b51f 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -92,8 +92,8 @@ namespace boost void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_vExtended) { - joint_m.setIndexes(i_id, 0, 0, i_vExtended); joint_m.setMimicIndexes(0, i_q, i_v, 0); + joint_m.setIndexes(i_id, 0, 0, i_vExtended); } }; From 5677d734ade70c9e25c75a437de3e4beeb4523ec Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 31 Jan 2025 17:11:41 +0100 Subject: [PATCH 213/310] [joint_mimic] Fix cast of joint --- include/pinocchio/multibody/joint/joint-mimic.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index bfbe57e097..79b008a677 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -778,6 +778,7 @@ namespace pinocchio m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), ScalarCast::cast(m_offset)); res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); + res.setMimicIndexes(m_jmodel_ref.id(), idx_q(), idx_v(), idx_vExtended()); return res; } From 1180f08a4d955031a8a611782c43f7100adb137b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 2 Feb 2025 19:22:28 +0100 Subject: [PATCH 214/310] [EtienneAr feedback] topic/mimic: pretty print for mimic joints --- .../multibody/joint/joint-composite.hpp | 46 +++++++------------ .../multibody/joint/joint-data-base.hpp | 4 +- .../pinocchio/multibody/joint/joint-mimic.hpp | 15 ++++++ .../multibody/joint/joint-model-base.hpp | 4 +- 4 files changed, 36 insertions(+), 33 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index ddeb632523..cf3b3503cc 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -150,21 +150,14 @@ namespace pinocchio { return classname(); } - }; - - template class JointCollectionTpl> - inline std::ostream & operator<<( - std::ostream & os, const JointDataCompositeTpl & jdata) - { - typedef typename JointDataCompositeTpl::JointDataVector - JointDataVector; - os << "JointDataComposite containing following models:\n"; - for (typename JointDataVector::const_iterator it = jdata.joints.begin(); - it != jdata.joints.end(); ++it) - os << " " << shortname(*it) << std::endl; - return os; - } + void disp(std::ostream & os) const + { + os << "JointDataComposite containing following models:\n"; + for (typename JointDataVector::const_iterator it = joints.begin(); it != joints.end(); ++it) + os << " " << it->shortname() << std::endl; + } + }; template< typename NewScalar, @@ -630,6 +623,16 @@ namespace pinocchio return A.middleCols(Base::i_vExtended, nvExtended()); } + void disp(std::ostream & os) const + { + typedef typename JointModelCompositeTpl::JointModelVector + JointModelVector; + + os << "JointModelComposite containing following models:\n"; + for (typename JointModelVector::const_iterator it = joints.begin(); it != joints.end(); ++it) + os << " " << it->shortname() << std::endl; + } + protected: friend struct Serialize; @@ -691,21 +694,6 @@ namespace pinocchio int njoints; }; - template class JointCollectionTpl> - inline std::ostream & operator<<( - std::ostream & os, const JointModelCompositeTpl & jmodel) - { - typedef typename JointModelCompositeTpl::JointModelVector - JointModelVector; - - os << "JointModelComposite containing following models:\n"; - for (typename JointModelVector::const_iterator it = jmodel.joints.begin(); - it != jmodel.joints.end(); ++it) - os << " " << shortname(*it) << std::endl; - - return os; - } - } // namespace pinocchio #include diff --git a/include/pinocchio/multibody/joint/joint-data-base.hpp b/include/pinocchio/multibody/joint/joint-data-base.hpp index e47c38c415..8782310985 100644 --- a/include/pinocchio/multibody/joint/joint-data-base.hpp +++ b/include/pinocchio/multibody/joint/joint-data-base.hpp @@ -273,9 +273,9 @@ namespace pinocchio os << shortname() << endl; } - friend std::ostream & operator<<(std::ostream & os, const JointDataBase & joint) + friend std::ostream & operator<<(std::ostream & os, const JointDataBase & jdata) { - joint.disp(os); + jdata.derived().disp(os); return os; } diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 79b008a677..55e8621527 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -562,6 +562,12 @@ namespace pinocchio return joint_v_transformed; } + void disp(std::ostream & os) const + { + Base::disp(os); + os << " Mimicking joint data: " << m_jdata_ref.shortname() << std::endl; + } + protected: RefJointData m_jdata_ref; @@ -886,6 +892,15 @@ namespace pinocchio Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); } + void disp(std::ostream & os) const + { + Base::disp(os); + os << " Mimicking joint type: " << m_jmodel_ref.shortname() << std::endl; + os << " Mimicked joint id: " << m_jmodel_ref.id() << std::endl; + os << " Mimic scaling: " << m_scaling << std::endl; + os << " Mimic offset: " << m_offset << std::endl; + } + }; // struct JointModelMimicTpl } // namespace pinocchio diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index 441f4f6d7f..6045f63721 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -227,9 +227,9 @@ namespace pinocchio << " nvExtended: " << nvExtended() << endl; } - friend std::ostream & operator<<(std::ostream & os, const JointModelBase & joint) + friend std::ostream & operator<<(std::ostream & os, const JointModelBase & jmodel) { - joint.disp(os); + jmodel.derived().disp(os); return os; } From c923af53d159383be1ab28ce6f2e22c51a4ba19d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Sun, 2 Feb 2025 19:58:54 +0100 Subject: [PATCH 215/310] [EtienneAr feedback] topic/mimic: Store nq/nv and remove mimicking joint visitor call --- .../pinocchio/multibody/joint/joint-mimic.hpp | 30 ++++++++++--------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 55e8621527..cd7f21dd6a 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -656,6 +656,8 @@ namespace pinocchio : m_jmodel_ref(checkMimic((JointModel)jmodel_mimicking.derived())) , m_scaling(scaling) , m_offset(offset) + , m_nqExtended(jmodel_mimicking.nq()) + , m_nvExtended(jmodel_mimicking.nvExtended()) { assert(jmodel_mimicking.nq() == jmodel_mimicked.nq()); assert(jmodel_mimicking.nv() == jmodel_mimicked.nv()); @@ -685,7 +687,7 @@ namespace pinocchio } inline int nvExtended_impl() const { - return m_jmodel_ref.nvExtended(); + return m_nvExtended; } void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended) @@ -714,8 +716,7 @@ namespace pinocchio 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_nqExtended, m_nvExtended); } const std::vector hasConfigurationLimit() const @@ -732,7 +733,7 @@ namespace pinocchio EIGEN_DONT_INLINE void calc(JointDataDerived & jdata, const typename Eigen::MatrixBase & qs) const { - jdata.joint_q = qs.segment(idx_q(), m_jmodel_ref.nq()); + jdata.joint_q = qs.segment(Base::i_q, m_nqExtended); configVectorAffineTransform( 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); @@ -744,8 +745,8 @@ namespace pinocchio const typename Eigen::MatrixBase & qs, const typename Eigen::MatrixBase & vs) const { - jdata.joint_q = qs.segment(idx_q(), m_jmodel_ref.nq()); - jdata.joint_v = vs.segment(idx_v(), m_jmodel_ref.nv()); + jdata.joint_q = qs.segment(Base::i_q, m_nqExtended); + jdata.joint_v = vs.segment(Base::i_v, m_nvExtended); configVectorAffineTransform( m_jmodel_ref, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); jdata.joint_v_transformed = m_scaling * jdata.joint_v; @@ -819,6 +820,7 @@ namespace pinocchio // data JointModel m_jmodel_ref; Scalar m_scaling, m_offset; + int m_nqExtended, m_nvExtended; public: /* Acces to dedicated segment in robot config space. */ @@ -827,7 +829,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::ConstType jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), Base::i_q, m_nqExtended); } // Non-const access @@ -835,7 +837,7 @@ namespace pinocchio typename SizeDepType::template SegmentReturn::Type jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const { - return SizeDepType::segment(a.derived(), idx_q(), m_jmodel_ref.nq()); + return SizeDepType::segment(a.derived(), Base::i_q, m_nqExtended); } /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ @@ -844,7 +846,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::ConstType jointCols_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleCols(A.derived(), Base::i_v, m_nvExtended); } // Non-const access @@ -852,7 +854,7 @@ namespace pinocchio typename SizeDepType::template ColsReturn::Type jointCols_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleCols(A.derived(), idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleCols(A.derived(), Base::i_v, m_nvExtended); } /* Acces to dedicated rows in a matrix.*/ @@ -861,7 +863,7 @@ namespace pinocchio typename SizeDepType::template RowsReturn::ConstType joinVelRows_impl(const Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleRows(A.derived(), Base::i_v, m_nvExtended); } // Non-const access @@ -869,7 +871,7 @@ namespace pinocchio typename SizeDepType::template RowsReturn::Type jointRows_impl(Eigen::MatrixBase & A) const { - return SizeDepType::middleRows(A.derived(), idx_v(), m_jmodel_ref.nv()); + return SizeDepType::middleRows(A.derived(), Base::i_v, m_nvExtended); } // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the @@ -880,7 +882,7 @@ namespace pinocchio jointBlock_impl(const Eigen::MatrixBase & Mat) const { return SizeDepType::block( - Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); + Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended); } // Non-const access @@ -889,7 +891,7 @@ namespace pinocchio jointBlock_impl(Eigen::MatrixBase & Mat) const { return SizeDepType::block( - Mat.derived(), idx_v(), idx_v(), m_jmodel_ref.nv(), m_jmodel_ref.nv()); + Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended); } void disp(std::ostream & os) const From 5dcc533163e4fcf07e0278b7916eec33d1d93813 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 12:03:03 +0100 Subject: [PATCH 216/310] [Joint] Re-introduce old api for setIndexes --- .../python/multibody/joint/joint-derived.hpp | 21 +++++++++++++++++-- .../bindings/python/multibody/joint/joint.hpp | 19 ++++++++++++++++- .../multibody/joint/joint-basic-visitors.hpp | 17 +++++++++++++++ .../multibody/joint/joint-basic-visitors.hxx | 7 +++++++ .../multibody/joint/joint-generic.hpp | 5 +++++ .../multibody/joint/joint-model-base.hpp | 5 +++++ 6 files changed, 71 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp b/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp index 3a51ca2c29..16041d2d2d 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp @@ -40,9 +40,10 @@ namespace pinocchio .add_property( "hasConfigurationLimitInTangent", &JointModelDerived::hasConfigurationLimitInTangent, "Return vector of boolean if joint has configuration limits in tangent space.") + .def("setIndexes", &setIndexes0, bp::args("self", "joint_id", "idx_q", "idx_v")) .def( - "setIndexes", &JointModelDerived::setIndexes, - bp::args("self", "joint_id", "idx_q", "idx_v")) + "setIndexes", &setIndexes1, + bp::args("self", "joint_id", "idx_q", "idx_v", "idx_vExtended")) .def("classname", &JointModelDerived::classname) .staticmethod("classname") .def("calc", &calc0, bp::args("self", "jdata", "q")) @@ -111,6 +112,22 @@ namespace pinocchio { self.calc(jdata, q, v); } + + static void + setIndexes0(JointModelDerived & self, const int & id, const int & idx_q, const int & idx_v) + { + self.setIndexes(id, idx_q, idx_v); + } + + static void setIndexes1( + JointModelDerived & self, + const int & id, + const int & idx_q, + const int & idx_v, + const int & idx_vExtended) + { + self.setIndexes(id, idx_q, idx_v, idx_vExtended); + } }; template diff --git a/include/pinocchio/bindings/python/multibody/joint/joint.hpp b/include/pinocchio/bindings/python/multibody/joint/joint.hpp index cf589de467..c66904b639 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joint.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joint.hpp @@ -35,7 +35,8 @@ namespace pinocchio .def( "hasConfigurationLimitInTangent", &JointModel::hasConfigurationLimitInTangent, "Return vector of boolean if joint has configuration limits in tangent space.") - .def("setIndexes", &JointModel::setIndexes, bp::args("self", "id", "idx_q", "idx_v")) + .def("setIndexes", setIndexes0, bp::args("self", "id", "idx_q", "idx_v")) + .def("setIndexes", setIndexes1, bp::args("self", "id", "idx_q", "idx_v", "idx_vExtended")) .def( "hasSameIndexes", &JointModel::hasSameIndexes, bp::args("self", "other"), "Check if this has same indexes than other.") @@ -82,6 +83,22 @@ namespace pinocchio return self.nv(); } + static void + setIndexes0(JointModelDerived & self, const int & id, const int & idx_q, const int & idx_v) + { + self.setIndexes(id, idx_q, idx_v); + } + + static void setIndexes1( + JointModelDerived & self, + const int & id, + const int & idx_q, + const int & idx_v, + const int & idx_vExtended) + { + self.setIndexes(id, idx_q, idx_v, idx_vExtended); + } + static void expose() { bp::class_("JointModel", "Generic Joint Model", bp::no_init) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index a9fbec6a2b..69b6899441 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -249,6 +249,23 @@ namespace pinocchio int v, int vExtended); + /** + * @brief Visit a JointModelTpl through JointSetIndexesVisitor to set + * the indexes of the joint in the kinematic chain + * + * @param[in] jmodel The JointModelVariant + * @param[in] id The index of joint in the kinematic chain + * @param[in] q The index in the full model configuration space corresponding to the first + * degree of freedom + * @param[in] v The index in the full model tangent space corresponding to the first joint + * tangent 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); + /** * @brief Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the * derived joint model diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index cdb2cbd85a..13998ac923 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -486,6 +486,13 @@ namespace pinocchio return JointSetIndexesVisitor::run(jmodel, id, q, v, vExtended); } + template class JointCollectionTpl> + inline void setIndexes( + JointModelTpl & jmodel, JointIndex id, int q, int v) + { + return JointSetIndexesVisitor::run(jmodel, id, q, v, v); + } + /** * @brief JointModelShortnameVisitor visitor */ diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index 2ef91fea6f..8c53ea90f5 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -447,6 +447,11 @@ namespace pinocchio return ::pinocchio::id(*this); } + void setIndexes(JointIndex id, int nq, int nv) + { + ::pinocchio::setIndexes(*this, id, nq, nv, nv); + } + void setIndexes(JointIndex id, int nq, int nv, int nvExtended) { ::pinocchio::setIndexes(*this, id, nq, nv, nvExtended); diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index 6045f63721..3623236883 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -202,6 +202,11 @@ namespace pinocchio return i_id; } + void setIndexes(JointIndex id, int q, int v) + { + derived().setIndexes_impl(id, q, v, v); + } + void setIndexes(JointIndex id, int q, int v, int vExtended) { derived().setIndexes_impl(id, q, v, vExtended); From 801b224af5bbdadab90449583d235d4b55e6f2e6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 12:03:11 +0100 Subject: [PATCH 217/310] Fix doc --- .../multibody/joint/joint-basic-visitors.hpp | 20 +++++++++---------- .../multibody/joint/joint-basic-visitors.hxx | 4 ++-- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp index 69b6899441..b40ed05a87 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hpp +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hpp @@ -145,12 +145,12 @@ namespace pinocchio inline int nq(const JointModelTpl & jmodel); /** - * @brief Visit a JointModelTpl through JointNjVisitor to get the dimension of - * the joint configuration space + * @brief Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of + * the joint extended tangent space * * @param[in] jmodel The JointModelVariant * - * @return The dimension of joint jacobian space + * @return The dimension of joint extended tangent space */ template class JointCollectionTpl> inline int nvExtended(const JointModelTpl & jmodel); @@ -192,7 +192,7 @@ namespace pinocchio inline int idx_q(const JointModelTpl & jmodel); /** - * @brief Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model + * @brief Visit a JointModelTpl through JointIdxVVisitor to get the index in the model * tangent space corresponding to the first joint tangent space degree * * @param[in] jmodel The JointModelVariant @@ -204,13 +204,13 @@ namespace pinocchio 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 + * @brief Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model + * extended tangent space corresponding to the joint first joint extended tangent space degree * * @param[in] jmodel The JointModelVariant * - * @return The index in the full model tangent space corresponding to the first - * joint jacobian space degree + * @return The index in the model extended tangent space corresponding to the first + * joint extended tangent space degree */ template class JointCollectionTpl> inline int idx_vExtended(const JointModelTpl & jmodel); @@ -236,8 +236,8 @@ 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] vExtended The index in the full model tangent space corresponding to the - * first joint jacobian space degree + * @param[in] vExtended The index in the model extended tangent space corresponding to the + * joint first extended tangent space degree * * @return The index of the joint in the kinematic chain */ diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index 13998ac923..c768d682f5 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -268,7 +268,7 @@ namespace pinocchio } /** - * @brief JointNjVisitor visitor + * @brief JointNvExtendedVisitor visitor */ struct JointNvExtendedVisitor : boost::static_visitor { @@ -392,7 +392,7 @@ namespace pinocchio } /** - * @brief JointIdxjVisitor visitor + * @brief JointIdxVExtendedVisitor visitor */ struct JointIdxVExtendedVisitor : boost::static_visitor { From 1b2a0dbcc1b386b47c6a6484d04ed49fe9348eae Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 12:03:21 +0100 Subject: [PATCH 218/310] Remove useless include --- include/pinocchio/multibody/joint/joint-mimic.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index cd7f21dd6a..a8a8bd9818 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -10,12 +10,6 @@ #include "pinocchio/macros.hpp" #include "pinocchio/multibody/joint/joint-base.hpp" #include "pinocchio/multibody/joint/joint-basic-visitors.hpp" -#include -#include -#include -#include -#include -#include namespace pinocchio { From b7abbcd104760af47dde600758f76603d45e45e7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 12:03:29 +0100 Subject: [PATCH 219/310] apply pre-commit --- unittest/model.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/unittest/model.cpp b/unittest/model.cpp index 40d52231a0..53eee98b04 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -996,7 +996,8 @@ BOOST_AUTO_TEST_CASE(test_build_reduced_model_mimic) idx = model.addJoint(idx, JointModelRX(), SE3::Identity(), "joint3"); model.addJointFrame(idx); - auto const mimic = JointModelMimic(boost::get(model.joints[model.getJointId("joint3")].toVariant()), 1., 0.); + auto const mimic = JointModelMimic( + boost::get(model.joints[model.getJointId("joint3")].toVariant()), 1., 0.); idx = model.addJoint(idx, mimic, SE3::Identity(), "joint4"); model.addJointFrame(idx); From 835f677ae81a308e338bf0613c026e7564bd8255 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 14:07:43 +0100 Subject: [PATCH 220/310] [EtienneAr feedback] topic/mimic: Minor visitor call remove --- 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 a8a8bd9818..0fef3aa67e 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -778,8 +778,8 @@ namespace pinocchio ReturnType res( m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), ScalarCast::cast(m_offset)); - res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); - res.setMimicIndexes(m_jmodel_ref.id(), idx_q(), idx_v(), idx_vExtended()); + res.setIndexes(id(), Base::i_q, Base::i_v, Base::i_vExtended); + res.setMimicIndexes(m_jmodel_ref.id(), Base::i_q, Base::i_v, Base::i_vExtended); return res; } From b96b8320f22edd79d1d7cecad86591a2477fd264 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 16:44:20 +0100 Subject: [PATCH 221/310] [parser/urdf] Simplify mimic parsing --- include/pinocchio/parsers/urdf/model.hxx | 105 +++++++++++------------ src/parsers/urdf/model.cpp | 18 ++-- 2 files changed, 59 insertions(+), 64 deletions(-) diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index a8641fbf51..4d2a94b527 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -15,7 +15,8 @@ #include #include #include -#include +#include +#include namespace pinocchio { @@ -52,6 +53,7 @@ namespace pinocchio typedef Eigen::Matrix Vector; typedef Eigen::Ref VectorRef; typedef Eigen::Ref VectorConstRef; + typedef MimicInfo MimicInfo_; virtual void setName(const std::string & name) = 0; @@ -70,7 +72,8 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) = 0; + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) = 0; virtual void addJointAndBody( JointType type, @@ -86,7 +89,8 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) = 0; + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) = 0; virtual void addFixedJointAndBody( const FrameIndex & parentFrameId, @@ -113,14 +117,6 @@ 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 & mimicked_name, - const Vector3 & axis, - const Scalar multiplier, - const Scalar offset, - JointType jointType) = 0; - UrdfVisitorBaseTpl() : log(NULL) { @@ -135,7 +131,6 @@ namespace pinocchio } std::ostream * log; - std::unordered_map> mimicInfo_map; }; template @@ -153,9 +148,7 @@ namespace pinocchio typedef typename UrdfVisitorBaseTpl<_Scalar, Options>::JointType JointType; JointType jointType; - MimicInfo() - { - } + MimicInfo() = default; MimicInfo( std::string _mimicked_name, @@ -188,6 +181,8 @@ namespace pinocchio typedef typename Model::JointModel JointModel; typedef typename Model::Frame Frame; + typedef MimicInfo MimicInfo_; + Model & model; std::string root_joint_name; @@ -228,11 +223,12 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) { 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, mimic_info); } void addJointAndBody( @@ -249,7 +245,8 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) + const VectorConstRef & damping, + const boost::optional & mimic_info = boost::none) { JointIndex joint_id; const Frame & frame = model.frames[parentFrameId]; @@ -298,30 +295,33 @@ namespace pinocchio max_config, friction, damping); break; case Base::MIMIC: - switch (this->mimicInfo_map[joint_name].jointType) - { - case Base::REVOLUTE: - joint_id = addMimicJoint< - typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, - typename JointCollection::JointModelRZ, - typename JointCollection::JointModelRevoluteUnaligned>( - frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, - friction, damping); - break; - case Base::PRISMATIC: - joint_id = addMimicJoint< - typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, - typename JointCollection::JointModelPZ, - typename JointCollection::JointModelPrismaticUnaligned>( - frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, - friction, damping); - break; - default: - PINOCCHIO_CHECK_INPUT_ARGUMENT( - false, - "Cannot mimic this type. Only revolute, prismatic and helicoidal can be mimicked"); - break; - } + if (mimic_info) + switch (mimic_info->jointType) + { + case Base::REVOLUTE: + joint_id = addMimicJoint< + typename JointCollection::JointModelRX, typename JointCollection::JointModelRY, + typename JointCollection::JointModelRZ, + typename JointCollection::JointModelRevoluteUnaligned>( + frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, + friction, damping, *mimic_info); + break; + case Base::PRISMATIC: + joint_id = addMimicJoint< + typename JointCollection::JointModelPX, typename JointCollection::JointModelPY, + typename JointCollection::JointModelPZ, + typename JointCollection::JointModelPrismaticUnaligned>( + frame, placement, joint_name, max_effort, max_velocity, min_config, max_config, + friction, damping, *mimic_info); + break; + default: + PINOCCHIO_CHECK_INPUT_ARGUMENT( + false, "Cannot mimic this type. Only revolute, prismatic and helicoidal can be " + "mimicked"); + break; + } + else + PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "Cannot create mimic joint. Missing info.") break; default: PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct."); @@ -479,18 +479,6 @@ namespace pinocchio } } - void addMimicInfo( - const std::string & mimic_name, - const std::string & mimicked_name, - const Vector3 & axis, - const Scalar multiplier, - const Scalar offset, - JointType jointType) - { - MimicInfo mimic_info(mimicked_name, multiplier, offset, axis, jointType); - this->mimicInfo_map.insert(std::make_pair(mimic_name, mimic_info)); - } - template JointIndex addMimicJoint( const Frame & frame, @@ -501,9 +489,13 @@ namespace pinocchio const VectorConstRef & min_config, const VectorConstRef & max_config, const VectorConstRef & friction, - const VectorConstRef & damping) + const VectorConstRef & damping, + const MimicInfo_ & mimic_info) { - const MimicInfo mimic_info = this->mimicInfo_map[joint_name]; + if (!model.existJointName(mimic_info.mimicked_name)) + PINOCCHIO_CHECK_INPUT_ARGUMENT( + false, "The parent joint of the mimic joint is not in the kinematic tree"); + auto mimicked_joint = model.joints[getJointId(mimic_info.mimicked_name)]; CartesianAxis axisType = extractCartesianAxis(mimic_info.axis); @@ -625,6 +617,7 @@ namespace pinocchio }; typedef UrdfVisitorBaseTpl UrdfVisitorBase; + typedef MimicInfo MimicInfo_; PINOCCHIO_PARSERS_DLLAPI void parseRootTree( const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = true); diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp index 1bd46bc8f3..44608f0446 100644 --- a/src/parsers/urdf/model.cpp +++ b/src/parsers/urdf/model.cpp @@ -147,13 +147,14 @@ namespace pinocchio 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); + MimicInfo_ mimic_info( + joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis, + UrdfVisitorBase::REVOLUTE); model.addJointAndBody( UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + link->name, max_effort, max_velocity, min_config, max_config, friction, damping, + mimic_info); } else model.addJointAndBody( @@ -219,13 +220,14 @@ namespace pinocchio 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); + MimicInfo_ mimic_info( + joint->mimic->joint_name, joint->mimic->multiplier, joint->mimic->offset, axis, + UrdfVisitorBase::PRISMATIC); model.addJointAndBody( UrdfVisitorBase::MIMIC, axis, parentFrameId, jointPlacement, joint->name, Y, - link->name, max_effort, max_velocity, min_config, max_config, friction, damping); + link->name, max_effort, max_velocity, min_config, max_config, friction, damping, + mimic_info); } else model.addJointAndBody( From 113f06766868dee6db6e691477a054a2f4dd1066 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 16:44:50 +0100 Subject: [PATCH 222/310] [parser/urdf] Mimic default to false --- include/pinocchio/parsers/urdf.hpp | 12 ++++++------ include/pinocchio/parsers/urdf/model.hxx | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/parsers/urdf.hpp b/include/pinocchio/parsers/urdf.hpp index c3dfebfb99..6ec709f442 100644 --- a/include/pinocchio/parsers/urdf.hpp +++ b/include/pinocchio/parsers/urdf.hpp @@ -62,7 +62,7 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic = true, + const bool mimic = false, const bool verbose = false); /// @@ -78,7 +78,7 @@ namespace pinocchio ModelTpl & buildModel( const std::string & filename, ModelTpl & model, - const bool mimic = true, + const bool mimic = false, const bool verbose = false); /// @@ -99,7 +99,7 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic = true, + const bool mimic = false, const bool verbose = false); /// @@ -140,7 +140,7 @@ namespace pinocchio ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, - const bool mimic = true, + const bool mimic = false, const bool verbose = false); /// @@ -184,7 +184,7 @@ namespace pinocchio const std::string & xml_stream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic = true, + const bool mimic = false, const bool verbose = false); /// @@ -202,7 +202,7 @@ namespace pinocchio ModelTpl & buildModelFromXML( const std::string & xml_stream, ModelTpl & model, - const bool mimic = true, + const bool mimic = false, const bool verbose = false); /** diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 4d2a94b527..f68a15a49d 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -620,13 +620,13 @@ namespace pinocchio typedef MimicInfo MimicInfo_; PINOCCHIO_PARSERS_DLLAPI void parseRootTree( - const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = true); + const ::urdf::ModelInterface * urdfTree, UrdfVisitorBase & model, const bool mimic = false); - PINOCCHIO_PARSERS_DLLAPI void - parseRootTree(const std::string & filename, UrdfVisitorBase & model, const bool mimic = true); + PINOCCHIO_PARSERS_DLLAPI void parseRootTree( + const std::string & filename, UrdfVisitorBase & model, const bool mimic = false); PINOCCHIO_PARSERS_DLLAPI void parseRootTreeFromXML( - const std::string & xmlString, UrdfVisitorBase & model, const bool mimic = true); + const std::string & xmlString, UrdfVisitorBase & model, const bool mimic = false); } // namespace details template class JointCollectionTpl> From ca07e420e4bdd335c4c506a8c5fa5a85fcb818c5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 16:45:14 +0100 Subject: [PATCH 223/310] [bindings/urdf] Parser default argument to bindings to not break api --- bindings/python/parsers/urdf/model.cpp | 81 ++++++++++---------------- 1 file changed, 31 insertions(+), 50 deletions(-) diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index 27d62666ab..a188632557 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -20,20 +20,20 @@ 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) { 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) { 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) { Model model; pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); @@ -44,7 +44,7 @@ namespace pinocchio const bp::object & filename, const JointModel & root_joint, const std::string & root_joint_name, - const bool mimic = false) + const bool mimic) { Model model; pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); @@ -52,10 +52,7 @@ namespace pinocchio } Model & buildModelFromUrdf( - const bp::object & filename, - const JointModel & root_joint, - Model & model, - const bool mimic = false) + const bp::object & filename, const JointModel & root_joint, Model & model, const bool mimic) { return pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); } @@ -65,13 +62,13 @@ namespace pinocchio const JointModel & root_joint, const std::string & root_joint_name, Model & model, - const bool mimic = false) + const bool mimic) { 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) + const std::string & xml_stream, const JointModel & root_joint, const bool mimic) { Model model; pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); @@ -82,7 +79,7 @@ namespace pinocchio const std::string & xml_stream, const JointModel & root_joint, const std::string & root_joint_name, - const bool mimic = false) + const bool mimic) { Model model; pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); @@ -93,7 +90,7 @@ namespace pinocchio const std::string & xml_stream, const JointModel & root_joint, Model & model, - const bool mimic = false) + const bool mimic) { pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); return model; @@ -104,21 +101,20 @@ namespace pinocchio const JointModel & root_joint, const std::string & root_joint_name, Model & model, - const bool mimic = false) + const bool mimic) { 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) { 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) { pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); return model; @@ -135,7 +131,7 @@ namespace pinocchio "buildModelFromUrdf", static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "mimic"), + (bp::arg("urdf_filename"), bp::arg("root"), bp::arg("mimic") = false), "Parse the URDF file given in input and return a pinocchio Model starting with the " "given root joint."); @@ -144,7 +140,8 @@ namespace pinocchio static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "root_joint_name", "mimic"), + (bp::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("mimic") = false), "Parse the URDF file given in input and return a pinocchio Model starting with the " "given root joint with its specified name."); @@ -152,14 +149,14 @@ namespace pinocchio "buildModelFromUrdf", static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "mimic"), + (bp::arg("urdf_filename"), bp::arg("mimic") = false), "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", "mimic"), + (bp::arg("urdf_filename"), bp::arg("model"), bp::arg("mimic") = false), "Append to a given model a URDF structure given by its filename.", bp::return_internal_reference<2>()); @@ -167,7 +164,8 @@ namespace pinocchio "buildModelFromUrdf", static_cast( pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "model", "mimic"), + (bp::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("model"), + bp::arg("mimic") = false), "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.", @@ -177,7 +175,8 @@ namespace pinocchio "buildModelFromUrdf", static_cast(pinocchio::python::buildModelFromUrdf), - bp::args("urdf_filename", "root_joint", "root_joint_name", "model", "mimic"), + (bp::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("model"), bp::arg("mimic") = false), "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," @@ -188,7 +187,7 @@ namespace pinocchio "buildModelFromXML", static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "mimic"), + (bp::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("mimic") = false), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " "the given root joint."); @@ -197,19 +196,8 @@ namespace pinocchio static_cast( pinocchio::python::buildModelFromXML), - 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( - pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "root_joint_name", "mimic"), + (bp::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("mimic") = false), "Parse the URDF XML stream given in input and return a pinocchio Model starting with " "the given root joint with its specified name."); @@ -217,7 +205,8 @@ namespace pinocchio "buildModelFromXML", static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "root_joint", "model", "mimic"), + (bp::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("model"), + bp::arg("mimic") = false), "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>()); @@ -226,16 +215,8 @@ namespace pinocchio "buildModelFromXML", static_cast(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", "root_joint", "root_joint_name", "model", "mimic"), + (bp::arg("urdf_xml_stream"), bp::arg("root_joint"), bp::arg("root_joint_name"), + bp::arg("model"), bp::arg("mimic") = false), "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>()); @@ -244,14 +225,14 @@ namespace pinocchio "buildModelFromXML", static_cast( pinocchio::python::buildModelFromXML), - bp::args("urdf_xml_stream", "mimic"), + (bp::arg("urdf_xml_stream"), bp::arg("mimic") = false), "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", "mimic"), + (bp::arg("urdf_xml_stream"), bp::arg("model"), bp::arg("mimic") = false), "Parse the URDF XML stream given in input and append it to the input model.", bp::return_internal_reference<2>()); #endif From 72db2e795021961c2ebcb935e48bf9b110da5e0b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 16:45:53 +0100 Subject: [PATCH 224/310] Revert mimic argument --- examples/collision-with-point-clouds.py | 2 +- examples/collisions.py | 2 +- examples/geometry-models.py | 2 +- examples/overview-urdf.py | 2 +- .../bindings_build_geom_from_urdf_memorycheck.py | 2 +- .../python/bindings_contact_inverse_dynamics.py | 2 +- unittest/python/bindings_geometry_model_urdf.py | 16 ++++------------ unittest/reachable-workspace.cpp | 2 +- 8 files changed, 11 insertions(+), 19 deletions(-) diff --git a/examples/collision-with-point-clouds.py b/examples/collision-with-point-clouds.py index 5a933f18de..dac0939b0d 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, package_dirs=mesh_dir + urdf_model_path, mesh_dir ) # Add point clouds 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/geometry-models.py b/examples/geometry-models.py index 7544003005..c1ae8d4579 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, mimic=False + urdf_model_path, mesh_dir ) print("model name: " + model.name) diff --git a/examples/overview-urdf.py b/examples/overview-urdf.py index 9e01ccf037..8aee4be196 100644 --- a/examples/overview-urdf.py +++ b/examples/overview-urdf.py @@ -15,7 +15,7 @@ ) # Load the urdf model -model = pinocchio.buildModelFromUrdf(urdf_filename, False) +model = pinocchio.buildModelFromUrdf(urdf_filename) print("model name: " + model.name) # Create data required by the algorithms diff --git a/unittest/python/bindings_build_geom_from_urdf_memorycheck.py b/unittest/python/bindings_build_geom_from_urdf_memorycheck.py index 3d517516da..e606a84e5c 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, False) + model = pin.buildModelFromUrdf(self.model_path) for _ in range(2): pin.buildGeomFromUrdf( model, diff --git a/unittest/python/bindings_contact_inverse_dynamics.py b/unittest/python/bindings_contact_inverse_dynamics.py index f4484c271e..7886ed5a88 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(), False + self.urdf_model_path, pin.JointModelFreeFlyer() ) 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 f3180d4f06..7ebb86b250 100644 --- a/unittest/python/bindings_geometry_model_urdf.py +++ b/unittest/python/bindings_geometry_model_urdf.py @@ -23,9 +23,7 @@ def test_load(self): self.model_dir / "romeo_description/meshes/V1/collision/LHipPitch.dae" ) - model = pin.buildModelFromUrdf( - self.model_path, pin.JointModelFreeFlyer(), mimic=True - ) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -38,9 +36,7 @@ def test_load(self): def test_self_load(self): hint_list = [self.mesh_path] - model = pin.buildModelFromUrdf( - self.model_path, pin.JointModelFreeFlyer(), mimic=True - ) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) collision_model_ref = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list ) @@ -86,9 +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(), mimic=True - ) + model = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) collision_model = pin.buildGeomFromUrdf( model, self.model_path, pin.GeometryType.COLLISION, hint_list @@ -107,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(), mimic=True + self.model_path, hint_list, pin.JointModelFreeFlyer() ) self.assertEqual(model, model_2) @@ -127,7 +121,6 @@ def test_multi_load(self): hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.COLLISION, - mimic=True, ) self.assertEqual(model, model_c) @@ -142,7 +135,6 @@ def test_multi_load(self): hint_list, pin.JointModelFreeFlyer(), geometry_types=pin.GeometryType.VISUAL, - mimic=True, ) self.assertEqual(model, model_v) diff --git a/unittest/reachable-workspace.cpp b/unittest/reachable-workspace.cpp index 1f19dbedd1..c4b56f83dc 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, true); + pinocchio::buildModels::manipulator(model); Eigen::VectorXd q = (model.upperPositionLimit + model.lowerPositionLimit) / 2; ReachableSetResults res; From 4e768fe8a758f2c0b44f9e499de2313640626d54 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 16:46:10 +0100 Subject: [PATCH 225/310] Revert setIndexes to old api --- unittest/all-joints.cpp | 20 ++++++++++---------- unittest/cppad/joints.cpp | 20 ++++++++++---------- unittest/finite-differences.cpp | 18 +++++++++--------- unittest/joint-generic.cpp | 24 ++++++++++++------------ unittest/joint-prismatic.cpp | 2 +- unittest/liegroups.cpp | 6 +++--- 6 files changed, 45 insertions(+), 45 deletions(-) diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index cdde132bfd..4c3b4683c7 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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -85,7 +85,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -102,7 +102,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -133,7 +133,7 @@ struct init> { JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -147,7 +147,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -162,7 +162,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -217,7 +217,7 @@ BOOST_AUTO_TEST_CASE(isEqual) BOOST_CHECK(joint_revolutex != joint_revolutey); JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0, 0); + jmodelx.setIndexes(0, 0, 0); TestJointModelIsEqual()(JointModel()); JointModel jmodel_any; diff --git a/unittest/cppad/joints.cpp b/unittest/cppad/joints.cpp index 3ab7e3ec33..28f53678db 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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -184,7 +184,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteTpl JointModelRX; typedef pinocchio::JointModelTpl JointModel; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -205,7 +205,7 @@ struct TestADOnJoints typedef pinocchio::JointModelCompositeTpl JointModel; JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index d4a06ceb1a..9b3958ef41 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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -158,7 +158,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -173,7 +173,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -190,7 +190,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 4a0ae184ab..1f993b7bb7 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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -197,7 +197,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -214,7 +214,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -259,7 +259,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); typename JointModel::JointDataDerived jdata = jmodel.createData(); test_joint_methods(jmodel, jdata); @@ -426,7 +426,7 @@ BOOST_AUTO_TEST_CASE(isEqual) BOOST_CHECK(joint_revolutex != joint_revolutey); JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0, 0); + jmodelx.setIndexes(0, 0, 0); JointModel jmodelx_copy = jmodelx; BOOST_CHECK(jmodelx_copy == jmodelx); @@ -443,7 +443,7 @@ BOOST_AUTO_TEST_CASE(cast) JointModelRX joint_revolutex; JointModel jmodelx(joint_revolutex); - jmodelx.setIndexes(0, 0, 0, 0); + jmodelx.setIndexes(0, 0, 0); BOOST_CHECK(jmodelx.cast() == jmodelx); BOOST_CHECK(jmodelx.cast().cast() == jmodelx); diff --git a/unittest/joint-prismatic.cpp b/unittest/joint-prismatic.cpp index eccc232c79..36918eb04d 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, 0); + joint_model.setIndexes(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 a72393a8ac..3249470680 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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); pinocchio::JointModelPrismaticUnaligned::JointDataDerived jdata = jmodel.createData(); run_tests(jmodel, jdata); From 86b1b0adea23a25067954d21b3f63e2680a5e858 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 17:53:09 +0100 Subject: [PATCH 226/310] [SampleModel] Simplify bindings --- bindings/python/multibody/sample-models.cpp | 60 ++------------------- examples/build-reduced-model.py | 2 +- 2 files changed, 6 insertions(+), 56 deletions(-) diff --git a/bindings/python/multibody/sample-models.cpp b/bindings/python/multibody/sample-models.cpp index d47338a474..9373db96ed 100644 --- a/bindings/python/multibody/sample-models.cpp +++ b/bindings/python/multibody/sample-models.cpp @@ -12,31 +12,10 @@ namespace pinocchio { namespace bp = boost::python; - Model buildSampleModelHumanoidRandom() - { - Model model; - buildModels::humanoidRandom(model); - return model; - } - - Model buildSampleModelHumanoidRandom(bool usingFF) - { - Model model; - buildModels::humanoidRandom(model, usingFF); - return model; - } - Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic) { Model model; - buildModels::humanoidRandom(model, usingFF, mimic); - return model; - } - - Model buildSampleModelManipulator() - { - Model model; - buildModels::manipulator(model); + buildModels::humanoidRandom(model); return model; } @@ -56,13 +35,6 @@ namespace pinocchio } #endif - Model buildSampleModelHumanoid() - { - Model model; - buildModels::humanoid(model); - return model; - } - Model buildSampleModelHumanoid(bool usingFF) { Model model; @@ -81,35 +53,17 @@ namespace pinocchio void exposeSampleModels() { - bp::def( - "buildSampleModelHumanoidRandom", - static_cast(pinocchio::python::buildSampleModelHumanoidRandom), - "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"), - "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"), + (bp::arg("using_free_flyer") = false, bp::arg("mimic") = false), "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."); + (bp::arg("mimic") = false), "Generate a (hard-coded) model of a simple manipulator."); #ifdef PINOCCHIO_WITH_HPP_FCL bp::def( @@ -119,15 +73,11 @@ namespace pinocchio bp::args("model"), "Generate a (hard-coded) geometry model of a simple manipulator."); #endif - bp::def( - "buildSampleModelHumanoid", - static_cast(pinocchio::python::buildSampleModelHumanoid), - "Generate a (hard-coded) model of a simple humanoid."); - bp::def( "buildSampleModelHumanoid", static_cast(pinocchio::python::buildSampleModelHumanoid), - bp::args("using_free_flyer"), "Generate a (hard-coded) model of a simple humanoid."); + (bp::arg("using_free_flyer") = false), + "Generate a (hard-coded) model of a simple humanoid."); #ifdef PINOCCHIO_WITH_HPP_FCL bp::def( diff --git a/examples/build-reduced-model.py b/examples/build-reduced-model.py index d61ad4da36..85269ae598 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, mimic=True) +robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir) reduced_robot = robot.buildReducedRobot( list_of_joints_to_lock=mixed_jointsToLockIDs, reference_configuration=initialJointConfig, From 33e18d08dfd8a31e990464482b698a1735667d0e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 3 Feb 2025 18:11:56 +0100 Subject: [PATCH 227/310] Revert setIndexes uses back to old api --- unittest/aba.cpp | 8 +++--- unittest/casadi/joints.cpp | 30 +++++++++++------------ unittest/joint-composite.cpp | 10 ++++---- unittest/joint-motion-subspace.cpp | 18 +++++++------- unittest/python/bindings_frame.py | 2 +- unittest/python/bindings_sample_models.py | 2 +- unittest/python/bindings_urdf.py | 20 ++++++--------- unittest/python/serialization.py | 6 ++--- unittest/serialization.cpp | 24 +++++++++--------- 9 files changed, 58 insertions(+), 62 deletions(-) diff --git a/unittest/aba.cpp b/unittest/aba.cpp index b213083589..a8204b9336 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, 0); + jmodel.setIndexes(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, 0); + jmodel_composite.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); test_joint_methods(jmodel); } diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 38ad2614e4..9147691a87 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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } @@ -232,7 +232,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } @@ -254,7 +254,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } @@ -300,7 +300,7 @@ struct TestADOnJoints { typedef pinocchio::JointModelHelicalTpl JointModel; JointModel jmodel(Scalar(0.4)); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -311,7 +311,7 @@ struct TestADOnJoints typedef pinocchio::JointModelUniversalTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::UnitX(), Vector3::UnitY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -322,7 +322,7 @@ struct TestADOnJoints typedef pinocchio::JointModelHelicalUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -333,7 +333,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -344,7 +344,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -355,7 +355,7 @@ struct TestADOnJoints typedef pinocchio::JointModelPrismaticUnalignedTpl JointModel; typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -366,7 +366,7 @@ struct TestADOnJoints typedef pinocchio::JointModelRevoluteTpl JointModelRX; typedef pinocchio::JointModelTpl JointModel; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } @@ -379,7 +379,7 @@ struct TestADOnJoints typedef pinocchio::JointModelCompositeTpl JointModel; JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test(jmodel); } diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 368dea9c0c..454d521e90 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -178,7 +178,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModel jmodel; - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test_joint_methods(jmodel); } @@ -196,7 +196,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test_joint_methods(jmodel); } @@ -204,7 +204,7 @@ struct TestJointComposite void operator()(const JointModelBase &) const { JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); test_joint_methods(jmodel); } }; @@ -237,7 +237,7 @@ BOOST_AUTO_TEST_CASE(chain) BOOST_AUTO_TEST_CASE(vsZYX) { JointModelSphericalZYX jmodel_spherical; - jmodel_spherical.setIndexes(0, 0, 0, 0); + jmodel_spherical.setIndexes(0, 0, 0); JointModelComposite jmodel_composite((JointModelRZ())); jmodel_composite.addJoint(JointModelRY()); @@ -249,7 +249,7 @@ BOOST_AUTO_TEST_CASE(vsZYX) BOOST_AUTO_TEST_CASE(vsTranslation) { JointModelTranslation jmodel_translation; - jmodel_translation.setIndexes(0, 0, 0, 0); + jmodel_translation.setIndexes(0, 0, 0); JointModelComposite jmodel_composite((JointModelPX())); jmodel_composite.addJoint(JointModelPY()); diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index ab2e80430f..c5fda672c3 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -288,7 +288,7 @@ struct init static JointModel_ run() { JointModel_ jmodel; - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -303,7 +303,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -318,7 +318,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -333,7 +333,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -348,7 +348,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -362,7 +362,7 @@ struct init> { JointModel jmodel(XAxis::vector(), YAxis::vector()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -382,7 +382,7 @@ struct init> jmodel.addJoint(JointModelRY(), SE3::Random()); jmodel.addJoint(JointModelRZ(), SE3::Random()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } @@ -413,7 +413,7 @@ struct init> { JointModel jmodel(static_cast(0.5)); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -428,7 +428,7 @@ struct init> typedef typename JointModel::Vector3 Vector3; JointModel jmodel(Vector3::Random().normalized()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; diff --git a/unittest/python/bindings_frame.py b/unittest/python/bindings_frame.py index 23ac326f9c..d0c545eb3b 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() self.parent_idx = ( self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") diff --git a/unittest/python/bindings_sample_models.py b/unittest/python/bindings_sample_models.py index f55d75b5fb..159a5f6139 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(True) + manipulator_1 = pin.buildSampleModelManipulator() if pin.WITH_HPP_FCL: _geometry_manipulator_1 = pin.buildSampleGeometryModelManipulator( diff --git a/unittest/python/bindings_urdf.py b/unittest/python/bindings_urdf.py index 33b77e7e86..45826f08cc 100644 --- a/unittest/python/bindings_urdf.py +++ b/unittest/python/bindings_urdf.py @@ -12,29 +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, False) - pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) + pin.buildModelFromUrdf(self.model_path) + pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) def test_self_load(self): model = pin.Model() - pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), model, False) - pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), False) + pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer(), model) + pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) 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 = pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer(), False) + model_ref = pin.buildModelFromUrdf(self.model_path, pin.JointModelFreeFlyer()) + model = pin.buildModelFromXML(file_content, pin.JointModelFreeFlyer()) 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) self.assertEqual(model_self, model_ref) def test_pickle(self): @@ -43,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, False) + model = pin.buildModelFromUrdf(model_path) filename = Path("model.pickle") with filename.open("wb") as f: pickle.dump(model, f) diff --git a/unittest/python/serialization.py b/unittest/python/serialization.py index 7adc0a8f3e..66c7d37b34 100644 --- a/unittest/python/serialization.py +++ b/unittest/python/serialization.py @@ -8,7 +8,7 @@ class TestSerialization(TestCase): def testTXT(self): - model = pin.buildSampleModelHumanoidRandom(True, True) + model = pin.buildSampleModelHumanoidRandom() 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(True, True) + model = pin.buildSampleModelHumanoidRandom() 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(True, True) + model = pin.buildSampleModelHumanoidRandom() filename = main_path + "/model.bin" model.saveToBinary(filename) diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index 0445d1be00..c15ad8826e 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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(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, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -346,7 +346,7 @@ struct init> { JointModel jmodel(static_cast(0.0)); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -361,7 +361,7 @@ struct init> typedef pinocchio::JointModelRevoluteTpl JointModelRX; JointModel jmodel((JointModelRX())); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -375,7 +375,7 @@ struct init> { JointModel jmodel(pinocchio::XAxis::vector(), pinocchio::YAxis::vector()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -392,7 +392,7 @@ struct init> JointModel jmodel((JointModelRX())); jmodel.addJoint(JointModelRY()); - jmodel.setIndexes(0, 0, 0, 0); + jmodel.setIndexes(0, 0, 0); return jmodel; } }; @@ -628,7 +628,7 @@ BOOST_AUTO_TEST_CASE(test_model_serialization) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model); generic_test(model, TEST_SERIALIZATION_FOLDER "/Model", "Model"); } @@ -638,7 +638,7 @@ BOOST_AUTO_TEST_CASE(test_throw_extension) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model); const std::string & fake_filename = "this_is_a_fake_filename"; @@ -673,7 +673,7 @@ BOOST_AUTO_TEST_CASE(test_data_serialization) using namespace pinocchio; Model model; - buildModels::humanoidRandom(model, true, true); + buildModels::humanoidRandom(model); Data data(model); From 42d4c4cf6b88dc92f79b11d6021cef4abdb960ae Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 10:10:53 +0100 Subject: [PATCH 228/310] [EtienneAr feedback] topic/mimic: rename m_jmodel_ref > m_jmodel_mimicking (resp data) --- .../pinocchio/multibody/joint/joint-mimic.hpp | 85 ++++++++++--------- 1 file changed, 44 insertions(+), 41 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 0fef3aa67e..04f8a139eb 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -370,8 +370,8 @@ namespace pinocchio JointDataMimicTpl( const RefJointData & jdata, const Scalar & scaling, const int & nq, const int & nv) - : m_jdata_ref(checkMimic(jdata.derived())) - , S(m_jdata_ref.S(), scaling) + : m_jdata_mimicking(checkMimic(jdata.derived())) + , S(m_jdata_mimicking.S(), scaling) { joint_q.resize(nq, 1); joint_q_transformed.resize(nq, 1); @@ -381,7 +381,7 @@ namespace pinocchio JointDataMimicTpl & operator=(const JointDataMimicTpl & other) { - m_jdata_ref = other.m_jdata_ref; + m_jdata_mimicking = other.m_jdata_mimicking; joint_q = other.joint_q; joint_q_transformed = other.joint_q_transformed; joint_v = other.joint_v; @@ -393,9 +393,9 @@ namespace pinocchio using Base::isEqual; bool isEqual(const JointDataMimicTpl & other) const { - return Base::isEqual(other) && m_jdata_ref == other.m_jdata_ref && joint_q == other.joint_q - && joint_q_transformed == other.joint_q_transformed && joint_v == other.joint_v - && joint_v_transformed == other.joint_v_transformed; + return Base::isEqual(other) && m_jdata_mimicking == other.m_jdata_mimicking + && joint_q == other.joint_q && joint_q_transformed == other.joint_q_transformed + && joint_v == other.joint_v && joint_v_transformed == other.joint_v_transformed; } static std::string classname() @@ -420,92 +420,92 @@ namespace pinocchio TansformTypeConstRef M_accessor() const { - M_ = m_jdata_ref.M(); + M_ = m_jdata_mimicking.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(); + M_ = m_jdata_mimicking.M(); return M_; } MotionTypeConstRef v_accessor() const { - v_ = m_jdata_ref.v(); + v_ = m_jdata_mimicking.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(); + v_ = m_jdata_mimicking.v(); return v_; } BiasTypeConstRef c_accessor() const { - c_ = m_jdata_ref.c(); + c_ = m_jdata_mimicking.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(); + c_ = m_jdata_mimicking.c(); return c_; } UTypeConstRef U_accessor() const { - U_ = m_jdata_ref.U(); + U_ = m_jdata_mimicking.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(); + U_ = m_jdata_mimicking.U(); return U_; } DTypeConstRef Dinv_accessor() const { - Dinv_ = m_jdata_ref.Dinv(); + Dinv_ = m_jdata_mimicking.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(); + Dinv_ = m_jdata_mimicking.Dinv(); return Dinv_; } UDTypeConstRef UDinv_accessor() const { - UDinv_ = m_jdata_ref.UDinv(); + UDinv_ = m_jdata_mimicking.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(); + UDinv_ = m_jdata_mimicking.UDinv(); return UDinv_; } DTypeConstRef StU_accessor() const { - StU_ = m_jdata_ref.StU(); + StU_ = m_jdata_mimicking.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"); - StU_ = m_jdata_ref.StU(); + StU_ = m_jdata_mimicking.StU(); return StU_; } @@ -513,11 +513,11 @@ namespace pinocchio const RefJointData & jdata() const { - return m_jdata_ref; + return m_jdata_mimicking; } RefJointData & jdata() { - return m_jdata_ref; + return m_jdata_mimicking; } ConfigVector_t & joint_q_accessor() @@ -559,11 +559,11 @@ namespace pinocchio void disp(std::ostream & os) const { Base::disp(os); - os << " Mimicking joint data: " << m_jdata_ref.shortname() << std::endl; + os << " Mimicking joint data: " << m_jdata_mimicking.shortname() << std::endl; } protected: - RefJointData m_jdata_ref; + RefJointData m_jdata_mimicking; /// \brief original configuration vector ConfigVector_t joint_q; @@ -647,7 +647,7 @@ namespace pinocchio const JointModelBase & jmodel_mimicked, const Scalar & scaling, const Scalar & offset) - : m_jmodel_ref(checkMimic((JointModel)jmodel_mimicking.derived())) + : m_jmodel_mimicking(checkMimic((JointModel)jmodel_mimicking.derived())) , m_scaling(scaling) , m_offset(offset) , m_nqExtended(jmodel_mimicking.nq()) @@ -687,7 +687,7 @@ namespace pinocchio void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended) { PINOCCHIO_THROW( - (id > m_jmodel_ref.id()), std::invalid_argument, + (id > m_jmodel_mimicking.id()), std::invalid_argument, "Mimic joint index is lower than its directing joint. Should never happen"); Base::i_id = id; // When setting the indexes q and v should remain on the mimicked joint @@ -701,7 +701,7 @@ namespace pinocchio void setMimicIndexes(JointIndex id, int q, int v, int vExtended) { // 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, vExtended); + m_jmodel_mimicking.setIndexes(id, 0, 0, vExtended); // idx_q, idx_v kept separately Base::i_q = q; Base::i_v = v; @@ -710,17 +710,18 @@ namespace pinocchio JointDataDerived createData() const { - return JointDataDerived(m_jmodel_ref.createData(), scaling(), m_nqExtended, m_nvExtended); + return JointDataDerived( + m_jmodel_mimicking.createData(), scaling(), m_nqExtended, m_nvExtended); } const std::vector hasConfigurationLimit() const { - return m_jmodel_ref.hasConfigurationLimit(); + return m_jmodel_mimicking.hasConfigurationLimit(); } const std::vector hasConfigurationLimitInTangent() const { - return m_jmodel_ref.hasConfigurationLimitInTangent(); + return m_jmodel_mimicking.hasConfigurationLimitInTangent(); } template @@ -729,8 +730,8 @@ namespace pinocchio { jdata.joint_q = qs.segment(Base::i_q, m_nqExtended); configVectorAffineTransform( - 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); + m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + m_jmodel_mimicking.calc(jdata.m_jdata_mimicking, jdata.joint_q_transformed); } template @@ -742,9 +743,10 @@ namespace pinocchio jdata.joint_q = qs.segment(Base::i_q, m_nqExtended); jdata.joint_v = vs.segment(Base::i_v, m_nvExtended); configVectorAffineTransform( - m_jmodel_ref, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); + m_jmodel_mimicking, 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); + m_jmodel_mimicking.calc( + jdata.m_jdata_mimicking, jdata.joint_q_transformed, jdata.joint_v_transformed); } template @@ -776,20 +778,21 @@ namespace pinocchio { typedef typename CastType::type ReturnType; ReturnType res( - m_jmodel_ref.template cast(), ScalarCast::cast(m_scaling), + m_jmodel_mimicking.template cast(), + ScalarCast::cast(m_scaling), ScalarCast::cast(m_offset)); res.setIndexes(id(), Base::i_q, Base::i_v, Base::i_vExtended); - res.setMimicIndexes(m_jmodel_ref.id(), Base::i_q, Base::i_v, Base::i_vExtended); + res.setMimicIndexes(m_jmodel_mimicking.id(), Base::i_q, Base::i_v, Base::i_vExtended); return res; } const JointModel & jmodel() const { - return m_jmodel_ref; + return m_jmodel_mimicking; } JointModel & jmodel() { - return m_jmodel_ref; + return m_jmodel_mimicking; } const Scalar & scaling() const @@ -812,7 +815,7 @@ namespace pinocchio protected: // data - JointModel m_jmodel_ref; + JointModel m_jmodel_mimicking; Scalar m_scaling, m_offset; int m_nqExtended, m_nvExtended; @@ -891,8 +894,8 @@ namespace pinocchio void disp(std::ostream & os) const { Base::disp(os); - os << " Mimicking joint type: " << m_jmodel_ref.shortname() << std::endl; - os << " Mimicked joint id: " << m_jmodel_ref.id() << std::endl; + os << " Mimicking joint type: " << m_jmodel_mimicking.shortname() << std::endl; + os << " Mimicked joint id: " << m_jmodel_mimicking.id() << std::endl; os << " Mimic scaling: " << m_scaling << std::endl; os << " Mimic offset: " << m_offset << std::endl; } From 810f75057acda8c38d509c36cd2f6c3d8505fb38 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 10:27:21 +0100 Subject: [PATCH 229/310] [EtienneAr feedback] topic/mimic: add size check for new data in checkdata --- include/pinocchio/algorithm/check-data.hxx | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 50b42af57f..8b645574a9 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -83,6 +83,9 @@ namespace pinocchio CHECK_DATA((int)data.parents_fromRow.size() == model.nvExtended); CHECK_DATA((int)data.idx_v_extended_fromRow.size() == model.nvExtended); CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.idx_v_extended_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.start_idx_v_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.end_idx_v_fromRow.size() == model.nvExtended); for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) { From 01706ce8e2f50c974b2f72910b4d15a0c96baa34 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 11:03:29 +0100 Subject: [PATCH 230/310] [EtienneAr feedback] topic/mimic: Minor comment update for mimic setIndexes --- .../pinocchio/multibody/joint/joint-mimic.hpp | 23 +++++++++++++++---- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 04f8a139eb..9e3c6fab1a 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -684,25 +684,38 @@ namespace pinocchio return m_nvExtended; } + /** + * @note q and v are ignored in the _impl for mimic joint because most algorithms will pass + * indexes of their current position in the tree, while in this case idx_q and idx_v should + * remain pointing to the mimicked joint. (See setMimicIndexes) + */ void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended) { PINOCCHIO_THROW( (id > m_jmodel_mimicking.id()), std::invalid_argument, "Mimic joint index is lower than its directing joint. Should never happen"); 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_vExtended = vExtended; } - // Specific way for mimic joints to set the mimicked q,v indexes. - // Used for manipulating tree (e.g. appendModel) + /** + * @brief Specific way for mimic joints to set the mimicked q,v indexes. + * Used for manipulating tree (e.g. appendModel) + * + * @param id Set the mimicking joint id + * @param q Set the mimic joint idx_q (should point to the mimicked joint) + * @param v Set the mimic joint idx_v (should point to the mimicked joint) + * @param vExtended Set the mimicking idx_vExtended + */ void setMimicIndexes(JointIndex id, int q, int v, int vExtended) { - // Set idx_q, idx_v to zero so that only sub segment of q,v can be passed to ref joint + // Set idx_q, idx_v to zero because only the corresponding subsegment of q,v are passed to the + // m_jmodel_mimicking, thus, its indexes starts at 0 m_jmodel_mimicking.setIndexes(id, 0, 0, vExtended); - // idx_q, idx_v kept separately + + // idx_q, idx_v are kept separately to extract the subsegment Base::i_q = q; Base::i_v = v; } From 9e318714174b13d03a7fbc348152d0c88ee5bb47 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 13:22:47 +0100 Subject: [PATCH 231/310] [EtienneAr feedback] topic/mimic: Fix mimic serialization --- include/pinocchio/serialization/joints-model.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index f6fa94b51f..af0a2e3ed9 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -92,7 +92,7 @@ namespace boost void run(pinocchio::JointIndex i_id, int i_q, int i_v, int i_vExtended) { - joint_m.setMimicIndexes(0, i_q, i_v, 0); + joint_m.setMimicIndexes(joint_m.jmodel().id(), i_q, i_v, joint_m.jmodel().idx_vExtended()); joint_m.setIndexes(i_id, 0, 0, i_vExtended); } }; @@ -332,13 +332,13 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelMimicTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); - fix::serialize(ar, *static_cast *>(&joint), version); - ar & make_nvp("jmodel", joint.jmodel()); ar & make_nvp("scaling", joint.scaling()); ar & make_nvp("offset", joint.offset()); + + // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase + // >(joint)); + fix::serialize(ar, *static_cast *>(&joint), version); } } // namespace serialization From cce159e781238df9af9ef6610e466a2cc90d4d35 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 13:49:09 +0100 Subject: [PATCH 232/310] [bindings/sampleModel] Add missing arguments --- bindings/python/multibody/sample-models.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/multibody/sample-models.cpp b/bindings/python/multibody/sample-models.cpp index 9373db96ed..18ab9c37e6 100644 --- a/bindings/python/multibody/sample-models.cpp +++ b/bindings/python/multibody/sample-models.cpp @@ -15,7 +15,7 @@ namespace pinocchio Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic) { Model model; - buildModels::humanoidRandom(model); + buildModels::humanoidRandom(model, usingFF, mimic); return model; } From 9e374dd92901b95ad38b3ab3bdd77bb385c91c06 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 14:44:14 +0100 Subject: [PATCH 233/310] [bindings/model] Expose transformJointIntoMimic --- bindings/python/algorithm/expose-model.cpp | 28 ++++++++++++++++++++++ unittest/model.cpp | 2 +- 2 files changed, 29 insertions(+), 1 deletion(-) diff --git a/bindings/python/algorithm/expose-model.cpp b/bindings/python/algorithm/expose-model.cpp index 0b38ec4630..a2e24559a2 100644 --- a/bindings/python/algorithm/expose-model.cpp +++ b/bindings/python/algorithm/expose-model.cpp @@ -83,6 +83,19 @@ namespace pinocchio return bp::make_tuple(ancestor_id, index_ancestor_in_support1, index_ancestor_in_support2); } + Model transformJointIntoMimic( + const context::Model & input_model, + const JointIndex & index_primary, + const JointIndex & index_secondary, + const context::Scalar & scaling, + const context::Scalar & offset) + { + Model model; + pinocchio::transformJointIntoMimic( + input_model, index_primary, index_secondary, scaling, offset, model); + return model; + } + void exposeModelAlgo() { using namespace Eigen; @@ -175,6 +188,21 @@ namespace pinocchio "\tjoint2_id: index of the second joint\n" "Returns a tuple containing the index of the common joint ancestor, the position of this " "ancestor in model.supports[joint1_id] and model.supports[joint2_id].\n"); + + bp::def( + "transformJointIntoMimic", + static_cast( + pinocchio::python::transformJointIntoMimic), + bp::args("input_model", "index_primary", "index_secondary", "scaling", "offset"), + "Transform of a joint of the model into a mimic joint. Keep the type of the joint as it " + "was previously. \n\n" + "Parameters:\n" + "\tinput_model: model the input model to take joints from.\n" + "\tindex_primary: index of the joint to mimic\n" + "\tindex_secondary: index of the joint that will mimic\n" + "\tscaling: Scaling of joint velocity and configuration\n" + "\toffset: Offset of joint configuration\n"); } } // namespace python diff --git a/unittest/model.cpp b/unittest/model.cpp index 53eee98b04..a583474d02 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -885,7 +885,7 @@ 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) +BOOST_AUTO_TEST_CASE(test_transform_to_mimic) { Model humanoid_model, humanoid_mimic; buildModels::humanoid(humanoid_model); From 9e21813241ececec1895a9d55fa159625e01d19a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 14:45:11 +0100 Subject: [PATCH 234/310] [bindings/joint] Expose new aatributes idx_vExtended and nvExtended --- .../bindings/python/multibody/joint/joint-derived.hpp | 11 +++++++++++ .../bindings/python/multibody/joint/joint.hpp | 10 ++++++++++ 2 files changed, 21 insertions(+) diff --git a/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp b/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp index 16041d2d2d..2d254c77bc 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joint-derived.hpp @@ -32,8 +32,10 @@ namespace pinocchio .add_property("id", &get_id) .add_property("idx_q", &get_idx_q) .add_property("idx_v", &get_idx_v) + .add_property("idx_vExtended", &get_idx_vExtended) .add_property("nq", &get_nq) .add_property("nv", &get_nv) + .add_property("nvExtended", &get_nvExtended) .add_property( "hasConfigurationLimit", &JointModelDerived::hasConfigurationLimit, "Return vector of boolean if joint has configuration limits.") @@ -91,6 +93,10 @@ namespace pinocchio { return self.idx_v(); } + static int get_idx_vExtended(const JointModelDerived & self) + { + return self.idx_vExtended(); + } static int get_nq(const JointModelDerived & self) { return self.nq(); @@ -99,6 +105,11 @@ namespace pinocchio { return self.nv(); } + static int get_nvExtended(const JointModelDerived & self) + { + return self.nvExtended(); + } + static void calc0(const JointModelDerived & self, JointDataDerived & jdata, const context::VectorXs & q) { diff --git a/include/pinocchio/bindings/python/multibody/joint/joint.hpp b/include/pinocchio/bindings/python/multibody/joint/joint.hpp index c66904b639..e0dd2af48c 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joint.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joint.hpp @@ -27,8 +27,10 @@ namespace pinocchio .add_property("id", &getId) .add_property("idx_q", &getIdx_q) .add_property("idx_v", &getIdx_v) + .add_property("idx_vExtended", &getIdx_vExtended) .add_property("nq", &getNq) .add_property("nv", &getNv) + .add_property("nvExtended", &getNvExtended) .def( "hasConfigurationLimit", &JointModel::hasConfigurationLimit, "Return vector of boolean if joint has configuration limits.") @@ -74,6 +76,10 @@ namespace pinocchio { return self.idx_v(); } + static int getIdx_vExtended(const JointModel & self) + { + return self.idx_vExtended(); + } static int getNq(const JointModel & self) { return self.nq(); @@ -82,6 +88,10 @@ namespace pinocchio { return self.nv(); } + static int getNvExtended(const JointModel & self) + { + return self.nvExtended(); + } static void setIndexes0(JointModelDerived & self, const int & id, const int & idx_q, const int & idx_v) From 1a013e0b183ac1fc53df6c27f6d668ee442e3614 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 15:36:30 +0100 Subject: [PATCH 235/310] [bindings/sampleModel] Coherent with cpp implementation --- bindings/python/multibody/sample-models.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bindings/python/multibody/sample-models.cpp b/bindings/python/multibody/sample-models.cpp index 18ab9c37e6..1ae4679dbf 100644 --- a/bindings/python/multibody/sample-models.cpp +++ b/bindings/python/multibody/sample-models.cpp @@ -56,7 +56,7 @@ namespace pinocchio bp::def( "buildSampleModelHumanoidRandom", static_cast(pinocchio::python::buildSampleModelHumanoidRandom), - (bp::arg("using_free_flyer") = false, bp::arg("mimic") = false), + (bp::arg("using_free_flyer") = true, bp::arg("mimic") = false), "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint " "placements.\nOnly meant for unit tests."); @@ -76,7 +76,7 @@ namespace pinocchio bp::def( "buildSampleModelHumanoid", static_cast(pinocchio::python::buildSampleModelHumanoid), - (bp::arg("using_free_flyer") = false), + (bp::arg("using_free_flyer") = true), "Generate a (hard-coded) model of a simple humanoid."); #ifdef PINOCCHIO_WITH_HPP_FCL From 1b4ca4cd3a0d775b455f7033240fcae19920a9a5 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 4 Feb 2025 15:37:03 +0100 Subject: [PATCH 236/310] [joint/Mimic] Add velocitySelector and fix typo --- .../pinocchio/multibody/joint/joint-mimic.hpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 9e3c6fab1a..039c3e6f2d 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -849,6 +849,22 @@ namespace pinocchio { return SizeDepType::segment(a.derived(), Base::i_q, m_nqExtended); } + /* Acces to dedicated segment in robot tangent space. */ + // Const access + template + typename SizeDepType::template SegmentReturn::ConstType + jointVelocityExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), Base::i_v, m_nvExtended); + } + + // Non-const access + template + typename SizeDepType::template SegmentReturn::Type + jointVelocityExtendedModelSelector_impl(Eigen::MatrixBase & a) const + { + return SizeDepType::segment(a.derived(), Base::i_v, m_nvExtended); + } /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/ // Const access @@ -871,7 +887,7 @@ namespace pinocchio // Const access template typename SizeDepType::template RowsReturn::ConstType - joinVelRows_impl(const Eigen::MatrixBase & A) const + jointRows_impl(const Eigen::MatrixBase & A) const { return SizeDepType::middleRows(A.derived(), Base::i_v, m_nvExtended); } From bc5982530757d7dc469f0fbbdeb0b3257dd19ef9 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 5 Feb 2025 15:04:18 +0100 Subject: [PATCH 237/310] [bindings/model] Fix scalar of transformJointIntoMimic --- bindings/python/algorithm/expose-model.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/bindings/python/algorithm/expose-model.cpp b/bindings/python/algorithm/expose-model.cpp index a2e24559a2..28bfb4d19c 100644 --- a/bindings/python/algorithm/expose-model.cpp +++ b/bindings/python/algorithm/expose-model.cpp @@ -83,13 +83,16 @@ namespace pinocchio return bp::make_tuple(ancestor_id, index_ancestor_in_support1, index_ancestor_in_support2); } - Model transformJointIntoMimic( - const context::Model & input_model, + template class JointCollectionTpl> + ModelTpl transformJointIntoMimic( + const ModelTpl & input_model, const JointIndex & index_primary, const JointIndex & index_secondary, - const context::Scalar & scaling, - const context::Scalar & offset) + const Scalar & scaling, + const Scalar & offset) { + typedef ModelTpl Model; + Model model; pinocchio::transformJointIntoMimic( input_model, index_primary, index_secondary, scaling, offset, model); @@ -191,9 +194,9 @@ namespace pinocchio bp::def( "transformJointIntoMimic", - static_cast( - pinocchio::python::transformJointIntoMimic), + (Model(*)( + const Model &, const JointIndex &, const JointIndex &, const double &, const double &))( + transformJointIntoMimic), bp::args("input_model", "index_primary", "index_secondary", "scaling", "offset"), "Transform of a joint of the model into a mimic joint. Keep the type of the joint as it " "was previously. \n\n" From 2f2e5ee410ab98311839f44b1b1bb5ba2514175e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 5 Feb 2025 15:12:06 +0100 Subject: [PATCH 238/310] [algo/model] Fix reduced model for mimic --- include/pinocchio/algorithm/model.hxx | 45 +++++++++++++++++++++------ 1 file changed, 35 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 8a4aa56896..32d823341c 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -639,16 +639,41 @@ namespace pinocchio } else { - // the joint should be added to the Model as it is - 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.jointConfigSelector(input_model.lowerPositionLimit), - joint_input_model.jointConfigSelector(input_model.upperPositionLimit), - joint_input_model.jointVelocitySelector(input_model.friction), - joint_input_model.jointVelocitySelector(input_model.damping)); + JointIndex reduced_joint_id; + if (boost::get>(&joint_input_model)) + { + auto mimic_joint = + boost::get>(joint_input_model); + + JointIndex mimicked_id = + reduced_model.getJointId(input_model.names[mimic_joint.jmodel().id()]); + mimic_joint.setMimicIndexes( + mimicked_id, reduced_model.idx_qs[mimicked_id], reduced_model.idx_vs[mimicked_id], + reduced_model.idx_vExtendeds[mimicked_id]); + + reduced_joint_id = reduced_model.addJoint( + reduced_parent_joint_index, mimic_joint, + parent_frame_placement * input_model.jointPlacements[joint_id], joint_name, + mimic_joint.jointVelocitySelector(input_model.effortLimit), + mimic_joint.jointVelocitySelector(input_model.velocityLimit), + mimic_joint.jointConfigSelector(input_model.lowerPositionLimit), + mimic_joint.jointConfigSelector(input_model.upperPositionLimit), + mimic_joint.jointVelocitySelector(input_model.friction), + mimic_joint.jointVelocitySelector(input_model.damping)); + } + else + { + // the joint should be added to the Model as it is + 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.jointConfigSelector(input_model.lowerPositionLimit), + joint_input_model.jointConfigSelector(input_model.upperPositionLimit), + joint_input_model.jointVelocitySelector(input_model.friction), + joint_input_model.jointVelocitySelector(input_model.damping)); + } // Append inertia reduced_model.appendBodyToJoint( reduced_joint_id, input_model.inertias[joint_id], SE3::Identity()); From 65adbb17d1404c0be5e63911e03914641e74bc1a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 5 Feb 2025 15:24:31 +0100 Subject: [PATCH 239/310] [algo/model] Remove useless typedef that causes compilation warning --- include/pinocchio/algorithm/model.hxx | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 32d823341c..0b2e7145cb 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -789,8 +789,6 @@ namespace pinocchio ModelTpl & reduced_model, std::vector & list_of_reduced_geom_models) { - - typedef ModelTpl Model; buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model); // for all GeometryModels From 5046018035041e2b55293e3cce7661bfeefa084d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 6 Feb 2025 17:41:47 +0100 Subject: [PATCH 240/310] [data] Use nvExtended instead of nv in nvSubtree --- include/pinocchio/algorithm/check-data.hxx | 10 ++-------- include/pinocchio/multibody/data.hxx | 15 +++------------ 2 files changed, 5 insertions(+), 20 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 8b645574a9..9cdcd7d122 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -102,17 +102,11 @@ namespace pinocchio { JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); - int nv; - if (boost::get>(&model.joints[j])) - nv = boost::get>(model.joints[j]) - .jmodel() - .nv(); - else - nv = model.joints[j].nv(); + int nv = model.joints[j].nvExtended(); for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant { CHECK_DATA(model.parents[d] >= j); - nv += model.joints[d].nv(); + nv += model.joints[d].nvExtended(); } CHECK_DATA(nv == data.nvSubtree[j]); diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index ead6b747c2..6f7d3d3eb2 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -203,19 +203,10 @@ namespace pinocchio const Index & parent = model.parents[(Index)i]; lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); - 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]]); + int nv_ = model.joints[(Index)lastChild[(Index)i]].nvExtended(); - 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_vExtended() + nv_ + - model.joints[(Index)i].idx_vExtended(); } } From e7d89a21a3ca1303918e23e2cb95029c6430f4f2 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 11:48:57 +0100 Subject: [PATCH 241/310] [unittest] Reactivate mimic in reachable workspace --- unittest/reachable-workspace.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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; From 735b807c91934c5966106b1ba901944919406ed6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 11:49:43 +0100 Subject: [PATCH 242/310] [unittest] Build mimic model generic --- unittest/utils/model-generator.hpp | 39 ++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index d5ad33a283..e39c0eebb2 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -3,6 +3,7 @@ // #include "pinocchio/multibody/model.hpp" +#include "pinocchio/algorithm/model.hpp" namespace pinocchio { @@ -65,4 +66,42 @@ namespace pinocchio "translation", Inertia::Random()); } + void buildMimicModel( + const Model & model_full, + const std::vector & primary_ids, + const std::vector & secondary_ids, + const double & ratio, + const double & offset, + Model & model_mimic, + Eigen::MatrixXd & G) + { + Model model_temp; + model_temp = model_full; + for (int i = 0; i < primary_ids.size(); i++) + { + pinocchio::transformJointIntoMimic( + model_temp, primary_ids[i], secondary_ids[i], ratio, offset, model_mimic); + model_temp = model_mimic; + } + // Initialize G as a zero matrix + G.resize(model_full.nv, model_mimic.nv); + G.setZero(); + // Set ones on the pseudo-diagonal + for (int j = 0; j < model_full.njoints; ++j) + { + if (std::find(secondary_ids.begin(), secondary_ids.end(), j) == secondary_ids.end()) + G.block( + model_full.joints[j].idx_v(), model_mimic.joints[j].idx_v(), model_full.joints[j].nv(), + model_mimic.joints[j].nv()) + .setIdentity(); + } + + // Set specific values for primary-secondary joint pairs + for (size_t i = 0; i < primary_ids.size(); ++i) + { + G(model_full.joints[secondary_ids[i]].idx_v(), model_full.joints[primary_ids[i]].idx_v()) = + ratio; + } + } + } // namespace pinocchio From 628a599eab0e7ece1a4082073f16faf82afcebcc Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 11:50:10 +0100 Subject: [PATCH 243/310] [unittest] Use generic buildMimic function and add test for double mimic in a model --- unittest/centroidal.cpp | 92 +++++++++++++++++++----------- unittest/crba.cpp | 120 ++++++++++++++++++++++++---------------- unittest/rnea.cpp | 87 +++++++++++++++++------------ 3 files changed, 181 insertions(+), 118 deletions(-) diff --git a/unittest/centroidal.cpp b/unittest/centroidal.cpp index 157c8feab5..5f928a1a2d 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -18,6 +18,8 @@ #include #include +#include "utils/model-generator.hpp" + template static void addJointAndBody( pinocchio::Model & model, @@ -92,42 +94,36 @@ BOOST_AUTO_TEST_CASE(test_ccrba) void test_mimic_against_full_model( const pinocchio::Model & model_full, - const pinocchio::JointIndex & primary_id, - const pinocchio::JointIndex & secondary_id) + const std::vector & primary_ids, + const std::vector & secondary_ids) { - // 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); + Eigen::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + 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 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); + double joint_ratio = 1.0; + double joint_offset = 0.0; + if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) + { + joint_ratio = ratio; + joint_offset = offset; + } model_full.joints[n].jointConfigExtendedModelSelector(q_full) = joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q) + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); @@ -172,20 +168,50 @@ 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")); + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("rleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Parallel + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("lleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); - // Test for spaced parent/child joint mimic - test_mimic_against_full_model( - humanoid_model, humanoid_model.getJointId("rleg1_joint"), - humanoid_model.getJointId("rleg4_joint")); + // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; - // // Test for parallel joint mimic - // test_mimic_against_full_model( - // humanoid_model, humanoid_model.getJointId("lleg4_joint"), - // humanoid_model.getJointId("rleg4_joint")); + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); } BOOST_AUTO_TEST_CASE(test_centroidal_mapping) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 4563b4f2ed..c70e90bf9e 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -31,6 +31,8 @@ #include #include +#include "utils/model-generator.hpp" + template static void addJointAndBody( pinocchio::Model & model, @@ -128,57 +130,47 @@ BOOST_AUTO_TEST_CASE(test_crba) void test_mimic_against_full_model( const pinocchio::Model & model_full, - const pinocchio::JointIndex & primary_id, - const pinocchio::JointIndex & secondary_id) + const std::vector & primary_ids, + const std::vector & secondary_ids) { - // 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_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); - 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::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); Eigen::VectorXd q_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); - - // World vs local - 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)); + Eigen::VectorXd as_mimic = Eigen::VectorXd::Random(model_mimic.nv); Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); Eigen::VectorXd v_full = G * v_mimic; - Eigen::VectorXd a_full = G * a_mimic; + Eigen::VectorXd a_full = G * v_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); + double joint_ratio = 1.0; + double joint_offset = 0.0; + if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) + { + joint_ratio = ratio; + joint_offset = offset; + } model_full.joints[n].jointConfigExtendedModelSelector(q_full) = joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q_mimic) + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); } - // // // Run crba - // // pinocchio::crba(model_mimic, data_mimic, q_mimic); + pinocchio::Data data_full(model_full); + pinocchio::Data data_mimic(model_mimic); + pinocchio::Data data_ref_mimic(model_mimic); + + // World vs local + 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)); + pinocchio::crba(model_full, data_full, q_full); // Compute other half of matrix @@ -194,22 +186,52 @@ void test_mimic_against_full_model( 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 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")); + pinocchio::buildModels::humanoidRandom(humanoid_model); + + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("rleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Parallel + primaries.clear(); + secondaries.clear(); + + primaries = {humanoid_model.getJointId("lleg1_joint")}; + + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); } BOOST_AUTO_TEST_CASE(test_minimal_crba) diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 3bbed9b5f3..8f908768b0 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -34,6 +34,8 @@ #include #include +#include "utils/model-generator.hpp" + BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_rnea) @@ -347,32 +349,19 @@ 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) + const std::vector & primary_ids, + const std::vector & secondary_ids) { - // 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); + Eigen::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + 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); @@ -383,8 +372,13 @@ void test_mimic_against_full_model( 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); + double joint_ratio = 1.0; + double joint_offset = 0.0; + if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) + { + joint_ratio = ratio; + joint_offset = offset; + } model_full.joints[n].jointConfigExtendedModelSelector(q_full) = joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q) + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); @@ -397,7 +391,6 @@ void test_mimic_against_full_model( // // 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)); @@ -407,7 +400,7 @@ void test_mimic_against_full_model( 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)); + BOOST_CHECK(tau_nle.isApprox(tau_ref_nle)); // Generalized Gravity pinocchio::Data data_ref_gg(model_mimic); @@ -438,20 +431,42 @@ 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")); + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("rleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Parallel + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); } BOOST_AUTO_TEST_SUITE_END() From ccd02416b0b10b65e62f313b432eeb691508702c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 16:49:34 +0100 Subject: [PATCH 244/310] [Common-operation] Assert that it's not possible --- .../multibody/joint/joint-common-operations.hpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index 9267bb4141..8c6f0f6567 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -96,14 +96,7 @@ namespace pinocchio const Scalar & offset, const Eigen::MatrixBase & qOut) { - 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; + assert(false && "Joint cannot be used with JointMimic."); } }; From 76c6ce829190f38037bfe3c0b6a695ccd476ca28 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 16:50:13 +0100 Subject: [PATCH 245/310] [joint/helical] Add affine transform to mimic --- include/pinocchio/multibody/joint/joint-helical.hpp | 6 ++++++ unittest/utils/model-generator.hpp | 1 - 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-helical.hpp b/include/pinocchio/multibody/joint/joint-helical.hpp index a8c7ae837d..ea5a9a5036 100644 --- a/include/pinocchio/multibody/joint/joint-helical.hpp +++ b/include/pinocchio/multibody/joint/joint-helical.hpp @@ -971,6 +971,12 @@ namespace pinocchio typedef JointDataHelicalTpl JointDataHZ; typedef JointModelHelicalTpl JointModelHZ; + template + struct ConfigVectorAffineTransform> + { + typedef LinearAffineTransform Type; + }; + } // namespace pinocchio #include diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index e39c0eebb2..5da98941d9 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -103,5 +103,4 @@ namespace pinocchio ratio; } } - } // namespace pinocchio From 11998b4b166436c6afd5e45e074dc6872fdba39a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 16:50:28 +0100 Subject: [PATCH 246/310] [unittest] Fix acceleration --- unittest/centroidal.cpp | 2 +- unittest/crba.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/unittest/centroidal.cpp b/unittest/centroidal.cpp index 5f928a1a2d..86e46e454e 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -113,7 +113,7 @@ void test_mimic_against_full_model( Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); Eigen::VectorXd v_full = G * v; - Eigen::VectorXd a_full = G * v; + Eigen::VectorXd a_full = G * a; for (int n = 1; n < model_full.njoints; n++) { diff --git a/unittest/crba.cpp b/unittest/crba.cpp index c70e90bf9e..b35537e543 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -145,7 +145,7 @@ void test_mimic_against_full_model( Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_full.nq); Eigen::VectorXd v_full = G * v_mimic; - Eigen::VectorXd a_full = G * v_mimic; + Eigen::VectorXd a_full = G * as_mimic; for (int n = 1; n < model_full.njoints; n++) { From bca6914bc17b3cda8ba936e3996646cd02a29241 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 16:51:27 +0100 Subject: [PATCH 247/310] [EtienneAr Feedback] joint-mimic: Remove mutable from joint data --- .../pinocchio/multibody/joint/joint-mimic.hpp | 101 ++++++++---------- 1 file changed, 45 insertions(+), 56 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 039c3e6f2d..6ae5757829 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -313,9 +313,9 @@ namespace pinocchio 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; @@ -324,12 +324,31 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; + typedef Constraint_t ConstraintTypeConstRef; + typedef Constraint_t ConstraintTypeRef; + typedef Transformation_t TansformTypeConstRef; + typedef Transformation_t TansformTypeRef; + typedef Motion_t MotionTypeConstRef; + typedef Motion_t MotionTypeRef; + typedef Bias_t BiasTypeConstRef; + typedef Bias_t BiasTypeRef; + typedef U_t UTypeConstRef; + typedef U_t UTypeRef; + typedef D_t DTypeConstRef; + typedef D_t DTypeRef; + typedef UD_t UDTypeConstRef; + typedef UD_t UDTypeRef; + typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; - typedef boost::mpl::false_ is_mimicable_t; + typedef ConfigVector_t ConfigVectorTypeConstRef; + typedef ConfigVector_t ConfigVectorTypeRef; + typedef TangentVector_t TangentVectorTypeConstRef; + typedef TangentVector_t TangentVectorTypeRef; - PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE + typedef boost::mpl::false_ is_mimicable_t; + // PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; template class JointCollectionTpl> @@ -420,93 +439,74 @@ namespace pinocchio TansformTypeConstRef M_accessor() const { - M_ = m_jdata_mimicking.M(); - return M_; + return m_jdata_mimicking.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_mimicking.M(); - return M_; + assert( + false + && "Changes to non const ref on mimic joints won't be taken into account. Use const ref"); + return m_jdata_mimicking.M(); } MotionTypeConstRef v_accessor() const { - v_ = m_jdata_mimicking.v(); - return v_; + return m_jdata_mimicking.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_mimicking.v(); - return v_; + assert(false && "Changes to non const ref on mimic joints won't be taken into account."); + return m_jdata_mimicking.v(); } BiasTypeConstRef c_accessor() const { - c_ = m_jdata_mimicking.c(); - return c_; + return m_jdata_mimicking.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_mimicking.c(); - return c_; + assert(false && "Changes to non const ref on mimic joints won't be taken into account."); + return m_jdata_mimicking.c(); } UTypeConstRef U_accessor() const { - U_ = m_jdata_mimicking.U(); - return U_; + return m_jdata_mimicking.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_mimicking.U(); - return U_; + assert(false && "Changes to non const ref on mimic joints won't be taken into account. "); + return m_jdata_mimicking.U(); } DTypeConstRef Dinv_accessor() const { - Dinv_ = m_jdata_mimicking.Dinv(); - return Dinv_; + return m_jdata_mimicking.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_mimicking.Dinv(); - return Dinv_; + assert(false && "Changes to non const ref on mimic joints won't be taken into account."); + return m_jdata_mimicking.Dinv(); } UDTypeConstRef UDinv_accessor() const { - UDinv_ = m_jdata_mimicking.UDinv(); - return UDinv_; + return m_jdata_mimicking.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_mimicking.UDinv(); - return UDinv_; + assert(false && "Changes to non const ref on mimic joints won't be taken into account."); + return m_jdata_mimicking.UDinv(); } DTypeConstRef StU_accessor() const { - StU_ = m_jdata_mimicking.StU(); - return StU_; + return m_jdata_mimicking.StU(); } DTypeRef StU_accessor() { - // assert(false && "Changes to non const ref on mimic joints won't be taken into account. Use - // const ref"); - StU_ = m_jdata_mimicking.StU(); - return StU_; + assert(false && "Changes to non const ref on mimic joints won't be taken into account."); + return m_jdata_mimicking.StU(); } friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>; @@ -577,17 +577,6 @@ namespace pinocchio public: // data Constraint_t S; - - 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< From 8695187a6c91c5ff50b2a75515873801df2d51e2 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 7 Feb 2025 17:26:30 +0100 Subject: [PATCH 248/310] [serialization] Fix mimic data serialization --- .../pinocchio/serialization/joints-data.hpp | 38 ++++++++++--------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 1a4fc0e432..587b0e8efe 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -62,22 +62,24 @@ namespace boost ar & make_nvp("StU", joint_data.StU()); } - 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("joint_q", joint_data.joint_q()); - ar & make_nvp("joint_v", joint_data.joint_v()); - - ar & make_nvp("S", joint_data.S()); - } + // template< + // class Archive, + // typename Scalar, + // int Options, + // template class JointCollection> + // void serialize( + // Archive & ar, + // pinocchio::JointDataBase> & + // joint_data, + // const unsigned int /*version*/) + // { + // typedef pinocchio::JointDataMimicTpl JointType; + // fix::serialize(ar, static_cast &>(joint_data), version); + // // ar & make_nvp("joint_q", joint_data.joint_q()); + // // ar & make_nvp("joint_v", joint_data.joint_v()); + + // // ar & make_nvp("S", joint_data.S()); + // } } // namespace fix @@ -261,8 +263,8 @@ namespace boost pinocchio::JointDataMimicTpl & joint, const unsigned int version) { - typedef pinocchio::JointDataMimicTpl JointType; - fix::serialize(ar, static_cast &>(joint), version); + // 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()); From 0d14165cef65232521922f51f208ec20523d83a6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 10 Feb 2025 09:26:03 +0100 Subject: [PATCH 249/310] [algo/centroidal] Add comments to explain mimic --- include/pinocchio/algorithm/centroidal.hxx | 3 +++ 1 file changed, 3 insertions(+) diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 64c1a4feae..58884242df 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -140,6 +140,7 @@ namespace pinocchio J_cols = data.oMi[i].act(jdata.S()); ColsBlock Ag_cols = jmodel.jointCols(data.Ag); + // Mimic joint contributes to the inertia of their primary and don't have their own motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); data.oYcrb[parent] += data.oYcrb[i]; @@ -174,6 +175,7 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -222,6 +224,7 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { From 6c711ed2ad21e0c45dd4d6df8a6a39009ce072a9 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 10 Feb 2025 09:26:17 +0100 Subject: [PATCH 250/310] [algo/rnea] Add comment to explain mimic --- include/pinocchio/algorithm/rnea.hxx | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 3125f6348b..594d7e8f74 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -99,6 +99,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; + // Mimic joint contributes to the torque of their primary and don't have their own jmodel.jointVelocityExtendedModelSelector(data.tau) += jdata.S().transpose() * data.f[i]; if (parent > 0) @@ -130,6 +131,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -184,6 +186,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -280,6 +283,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; + // Mimic joint contributes to the nle of their primary and don't have their own jmodel.jointVelocityExtendedModelSelector(data.nle) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); @@ -307,6 +311,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account data.nle.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -388,7 +393,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; - + // Mimic joint contributes to the gravity of their primary and don't have their own jmodel.jointVelocityExtendedModelSelector(g) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); @@ -413,6 +418,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account data.g.setZero(); data.a_gf[0] = -model.gravity; @@ -467,7 +473,7 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); data.f[i] -= fext[i]; } - + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account data.tau.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) From 8b6e9f4336d7e935dc8d27b27b7c34bbe759d5fd Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 10 Feb 2025 09:29:21 +0100 Subject: [PATCH 251/310] [unittest/mimic] Remove test for no transform (since it throws now) --- unittest/joint-mimic.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index d1062c0010..9adb404703 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -270,17 +270,6 @@ BOOST_AUTO_TEST_CASE(test_transform_linear_revolute_unbounded) == 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) { JointModelRX jmodel_ref; From 1a30833e2e6cf9d0124a9a6b911dabb145cb0645 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 10 Feb 2025 17:16:32 +0100 Subject: [PATCH 252/310] Apply pre-commit --- include/pinocchio/algorithm/centroidal.hxx | 6 ++++-- include/pinocchio/algorithm/rnea.hxx | 15 ++++++++++----- .../pinocchio/serialization/joints-constraint.hpp | 0 3 files changed, 14 insertions(+), 7 deletions(-) delete mode 100644 include/pinocchio/serialization/joints-constraint.hpp diff --git a/include/pinocchio/algorithm/centroidal.hxx b/include/pinocchio/algorithm/centroidal.hxx index 58884242df..be4b9a71f9 100644 --- a/include/pinocchio/algorithm/centroidal.hxx +++ b/include/pinocchio/algorithm/centroidal.hxx @@ -175,7 +175,8 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; - // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { @@ -224,7 +225,8 @@ namespace pinocchio data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); typedef CcrbaBackwardStep Pass2; - // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account data.Ag.setZero(); for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index 594d7e8f74..d81e0d1bc4 100644 --- a/include/pinocchio/algorithm/rnea.hxx +++ b/include/pinocchio/algorithm/rnea.hxx @@ -131,7 +131,8 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; - // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -186,7 +187,8 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; - // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account data.tau.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -311,7 +313,8 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; - // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account data.nle.setZero(); data.v[0].setZero(); data.a_gf[0] = -model.gravity; @@ -418,7 +421,8 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; - // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account data.g.setZero(); data.a_gf[0] = -model.gravity; @@ -473,7 +477,8 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); data.f[i] -= fext[i]; } - // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to take mimic joint into account + // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to + // take mimic joint into account data.tau.setZero(); typedef ComputeGeneralizedGravityBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) diff --git a/include/pinocchio/serialization/joints-constraint.hpp b/include/pinocchio/serialization/joints-constraint.hpp deleted file mode 100644 index e69de29bb2..0000000000 From 95688f73f51670d9baf9851ed8250aa2ed04bc49 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 10 Feb 2025 17:16:51 +0100 Subject: [PATCH 253/310] [Serialization/Data] Fix mimic data serialization --- .../pinocchio/multibody/joint/joint-mimic.hpp | 78 +++++-------------- .../pinocchio/serialization/joints-data.hpp | 45 +++++------ unittest/serialization.cpp | 15 +++- 3 files changed, 51 insertions(+), 87 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 6ae5757829..2de379d4fa 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -324,8 +324,8 @@ namespace pinocchio typedef Eigen::Matrix D_t; typedef Eigen::Matrix UD_t; - typedef Constraint_t ConstraintTypeConstRef; - typedef Constraint_t ConstraintTypeRef; + typedef const Constraint_t & ConstraintTypeConstRef; + typedef Constraint_t & ConstraintTypeRef; typedef Transformation_t TansformTypeConstRef; typedef Transformation_t TansformTypeRef; typedef Motion_t MotionTypeConstRef; @@ -342,13 +342,12 @@ namespace pinocchio typedef Eigen::Matrix ConfigVector_t; typedef Eigen::Matrix TangentVector_t; - typedef ConfigVector_t ConfigVectorTypeConstRef; - typedef ConfigVector_t ConfigVectorTypeRef; - typedef TangentVector_t TangentVectorTypeConstRef; - typedef TangentVector_t TangentVectorTypeRef; + typedef const ConfigVector_t & ConfigVectorTypeConstRef; + typedef ConfigVector_t & ConfigVectorTypeRef; + typedef const TangentVector_t TangentVectorTypeConstRef; + typedef TangentVector_t & TangentVectorTypeRef; typedef boost::mpl::false_ is_mimicable_t; - // PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE }; template class JointCollectionTpl> @@ -437,75 +436,38 @@ namespace pinocchio return S; } - TansformTypeConstRef M_accessor() const + Transformation_t M_accessor() const { return m_jdata_mimicking.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_jdata_mimicking.M(); - } - MotionTypeConstRef v_accessor() const - { - return m_jdata_mimicking.v(); - } - MotionTypeRef v_accessor() + Motion_t v_accessor() const { - assert(false && "Changes to non const ref on mimic joints won't be taken into account."); return m_jdata_mimicking.v(); } - BiasTypeConstRef c_accessor() const + Bias_t c_accessor() const { return m_jdata_mimicking.c(); } - BiasTypeRef c_accessor() - { - assert(false && "Changes to non const ref on mimic joints won't be taken into account."); - return m_jdata_mimicking.c(); - } - UTypeConstRef U_accessor() const + U_t U_accessor() const { return m_jdata_mimicking.U(); } - UTypeRef U_accessor() - { - assert(false && "Changes to non const ref on mimic joints won't be taken into account. "); - return m_jdata_mimicking.U(); - } - DTypeConstRef Dinv_accessor() const + D_t Dinv_accessor() const { return m_jdata_mimicking.Dinv(); } - DTypeRef Dinv_accessor() - { - assert(false && "Changes to non const ref on mimic joints won't be taken into account."); - return m_jdata_mimicking.Dinv(); - } - UDTypeConstRef UDinv_accessor() const + UD_t UDinv_accessor() const { return m_jdata_mimicking.UDinv(); } - UDTypeRef UDinv_accessor() - { - assert(false && "Changes to non const ref on mimic joints won't be taken into account."); - return m_jdata_mimicking.UDinv(); - } - DTypeConstRef StU_accessor() const - { - return m_jdata_mimicking.StU(); - } - DTypeRef StU_accessor() + D_t StU_accessor() const { - assert(false && "Changes to non const ref on mimic joints won't be taken into account."); return m_jdata_mimicking.StU(); } @@ -520,11 +482,11 @@ namespace pinocchio return m_jdata_mimicking; } - ConfigVector_t & joint_q_accessor() + ConfigVectorTypeRef joint_q_accessor() { return joint_q; } - const ConfigVector_t & joint_q_accessor() const + ConfigVectorTypeConstRef joint_q_accessor() const { return joint_q; } @@ -537,12 +499,11 @@ namespace pinocchio { return joint_q_transformed; } - - TangentVector_t & joint_v_accessor() + TangentVectorTypeRef joint_v_accessor() { return joint_v; } - const TangentVector_t & joint_v_accessor() const + TangentVectorTypeConstRef joint_v_accessor() const { return joint_v; } @@ -562,7 +523,6 @@ namespace pinocchio os << " Mimicking joint data: " << m_jdata_mimicking.shortname() << std::endl; } - protected: RefJointData m_jdata_mimicking; /// \brief original configuration vector @@ -573,8 +533,6 @@ namespace pinocchio TangentVector_t joint_v; /// \brief Transform velocity vector. TangentVector_t joint_v_transformed; - - public: // data Constraint_t S; }; // struct JointDataMimicTpl @@ -711,7 +669,6 @@ namespace pinocchio JointDataDerived createData() const { - return JointDataDerived( m_jmodel_mimicking.createData(), scaling(), m_nqExtended, m_nvExtended); } @@ -747,6 +704,7 @@ namespace pinocchio configVectorAffineTransform( m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed); jdata.joint_v_transformed = m_scaling * jdata.joint_v; + m_jmodel_mimicking.calc( jdata.m_jdata_mimicking, jdata.joint_q_transformed, jdata.joint_v_transformed); } diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 587b0e8efe..157d3ba7d7 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -62,24 +62,22 @@ namespace boost ar & make_nvp("StU", joint_data.StU()); } - // template< - // class Archive, - // typename Scalar, - // int Options, - // template class JointCollection> - // void serialize( - // Archive & ar, - // pinocchio::JointDataBase> & - // joint_data, - // const unsigned int /*version*/) - // { - // typedef pinocchio::JointDataMimicTpl JointType; - // fix::serialize(ar, static_cast &>(joint_data), version); - // // ar & make_nvp("joint_q", joint_data.joint_q()); - // // ar & make_nvp("joint_v", joint_data.joint_v()); - - // // ar & make_nvp("S", joint_data.S()); - // } + 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("joint_q", joint_data.joint_q()); + ar & make_nvp("joint_v", joint_data.joint_v()); + + ar & make_nvp("S", joint_data.S()); + } } // namespace fix @@ -235,7 +233,6 @@ namespace boost { typedef pinocchio::JointDataCompositeTpl JointType; fix::serialize(ar, static_cast &>(joint), version); - ::pinocchio::Serialize::run(ar, joint); } @@ -257,19 +254,19 @@ namespace boost class Archive, typename Scalar, int Options, - template typename JointCollection> + template class JointCollectionTpl> void serialize( Archive & ar, - pinocchio::JointDataMimicTpl & joint, + pinocchio::JointDataMimicTpl & joint, const unsigned int version) { - // typedef pinocchio::JointDataMimicTpl JointType; - // fix::serialize(ar, static_cast &>(joint), version); + ar & make_nvp("jdata", joint.jdata()); ar & make_nvp("m_q_transform", joint.q_transformed()); ar & make_nvp("m_v_transform", joint.v_transformed()); - ar & make_nvp("jdata", joint.jdata()); + typedef pinocchio::JointDataMimicTpl JointType; + fix::serialize(ar, static_cast &>(joint), version); } } // namespace serialization diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index c15ad8826e..fc810abb67 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -132,7 +132,6 @@ void generic_test(const T & object, const std::string & filename, const std::str delete &object_loaded; } - // Load and save as XML const std::string xml_filename = filename + ".xml"; saveToXML(object, xml_filename, tag_name); @@ -140,7 +139,6 @@ void generic_test(const T & object, const std::string & filename, const std::str { T & object_loaded = *empty_contructor(); loadFromXML(object_loaded, xml_filename, tag_name); - // Check BOOST_CHECK(run_call_equality_op(object_loaded, object)); @@ -607,7 +605,18 @@ struct TestJointData template class JointCollection> void operator()(const pinocchio::JointModelMimicTpl &) { - // Do nothing + typedef pinocchio::JointModelMimicTpl JointModel; + typedef typename JointModel::JointDerived JointDerived; + typedef typename pinocchio::traits::JointDataDerived JointData; + + JointModel jmodel = init::run(); + JointData jdata = jmodel.createData(); + + Eigen::VectorXd q_random = Eigen::VectorXd::Random(jmodel.jmodel().nq()); + Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.jmodel().nv()); + jmodel.calc(jdata, q_random, v_random); + + test(jdata); } template From 4df852a73e4cbf9ea7d6b8f7445a33afef75e4bc Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 10 Feb 2025 18:45:30 +0100 Subject: [PATCH 254/310] [parser/urdf] Don't break api --- bindings/python/parsers/urdf/model.cpp | 2 +- include/pinocchio/parsers/urdf.hpp | 36 ++++++++++++------------ include/pinocchio/parsers/urdf/model.hxx | 32 ++++++++++----------- 3 files changed, 35 insertions(+), 35 deletions(-) diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index a188632557..2d66ae2336 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -131,7 +131,7 @@ namespace pinocchio "buildModelFromUrdf", static_cast( pinocchio::python::buildModelFromUrdf), - (bp::arg("urdf_filename"), bp::arg("root"), bp::arg("mimic") = false), + (bp::arg("urdf_filename"), bp::arg("root_joint"), bp::arg("mimic") = false), "Parse the URDF file given in input and return a pinocchio Model starting with the " "given root joint."); diff --git a/include/pinocchio/parsers/urdf.hpp b/include/pinocchio/parsers/urdf.hpp index 6ec709f442..91c9223523 100644 --- a/include/pinocchio/parsers/urdf.hpp +++ b/include/pinocchio/parsers/urdf.hpp @@ -43,8 +43,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF file with a particular joint as root of the model tree @@ -62,8 +62,8 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF file with a fixed joint as root of the model tree. @@ -78,8 +78,8 @@ namespace pinocchio ModelTpl & buildModel( const std::string & filename, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF model with a particular joint as root of the model tree @@ -99,8 +99,8 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF model with a particular joint as root of the model tree @@ -122,8 +122,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from a URDF model @@ -140,8 +140,8 @@ namespace pinocchio ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from an XML stream with a particular joint as root of the model tree @@ -163,8 +163,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from an XML stream with a particular joint as root of the model tree @@ -184,8 +184,8 @@ namespace pinocchio const std::string & xml_stream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /// /// \brief Build the model from an XML stream @@ -202,8 +202,8 @@ namespace pinocchio ModelTpl & buildModelFromXML( const std::string & xml_stream, ModelTpl & model, - const bool mimic = false, - const bool verbose = false); + const bool verbose = false, + const bool mimic = false); /** * @brief Build The GeometryModel from a URDF file. Search for meshes diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index f68a15a49d..2da7c65381 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -635,8 +635,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { if (rootJointName.empty()) throw std::invalid_argument( @@ -655,8 +655,8 @@ namespace pinocchio const std::string & filename, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { return buildModel(filename, rootJoint, "root_joint", model, mimic, verbose); } @@ -665,8 +665,8 @@ namespace pinocchio ModelTpl & buildModel( const std::string & filename, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { details::UrdfVisitor visitor(model); if (verbose) @@ -681,8 +681,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { if (rootJointName.empty()) throw std::invalid_argument( @@ -701,8 +701,8 @@ namespace pinocchio const std::string & xmlStream, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, mimic, verbose); } @@ -711,8 +711,8 @@ namespace pinocchio ModelTpl & buildModelFromXML( const std::string & xmlStream, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { details::UrdfVisitor visitor(model); if (verbose) @@ -727,8 +727,8 @@ namespace pinocchio const typename ModelTpl::JointModel & rootJoint, const std::string & rootJointName, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { if (rootJointName.empty()) throw std::invalid_argument( @@ -748,8 +748,8 @@ namespace pinocchio const std::shared_ptr<::urdf::ModelInterface> urdfTree, const typename ModelTpl::JointModel & rootJoint, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { return buildModel(urdfTree, rootJoint, "root_joint", model, mimic, verbose); } From 9eebd7e9d6b4f1bdc6ffd6322b4a87bde213bd92 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 11 Feb 2025 09:51:16 +0100 Subject: [PATCH 255/310] [bindings/model] Use proxy --- bindings/python/algorithm/expose-model.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/bindings/python/algorithm/expose-model.cpp b/bindings/python/algorithm/expose-model.cpp index 28bfb4d19c..8e3b3b0048 100644 --- a/bindings/python/algorithm/expose-model.cpp +++ b/bindings/python/algorithm/expose-model.cpp @@ -84,7 +84,7 @@ namespace pinocchio } template class JointCollectionTpl> - ModelTpl transformJointIntoMimic( + ModelTpl transformJointIntoMimic_proxy( const ModelTpl & input_model, const JointIndex & index_primary, const JointIndex & index_secondary, @@ -194,9 +194,7 @@ namespace pinocchio bp::def( "transformJointIntoMimic", - (Model(*)( - const Model &, const JointIndex &, const JointIndex &, const double &, const double &))( - transformJointIntoMimic), + transformJointIntoMimic_proxy, bp::args("input_model", "index_primary", "index_secondary", "scaling", "offset"), "Transform of a joint of the model into a mimic joint. Keep the type of the joint as it " "was previously. \n\n" From a0043125ea6eeee4a8b93d97ff3e9213a5a2b3b3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 11 Feb 2025 09:52:36 +0100 Subject: [PATCH 256/310] [model] Iintroduce mimic_joints and mimic_pairs value (missing serialize and bindigns for mimic_pairs) --- include/pinocchio/algorithm/check-model.hxx | 8 ++------ .../pinocchio/bindings/python/multibody/model.hpp | 2 ++ include/pinocchio/multibody/model.hpp | 10 ++++++++++ include/pinocchio/multibody/model.hxx | 15 ++++++++++++++- include/pinocchio/serialization/model.hpp | 1 + 5 files changed, 29 insertions(+), 7 deletions(-) diff --git a/include/pinocchio/algorithm/check-model.hxx b/include/pinocchio/algorithm/check-model.hxx index d957fe3e3a..d7ad286da1 100644 --- a/include/pinocchio/algorithm/check-model.hxx +++ b/include/pinocchio/algorithm/check-model.hxx @@ -60,12 +60,8 @@ namespace pinocchio inline bool MimicChecker::checkModel_impl(const ModelTpl & model) const { - typedef ModelTpl Model; - typedef typename Model::JointIndex JointIndex; - - for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) - if (boost::get>(&model.joints[j])) - return false; + if (!model.mimic_joints.empty()) + return false; return true; } diff --git a/include/pinocchio/bindings/python/multibody/model.hpp b/include/pinocchio/bindings/python/multibody/model.hpp index d8e273c335..2288ce64df 100644 --- a/include/pinocchio/bindings/python/multibody/model.hpp +++ b/include/pinocchio/bindings/python/multibody/model.hpp @@ -135,6 +135,8 @@ namespace pinocchio "subtrees", &Model::subtrees, "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.") + .def_readwrite("mimic_joints", &Model::mimic_joints, "Vector of mimic joints indexes") + .def_readwrite( "gravity", &Model::gravity, "Motion vector corresponding to the gravity field expressed in the world Frame.") diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index 16cee1df8e..eb3c38b735 100644 --- a/include/pinocchio/multibody/model.hpp +++ b/include/pinocchio/multibody/model.hpp @@ -94,6 +94,9 @@ namespace pinocchio /// It also handles the notion of co-tangent vector (e.g. torque, etc). typedef VectorXs TangentVectorType; + /// \brief Pairs of index for common ancestor of a mimic and its primary + typedef std::pair MimicAncestorPair; + /// \brief Dimension of the configuration vector representation. int nq; @@ -147,6 +150,13 @@ namespace pinocchio /// the set (i==parents[k] for k in mu(i)). std::vector children; + /// \brief Vector of mimic joints in the tree + std::vector mimic_joints; + + /// \brief Vector of mimic joints and its primary common ancestors. It corresponds to the index + /// in their respective model.supports. + std::vector mimic_pairs; + /// \brief Name of the joints. std::vector names; diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index 7b2c753604..c039851157 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -8,6 +8,7 @@ #include "pinocchio/utils/string-generator.hpp" #include "pinocchio/multibody/liegroup/liegroup-algo.hpp" +#include "pinocchio/algorithm/model.hpp" /// @cond DEV @@ -156,6 +157,14 @@ namespace pinocchio supports.push_back(supports[parent]); supports[joint_id].push_back(joint_id); + if (auto jmodel_ = boost::get>(&jmodel)) + { + mimic_joints.push_back(jmodel.id()); + + size_t anc_prim, anc_sec; + findCommonAncestor(*this, jmodel.id(), jmodel_->jmodel().id(), anc_sec, anc_prim); + mimic_pairs.push_back(std::make_pair(anc_prim, anc_sec)); + } return joint_id; } @@ -242,6 +251,8 @@ namespace pinocchio res.names = names; res.subtrees = subtrees; res.supports = supports; + res.mimic_joints = mimic_joints; + res.mimic_pairs = mimic_pairs; res.gravity = gravity.template cast(); res.name = name; @@ -297,7 +308,9 @@ namespace pinocchio bool res = other.nq == nq && other.nv == nv && other.nvExtended == nvExtended && 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; + && other.subtrees == subtrees && other.mimic_joints == mimic_joints + && other.mimic_pairs == mimic_pairs && 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_vExtendeds == idx_vExtendeds && other.nvExtendeds == nvExtendeds; diff --git a/include/pinocchio/serialization/model.hpp b/include/pinocchio/serialization/model.hpp index b07f4221ca..1242d1941f 100644 --- a/include/pinocchio/serialization/model.hpp +++ b/include/pinocchio/serialization/model.hpp @@ -48,6 +48,7 @@ namespace boost ar & make_nvp("names", model.names); ar & make_nvp("supports", model.supports); ar & make_nvp("subtrees", model.subtrees); + ar & make_nvp("mimic_joints", model.mimic_joints); ar & make_nvp("gravity", model.gravity); ar & make_nvp("name", model.name); From 017368da4fc6ad1cb07dfb433feffbc3863ddca3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 11 Feb 2025 10:44:04 +0100 Subject: [PATCH 257/310] [algo/model] Add new model attributes to transformMimicJoint --- include/pinocchio/algorithm/model.hxx | 5 +++++ include/pinocchio/multibody/model.hxx | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 0b2e7145cb..cc27a50498 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -961,6 +961,11 @@ namespace pinocchio jmodel_input.jointVelocitySelector(input_model.damping); } } + + output_model.mimic_joints.push_back(index_secondary); + size_t anc_prim, anc_sec; + findCommonAncestor(output_model, index_primary, index_secondary, anc_prim, anc_sec); + output_model.mimic_pairs.push_back(std::make_pair(anc_prim, anc_sec)); } template class JointCollectionTpl> diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index c039851157..f7d138548a 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -162,7 +162,7 @@ namespace pinocchio mimic_joints.push_back(jmodel.id()); size_t anc_prim, anc_sec; - findCommonAncestor(*this, jmodel.id(), jmodel_->jmodel().id(), anc_sec, anc_prim); + findCommonAncestor(*this, jmodel_->jmodel().id(), jmodel.id(), anc_sec, anc_prim); mimic_pairs.push_back(std::make_pair(anc_prim, anc_sec)); } return joint_id; From 22f87ea1d1ed00a0cce9dc06ddb3e2a06b977f5e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 11 Feb 2025 18:11:08 +0100 Subject: [PATCH 258/310] [data] Fix nvSubtree --- include/pinocchio/algorithm/check-data.hxx | 3 ++- include/pinocchio/multibody/data.hxx | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 9cdcd7d122..4715dbf286 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -106,7 +106,8 @@ namespace pinocchio for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant { CHECK_DATA(model.parents[d] >= j); - nv += model.joints[d].nvExtended(); + + nv += model.joints[d].nv(); } CHECK_DATA(nv == data.nvSubtree[j]); diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 6f7d3d3eb2..02256b3ed0 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -205,8 +205,8 @@ namespace pinocchio lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); int nv_ = model.joints[(Index)lastChild[(Index)i]].nvExtended(); - nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_vExtended() + nv_ - - model.joints[(Index)i].idx_vExtended(); + nvSubtree[(Index)i] = + model.joints[(Index)lastChild[(Index)i]].idx_v() + nv_ - model.joints[(Index)i].idx_v(); } } From bb5464fb2f0780796d13e796f94bb3e75119379a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 12 Feb 2025 13:51:31 +0100 Subject: [PATCH 259/310] [parser/urdf] Add false to verbose argument and fix argument order issues --- bindings/python/parsers/urdf/model.cpp | 27 +++++++++++++----------- include/pinocchio/parsers/urdf/model.hxx | 10 ++++----- 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/bindings/python/parsers/urdf/model.cpp b/bindings/python/parsers/urdf/model.cpp index 2d66ae2336..d94ff3539b 100644 --- a/bindings/python/parsers/urdf/model.cpp +++ b/bindings/python/parsers/urdf/model.cpp @@ -23,20 +23,20 @@ namespace pinocchio Model buildModelFromUrdf(const bp::object & filename, const bool mimic) { Model model; - pinocchio::urdf::buildModel(path(filename), model, mimic); + pinocchio::urdf::buildModel(path(filename), model, false, mimic); return model; } Model & buildModelFromUrdf(const bp::object & filename, Model & model, const bool mimic) { - return pinocchio::urdf::buildModel(path(filename), model, mimic); + return pinocchio::urdf::buildModel(path(filename), model, false, mimic); } Model buildModelFromUrdf(const bp::object & filename, const JointModel & root_joint, const bool mimic) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); + pinocchio::urdf::buildModel(path(filename), root_joint, model, false, mimic); return model; } @@ -47,14 +47,14 @@ namespace pinocchio const bool mimic) { Model model; - pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); + pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, false, mimic); return model; } Model & buildModelFromUrdf( const bp::object & filename, const JointModel & root_joint, Model & model, const bool mimic) { - return pinocchio::urdf::buildModel(path(filename), root_joint, model, mimic); + return pinocchio::urdf::buildModel(path(filename), root_joint, model, false, mimic); } Model & buildModelFromUrdf( @@ -64,14 +64,15 @@ namespace pinocchio Model & model, const bool mimic) { - return pinocchio::urdf::buildModel(path(filename), root_joint, root_joint_name, model, mimic); + return pinocchio::urdf::buildModel( + path(filename), root_joint, root_joint_name, model, false, mimic); } Model buildModelFromXML( const std::string & xml_stream, const JointModel & root_joint, const bool mimic) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, false, mimic); return model; } @@ -82,7 +83,8 @@ namespace pinocchio const bool mimic) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); + pinocchio::urdf::buildModelFromXML( + xml_stream, root_joint, root_joint_name, model, false, mimic); return model; } @@ -92,7 +94,7 @@ namespace pinocchio Model & model, const bool mimic) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, mimic); + pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, model, false, mimic); return model; } @@ -103,20 +105,21 @@ namespace pinocchio Model & model, const bool mimic) { - pinocchio::urdf::buildModelFromXML(xml_stream, root_joint, root_joint_name, model, mimic); + pinocchio::urdf::buildModelFromXML( + xml_stream, root_joint, root_joint_name, model, false, mimic); return model; } Model buildModelFromXML(const std::string & xml_stream, const bool mimic) { Model model; - pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); + pinocchio::urdf::buildModelFromXML(xml_stream, model, false, mimic); return model; } Model & buildModelFromXML(const std::string & xml_stream, Model & model, const bool mimic) { - pinocchio::urdf::buildModelFromXML(xml_stream, model, mimic); + pinocchio::urdf::buildModelFromXML(xml_stream, model, false, mimic); return model; } diff --git a/include/pinocchio/parsers/urdf/model.hxx b/include/pinocchio/parsers/urdf/model.hxx index 2da7c65381..b68667eb20 100644 --- a/include/pinocchio/parsers/urdf/model.hxx +++ b/include/pinocchio/parsers/urdf/model.hxx @@ -658,7 +658,7 @@ namespace pinocchio const bool verbose, const bool mimic) { - return buildModel(filename, rootJoint, "root_joint", model, mimic, verbose); + return buildModel(filename, rootJoint, "root_joint", model, verbose, mimic); } template class JointCollectionTpl> @@ -704,7 +704,7 @@ namespace pinocchio const bool verbose, const bool mimic) { - return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, mimic, verbose); + return buildModelFromXML(xmlStream, rootJoint, "root_joint", model, verbose, mimic); } template class JointCollectionTpl> @@ -751,15 +751,15 @@ namespace pinocchio const bool verbose, const bool mimic) { - return buildModel(urdfTree, rootJoint, "root_joint", model, mimic, verbose); + return buildModel(urdfTree, rootJoint, "root_joint", model, verbose, mimic); } template class JointCollectionTpl> ModelTpl & buildModel( const std::shared_ptr<::urdf::ModelInterface> urdfTree, ModelTpl & model, - const bool mimic, - const bool verbose) + const bool verbose, + const bool mimic) { PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL); details::UrdfVisitor visitor(model); From 98daec1cd5f7ec8daed826c167e89be0f067397a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 12 Feb 2025 14:51:50 +0100 Subject: [PATCH 260/310] [unittest] Factorize fonction --- unittest/centroidal.cpp | 14 +------------- unittest/crba.cpp | 14 +------------- unittest/rnea.cpp | 14 +------------- unittest/utils/model-generator.hpp | 24 ++++++++++++++++++++++++ 4 files changed, 27 insertions(+), 39 deletions(-) diff --git a/unittest/centroidal.cpp b/unittest/centroidal.cpp index 86e46e454e..89bc337e90 100644 --- a/unittest/centroidal.cpp +++ b/unittest/centroidal.cpp @@ -115,19 +115,7 @@ void test_mimic_against_full_model( Eigen::VectorXd v_full = G * v; Eigen::VectorXd a_full = G * a; - for (int n = 1; n < model_full.njoints; n++) - { - double joint_ratio = 1.0; - double joint_offset = 0.0; - if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) - { - joint_ratio = ratio; - joint_offset = offset; - } - model_full.joints[n].jointConfigExtendedModelSelector(q_full) = - joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q) - + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); - } + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q, q_full); model_mimic.lowerPositionLimit.head<3>().fill(-1.); model_mimic.upperPositionLimit.head<3>().fill(1.); diff --git a/unittest/crba.cpp b/unittest/crba.cpp index b35537e543..d8d4b2104b 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -147,19 +147,7 @@ void test_mimic_against_full_model( Eigen::VectorXd v_full = G * v_mimic; Eigen::VectorXd a_full = G * as_mimic; - for (int n = 1; n < model_full.njoints; n++) - { - double joint_ratio = 1.0; - double joint_offset = 0.0; - if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) - { - joint_ratio = ratio; - joint_offset = offset; - } - model_full.joints[n].jointConfigExtendedModelSelector(q_full) = - joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q_mimic) - + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); - } + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q_mimic, q_full); pinocchio::Data data_full(model_full); pinocchio::Data data_mimic(model_mimic); diff --git a/unittest/rnea.cpp b/unittest/rnea.cpp index 8f908768b0..3375608dee 100644 --- a/unittest/rnea.cpp +++ b/unittest/rnea.cpp @@ -370,19 +370,7 @@ void test_mimic_against_full_model( Eigen::VectorXd v_full = G * v; Eigen::VectorXd a_full = G * v; - for (int n = 1; n < model_full.njoints; n++) - { - double joint_ratio = 1.0; - double joint_offset = 0.0; - if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) - { - joint_ratio = ratio; - joint_offset = offset; - } - model_full.joints[n].jointConfigExtendedModelSelector(q_full) = - joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q) - + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); - } + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q, q_full); pinocchio::crba(model_full, data_full, q_full, pinocchio::Convention::WORLD); data_full.M.triangularView() = diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index 5da98941d9..26b92089c5 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -66,6 +66,30 @@ namespace pinocchio "translation", Inertia::Random()); } + void toFull( + const Model & model_full, + const Model & model_mimic, + const std::vector & secondary_ids, + const double & ratio, + const double & offset, + const Eigen::VectorXd & q, + Eigen::VectorXd & q_full) + { + for (int n = 1; n < model_full.njoints; n++) + { + double joint_ratio = 1.0; + double joint_offset = 0.0; + if (std::find(secondary_ids.begin(), secondary_ids.end(), n) != secondary_ids.end()) + { + joint_ratio = ratio; + joint_offset = offset; + } + model_full.joints[n].jointConfigExtendedModelSelector(q_full) = + joint_ratio * model_mimic.joints[n].jointConfigExtendedModelSelector(q) + + joint_offset * Eigen::VectorXd::Ones(model_full.joints[n].nq()); + } + } + void buildMimicModel( const Model & model_full, const std::vector & primary_ids, From 17cfcd9d466e092808241dfddbce2c41a8e13c81 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 12 Feb 2025 14:52:07 +0100 Subject: [PATCH 261/310] [unittest] Test mimic manipulator --- unittest/sample-models.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index b716b43b5c..db1f398375 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -65,10 +65,16 @@ BOOST_AUTO_TEST_CASE(build_model_sample_humanoid_random) BOOST_AUTO_TEST_CASE(build_model_sample_manipulator) { pinocchio::Model model; - pinocchio::buildModels::manipulator(model, true); + pinocchio::buildModels::manipulator(model); + + BOOST_CHECK(model.nq == 6); + BOOST_CHECK(model.nv == 6); + + pinocchio::Model model_m; + pinocchio::buildModels::manipulator(model_m, true); - BOOST_CHECK(model.nq == 5); - BOOST_CHECK(model.nv == 5); + BOOST_CHECK(model_m.nq == 5); + BOOST_CHECK(model_m.nv == 5); #ifdef PINOCCHIO_WITH_HPP_FCL pinocchio::Data data(model); From 42cc6bfe589cf27dda8b6a94fdce3a69ea5b2841 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 12 Feb 2025 14:52:26 +0100 Subject: [PATCH 262/310] [unittest] add mimic kinematics test --- unittest/kinematics.cpp | 79 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) diff --git a/unittest/kinematics.cpp b/unittest/kinematics.cpp index 02d3864e0b..dd1595124b 100644 --- a/unittest/kinematics.cpp +++ b/unittest/kinematics.cpp @@ -14,6 +14,8 @@ #include #include +#include "utils/model-generator.hpp" + BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input) @@ -334,4 +336,81 @@ BOOST_AUTO_TEST_CASE(test_kinematic_getters) ac_align.isApprox(getClassicalAcceleration(model, data, jointId, LOCAL_WORLD_ALIGNED))); } +void test_mimic_against_full_model( + const pinocchio::Model & model_full, + const std::vector & primary_ids, + const std::vector & secondary_ids) +{ + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + + 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; + + toFull(model_full, model_mimic, secondary_ids, ratio, offset, q, q_full); + + pinocchio::Data dataFKFull(model_full); + pinocchio::forwardKinematics(model_full, dataFKFull, q_full, v_full, a_full); + + pinocchio::Data dataFKRed(model_mimic); + pinocchio::forwardKinematics(model_mimic, dataFKRed, q, v, a); + + for (int i = 0; i < model_full.njoints; i++) + { + BOOST_CHECK(dataFKRed.oMi[i].isApprox(dataFKFull.oMi[i])); + BOOST_CHECK(dataFKRed.liMi[i].isApprox(dataFKFull.liMi[i])); + } +} + +BOOST_AUTO_TEST_CASE(test_kinematics_mimic) +{ + pinocchio::Model humanoid_model; + pinocchio::buildModels::humanoidRandom(humanoid_model); + + // Direct parent/Child + std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; + std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Spaced Parent/Child + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("rleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg4_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Parallel + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint")}; + secondaries = {humanoid_model.getJointId("rleg1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double mimic, not same primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // Double Mimic, Same Primary + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; + + secondaries = { + humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); +} + BOOST_AUTO_TEST_SUITE_END() From cb60868f9917422cdd53dd5f7324e150eedcfd91 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 12 Feb 2025 14:52:38 +0100 Subject: [PATCH 263/310] [unittest] Remove dead code --- unittest/joint-mimic.cpp | 4 +--- unittest/joint-motion-subspace.cpp | 17 ----------------- 2 files changed, 1 insertion(+), 20 deletions(-) diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 9adb404703..18863bd9dd 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -120,9 +120,7 @@ BOOST_AUTO_TEST_CASE(test_constraint) using namespace pinocchio; typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelPX, - JointModelPY, JointModelPZ, JointModelPrismaticUnaligned - //, JointModelRUBX, JointModelRUBY, JointModelRUBZ - > + JointModelPY, JointModelPZ, JointModelPrismaticUnaligned> Variant; boost::mpl::for_each(TestJointConstraint()); diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index c5fda672c3..2940a930bc 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -114,21 +114,6 @@ struct buildModel } }; -// 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"); - -// return model; -// } -// }; - template void test_constraint_operations(const JointModelBase & jmodel) { @@ -440,8 +425,6 @@ struct TestJointConstraint void operator()(const JointModelBase &) const { JointModel jmodel = init::run(); - // jmodel.setIndexes(0, 0, 0, 0); - test_constraint_operations(jmodel); } }; From c2da672bd27854de6e7be752419220b11ee055e4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 12 Feb 2025 18:04:19 +0100 Subject: [PATCH 264/310] [data & check-data] change nvSubtree convention for mimic --- include/pinocchio/algorithm/check-data.hxx | 9 +++++++-- include/pinocchio/multibody/data.hxx | 20 +++++++++++++++++--- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 4715dbf286..6dd644529a 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -102,11 +102,16 @@ namespace pinocchio { JointIndex c = (JointIndex)data.lastChild[j]; CHECK_DATA((int)c < model.njoints); - int nv = model.joints[j].nvExtended(); + int nv; + // For mimic, since in nvSubtree we're using the idx_vExtended, we need to do the same here + if (boost::get>(&model.joints[j])) + nv = model.joints[j].nvExtended(); + else + nv = model.joints[j].nv(); for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant { CHECK_DATA(model.parents[d] >= j); - + nv += model.joints[d].nv(); } CHECK_DATA(nv == data.nvSubtree[j]); diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 02256b3ed0..addcdb968b 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -203,10 +203,24 @@ namespace pinocchio const Index & parent = model.parents[(Index)i]; lastChild[parent] = std::max(lastChild[(Index)i], lastChild[parent]); - int nv_ = model.joints[(Index)lastChild[(Index)i]].nvExtended(); - nvSubtree[(Index)i] = - model.joints[(Index)lastChild[(Index)i]].idx_v() + nv_ - model.joints[(Index)i].idx_v(); + // Build a "correct" representation of mimic nvSubtree by using nvExtended, which will cover + // its children nv, and allow for a simple check + if (boost::get>(&model.joints[i])) + nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_vExtended() + + model.joints[(Index)lastChild[(Index)i]].nvExtended() + - model.joints[(Index)i].idx_vExtended(); + else + { + int nv_; + if (boost::get>( + &model.joints[(Index)lastChild[(Index)i]])) + nv_ = model.joints[(Index)lastChild[(Index)i]].nvExtended(); + else + nv_ = model.joints[(Index)lastChild[(Index)i]].nv(); + nvSubtree[(Index)i] = + model.joints[(Index)lastChild[(Index)i]].idx_v() + nv_ - model.joints[(Index)i].idx_v(); + } } } From a5c6f91093db160fb613e073768004850db68c03 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 12 Feb 2025 18:06:28 +0100 Subject: [PATCH 265/310] [unittest] Remove test for sample-model --- unittest/sample-models.cpp | 67 -------------------------------------- 1 file changed, 67 deletions(-) diff --git a/unittest/sample-models.cpp b/unittest/sample-models.cpp index db1f398375..6e60d7ea60 100644 --- a/unittest/sample-models.cpp +++ b/unittest/sample-models.cpp @@ -106,71 +106,4 @@ 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])); - } - - // 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); - 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); - - BOOST_CHECK((M_reduced_computed - dataCRBAMimic.M).isZero()); - - // Test rnea - pinocchio::Data dataBiasFull(model); - pinocchio::nonLinearEffects(model, dataBiasFull, q_full, v_full); - Eigen::VectorXd C_full = dataBiasFull.nle; - - // // Use equation of motion to compute tau from a_reduced - 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); - - BOOST_CHECK((dataRneaRed.tau - tau_reduced_computed).isZero()); -} - BOOST_AUTO_TEST_SUITE_END() From 696663fcc065c52341f7730915ba88496a3d3006 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 13 Feb 2025 10:40:41 +0100 Subject: [PATCH 266/310] [serialization] Remove dead code --- .../pinocchio/serialization/joints-model.hpp | 30 ------------------- 1 file changed, 30 deletions(-) diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index af0a2e3ed9..8a9cdda041 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -119,8 +119,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -131,8 +129,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteUnboundedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -143,8 +139,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelPrismaticTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -178,8 +172,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelFreeFlyerTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -190,8 +182,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelPlanarTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -202,8 +192,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelSphericalTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -214,8 +202,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelSphericalZYXTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -226,8 +212,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelTranslationTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } @@ -238,8 +222,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteUnalignedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis", joint.axis); } @@ -251,8 +233,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis", joint.axis); } @@ -264,8 +244,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelPrismaticUnalignedTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis", joint.axis); } @@ -277,8 +255,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelUniversalTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ar & make_nvp("axis1", joint.axis1); ar & make_nvp("axis2", joint.axis2); @@ -295,8 +271,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelCompositeTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); ::pinocchio::Serialize::run(ar, joint); @@ -313,8 +287,6 @@ namespace boost const unsigned int version) { typedef pinocchio::JointModelTpl JointType; - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); typedef typename JointCollectionTpl::JointModelVariant JointModelVariant; @@ -336,8 +308,6 @@ namespace boost ar & make_nvp("scaling", joint.scaling()); ar & make_nvp("offset", joint.offset()); - // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase - // >(joint)); fix::serialize(ar, *static_cast *>(&joint), version); } From 749dc76f17a069faf62fa0ed3c46513526fe1790 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 13 Feb 2025 10:55:18 +0100 Subject: [PATCH 267/310] [algo/center-of-mass] Add assert for jacobian subtree --- include/pinocchio/algorithm/center-of-mass.hxx | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/pinocchio/algorithm/center-of-mass.hxx b/include/pinocchio/algorithm/center-of-mass.hxx index 4c865eaa6e..700ab33234 100644 --- a/include/pinocchio/algorithm/center-of-mass.hxx +++ b/include/pinocchio/algorithm/center-of-mass.hxx @@ -412,6 +412,7 @@ namespace pinocchio typedef ModelTpl Model; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT((int)rootSubtreeId < model.njoints, "Invalid joint id."); PINOCCHIO_CHECK_ARGUMENT_SIZE( res.rows(), 3, "the resulting matrix does not have the right size."); @@ -503,6 +504,7 @@ namespace pinocchio typedef DataTpl Data; assert(model.check(data) && "data is not consistent with model."); + assert(model.check(MimicChecker()) && "Function does not support mimic joints"); PINOCCHIO_CHECK_INPUT_ARGUMENT(((int)rootSubtreeId < model.njoints), "Invalid joint id."); PINOCCHIO_CHECK_ARGUMENT_SIZE( res.rows(), 3, "the resulting matrix does not have the right size."); From 26488054903d9508b1f6d87a9d47e892852f326c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 13 Feb 2025 13:56:27 +0100 Subject: [PATCH 268/310] [unittest] Fix model generator --- unittest/kinematics.cpp | 1 + unittest/utils/model-generator.hpp | 5 ++--- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/unittest/kinematics.cpp b/unittest/kinematics.cpp index dd1595124b..2f2e6b5210 100644 --- a/unittest/kinematics.cpp +++ b/unittest/kinematics.cpp @@ -367,6 +367,7 @@ void test_mimic_against_full_model( { BOOST_CHECK(dataFKRed.oMi[i].isApprox(dataFKFull.oMi[i])); BOOST_CHECK(dataFKRed.liMi[i].isApprox(dataFKFull.liMi[i])); + BOOST_CHECK(model_full.inertias[i].isApprox(model_mimic.inertias[i])); } } diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index 26b92089c5..7202c6f4cd 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -111,7 +111,8 @@ namespace pinocchio G.resize(model_full.nv, model_mimic.nv); G.setZero(); // Set ones on the pseudo-diagonal - for (int j = 0; j < model_full.njoints; ++j) + std::cout << model_full.nv << " " << model_mimic.nv << std::endl; + for (int j = 1; j < model_full.njoints; ++j) { if (std::find(secondary_ids.begin(), secondary_ids.end(), j) == secondary_ids.end()) G.block( @@ -122,9 +123,7 @@ namespace pinocchio // Set specific values for primary-secondary joint pairs for (size_t i = 0; i < primary_ids.size(); ++i) - { G(model_full.joints[secondary_ids[i]].idx_v(), model_full.joints[primary_ids[i]].idx_v()) = ratio; - } } } // namespace pinocchio From 901998e690eeafba7d5d596fe1cc1667c9a2ecc3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 13 Feb 2025 14:25:13 +0100 Subject: [PATCH 269/310] [unittest] Fix G matrix --- unittest/utils/model-generator.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index 7202c6f4cd..a95261c472 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -123,7 +123,9 @@ namespace pinocchio // Set specific values for primary-secondary joint pairs for (size_t i = 0; i < primary_ids.size(); ++i) - G(model_full.joints[secondary_ids[i]].idx_v(), model_full.joints[primary_ids[i]].idx_v()) = + { + G(model_full.joints[secondary_ids[i]].idx_v(), model_mimic.joints[secondary_ids[i]].idx_v()) = ratio; + } } } // namespace pinocchio From ea4fd583cdb9a83fdb2d46c2590526bcdba00c53 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 13 Feb 2025 14:55:57 +0100 Subject: [PATCH 270/310] [unittest] Remove cout --- unittest/utils/model-generator.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index a95261c472..25754b22b6 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -111,7 +111,6 @@ namespace pinocchio G.resize(model_full.nv, model_mimic.nv); G.setZero(); // Set ones on the pseudo-diagonal - std::cout << model_full.nv << " " << model_mimic.nv << std::endl; for (int j = 1; j < model_full.njoints; ++j) { if (std::find(secondary_ids.begin(), secondary_ids.end(), j) == secondary_ids.end()) From ac09de17610bf355c10da5fcdbd237d0e3186b37 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 11 Feb 2025 18:05:09 +0100 Subject: [PATCH 271/310] wip for crba patch --- include/pinocchio/algorithm/crba.hxx | 324 ++++++++++++--------------- 1 file changed, 145 insertions(+), 179 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index f947160dbd..cb0bae9fb0 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -11,6 +11,8 @@ #include "pinocchio/algorithm/check.hpp" #include "pinocchio/algorithm/model.hpp" +#include + /// @cond DEV namespace pinocchio @@ -58,65 +60,6 @@ namespace pinocchio } }; - /// \brief Patch to the crba algorithm for joint mimic (in local convention) - template< - typename JointModel, - typename Scalar, - int Options, - template class JointCollectionTpl> - static void mimic_patch_CrbaWorldConventionBackwardStep( - const JointModelBase &, - const ModelTpl &, - DataTpl &) - { - } - - /// \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 void mimic_patch_CrbaWorldConventionBackwardStep( - const JointModelBase> & jmodel, - const ModelTpl & model, - DataTpl & data) - { - const JointIndex secondary_id = jmodel.id(); - const JointIndex primary_id = jmodel.derived().jmodel().id(); - PINOCCHIO_THROW( - secondary_id > primary_id, std::invalid_argument, - std::string("Mimicking joint id is before the primary in the tree")); - - 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 (size_t k = model.supports[secondary_id].size() - 2; k >= ancestor_sec; k--) - { - const JointIndex i = model.supports[secondary_id][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.jointRows(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[i].jointExtendedModelCols(data.J); - } - // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint - for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) - { - const JointIndex i = model.supports[primary_id][k]; - jmodel.jointCols(data.M) - .middleRows(model.joints[i].idx_v(), model.joints[i].nv()) - .noalias() -= model.joints[i].jointExtendedModelCols(data.J).transpose() - * data.Ag.middleCols(jmodel.idx_v(), jmodel.derived().jmodel().nv()); - } - } - template class JointCollectionTpl> struct CrbaWorldConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -127,7 +70,11 @@ namespace pinocchio typedef boost::fusion::vector ArgsType; - template + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> static void algo(const JointModelBase & jmodel, const Model & model, Data & data) { typedef typename Model::JointIndex JointIndex; @@ -136,21 +83,63 @@ namespace pinocchio ColsBlock; const JointIndex & i = jmodel.id(); - // Centroidal momentum map + // // Centroidal momentum map ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); - motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); + ColsBlock J_cols = jmodel.jointCols(data.J); + motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); - // Joint Space Inertia Matrix - jmodel.jointRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + // // Joint Space Inertia Matrix + data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), 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]; + } + + template< + typename JointModel = JointModelMimicTpl, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo(const JointModelBase & jmodel, const Model & model, Data & data) + { + typedef typename Model::JointIndex JointIndex; + const JointIndex & i = jmodel.id(); + const JointIndex & parent = model.parents[i]; data.oYcrb[parent] += data.oYcrb[i]; } }; + template class JointCollectionTpl> + struct CrbaWorldConventionMimicStep + : public fusion::JointUnaryVisitorBase< + CrbaWorldConventionMimicStep> + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> + static void algo(const JointModelBase & jmodel, const Model & model, Data & data) + { + } + + template< + typename JointModel, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo(const JointModelBase & jmodel, const Model & model, Data & data) + { + } + }; + template< typename Scalar, int Options, @@ -183,108 +172,6 @@ namespace pinocchio } }; - // /// \brief Patch to the crba algorithm for joint mimic (in local convention) - template< - typename JointModel, - typename Scalar, - int Options, - template class JointCollectionTpl> - static void mimic_patch_CrbaLocalConventionBackwardStep( - const JointModelBase &, - const ModelTpl &, - DataTpl &) - { - } - - 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 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 void mimic_patch_CrbaLocalConventionBackwardStep( - const JointModelBase> & jmodel, - const ModelTpl & model, - DataTpl & data) - { - typedef JointModelMimicTpl JointModel; - typedef typename Eigen::Matrix< - Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel::MaxNVExtended> - SpatialForcesX; - typedef JointDataTpl JointData; - typedef - typename Eigen::Matrix - MotionSubspace; - typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; - typedef SE3Tpl SE3; - - const JointIndex secondary_id = jmodel.id(); - const JointIndex primary_id = jmodel.derived().jmodel().id(); - - PINOCCHIO_THROW( - secondary_id > primary_id, std::invalid_argument, - std::string("Mimicking joint id is before the primary in the tree")); - size_t ancestor_prim, ancestor_sec; - findCommonAncestor(model, primary_id, secondary_id, ancestor_prim, ancestor_sec); - - SpatialForcesX iF(6, jmodel.nvExtended()); // 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--) - { - const JointIndex i = model.supports[secondary_id][k]; - const JointIndex ui = - model.supports[secondary_id][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; - - forceSet::se3Action(iMj, jmodel.jointCols(data.Fcrb[secondary_id]), iF); - - jmodel.jointRows(data.M) - .middleCols(model.joints[i].idx_v(), model.joints[i].nv()) - .noalias() += GetSNoMalloc::run(data.joints[i]).transpose() * iF; - } - // Traverse the kinematic tree forward from common ancestor to mimicked (primary) joint - for (size_t k = ancestor_prim + 1; k < model.supports[primary_id].size(); k++) - { - const JointIndex i = model.supports[primary_id][k]; - iMj = data.liMi[i].actInv(iMj); - - forceSet::se3Action(iMj, jmodel.jointCols(data.Fcrb[secondary_id]), iF); - - jmodel.jointCols(data.M) - .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) - { - forceSet::se3Action( - iMj, data.Fcrb[secondary_id].col(jmodel.idx_v()), - data.Fcrb[primary_id].col(jmodel.idx_v())); - } - } - template class JointCollectionTpl> struct CrbaLocalConventionBackwardStep : public fusion::JointUnaryVisitorBase< @@ -295,7 +182,11 @@ namespace pinocchio typedef boost::fusion::vector ArgsType; - template + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> static void algo( const JointModelBase & jmodel, JointDataBase & jdata, @@ -316,10 +207,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.jointCols(data.Fcrb[i]) += data.Ycrb[i] * jdata.S(); + jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ - jmodel.jointRows(data.M).middleCols(jmodel.idx_v(), data.nvSubtree[i]).noalias() += + data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); const JointIndex & parent = model.parents[i]; @@ -331,10 +222,76 @@ 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); + } + } + + template< + typename JointModel, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data) + { + + typedef typename Model::JointIndex JointIndex; + typedef typename Data::Matrix6x::ColsBlockXpr Block; + const JointIndex & i = jmodel.id(); + + /* F[1:6,i] = Y*S */ + jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); + const JointIndex & parent = model.parents[i]; + if (parent > 0) + { + /* Yli += liXi Yi */ + data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); + + /* 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); } + } + }; - mimic_patch_CrbaLocalConventionBackwardStep(jmodel, model, data); + template class JointCollectionTpl> + struct CrbaLocalConventionMimicStep + : public fusion::JointUnaryVisitorBase< + CrbaLocalConventionMimicStep> + { + typedef ModelTpl Model; + typedef DataTpl Data; + + typedef boost::fusion::vector ArgsType; + + template< + typename JointModel, + typename boost::disable_if< + std::is_same>, + int>::type = 0> + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data) + { + } + + template< + typename JointModel, + typename boost::enable_if< + std::is_same>, + int>::type = 0> + static void algo( + const JointModelBase & jmodel, + JointDataBase & jdata, + const Model & model, + Data & data) + { } }; @@ -362,17 +319,20 @@ 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) { Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); } + typedef CrbaLocalConventionMimicStep Pass3; + for (int i = 0; i < model.mimic_joints.size(); i++) + { + Pass3::run( + model.joints[model.mimic_joints[i]], data.joints[model.mimic_joints[i]], + typename Pass3::ArgsType(model, data)); + } + // Add the armature contribution data.M.diagonal() += model.armature; @@ -404,14 +364,20 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } - data.M.setZero(); data.Ag.setZero(); + data.M.setZero(); typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data)); } + typedef CrbaWorldConventionMimicStep Pass3; + for (int i = 0; i < model.mimic_joints.size(); i++) + { + Pass3::run(model.joints[model.mimic_joints[i]], typename Pass3::ArgsType(model, data)); + } + // Add the armature contribution data.M.diagonal() += model.armature; From b22fbb67de739d2ab087a088c978b17c53857ed1 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 14 Feb 2025 11:58:11 +0100 Subject: [PATCH 272/310] [algo/crba] Use new patch for crba with local mimic --- include/pinocchio/algorithm/crba.hxx | 67 +++++++++++++++++++++++----- 1 file changed, 57 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index cb0bae9fb0..71fcb74d9b 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -204,12 +204,11 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Matrix6x::ColsBlockXpr Block; const JointIndex & i = jmodel.id(); - /* 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(); - /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ + /* M[i,SUBTREE] = S'*F[1:6,SUBTR EE] */ data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); @@ -237,13 +236,10 @@ namespace pinocchio const Model & model, Data & data) { - typedef typename Model::JointIndex JointIndex; - typedef typename Data::Matrix6x::ColsBlockXpr Block; - const JointIndex & i = jmodel.id(); + typedef typename Data::Matrix6x & Matrix; - /* F[1:6,i] = Y*S */ - jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S(); + const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; if (parent > 0) { @@ -251,9 +247,12 @@ namespace pinocchio data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]); /* 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); + Matrix jF = data.Fcrb[parent]; + Matrix iF = data.Fcrb[i]; + // Since we don't just copy columns, we need to use ADDTO method, to avoid ecrasing + // already filled columns, in case of parallel arms, it is important to not ecrase already + // filled data from the other part of the tree + forceSet::se3Action(data.liMi[i], iF, jF); } } }; @@ -292,6 +291,54 @@ namespace pinocchio const Model & model, Data & data) { + typedef JointModelMimicTpl JointModel_; + typedef typename Eigen::Matrix< + Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel_::MaxNVExtended> + SpatialForcesX; + typedef JointDataTpl JointData; + typedef + typename Eigen::Matrix + MotionSubspace; + + const JointIndex & secondary_id = jmodel.id(); + const JointIndex & primary_id = jmodel.derived().jmodel().id(); + auto & F = data.Fcrb[secondary_id]; + + /* F[1:6,i] = Y*S */ + jmodel.jointCols(F) = data.Ycrb[secondary_id] * jdata.S(); + // Add contribution of mimic to primary. Here all Fcrb is multiplied but there could be an + // optimisation to only get the needed values + jmodel.jointRows(data.M) += jdata.S().transpose() * F; + + const auto & supports = model.supports[secondary_id]; + int j = supports.size() - 2; + for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--) + { + const JointIndex & li = supports[j]; + const JointIndex & i = supports[j + 1]; // Child link to compute placement + + forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F)); + int row_idx = jmodel.idx_v(); + int col_idx = model.idx_vs[li]; + + data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += + data.joints[li].S().transpose() * jmodel.jointCols(F); + if (li == primary_id) + data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += + data.joints[li].S().transpose() * jmodel.jointCols(F); + } + for (; j > 0; j--) + { + const JointIndex & li = supports[j]; + const JointIndex & i = supports[j + 1]; // Child link to compute placement + + forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F)); + int col_idx = jmodel.idx_v(); + int row_idx = model.idx_vs[li]; + + data.M.block(row_idx, col_idx, model.nvs[li], jmodel.nvExtended()).noalias() += + data.joints[li].S().transpose() * jmodel.jointCols(F); + } } }; From eb0dc20a3909029db03ba8c662569ff1438ed72f Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 13 Feb 2025 18:44:22 +0100 Subject: [PATCH 273/310] [EtienneAr feedback] topic/mimic: restore jacobian perfomrances (remove +=) and add mimicked_joint id vector in robot model --- include/pinocchio/algorithm/check-model.hxx | 2 +- include/pinocchio/algorithm/jacobian.hxx | 133 ++++++++++++++++-- include/pinocchio/algorithm/model.hxx | 3 +- .../bindings/python/multibody/model.hpp | 8 +- include/pinocchio/multibody/model.hpp | 9 +- include/pinocchio/multibody/model.hxx | 12 +- include/pinocchio/serialization/model.hpp | 3 +- 7 files changed, 149 insertions(+), 21 deletions(-) diff --git a/include/pinocchio/algorithm/check-model.hxx b/include/pinocchio/algorithm/check-model.hxx index d7ad286da1..3ce010fae7 100644 --- a/include/pinocchio/algorithm/check-model.hxx +++ b/include/pinocchio/algorithm/check-model.hxx @@ -60,7 +60,7 @@ namespace pinocchio inline bool MimicChecker::checkModel_impl(const ModelTpl & model) const { - if (!model.mimic_joints.empty()) + if (!model.mimicking_joints.empty()) return false; return true; diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index a8f3f0c703..73dcc1bbd3 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -149,7 +149,6 @@ 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; @@ -191,7 +190,6 @@ namespace pinocchio PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv); Matrix6xLikeOut & Jout_ = Jout.const_cast_derived(); - Jout_.setZero(); typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn; typedef const MotionRef MotionIn; @@ -210,7 +208,33 @@ namespace pinocchio MotionIn v_in(Jin.col(jExtended)); MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - v_out += v_in; + v_out = v_in; + } + // During the backward pass, mimicking joint jacobian might have been overwritten by the + // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + for (size_t i = 0; i < model.mimicking_joints.size(); i++) + { + const JointIndex mimicking_id = model.mimicking_joints[i]; + const JointIndex mimicked_id = model.mimicked_joints[i]; + const Model::IndexVector & joint_support = model.supports[joint_id]; + if ( + std::find(joint_support.begin(), joint_support.end(), mimicking_id) + == joint_support.end()) + continue; // This mimicking joint does not support the selected joint, skip + if ( + std::find(joint_support.begin(), joint_support.end(), mimicked_id) + == joint_support.end()) + continue; // This mimicked joint does not support the selected joint, skip + for (Eigen::DenseIndex jExtended = idx_vExtended(model.joints[mimicking_id]); + jExtended + < nvExtended(model.joints[mimicking_id]) + idx_vExtended(model.joints[mimicking_id]); + jExtended++) + { + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += v_in; + } } break; } @@ -221,9 +245,36 @@ namespace pinocchio MotionIn v_in(Jin.col(jExtended)); MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - v_out += v_in; + v_out = v_in; v_out.linear() -= placement.translation().cross(v_in.angular()); } + // During the backward pass, mimicking joint jacobian might have been overwritten by the + // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + for (size_t i = 0; i < model.mimicking_joints.size(); i++) + { + const JointIndex mimicking_id = model.mimicking_joints[i]; + const JointIndex mimicked_id = model.mimicked_joints[i]; + const Model::IndexVector & joint_support = model.supports[joint_id]; + if ( + std::find(joint_support.begin(), joint_support.end(), mimicking_id) + == joint_support.end()) + continue; // This mimicking joint does not support the selected joint, skip + if ( + std::find(joint_support.begin(), joint_support.end(), mimicked_id) + == joint_support.end()) + continue; // This mimicked joint does not support the selected joint, skip + for (Eigen::DenseIndex jExtended = idx_vExtended(model.joints[mimicking_id]); + jExtended + < nvExtended(model.joints[mimicking_id]) + idx_vExtended(model.joints[mimicking_id]); + jExtended++) + { + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += v_in; + v_out.linear() -= placement.translation().cross(v_in.angular()); + } + } break; } case LOCAL: { @@ -233,7 +284,33 @@ namespace pinocchio MotionIn v_in(Jin.col(jExtended)); MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - v_out += placement.actInv(v_in); + v_out = placement.actInv(v_in); + } + // During the backward pass, mimicking joint jacobian might have been overwritten by the + // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + for (size_t i = 0; i < model.mimicking_joints.size(); i++) + { + const JointIndex mimicking_id = model.mimicking_joints[i]; + const JointIndex mimicked_id = model.mimicked_joints[i]; + const Model::IndexVector & joint_support = model.supports[joint_id]; + if ( + std::find(joint_support.begin(), joint_support.end(), mimicking_id) + == joint_support.end()) + continue; // This mimicking joint does not support the selected joint, skip + if ( + std::find(joint_support.begin(), joint_support.end(), mimicked_id) + == joint_support.end()) + continue; // This mimicked joint does not support the selected joint, skip + for (Eigen::DenseIndex jExtended = idx_vExtended(model.joints[mimicking_id]); + jExtended + < nvExtended(model.joints[mimicking_id]) + idx_vExtended(model.joints[mimicking_id]); + jExtended++) + { + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += placement.actInv(v_in); + } } break; } @@ -291,14 +368,16 @@ namespace pinocchio int Options, template class JointCollectionTpl, typename ConfigVectorType, - typename Matrix6xLike> + typename Matrix6xLike, + AssignmentOperatorType op = SETTO> struct JointJacobianForwardStep : public fusion::JointUnaryVisitorBase> + Matrix6xLike, + op>> { typedef ModelTpl Model; typedef DataTpl Data; @@ -325,7 +404,21 @@ namespace pinocchio data.iMf[parent] = data.liMi[i] * data.iMf[i]; Matrix6xLike & J_ = J.const_cast_derived(); - jmodel.jointCols(J_) += data.iMf[i].actInv(jdata.S()); + switch (op) + { + case SETTO: + jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S()); + break; + case ADDTO: + jmodel.jointCols(J_) += data.iMf[i].actInv(jdata.S()); + break; + case RMTO: + jmodel.jointCols(J_) -= data.iMf[i].actInv(jdata.S()); + break; + default: + assert(false && "Wrong Op requesed value"); + break; + } } }; @@ -351,7 +444,6 @@ namespace pinocchio data.iMf[jointId].setIdentity(); Matrix6xLike & J_ = J.const_cast_derived(); - J_.setZero(); typedef JointJacobianForwardStep< Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike> Pass; @@ -360,6 +452,28 @@ namespace pinocchio Pass::run( model.joints[i], data.joints[i], typename Pass::ArgsType(model, data, q.derived(), J_)); } + + typedef JointJacobianForwardStep< + Scalar, Options, JointCollectionTpl, ConfigVectorType, Matrix6xLike, ADDTO> + MimicPass; + // During the backward pass, mimicking joint jacobian might have been overwritten by the + // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + for (size_t i = 0; i < model.mimicking_joints.size(); i++) + { + const JointIndex mimicking_id = model.mimicking_joints[i]; + const JointIndex mimicked_id = model.mimicked_joints[i]; + const typename Model::IndexVector & joint_support = model.supports[jointId]; + if ( + std::find(joint_support.begin(), joint_support.end(), mimicking_id) + == joint_support.end()) + continue; // This mimicking joint does not support the selected joint, skip + if ( + std::find(joint_support.begin(), joint_support.end(), mimicked_id) == joint_support.end()) + continue; // This mimicked joint does not support the selected joint, skip + MimicPass::run( + model.joints[mimicking_id], data.joints[mimicking_id], + typename Pass::ArgsType(model, data, q.derived(), J_)); + } } template< @@ -488,7 +602,6 @@ 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 diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index cc27a50498..671a565250 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -962,7 +962,8 @@ namespace pinocchio } } - output_model.mimic_joints.push_back(index_secondary); + output_model.mimicking_joints.push_back(index_secondary); + output_model.mimicked_joints.push_back(index_primary); size_t anc_prim, anc_sec; findCommonAncestor(output_model, index_primary, index_secondary, anc_prim, anc_sec); output_model.mimic_pairs.push_back(std::make_pair(anc_prim, anc_sec)); diff --git a/include/pinocchio/bindings/python/multibody/model.hpp b/include/pinocchio/bindings/python/multibody/model.hpp index 2288ce64df..b94509f167 100644 --- a/include/pinocchio/bindings/python/multibody/model.hpp +++ b/include/pinocchio/bindings/python/multibody/model.hpp @@ -135,7 +135,13 @@ namespace pinocchio "subtrees", &Model::subtrees, "Vector of subtrees. subtree[j] corresponds to the subtree supported by the joint j.") - .def_readwrite("mimic_joints", &Model::mimic_joints, "Vector of mimic joints indexes") + .def_readwrite( + "mimicking_joints", &Model::mimicking_joints, + "Vector of mimicking joints in the tree (with type MimicTpl)") + + .def_readwrite( + "mimicked_joints", &Model::mimicked_joints, + "Vector of mimicked joints in the tree (can be any joint type)") .def_readwrite( "gravity", &Model::gravity, diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index eb3c38b735..4f19f82180 100644 --- a/include/pinocchio/multibody/model.hpp +++ b/include/pinocchio/multibody/model.hpp @@ -150,8 +150,13 @@ namespace pinocchio /// the set (i==parents[k] for k in mu(i)). std::vector children; - /// \brief Vector of mimic joints in the tree - std::vector mimic_joints; + /// \brief Vector of mimicking joints in the tree (with type MimicTpl) + std::vector mimicking_joints; + + /// \brief Vector of mimicked joints in the tree (can be any joint type) + /// The i-th element of this vector correspond to the mimicked joint of the i-th mimicking + /// vector in mimicking_joints + std::vector mimicked_joints; /// \brief Vector of mimic joints and its primary common ancestors. It corresponds to the index /// in their respective model.supports. diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index f7d138548a..a9bd197f60 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -159,7 +159,8 @@ namespace pinocchio if (auto jmodel_ = boost::get>(&jmodel)) { - mimic_joints.push_back(jmodel.id()); + mimicking_joints.push_back(jmodel.id()); + mimicked_joints.push_back(jmodel_->jmodel().id()); size_t anc_prim, anc_sec; findCommonAncestor(*this, jmodel_->jmodel().id(), jmodel.id(), anc_sec, anc_prim); @@ -251,7 +252,8 @@ namespace pinocchio res.names = names; res.subtrees = subtrees; res.supports = supports; - res.mimic_joints = mimic_joints; + res.mimicking_joints = mimicking_joints; + res.mimicked_joints = mimicked_joints; res.mimic_pairs = mimic_pairs; res.gravity = gravity.template cast(); res.name = name; @@ -308,9 +310,9 @@ namespace pinocchio bool res = other.nq == nq && other.nv == nv && other.nvExtended == nvExtended && other.njoints == njoints && other.nbodies == nbodies && other.nframes == nframes && other.parents == parents && other.children == children && other.names == names - && other.subtrees == subtrees && other.mimic_joints == mimic_joints - && other.mimic_pairs == mimic_pairs && other.gravity == gravity - && other.name == name; + && other.subtrees == subtrees && other.mimicking_joints == mimicking_joints + && other.mimicked_joints == mimicked_joints && other.mimic_pairs == mimic_pairs + && 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_vExtendeds == idx_vExtendeds && other.nvExtendeds == nvExtendeds; diff --git a/include/pinocchio/serialization/model.hpp b/include/pinocchio/serialization/model.hpp index 1242d1941f..96c0420ce8 100644 --- a/include/pinocchio/serialization/model.hpp +++ b/include/pinocchio/serialization/model.hpp @@ -48,7 +48,8 @@ namespace boost ar & make_nvp("names", model.names); ar & make_nvp("supports", model.supports); ar & make_nvp("subtrees", model.subtrees); - ar & make_nvp("mimic_joints", model.mimic_joints); + ar & make_nvp("mimicking_joints", model.mimicking_joints); + ar & make_nvp("mimicked_joints", model.mimicked_joints); ar & make_nvp("gravity", model.gravity); ar & make_nvp("name", model.name); From 9b4e2376099287398f4d8cb473d2e1685b3da99b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Thu, 13 Feb 2025 19:05:08 +0100 Subject: [PATCH 274/310] [EtienneAr feedback] topic/mimic: Rename Joint[Velocity/Config]ExtendedModelSelector into JointMapped[Velocity/Config]Selector that is more explicit --- include/pinocchio/algorithm/kinematics.hxx | 4 +-- include/pinocchio/algorithm/rnea.hxx | 8 ++--- .../multibody/joint/joint-basic-visitors.hxx | 9 +++--- .../multibody/joint/joint-composite.hpp | 16 +++++----- .../multibody/joint/joint-generic.hpp | 8 ++--- .../pinocchio/multibody/joint/joint-mimic.hpp | 8 ++--- .../multibody/joint/joint-model-base.hpp | 24 +++++++------- unittest/joint-composite.cpp | 14 ++++---- unittest/model.cpp | 32 +++++++++---------- unittest/utils/model-generator.hpp | 4 +-- 10 files changed, 62 insertions(+), 65 deletions(-) diff --git a/include/pinocchio/algorithm/kinematics.hxx b/include/pinocchio/algorithm/kinematics.hxx index 6a13a9bb50..6316acd36b 100644 --- a/include/pinocchio/algorithm/kinematics.hxx +++ b/include/pinocchio/algorithm/kinematics.hxx @@ -233,8 +233,8 @@ namespace pinocchio else data.oMi[i] = data.liMi[i]; - data.a[i] = jdata.S() * jmodel.jointVelocityExtendedModelSelector(a) + jdata.c() - + (data.v[i] ^ jdata.v()); + data.a[i] = + jdata.S() * jmodel.JointMappedVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v()); data.a[i] += data.liMi[i].actInv(data.a[parent]); } }; diff --git a/include/pinocchio/algorithm/rnea.hxx b/include/pinocchio/algorithm/rnea.hxx index d81e0d1bc4..9eca6e7369 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.jointVelocityExtendedModelSelector(a); + data.a_gf[i] += jdata.S() * jmodel.JointMappedVelocitySelector(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]); @@ -100,7 +100,7 @@ namespace pinocchio const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; // Mimic joint contributes to the torque of their primary and don't have their own - jmodel.jointVelocityExtendedModelSelector(data.tau) += jdata.S().transpose() * data.f[i]; + jmodel.JointMappedVelocitySelector(data.tau) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); @@ -286,7 +286,7 @@ namespace pinocchio const JointIndex & parent = model.parents[i]; // Mimic joint contributes to the nle of their primary and don't have their own - jmodel.jointVelocityExtendedModelSelector(data.nle) += jdata.S().transpose() * data.f[i]; + jmodel.JointMappedVelocitySelector(data.nle) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[parent] += data.liMi[i].act(data.f[i]); } @@ -397,7 +397,7 @@ namespace pinocchio const JointIndex & i = jmodel.id(); const JointIndex & parent = model.parents[i]; // Mimic joint contributes to the gravity of their primary and don't have their own - jmodel.jointVelocityExtendedModelSelector(g) += jdata.S().transpose() * data.f[i]; + jmodel.JointMappedVelocitySelector(g) += jdata.S().transpose() * data.f[i]; if (parent > 0) data.f[(size_t)parent] += data.liMi[i].act(data.f[i]); } diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index c768d682f5..bee3714503 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -194,10 +194,9 @@ namespace pinocchio } template - struct jointConfigExtendedModelSelectorVisitor - : fusion::JointUnaryVisitorBase< - jointConfigExtendedModelSelectorVisitor, - ReturnType> + struct JointMappedConfigSelectorVisitor + : fusion:: + JointUnaryVisitorBase, ReturnType> { typedef boost::fusion::vector ArgsType; @@ -206,7 +205,7 @@ namespace pinocchio { // 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.jointConfigExtendedModelSelector(a); + auto vectorBlock = jmodel.JointMappedConfigSelector(a); // VectorBlock does not implemet such getter, hack the Eigen::Block base class to retreive // such values. diff --git a/include/pinocchio/multibody/joint/joint-composite.hpp b/include/pinocchio/multibody/joint/joint-composite.hpp index cf3b3503cc..e36ec56b18 100644 --- a/include/pinocchio/multibody/joint/joint-composite.hpp +++ b/include/pinocchio/multibody/joint/joint-composite.hpp @@ -472,13 +472,13 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointConfigExtendedModelSelector(const Eigen::MatrixBase & a) const + JointMappedConfigSelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigExtendedModelSelector(Eigen::MatrixBase & a) const + JointMappedConfigSelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } @@ -498,13 +498,13 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityExtendedModelSelector(const Eigen::MatrixBase & a) const + JointMappedVelocitySelector(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityExtendedModelSelector(Eigen::MatrixBase & a) const + JointMappedVelocitySelector(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } @@ -561,26 +561,26 @@ namespace pinocchio template typename SizeDepType::template SegmentReturn::ConstType - jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::Type - jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_q, nq()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + JointMappedVelocitySelector_impl(const Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityExtendedModelSelector_impl(Eigen::MatrixBase & a) const + JointMappedVelocitySelector_impl(Eigen::MatrixBase & a) const { return a.segment(Base::i_v, nv()); } diff --git a/include/pinocchio/multibody/joint/joint-generic.hpp b/include/pinocchio/multibody/joint/joint-generic.hpp index 8c53ea90f5..975f614544 100644 --- a/include/pinocchio/multibody/joint/joint-generic.hpp +++ b/include/pinocchio/multibody/joint/joint-generic.hpp @@ -386,11 +386,11 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(const Eigen::MatrixBase & a) const { typedef const Eigen::MatrixBase & InputType; typedef typename SizeDepType::template SegmentReturn::ConstType ReturnType; - typedef jointConfigExtendedModelSelectorVisitor Visitor; + typedef JointMappedConfigSelectorVisitor Visitor; typename Visitor::ArgsType arg(a); return Visitor::run(*this, arg); } @@ -398,11 +398,11 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const { typedef Eigen::MatrixBase & InputType; typedef typename SizeDepType::template SegmentReturn::Type ReturnType; - typedef jointConfigExtendedModelSelectorVisitor Visitor; + typedef JointMappedConfigSelectorVisitor Visitor; typename Visitor::ArgsType arg(a); return Visitor::run(*this, arg); } diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 2de379d4fa..2216634b21 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -784,7 +784,7 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), Base::i_q, m_nqExtended); } @@ -792,7 +792,7 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), Base::i_q, m_nqExtended); } @@ -800,7 +800,7 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + JointMappedVelocitySelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), Base::i_v, m_nvExtended); } @@ -808,7 +808,7 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointVelocityExtendedModelSelector_impl(Eigen::MatrixBase & a) const + JointMappedVelocitySelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), Base::i_v, m_nvExtended); } diff --git a/include/pinocchio/multibody/joint/joint-model-base.hpp b/include/pinocchio/multibody/joint/joint-model-base.hpp index 3623236883..93e423dbea 100644 --- a/include/pinocchio/multibody/joint/joint-model-base.hpp +++ b/include/pinocchio/multibody/joint/joint-model-base.hpp @@ -289,14 +289,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointConfigExtendedModelSelector(const Eigen::MatrixBase & a) const + JointMappedConfigSelector(const Eigen::MatrixBase & a) const { - return derived().jointConfigExtendedModelSelector_impl(a); + return derived().JointMappedConfigSelector_impl(a); } template typename SizeDepType::template SegmentReturn::ConstType - jointConfigExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_q(), nq()); } @@ -304,14 +304,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointConfigExtendedModelSelector(Eigen::MatrixBase & a) const + JointMappedConfigSelector(Eigen::MatrixBase & a) const { - return derived().jointConfigExtendedModelSelector_impl(a); + return derived().JointMappedConfigSelector_impl(a); } template typename SizeDepType::template SegmentReturn::Type - jointConfigExtendedModelSelector_impl(Eigen::MatrixBase & a) const + JointMappedConfigSelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a, idx_q(), nq()); } @@ -350,14 +350,14 @@ namespace pinocchio // Const access template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityExtendedModelSelector(const Eigen::MatrixBase & a) const + JointMappedVelocitySelector(const Eigen::MatrixBase & a) const { - return derived().jointVelocityExtendedModelSelector_impl(a.derived()); + return derived().JointMappedVelocitySelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::ConstType - jointVelocityExtendedModelSelector_impl(const Eigen::MatrixBase & a) const + JointMappedVelocitySelector_impl(const Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nvExtended()); } @@ -365,14 +365,14 @@ namespace pinocchio // Non-const access template typename SizeDepType::template SegmentReturn::Type - jointVelocityExtendedModelSelector(Eigen::MatrixBase & a) const + JointMappedVelocitySelector(Eigen::MatrixBase & a) const { - return derived().jointVelocityExtendedModelSelector_impl(a.derived()); + return derived().JointMappedVelocitySelector_impl(a.derived()); } template typename SizeDepType::template SegmentReturn::Type - jointVelocityExtendedModelSelector_impl(Eigen::MatrixBase & a) const + JointMappedVelocitySelector_impl(Eigen::MatrixBase & a) const { return SizeDepType::segment(a.derived(), idx_v(), nvExtended()); } diff --git a/unittest/joint-composite.cpp b/unittest/joint-composite.cpp index 454d521e90..d48eb1733c 100644 --- a/unittest/joint-composite.cpp +++ b/unittest/joint-composite.cpp @@ -143,11 +143,10 @@ void test_joint_methods( const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m)); BOOST_CHECK( - jmodel.jointConfigExtendedModelSelector(vec) - == jmodel_composite.jointConfigExtendedModelSelector(vec)); + jmodel.JointMappedConfigSelector(vec) == jmodel_composite.JointMappedConfigSelector(vec)); BOOST_CHECK( - jmodel.jointConfigExtendedModelSelector(vec_const) - == jmodel_composite.jointConfigExtendedModelSelector(vec_const)); + jmodel.JointMappedConfigSelector(vec_const) + == jmodel_composite.JointMappedConfigSelector(vec_const)); BOOST_CHECK(jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec)); BOOST_CHECK( @@ -158,11 +157,10 @@ void test_joint_methods( jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const)); BOOST_CHECK( - jmodel.jointVelocityExtendedModelSelector(vec) - == jmodel_composite.jointVelocityExtendedModelSelector(vec)); + jmodel.JointMappedVelocitySelector(vec) == jmodel_composite.JointMappedVelocitySelector(vec)); BOOST_CHECK( - jmodel.jointVelocityExtendedModelSelector(vec_const) - == jmodel_composite.jointVelocityExtendedModelSelector(vec_const)); + jmodel.JointMappedVelocitySelector(vec_const) + == jmodel_composite.JointMappedVelocitySelector(vec_const)); BOOST_CHECK(jmodel.jointCols(mat) == jmodel_composite.jointCols(mat)); BOOST_CHECK(jmodel.jointCols(mat_const) == jmodel_composite.jointCols(mat_const)); diff --git a/unittest/model.cpp b/unittest/model.cpp index a583474d02..e398e2d904 100644 --- a/unittest/model.cpp +++ b/unittest/model.cpp @@ -253,8 +253,8 @@ BOOST_AUTO_TEST_CASE(append) joint_model_humanoid.jointConfigSelector(humanoid_config->second) == joint_model1.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_humanoid.jointConfigExtendedModelSelector(humanoid_config->second) - == joint_model1.jointConfigExtendedModelSelector(config_vector)); + joint_model_humanoid.JointMappedConfigSelector(humanoid_config->second) + == joint_model1.JointMappedConfigSelector(config_vector)); // std::cerr<<"humanoid "<second) == joint_model1.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_manipulator.jointConfigExtendedModelSelector(manipulator_config->second) - == joint_model1.jointConfigExtendedModelSelector(config_vector)); + joint_model_manipulator.JointMappedConfigSelector(manipulator_config->second) + == joint_model1.JointMappedConfigSelector(config_vector)); // std::cerr<<"manipulator "<second) == joint_model.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_humanoid.jointConfigExtendedModelSelector(humanoid_config->second) - == joint_model.jointConfigExtendedModelSelector(config_vector)); + joint_model_humanoid.JointMappedConfigSelector(humanoid_config->second) + == joint_model.JointMappedConfigSelector(config_vector)); // std::cerr<<"humanoid "<second) == joint_model.jointConfigSelector(config_vector)); BOOST_CHECK( - joint_model_manipulator.jointConfigExtendedModelSelector(manipulator_config->second) - == joint_model.jointConfigExtendedModelSelector(config_vector)); + joint_model_manipulator.JointMappedConfigSelector(manipulator_config->second) + == joint_model.JointMappedConfigSelector(config_vector)); // std::cerr<<"manipulator "< Date: Fri, 14 Feb 2025 10:19:08 +0100 Subject: [PATCH 275/310] [EtienneAr feedback] topic/mimic: Add more jacobian tests for mimic joints --- unittest/joint-jacobian.cpp | 71 +++++++++++++++++++++++++++++++++---- 1 file changed, 64 insertions(+), 7 deletions(-) diff --git a/unittest/joint-jacobian.cpp b/unittest/joint-jacobian.cpp index 94cca9e990..c75aa4e435 100644 --- a/unittest/joint-jacobian.cpp +++ b/unittest/joint-jacobian.cpp @@ -12,10 +12,9 @@ #include "pinocchio/utils/timer.hpp" #include "pinocchio/algorithm/joint-configuration.hpp" -#include - #include #include +#include "utils/model-generator.hpp" template inline bool isFinite(const Eigen::MatrixBase & x) @@ -23,15 +22,11 @@ inline bool isFinite(const Eigen::MatrixBase & x) return ((x - x).array() == (x - x).array()).all(); } -BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) - -BOOST_AUTO_TEST_CASE(test_jacobian) +void test_jacobian_impl(const pinocchio::Model & model) { using namespace Eigen; using namespace pinocchio; - pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, false, true); pinocchio::Data data(model); VectorXd q = VectorXd::Zero(model.nq); @@ -71,6 +66,68 @@ BOOST_AUTO_TEST_CASE(test_jacobian) BOOST_CHECK(data_fk.J.isApprox(data.J)); } +BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) + +BOOST_AUTO_TEST_CASE(test_jacobian) +{ + pinocchio::Model model; + pinocchio::buildModels::humanoidRandom(model, false); + + test_jacobian_impl(model); +} + +BOOST_AUTO_TEST_CASE(test_jacobian_mimic) +{ + using namespace Eigen; + using namespace pinocchio; + + // Full model and mimic var + pinocchio::Model model_full; + pinocchio::buildModels::humanoidRandom(model_full, false); + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + + // Direct parent/Child + std::vector primary_ids = {model_full.getJointId("rleg1_joint")}; + std::vector secondary_ids = {model_full.getJointId("rleg2_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Spaced Parent/Child + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("rleg1_joint")}; + secondary_ids = {model_full.getJointId("rleg4_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Parallel + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("lleg1_joint")}; + secondary_ids = {model_full.getJointId("rleg1_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Double mimic, not same primary + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("lleg1_joint"), model_full.getJointId("rarm1_joint")}; + secondary_ids = {model_full.getJointId("rleg1_joint"), model_full.getJointId("larm1_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); + + // Double Mimic, Same Primary + primary_ids.clear(); + secondary_ids.clear(); + primary_ids = {model_full.getJointId("lleg1_joint"), model_full.getJointId("lleg1_joint")}; + secondary_ids = {model_full.getJointId("rleg1_joint"), model_full.getJointId("lleg2_joint")}; + pinocchio::buildMimicModel(model_full, primary_ids, secondary_ids, ratio, offset, model_mimic, G); + test_jacobian_impl(model_mimic); +} + BOOST_AUTO_TEST_CASE(test_jacobian_time_variation) { using namespace Eigen; From b4f0d8abeec2a067578387385697178ec3ca516b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Fri, 14 Feb 2025 18:01:41 +0100 Subject: [PATCH 276/310] [algo/crba] Add patch for crba world with mimic joints --- include/pinocchio/algorithm/check-data.hxx | 12 +-- include/pinocchio/algorithm/crba.hxx | 94 +++++++++++++++++----- include/pinocchio/multibody/data.hpp | 1 + include/pinocchio/multibody/data.hxx | 19 ++++- 4 files changed, 96 insertions(+), 30 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 6dd644529a..4c8df9c944 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -105,14 +105,16 @@ namespace pinocchio int nv; // For mimic, since in nvSubtree we're using the idx_vExtended, we need to do the same here if (boost::get>(&model.joints[j])) - nv = model.joints[j].nvExtended(); + nv = 0; else - nv = model.joints[j].nv(); - for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant { - CHECK_DATA(model.parents[d] >= j); + nv = model.joints[j].nv(); + for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant + { + CHECK_DATA(model.parents[d] >= j); - nv += model.joints[d].nv(); + nv += model.joints[d].nv(); + } } CHECK_DATA(nv == data.nvSubtree[j]); diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 71fcb74d9b..d4c79503cb 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -85,7 +85,7 @@ namespace pinocchio // // Centroidal momentum map ColsBlock Ag_cols = jmodel.jointCols(data.Ag); - ColsBlock J_cols = jmodel.jointCols(data.J); + ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); // // Joint Space Inertia Matrix @@ -119,14 +119,18 @@ namespace pinocchio typedef ModelTpl Model; typedef DataTpl Data; - typedef boost::fusion::vector ArgsType; + typedef boost::fusion::vector ArgsType; template< typename JointModel, typename boost::disable_if< std::is_same>, int>::type = 0> - static void algo(const JointModelBase & jmodel, const Model & model, Data & data) + static void algo( + const JointModelBase & jmodel, + const Model & model, + Data & data, + const int & mims_id) { } @@ -135,8 +139,63 @@ namespace pinocchio typename boost::enable_if< std::is_same>, int>::type = 0> - static void algo(const JointModelBase & jmodel, const Model & model, Data & data) + static void algo( + const JointModelBase & jmodel, + const Model & model, + Data & data, + const int & mims_id) { + JointIndex secondary_id = jmodel.id(); + JointIndex primary_id = jmodel.derived().jmodel().id(); + + Eigen::Vector J_cols = data.J.col(jmodel.idx_vExtended()); + Eigen::Vector Ag_sec; + + motionSet::inertiaAction(data.oYcrb[secondary_id], J_cols, Ag_sec); + + // // Compute secondary terms that were previously in the diagonal + data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nvExtended(), jmodel.nvExtended()) + .noalias() += J_cols.transpose() * Ag_sec; + + // Compute for the "subtree" of the mimic + + JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; + std::cout << sub_mimic_id << " " << model.idx_vs[sub_mimic_id] << " " + << data.nvSubtree[sub_mimic_id] << std::endl; + jmodel.jointRows(data.M) + .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) + .noalias() += + J_cols.transpose() + * data.Ag.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); + const auto & supports = model.supports[secondary_id]; + int j = supports.size() - 2; + for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--) + { + int sup_idx_v = model.idx_vs[supports[j]]; + int sup_nv = model.nvs[supports[j]]; + int sup_idx_vExtended = model.idx_vExtendeds[supports[j]]; + int sup_nvExtended = model.nvExtendeds[supports[j]]; + + data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += + data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; + if (supports[j] == primary_id) + data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += + data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; + } + + for (; j > 0; j--) + { + int sup_idx_v = model.idx_vs[supports[j]]; + int sup_nv = model.nvs[supports[j]]; + int sup_idx_vExtended = model.idx_vExtendeds[supports[j]]; + int sup_nvExtended = model.nvExtendeds[supports[j]]; + data.M.block(sup_idx_v, jmodel.idx_v(), sup_nv, jmodel.nvExtended()).noalias() += + data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; + } + + // Add Ag of secondary to its primary + motionSet::inertiaAction( + data.oYcrb[secondary_id], J_cols, jmodel.jointCols(data.Ag)); } }; @@ -291,15 +350,6 @@ namespace pinocchio const Model & model, Data & data) { - typedef JointModelMimicTpl JointModel_; - typedef typename Eigen::Matrix< - Scalar, 6, Eigen::Dynamic, Data::Matrix6x::Options, 6, JointModel_::MaxNVExtended> - SpatialForcesX; - typedef JointDataTpl JointData; - typedef - typename Eigen::Matrix - MotionSubspace; - const JointIndex & secondary_id = jmodel.id(); const JointIndex & primary_id = jmodel.derived().jmodel().id(); auto & F = data.Fcrb[secondary_id]; @@ -320,12 +370,11 @@ namespace pinocchio forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F)); int row_idx = jmodel.idx_v(); int col_idx = model.idx_vs[li]; - - data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += - data.joints[li].S().transpose() * jmodel.jointCols(F); + Scalar coeff = 1; if (li == primary_id) - data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += - data.joints[li].S().transpose() * jmodel.jointCols(F); + coeff = 2; + data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += + coeff * (data.joints[li].S().transpose() * jmodel.jointCols(F)); } for (; j > 0; j--) { @@ -373,10 +422,10 @@ namespace pinocchio } typedef CrbaLocalConventionMimicStep Pass3; - for (int i = 0; i < model.mimic_joints.size(); i++) + for (int i = 0; i < model.mimicking_joints.size(); i++) { Pass3::run( - model.joints[model.mimic_joints[i]], data.joints[model.mimic_joints[i]], + model.joints[model.mimicking_joints[i]], data.joints[model.mimicking_joints[i]], typename Pass3::ArgsType(model, data)); } @@ -420,9 +469,10 @@ namespace pinocchio } typedef CrbaWorldConventionMimicStep Pass3; - for (int i = 0; i < model.mimic_joints.size(); i++) + for (int i = 0; i < model.mimicking_joints.size(); i++) { - Pass3::run(model.joints[model.mimic_joints[i]], typename Pass3::ArgsType(model, data)); + Pass3::run( + model.joints[model.mimicking_joints[i]], typename Pass3::ArgsType(model, data, i)); } // Add the armature contribution diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index 4fbb752d4f..d50ce879ad 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -328,6 +328,7 @@ namespace pinocchio /// idx_v) std::vector idx_v_extended_fromRow; + std::vector mimic_subtree_joint; /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a /// Cholesky Decomposition. MatrixXs U; diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index addcdb968b..f57a22c097 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -207,9 +207,7 @@ namespace pinocchio // Build a "correct" representation of mimic nvSubtree by using nvExtended, which will cover // its children nv, and allow for a simple check if (boost::get>(&model.joints[i])) - nvSubtree[(Index)i] = model.joints[(Index)lastChild[(Index)i]].idx_vExtended() - + model.joints[(Index)lastChild[(Index)i]].nvExtended() - - model.joints[(Index)i].idx_vExtended(); + nvSubtree[(Index)i] = 0; else { int nv_; @@ -222,6 +220,21 @@ namespace pinocchio model.joints[(Index)lastChild[(Index)i]].idx_v() + nv_ - model.joints[(Index)i].idx_v(); } } + // fill mimic data + for (const JointIndex mimicking_id : model.mimicking_joints) + { + const auto & mimicking_sub = model.subtrees[mimicking_id]; + int j = 1; + for (; j < mimicking_sub.size(); j++) + { + if (model.nvs[mimicking_sub[j]] != 0) + break; + } + if (mimicking_sub.size() == 1) + mimic_subtree_joint.push_back(0); + else + mimic_subtree_joint.push_back(mimicking_sub[j]); + } } template class JointCollectionTpl> From 15d69c6b05f522039285c4fb89b25ba6c14ba22b Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 11:14:10 +0100 Subject: [PATCH 277/310] [data] Add check data test for mimic_subtree_joint --- include/pinocchio/algorithm/check-data.hxx | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 4c8df9c944..13f7b42a25 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -135,6 +135,26 @@ namespace pinocchio CHECK_DATA( jparent.idx_vExtended() + jparent.nvExtended() - 1 == data.parents_fromRow[(size_t)row]); } + + if (model.mimicking_joints.size() != 0) + { + for (JointIndex k : model.mimicking_joints) + { + // Check the mimic_subtree_joint + const auto & mimicking_sub = model.subtrees[k]; + int j = 1; + JointIndex id_subtree = 0; + for (; j < mimicking_sub.size(); j++) + { + if (model.nvs[mimicking_sub[j]] != 0) + { + id_subtree = mimicking_sub[j]; + } + break; + } + CHECK_DATA(id_subtree == data.mimic_subtree_joint[k]); + } + } } #undef CHECK_DATA From 23fe8bd305eb61d328323276f95b76e4dc8dce0c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 11:14:34 +0100 Subject: [PATCH 278/310] [algo/crba] Add comments and use new data struct in local mimic patch --- include/pinocchio/algorithm/crba.hxx | 53 ++++++++++++++++++++-------- 1 file changed, 39 insertions(+), 14 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index d4c79503cb..6b7eb885c3 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -111,6 +111,15 @@ namespace pinocchio } }; + // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in + // order not to replace the mimicked joint(s) info in the matrix. This step allows for the + // mimicking joint(s) contribution to be added. It is done in 3 steps: + // - First we compute the mimicking joint(s) Force matrix + // - Then we compute the "old" row of the mimicking joint in the matrix, by getting the subtree + // of mimicking joint + // - Since here only the upper part of the matrix is computed, we need to go over the support of + // the mimicking joint, and compute how the force matrix of the mimicking joint is affecting + // each joint ATTENTION : the last loop is spilt in 2, to only fill the upper part of the matrix template class JointCollectionTpl> struct CrbaWorldConventionMimicStep : public fusion::JointUnaryVisitorBase< @@ -148,8 +157,8 @@ namespace pinocchio JointIndex secondary_id = jmodel.id(); JointIndex primary_id = jmodel.derived().jmodel().id(); - Eigen::Vector J_cols = data.J.col(jmodel.idx_vExtended()); - Eigen::Vector Ag_sec; + Eigen::Matrix J_cols = data.J.col(jmodel.idx_vExtended()); + Eigen::Matrix Ag_sec; motionSet::inertiaAction(data.oYcrb[secondary_id], J_cols, Ag_sec); @@ -158,10 +167,7 @@ namespace pinocchio .noalias() += J_cols.transpose() * Ag_sec; // Compute for the "subtree" of the mimic - JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; - std::cout << sub_mimic_id << " " << model.idx_vs[sub_mimic_id] << " " - << data.nvSubtree[sub_mimic_id] << std::endl; jmodel.jointRows(data.M) .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) .noalias() += @@ -316,6 +322,15 @@ namespace pinocchio } }; + // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in + // order not to replace the mimicked joint(s) info in the matrix. This step allows for the + // mimicking joint(s) contribution to be added. It is done in 3 steps: + // - First we compute the mimicking joint(s) Force matrix + // - Then we compute the "old" row of the mimicking joint in the matrix, by getting the subtree + // of mimicking joint + // - Since here only the upper part of the matrix is computed, we need to go over the support of + // the mimicking joint, and compute how the force matrix of the mimicking joint is affecting + // each joint ATTENTION : the last loop is spilt in 2, to only fill the upper part of the matrix template class JointCollectionTpl> struct CrbaLocalConventionMimicStep : public fusion::JointUnaryVisitorBase< @@ -324,7 +339,7 @@ namespace pinocchio typedef ModelTpl Model; typedef DataTpl Data; - typedef boost::fusion::vector ArgsType; + typedef boost::fusion::vector ArgsType; template< typename JointModel, @@ -335,7 +350,8 @@ namespace pinocchio const JointModelBase & jmodel, JointDataBase & jdata, const Model & model, - Data & data) + Data & data, + const int & mims_id) { } @@ -348,7 +364,8 @@ namespace pinocchio const JointModelBase & jmodel, JointDataBase & jdata, const Model & model, - Data & data) + Data & data, + const int & mims_id) { const JointIndex & secondary_id = jmodel.id(); const JointIndex & primary_id = jmodel.derived().jmodel().id(); @@ -356,9 +373,17 @@ namespace pinocchio /* F[1:6,i] = Y*S */ jmodel.jointCols(F) = data.Ycrb[secondary_id] * jdata.S(); - // Add contribution of mimic to primary. Here all Fcrb is multiplied but there could be an - // optimisation to only get the needed values - jmodel.jointRows(data.M) += jdata.S().transpose() * F; + + // Add mimicking joint contribution to the Matrix + jmodel.jointBlock(data.M) += jdata.S().transpose() * jmodel.jointCols(F); + + // Add mimicking subtree contribution to the matrix. + JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; + + jmodel.jointRows(data.M) + .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) + .noalias() += jdata.S().transpose() + * F.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); const auto & supports = model.supports[secondary_id]; int j = supports.size() - 2; @@ -370,9 +395,9 @@ namespace pinocchio forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F)); int row_idx = jmodel.idx_v(); int col_idx = model.idx_vs[li]; - Scalar coeff = 1; + Scalar coeff(1.0); if (li == primary_id) - coeff = 2; + coeff = 2.0; data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += coeff * (data.joints[li].S().transpose() * jmodel.jointCols(F)); } @@ -426,7 +451,7 @@ namespace pinocchio { Pass3::run( model.joints[model.mimicking_joints[i]], data.joints[model.mimicking_joints[i]], - typename Pass3::ArgsType(model, data)); + typename Pass3::ArgsType(model, data, i)); } // Add the armature contribution From 4fa75a6fb764e1e9169902ad89461a19f3d32c2d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 11:14:54 +0100 Subject: [PATCH 279/310] [model] Remove useless structure --- include/pinocchio/algorithm/model.hxx | 3 --- include/pinocchio/multibody/model.hpp | 7 ------- include/pinocchio/multibody/model.hxx | 9 ++------- 3 files changed, 2 insertions(+), 17 deletions(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index 671a565250..f844c136aa 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -964,9 +964,6 @@ namespace pinocchio output_model.mimicking_joints.push_back(index_secondary); output_model.mimicked_joints.push_back(index_primary); - size_t anc_prim, anc_sec; - findCommonAncestor(output_model, index_primary, index_secondary, anc_prim, anc_sec); - output_model.mimic_pairs.push_back(std::make_pair(anc_prim, anc_sec)); } template class JointCollectionTpl> diff --git a/include/pinocchio/multibody/model.hpp b/include/pinocchio/multibody/model.hpp index 4f19f82180..4de5152785 100644 --- a/include/pinocchio/multibody/model.hpp +++ b/include/pinocchio/multibody/model.hpp @@ -94,9 +94,6 @@ namespace pinocchio /// It also handles the notion of co-tangent vector (e.g. torque, etc). typedef VectorXs TangentVectorType; - /// \brief Pairs of index for common ancestor of a mimic and its primary - typedef std::pair MimicAncestorPair; - /// \brief Dimension of the configuration vector representation. int nq; @@ -158,10 +155,6 @@ namespace pinocchio /// vector in mimicking_joints std::vector mimicked_joints; - /// \brief Vector of mimic joints and its primary common ancestors. It corresponds to the index - /// in their respective model.supports. - std::vector mimic_pairs; - /// \brief Name of the joints. std::vector names; diff --git a/include/pinocchio/multibody/model.hxx b/include/pinocchio/multibody/model.hxx index a9bd197f60..d36fe9c93a 100644 --- a/include/pinocchio/multibody/model.hxx +++ b/include/pinocchio/multibody/model.hxx @@ -161,10 +161,6 @@ namespace pinocchio { mimicking_joints.push_back(jmodel.id()); mimicked_joints.push_back(jmodel_->jmodel().id()); - - size_t anc_prim, anc_sec; - findCommonAncestor(*this, jmodel_->jmodel().id(), jmodel.id(), anc_sec, anc_prim); - mimic_pairs.push_back(std::make_pair(anc_prim, anc_sec)); } return joint_id; } @@ -254,7 +250,6 @@ namespace pinocchio res.supports = supports; res.mimicking_joints = mimicking_joints; res.mimicked_joints = mimicked_joints; - res.mimic_pairs = mimic_pairs; res.gravity = gravity.template cast(); res.name = name; @@ -311,8 +306,8 @@ namespace pinocchio && other.njoints == njoints && other.nbodies == nbodies && other.nframes == nframes && other.parents == parents && other.children == children && other.names == names && other.subtrees == subtrees && other.mimicking_joints == mimicking_joints - && other.mimicked_joints == mimicked_joints && other.mimic_pairs == mimic_pairs - && other.gravity == gravity && other.name == name; + && other.mimicked_joints == mimicked_joints && 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_vExtendeds == idx_vExtendeds && other.nvExtendeds == nvExtendeds; From 5d1761ce73ddd0652c801b37147285461a0457fa Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 11:15:20 +0100 Subject: [PATCH 280/310] [data] add comment for new vector (special mimic) --- include/pinocchio/bindings/python/multibody/data.hpp | 3 +++ include/pinocchio/multibody/data.hpp | 2 ++ 2 files changed, 5 insertions(+) diff --git a/include/pinocchio/bindings/python/multibody/data.hpp b/include/pinocchio/bindings/python/multibody/data.hpp index 08eb44c98e..c939e05312 100644 --- a/include/pinocchio/bindings/python/multibody/data.hpp +++ b/include/pinocchio/bindings/python/multibody/data.hpp @@ -156,6 +156,9 @@ namespace pinocchio "(idx_v_extended_fromRow[idx_v_extended] = idx_v)") .ADD_DATA_PROPERTY( nvSubtree_fromRow, "Subtree of the current row index (used in Cholesky)") + .ADD_DATA_PROPERTY( + mimic_subtree_joint, "When mimic joints are in the tree, this will store the index of " + "the first non mimic child of each mimic joint. (useful in crba)") .ADD_DATA_PROPERTY(J, "Jacobian of joint placement") .ADD_DATA_PROPERTY(dJ, "Time variation of the Jacobian of joint placement (data.J).") .ADD_DATA_PROPERTY(iMf, "Body placement wrt to algorithm end effector.") diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index d50ce879ad..8570b6543a 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -328,6 +328,8 @@ namespace pinocchio /// idx_v) std::vector idx_v_extended_fromRow; + /// \brief When mimic joints are in the tree, this will store the index of the first non mimic + /// child of each mimic joint. (useful in crba) std::vector mimic_subtree_joint; /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a /// Cholesky Decomposition. From be09e50c8184fdb3572a8406132c704978c53d3c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 11:52:56 +0100 Subject: [PATCH 281/310] [unittest/data] Add test for new vector --- unittest/crba.cpp | 13 +++++-------- unittest/data.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 32 insertions(+), 8 deletions(-) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index d8d4b2104b..d345f43944 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -153,14 +153,13 @@ void test_mimic_against_full_model( pinocchio::Data data_mimic(model_mimic); pinocchio::Data data_ref_mimic(model_mimic); + pinocchio::crba(model_full, data_full, q_full); // World vs local 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)); - pinocchio::crba(model_full, data_full, q_full); - // Compute other half of matrix data_mimic.M.triangularView() = data_mimic.M.transpose().triangularView(); @@ -178,9 +177,7 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) // Direct parent/Child std::vector primaries = {humanoid_model.getJointId("rleg1_joint")}; - std::vector secondaries = {humanoid_model.getJointId("rleg2_joint")}; - test_mimic_against_full_model(humanoid_model, primaries, secondaries); // Spaced Parent/Child @@ -193,7 +190,7 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) test_mimic_against_full_model(humanoid_model, primaries, secondaries); - // Parallel + // // Parallel primaries.clear(); secondaries.clear(); @@ -203,7 +200,7 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) test_mimic_against_full_model(humanoid_model, primaries, secondaries); - // Double mimic, not same primary + // // Double mimic, not same primary primaries.clear(); secondaries.clear(); primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("rarm1_joint")}; @@ -212,7 +209,7 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("larm1_joint")}; test_mimic_against_full_model(humanoid_model, primaries, secondaries); - // Double Mimic, Same Primary + // // Double Mimic, Same Primary primaries.clear(); secondaries.clear(); primaries = {humanoid_model.getJointId("lleg1_joint"), humanoid_model.getJointId("lleg1_joint")}; @@ -300,7 +297,7 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) const Eigen::VectorXd q = pinocchio::neutral(model); Eigen::internal::set_is_malloc_allowed(false); - crba(model, data, q); + crba(model, data, q, pinocchio::Convention::WORLD); Eigen::internal::set_is_malloc_allowed(true); } diff --git a/unittest/data.cpp b/unittest/data.cpp index e20d74dc05..ef44597526 100644 --- a/unittest/data.cpp +++ b/unittest/data.cpp @@ -11,6 +11,8 @@ #include #include +#include "utils/model-generator.hpp" + using namespace pinocchio; BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) @@ -107,4 +109,29 @@ BOOST_AUTO_TEST_CASE(test_std_vector_of_Data) datas.push_back(Data(model)); } +BOOST_AUTO_TEST_CASE(test_mimic_subtree) +{ + Model model; + buildModels::manipulator(model); + // Direct parent/Child + std::vector primaries = {model.getJointId("shoulder1_joint")}; + std::vector secondaries = {model.getJointId("shoulder2_joint")}; + + const double ratio = 2.5; + const double offset = 0.75; + pinocchio::Model model_mimic; + Eigen::MatrixXd G; + buildMimicModel(model, primaries, secondaries, ratio, offset, model_mimic, G); + + Data data_mimic(model_mimic); + Data data(model); + + // No mimic so should not be filled + BOOST_CHECK(data.mimic_subtree_joint.size() == 0); + + // it's a linear model with RX and RY so the joint after shoulder2 is not a mimic and is in its + // subtree + BOOST_CHECK(data_mimic.mimic_subtree_joint[0] == model_mimic.getJointId("shoulder3_joint")); +} + BOOST_AUTO_TEST_SUITE_END() From 48f17ffbcb8a7f1b37ec50fd425fc7b763948d42 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 14:05:49 +0100 Subject: [PATCH 282/310] [algo/check] Fix data check --- include/pinocchio/algorithm/check-data.hxx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 13f7b42a25..41514fb127 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -138,10 +138,10 @@ namespace pinocchio if (model.mimicking_joints.size() != 0) { - for (JointIndex k : model.mimicking_joints) + for (int k = 0; k < model.mimicking_joints.size(); k++) { // Check the mimic_subtree_joint - const auto & mimicking_sub = model.subtrees[k]; + const auto & mimicking_sub = model.subtrees[model.mimicking_joints[k]]; int j = 1; JointIndex id_subtree = 0; for (; j < mimicking_sub.size(); j++) From 334c76e0e59da6ce7dd2afee6cefba396e26a1b6 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 17:27:57 +0100 Subject: [PATCH 283/310] [algo/crba] [EtienneAr Feedback] Fix malloc error --- include/pinocchio/algorithm/crba.hxx | 27 ++++++++++++++++++++++++--- unittest/crba.cpp | 5 +++-- 2 files changed, 27 insertions(+), 5 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 6b7eb885c3..05af7c160b 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -322,6 +322,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); + } + }; + // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in // order not to replace the mimicked joint(s) info in the matrix. This step allows for the // mimicking joint(s) contribution to be added. It is done in 3 steps: @@ -367,6 +383,11 @@ namespace pinocchio Data & data, const int & mims_id) { + typedef JointDataTpl JointData; + typedef + typename Eigen::Matrix + MotionSubspace; + typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; const JointIndex & secondary_id = jmodel.id(); const JointIndex & primary_id = jmodel.derived().jmodel().id(); auto & F = data.Fcrb[secondary_id]; @@ -375,7 +396,7 @@ namespace pinocchio jmodel.jointCols(F) = data.Ycrb[secondary_id] * jdata.S(); // Add mimicking joint contribution to the Matrix - jmodel.jointBlock(data.M) += jdata.S().transpose() * jmodel.jointCols(F); + jmodel.jointBlock(data.M).noalias() += jdata.S().transpose() * jmodel.jointCols(F); // Add mimicking subtree contribution to the matrix. JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; @@ -399,7 +420,7 @@ namespace pinocchio if (li == primary_id) coeff = 2.0; data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += - coeff * (data.joints[li].S().transpose() * jmodel.jointCols(F)); + coeff * (GetSNoMalloc::run(data.joints[li]).transpose() * jmodel.jointCols(F)); } for (; j > 0; j--) { @@ -411,7 +432,7 @@ namespace pinocchio int row_idx = model.idx_vs[li]; data.M.block(row_idx, col_idx, model.nvs[li], jmodel.nvExtended()).noalias() += - data.joints[li].S().transpose() * jmodel.jointCols(F); + GetSNoMalloc::run(data.joints[li]).transpose() * jmodel.jointCols(F); } } }; diff --git a/unittest/crba.cpp b/unittest/crba.cpp index d345f43944..d6998f90f3 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -288,7 +288,7 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) { using namespace pinocchio; pinocchio::Model model; - pinocchio::buildModels::humanoidRandom(model, false, true); + pinocchio::buildModels::humanoidRandom(model, true, true); model.addJoint( size_t(model.njoints - 1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), @@ -297,7 +297,8 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) const Eigen::VectorXd q = pinocchio::neutral(model); Eigen::internal::set_is_malloc_allowed(false); - crba(model, data, q, pinocchio::Convention::WORLD); + // crba(model, data, q, pinocchio::Convention::WORLD); + crba(model, data, q, pinocchio::Convention::LOCAL); Eigen::internal::set_is_malloc_allowed(true); } From d5c4ead8f4754b93da5b12e15020c2bc266c77e8 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 17 Feb 2025 18:04:17 +0100 Subject: [PATCH 284/310] [EtienneAr feedback] topic/mimic: Rename MaxNVExtended into MaxNVMimicked and use it in crba patch --- include/pinocchio/algorithm/crba.hxx | 8 +++++--- include/pinocchio/multibody/joint/joint-mimic.hpp | 6 +++--- unittest/crba.cpp | 2 +- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 05af7c160b..01ae1b3c9d 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -157,8 +157,10 @@ namespace pinocchio JointIndex secondary_id = jmodel.id(); JointIndex primary_id = jmodel.derived().jmodel().id(); - Eigen::Matrix J_cols = data.J.col(jmodel.idx_vExtended()); - Eigen::Matrix Ag_sec; + Eigen::Matrix J_cols = + jmodel.jointExtendedModelCols(data.J); + Eigen::Matrix Ag_sec( + 6, jmodel.nvExtended()); motionSet::inertiaAction(data.oYcrb[secondary_id], J_cols, Ag_sec); @@ -385,7 +387,7 @@ namespace pinocchio { typedef JointDataTpl JointData; typedef - typename Eigen::Matrix + typename Eigen::Matrix MotionSubspace; typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; const JointIndex & secondary_id = jmodel.id(); diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp index 2216634b21..2bce4003fd 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -307,14 +307,14 @@ namespace pinocchio NQ = Eigen::Dynamic, NV = Eigen::Dynamic, NVExtended = Eigen::Dynamic, - MaxNVExtended = 6 + MaxNVMimicked = 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; typedef MotionTpl Bias_t; @@ -558,7 +558,7 @@ namespace pinocchio PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); enum { - MaxNVExtended = traits::MaxNVExtended + MaxNVMimicked = traits::MaxNVMimicked }; typedef JointCollectionTpl JointCollection; diff --git a/unittest/crba.cpp b/unittest/crba.cpp index d6998f90f3..91c90f3282 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -297,7 +297,7 @@ BOOST_AUTO_TEST_CASE(test_crba_malloc) const Eigen::VectorXd q = pinocchio::neutral(model); Eigen::internal::set_is_malloc_allowed(false); - // crba(model, data, q, pinocchio::Convention::WORLD); + crba(model, data, q, pinocchio::Convention::WORLD); crba(model, data, q, pinocchio::Convention::LOCAL); Eigen::internal::set_is_malloc_allowed(true); } From 9b7dfe1c4bda9cfc189319a7a02288e199dfc7bc Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 16:05:20 +0100 Subject: [PATCH 285/310] [algo/check-data] Put check for mimic_subtree outside the joint loop --- include/pinocchio/algorithm/check-data.hxx | 26 +++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 41514fb127..7e19bbdaf7 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -135,25 +135,25 @@ namespace pinocchio CHECK_DATA( jparent.idx_vExtended() + jparent.nvExtended() - 1 == data.parents_fromRow[(size_t)row]); } + } - if (model.mimicking_joints.size() != 0) + if (model.mimicking_joints.size() != 0) + { + for (int k = 0; k < model.mimicking_joints.size(); k++) { - for (int k = 0; k < model.mimicking_joints.size(); k++) + // Check the mimic_subtree_joint + const auto & mimicking_sub = model.subtrees[model.mimicking_joints[k]]; + int j = 1; + JointIndex id_subtree = -1; + for (; j < mimicking_sub.size(); j++) { - // Check the mimic_subtree_joint - const auto & mimicking_sub = model.subtrees[model.mimicking_joints[k]]; - int j = 1; - JointIndex id_subtree = 0; - for (; j < mimicking_sub.size(); j++) + if (model.nvs[mimicking_sub[j]] != 0) { - if (model.nvs[mimicking_sub[j]] != 0) - { - id_subtree = mimicking_sub[j]; - } - break; + id_subtree = mimicking_sub[j]; } - CHECK_DATA(id_subtree == data.mimic_subtree_joint[k]); + break; } + CHECK_DATA(id_subtree == data.mimic_subtree_joint[k]); } } From 9a51c9cd60176cd596a6a59c9240050ef633f84e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 16:05:37 +0100 Subject: [PATCH 286/310] [unittest] Add test when mimic is terminal --- unittest/crba.cpp | 8 ++++++++ unittest/data.cpp | 7 +++++++ 2 files changed, 15 insertions(+) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index 91c90f3282..faa68254bf 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -217,6 +217,14 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) secondaries = { humanoid_model.getJointId("rleg1_joint"), humanoid_model.getJointId("lleg2_joint")}; test_mimic_against_full_model(humanoid_model, primaries, secondaries); + + // test mimic terminal + primaries.clear(); + secondaries.clear(); + primaries = {humanoid_model.getJointId("larm4_joint")}; + + secondaries = {humanoid_model.getJointId("larm5_joint")}; + test_mimic_against_full_model(humanoid_model, primaries, secondaries); } BOOST_AUTO_TEST_CASE(test_minimal_crba) diff --git a/unittest/data.cpp b/unittest/data.cpp index ef44597526..1312cc3d78 100644 --- a/unittest/data.cpp +++ b/unittest/data.cpp @@ -132,6 +132,13 @@ BOOST_AUTO_TEST_CASE(test_mimic_subtree) // it's a linear model with RX and RY so the joint after shoulder2 is not a mimic and is in its // subtree BOOST_CHECK(data_mimic.mimic_subtree_joint[0] == model_mimic.getJointId("shoulder3_joint")); + + // Test when mimic is terminal + Model man_mimic; + buildModels::manipulator(man_mimic, true); + + Data data_man_mimic(man_mimic); + BOOST_CHECK(data_man_mimic.mimic_subtree_joint[0] == -1); } BOOST_AUTO_TEST_SUITE_END() From fbad2a43efe6f60858092e111701461fcd48ec86 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 16:05:55 +0100 Subject: [PATCH 287/310] [data] use -1 when mimic is terminal to be more explicit --- include/pinocchio/multibody/data.hxx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index f57a22c097..473152542d 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -231,7 +231,7 @@ namespace pinocchio break; } if (mimicking_sub.size() == 1) - mimic_subtree_joint.push_back(0); + mimic_subtree_joint.push_back(-1); else mimic_subtree_joint.push_back(mimicking_sub[j]); } From 2cd6dee5de602f07525f0f7e74f56016837fe258 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 16:06:44 +0100 Subject: [PATCH 288/310] [algo/crba] remove useless lines --- include/pinocchio/algorithm/crba.hxx | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 01ae1b3c9d..d25ce9692a 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -9,9 +9,6 @@ #include "pinocchio/spatial/act-on-set.hpp" #include "pinocchio/algorithm/kinematics.hpp" #include "pinocchio/algorithm/check.hpp" -#include "pinocchio/algorithm/model.hpp" - -#include /// @cond DEV @@ -83,12 +80,12 @@ namespace pinocchio ColsBlock; const JointIndex & i = jmodel.id(); - // // Centroidal momentum map + // Centroidal momentum map ColsBlock Ag_cols = jmodel.jointCols(data.Ag); ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J); motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols); - // // Joint Space Inertia Matrix + // Joint Space Inertia Matrix data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]); @@ -164,7 +161,7 @@ namespace pinocchio motionSet::inertiaAction(data.oYcrb[secondary_id], J_cols, Ag_sec); - // // Compute secondary terms that were previously in the diagonal + // Compute secondary terms that were previously in the diagonal data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nvExtended(), jmodel.nvExtended()) .noalias() += J_cols.transpose() * Ag_sec; @@ -261,7 +258,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 @@ -508,8 +505,6 @@ namespace pinocchio model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); } - data.Ag.setZero(); - data.M.setZero(); typedef CrbaWorldConventionBackwardStep Pass2; for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i) { From 9ca0a6ea382779cff6e0f4aa362550f6788ef329 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 16:07:12 +0100 Subject: [PATCH 289/310] [algo/crba] Fix compilation warning --- include/pinocchio/algorithm/crba.hxx | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index d25ce9692a..01459db504 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -132,11 +132,7 @@ namespace pinocchio typename boost::disable_if< std::is_same>, int>::type = 0> - static void algo( - const JointModelBase & jmodel, - const Model & model, - Data & data, - const int & mims_id) + static void algo(const JointModelBase &, const Model &, Data &, const int &) { } @@ -272,7 +268,7 @@ namespace pinocchio // 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(); - /* M[i,SUBTREE] = S'*F[1:6,SUBTR EE] */ + /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */ data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() = jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]); @@ -362,11 +358,11 @@ namespace pinocchio std::is_same>, int>::type = 0> static void algo( - const JointModelBase & jmodel, - JointDataBase & jdata, - const Model & model, - Data & data, - const int & mims_id) + const JointModelBase &, + JointDataBase &, + const Model &, + Data &, + const int &) { } From e1ecd8e3d02cf9fb3d372b0a1c0f9811488ab350 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 16:07:49 +0100 Subject: [PATCH 290/310] [algo/crba] use eigen::ref, and add comment to algorithm --- include/pinocchio/algorithm/crba.hxx | 51 ++++++++++++++++------------ 1 file changed, 30 insertions(+), 21 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 01459db504..db6765ecf8 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -150,8 +150,8 @@ namespace pinocchio JointIndex secondary_id = jmodel.id(); JointIndex primary_id = jmodel.derived().jmodel().id(); - Eigen::Matrix J_cols = - jmodel.jointExtendedModelCols(data.J); + Eigen::Ref> + J_cols = jmodel.jointExtendedModelCols(data.J); Eigen::Matrix Ag_sec( 6, jmodel.nvExtended()); @@ -163,13 +163,15 @@ namespace pinocchio // Compute for the "subtree" of the mimic JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; - jmodel.jointRows(data.M) - .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) - .noalias() += - J_cols.transpose() - * data.Ag.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); + if (sub_mimic_id != -1) + jmodel.jointRows(data.M) + .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) + .noalias() += + J_cols.transpose() + * data.Ag.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); + const auto & supports = model.supports[secondary_id]; - int j = supports.size() - 2; + size_t j = supports.size() - 2; for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--) { int sup_idx_v = model.idx_vs[supports[j]]; @@ -177,11 +179,12 @@ namespace pinocchio int sup_idx_vExtended = model.idx_vExtendeds[supports[j]]; int sup_nvExtended = model.nvExtendeds[supports[j]]; - data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += - data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; + Scalar coeff(1.0); if (supports[j] == primary_id) - data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += - data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; + coeff = 2.0; + + data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += + coeff * (data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec); } for (; j > 0; j--) @@ -194,9 +197,11 @@ namespace pinocchio data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; } - // Add Ag of secondary to its primary - motionSet::inertiaAction( - data.oYcrb[secondary_id], J_cols, jmodel.jointCols(data.Ag)); + // Mimic joint also have an effect on the centroidal map momentum, so it's important to + // compute it and add it to its mimicked. It is done here Ag[prim] += oYCRB[sec] * J[sec] + jmodel.jointCols(data.Ag) += Ag_sec; + // motionSet::inertiaAction( + // data.oYcrb[secondary_id], J_cols, jmodel.jointCols(data.Ag)); } }; @@ -395,14 +400,18 @@ namespace pinocchio // Add mimicking subtree contribution to the matrix. JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; - - jmodel.jointRows(data.M) - .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) - .noalias() += jdata.S().transpose() - * F.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); + if (sub_mimic_id != -1) + { + jmodel.jointRows(data.M) + .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) + .noalias() += jdata.S().transpose() + * F.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); + } + else + jmodel.jointRows(data.M).noalias() += jdata.S().transpose() * F; const auto & supports = model.supports[secondary_id]; - int j = supports.size() - 2; + size_t j = supports.size() - 2; for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--) { const JointIndex & li = supports[j]; From 8ad4023935d17b8ba7df695df3cd5f34957c22f7 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 18:19:48 +0100 Subject: [PATCH 291/310] [algo/model] Fix transform into mimic in the case a mimic is already in the joint list and has a index higher than the new mimic --- include/pinocchio/algorithm/model.hxx | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/algorithm/model.hxx b/include/pinocchio/algorithm/model.hxx index f844c136aa..b802b5b9c5 100644 --- a/include/pinocchio/algorithm/model.hxx +++ b/include/pinocchio/algorithm/model.hxx @@ -929,7 +929,17 @@ namespace pinocchio 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_vExtended()); - + if ( + boost::get>(&jmodel_output) + && joint_id != index_secondary) + { + auto & jmimic = + boost::get>(jmodel_output); + const JointIndex prim_id = jmimic.jmodel().id(); + jmimic.setMimicIndexes( + prim_id, output_model.idx_qs[prim_id], output_model.idx_vs[prim_id], + jmodel_input.idx_vExtended()); + } 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(); From 2dbd6d48bdbb270cde9139e87b81c801e7b9daa3 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 18:20:53 +0100 Subject: [PATCH 292/310] [algo/crba] Remove changes for -1 (does not work because joint indexes are unsigned int) --- include/pinocchio/algorithm/crba.hxx | 8 ++------ include/pinocchio/multibody/data.hxx | 4 +++- unittest/utils/model-generator.hpp | 5 ++--- 3 files changed, 7 insertions(+), 10 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index db6765ecf8..0f5c4644f8 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -163,7 +163,7 @@ namespace pinocchio // Compute for the "subtree" of the mimic JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; - if (sub_mimic_id != -1) + if (sub_mimic_id != 0) jmodel.jointRows(data.M) .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) .noalias() += @@ -200,8 +200,6 @@ namespace pinocchio // Mimic joint also have an effect on the centroidal map momentum, so it's important to // compute it and add it to its mimicked. It is done here Ag[prim] += oYCRB[sec] * J[sec] jmodel.jointCols(data.Ag) += Ag_sec; - // motionSet::inertiaAction( - // data.oYcrb[secondary_id], J_cols, jmodel.jointCols(data.Ag)); } }; @@ -400,15 +398,13 @@ namespace pinocchio // Add mimicking subtree contribution to the matrix. JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id]; - if (sub_mimic_id != -1) + if (sub_mimic_id != 0) { jmodel.jointRows(data.M) .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]) .noalias() += jdata.S().transpose() * F.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]); } - else - jmodel.jointRows(data.M).noalias() += jdata.S().transpose() * F; const auto & supports = model.supports[secondary_id]; size_t j = supports.size() - 2; diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 473152542d..8cf9606f07 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -123,6 +123,7 @@ namespace pinocchio , par_cons_ind((std::size_t)model.njoints, 0) , a_bias((std::size_t)model.njoints, Motion::Zero()) , KAS((std::size_t)model.njoints, MatrixXs::Zero(0, 0)) + #if EIGEN_VERSION_AT_LEAST(3, 2, 90) && !EIGEN_VERSION_AT_LEAST(3, 2, 93) , kinematic_hessians( 6, @@ -159,6 +160,7 @@ namespace pinocchio , constraints_supported_dim((std::size_t)model.njoints, 0) , constraints_supported((std::size_t)model.njoints) , constraints_on_joint((std::size_t)model.njoints) + , mimic_subtree_joint() { typedef typename Model::JointIndex JointIndex; @@ -231,7 +233,7 @@ namespace pinocchio break; } if (mimicking_sub.size() == 1) - mimic_subtree_joint.push_back(-1); + mimic_subtree_joint.push_back(0); else mimic_subtree_joint.push_back(mimicking_sub[j]); } diff --git a/unittest/utils/model-generator.hpp b/unittest/utils/model-generator.hpp index 84470b4d38..d40cf13528 100644 --- a/unittest/utils/model-generator.hpp +++ b/unittest/utils/model-generator.hpp @@ -121,10 +121,9 @@ namespace pinocchio } // Set specific values for primary-secondary joint pairs - for (size_t i = 0; i < primary_ids.size(); ++i) + for (size_t i = 0; i < secondary_ids.size(); ++i) { - G(model_full.joints[secondary_ids[i]].idx_v(), model_mimic.joints[secondary_ids[i]].idx_v()) = - ratio; + G(model_full.idx_vs[secondary_ids[i]], model_mimic.idx_vs[secondary_ids[i]]) = ratio; } } } // namespace pinocchio From bf2b3f08e9968f4a97921e60dc3d903bc6bcc1d4 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Tue, 18 Feb 2025 18:21:08 +0100 Subject: [PATCH 293/310] [joint] Remove compilation warning --- .../pinocchio/multibody/joint/joint-common-operations.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/pinocchio/multibody/joint/joint-common-operations.hpp b/include/pinocchio/multibody/joint/joint-common-operations.hpp index 8c6f0f6567..8030e507f6 100644 --- a/include/pinocchio/multibody/joint/joint-common-operations.hpp +++ b/include/pinocchio/multibody/joint/joint-common-operations.hpp @@ -91,10 +91,10 @@ namespace pinocchio { template static void run( - const Eigen::MatrixBase & qIn, - const Scalar & scaling, - const Scalar & offset, - const Eigen::MatrixBase & qOut) + const Eigen::MatrixBase &, + const Scalar &, + const Scalar &, + const Eigen::MatrixBase &) { assert(false && "Joint cannot be used with JointMimic."); } From 037514ee81b19ef4b3f4566402cf6f3720a28260 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 15:41:04 +0100 Subject: [PATCH 294/310] [data] Stop using -1 (not unsigned int safe) --- include/pinocchio/algorithm/check-data.hxx | 2 +- unittest/data.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 7e19bbdaf7..7030a293ab 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -144,7 +144,7 @@ namespace pinocchio // Check the mimic_subtree_joint const auto & mimicking_sub = model.subtrees[model.mimicking_joints[k]]; int j = 1; - JointIndex id_subtree = -1; + JointIndex id_subtree = 0; for (; j < mimicking_sub.size(); j++) { if (model.nvs[mimicking_sub[j]] != 0) diff --git a/unittest/data.cpp b/unittest/data.cpp index 1312cc3d78..1e336018e9 100644 --- a/unittest/data.cpp +++ b/unittest/data.cpp @@ -138,7 +138,7 @@ BOOST_AUTO_TEST_CASE(test_mimic_subtree) buildModels::manipulator(man_mimic, true); Data data_man_mimic(man_mimic); - BOOST_CHECK(data_man_mimic.mimic_subtree_joint[0] == -1); + BOOST_CHECK(data_man_mimic.mimic_subtree_joint[0] == 0); } BOOST_AUTO_TEST_SUITE_END() From ea8e03286171f3dbcbb3d801991c9522adc36588 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 15:41:25 +0100 Subject: [PATCH 295/310] [unittest/crba] test for mimic terminal --- unittest/crba.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/unittest/crba.cpp b/unittest/crba.cpp index faa68254bf..ef8b6b0293 100644 --- a/unittest/crba.cpp +++ b/unittest/crba.cpp @@ -221,9 +221,9 @@ BOOST_AUTO_TEST_CASE(test_crba_mimic) // test mimic terminal primaries.clear(); secondaries.clear(); - primaries = {humanoid_model.getJointId("larm4_joint")}; + primaries = {humanoid_model.getJointId("larm5_joint")}; - secondaries = {humanoid_model.getJointId("larm5_joint")}; + secondaries = {humanoid_model.getJointId("larm6_joint")}; test_mimic_against_full_model(humanoid_model, primaries, secondaries); } From f93c7202942eaf4d9f901d1408354b4b9541bb80 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 15:42:20 +0100 Subject: [PATCH 296/310] [algo/crba] Fix heap allocation --- include/pinocchio/algorithm/crba.hxx | 53 +++++++++++++++++++--------- 1 file changed, 37 insertions(+), 16 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 0f5c4644f8..09da6d71bb 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -172,6 +172,10 @@ namespace pinocchio const auto & supports = model.supports[secondary_id]; size_t j = supports.size() - 2; + Eigen::Matrix< + Scalar, Eigen::Dynamic, Eigen::Dynamic, Options, JointModel::MaxNVMimicked, + JointModel::MaxNVMimicked> + temp_JAG; for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--) { int sup_idx_v = model.idx_vs[supports[j]]; @@ -179,12 +183,13 @@ namespace pinocchio int sup_idx_vExtended = model.idx_vExtendeds[supports[j]]; int sup_nvExtended = model.nvExtendeds[supports[j]]; - Scalar coeff(1.0); - if (supports[j] == primary_id) - coeff = 2.0; - + temp_JAG.noalias() = + data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += - coeff * (data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec); + temp_JAG; + if (supports[j] == primary_id) + data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() += + temp_JAG; } for (; j > 0; j--) @@ -193,13 +198,15 @@ namespace pinocchio int sup_nv = model.nvs[supports[j]]; int sup_idx_vExtended = model.idx_vExtendeds[supports[j]]; int sup_nvExtended = model.nvExtendeds[supports[j]]; + data.M.block(sup_idx_v, jmodel.idx_v(), sup_nv, jmodel.nvExtended()).noalias() += data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec; } // Mimic joint also have an effect on the centroidal map momentum, so it's important to // compute it and add it to its mimicked. It is done here Ag[prim] += oYCRB[sec] * J[sec] - jmodel.jointCols(data.Ag) += Ag_sec; + motionSet::inertiaAction( + data.oYcrb[secondary_id], J_cols, jmodel.jointCols(data.Ag)); } }; @@ -320,19 +327,26 @@ namespace pinocchio } }; - template - struct GetMotionSubspaceTplNoMalloc : public boost::static_visitor + template + struct UseMotionSubspaceNoMalloc : public boost::static_visitor { + ForceType F; + + UseMotionSubspaceNoMalloc(ForceType F_) + : F(F_) + { + } + template ReturnType operator()(const JointDataBase & jdata) const { - assert(jdata.S().nv() <= ReturnType::MaxColsAtCompileTime); - return ReturnType(jdata.S().matrix()); + // assert(jdata.S().nv() <= ReturnType::MaxColsAtCompileTime); + return jdata.S().transpose() * F; } - static ReturnType run(const JointDataTpl & jdata) + static ReturnType run(const JointDataTpl & jdata, const ForceType & F_) { - return boost::apply_visitor(GetMotionSubspaceTplNoMalloc(), jdata); + return boost::apply_visitor(UseMotionSubspaceNoMalloc(F_), jdata); } }; @@ -384,8 +398,15 @@ namespace pinocchio typedef JointDataTpl JointData; typedef typename Eigen::Matrix - MotionSubspace; - typedef GetMotionSubspaceTplNoMalloc GetSNoMalloc; + Matrix6; + + typedef typename Eigen::Matrix< + Scalar, Eigen::Dynamic, Eigen::Dynamic, Options, JointModel::MaxNVMimicked, + JointModel::MaxNVMimicked> + MatrixMass; + + typedef UseMotionSubspaceNoMalloc UseSNoMalloc; + const JointIndex & secondary_id = jmodel.id(); const JointIndex & primary_id = jmodel.derived().jmodel().id(); auto & F = data.Fcrb[secondary_id]; @@ -420,7 +441,7 @@ namespace pinocchio if (li == primary_id) coeff = 2.0; data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += - coeff * (GetSNoMalloc::run(data.joints[li]).transpose() * jmodel.jointCols(F)); + coeff * UseSNoMalloc::run(data.joints[li], jmodel.jointCols(F)); } for (; j > 0; j--) { @@ -432,7 +453,7 @@ namespace pinocchio int row_idx = model.idx_vs[li]; data.M.block(row_idx, col_idx, model.nvs[li], jmodel.nvExtended()).noalias() += - GetSNoMalloc::run(data.joints[li]).transpose() * jmodel.jointCols(F); + UseSNoMalloc::run(data.joints[li], jmodel.jointCols(F)); } } }; From 67691f8cef0097d3094dafcf87e0a2106bdb5a45 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 10:09:32 +0100 Subject: [PATCH 297/310] [EtienneAr feedback] topic/mimic: WiP add (non_)mimic_parents_fromRow --- .../bindings/python/multibody/data.hpp | 6 ++++ include/pinocchio/multibody/data.hpp | 6 ++++ include/pinocchio/multibody/data.hxx | 36 +++++++++++++++++++ include/pinocchio/serialization/data.hpp | 2 ++ 4 files changed, 50 insertions(+) diff --git a/include/pinocchio/bindings/python/multibody/data.hpp b/include/pinocchio/bindings/python/multibody/data.hpp index c939e05312..1af9e5d837 100644 --- a/include/pinocchio/bindings/python/multibody/data.hpp +++ b/include/pinocchio/bindings/python/multibody/data.hpp @@ -151,6 +151,12 @@ namespace pinocchio .ADD_DATA_PROPERTY(U, "Joint Inertia square root (upper triangle)") .ADD_DATA_PROPERTY(D, "Diagonal of UDUT inertia decomposition") .ADD_DATA_PROPERTY(parents_fromRow, "First previous non-zero row in M (used in Cholesky)") + .ADD_DATA_PROPERTY( + mimic_parents_fromRow, + "First previous non-zero row belonging to a mimic joint in M (used in Jacobian).") + .ADD_DATA_PROPERTY( + non_mimic_parents_fromRow, + "First previous non-zero row belonging to a non mimic joint in M (used in Jacobian).") .ADD_DATA_PROPERTY( idx_v_extended_fromRow, "Extended model mapping of the joint rows " "(idx_v_extended_fromRow[idx_v_extended] = idx_v)") diff --git a/include/pinocchio/multibody/data.hpp b/include/pinocchio/multibody/data.hpp index 8570b6543a..3398432471 100644 --- a/include/pinocchio/multibody/data.hpp +++ b/include/pinocchio/multibody/data.hpp @@ -348,6 +348,12 @@ namespace pinocchio /// \brief First previous non-zero row in M (used in Cholesky Decomposition). std::vector parents_fromRow; + /// \brief First previous non-zero row belonging to a mimic joint in M (used in Jacobian). + std::vector mimic_parents_fromRow; + + /// \brief First previous non-zero row belonging to a non mimic joint in M (used in Jacobian). + std::vector non_mimic_parents_fromRow; + /// \brief Each element of this vector corresponds to the ordered list of indexes belonging to /// the supporting tree of the /// given index at the row level. It may be helpful to retrieve the sparsity pattern diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 8cf9606f07..7d139c352f 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -77,6 +77,8 @@ namespace pinocchio , Dinv(VectorXs::Zero(model.nv)) , tmp(VectorXs::Zero(model.nv)) , parents_fromRow((std::size_t)model.nvExtended, -1) + , mimic_parents_fromRow((std::size_t)model.nvExtended, -1) + , non_mimic_parents_fromRow((std::size_t)model.nvExtended, -1) , idx_v_extended_fromRow((std::size_t)model.nvExtended, -1) , supports_fromRow((std::size_t)model.nv) , nvSubtree_fromRow((std::size_t)model.nvExtended, -1) @@ -262,6 +264,36 @@ namespace pinocchio else parents_fromRow[(Index)idx_vExtended_j] = -1; + JointIndex first_non_mimic_parent_id = parent; + while (first_non_mimic_parent_id > 0 + && boost::get>( + &model.joints[first_non_mimic_parent_id])) + { + first_non_mimic_parent_id = model.parents[first_non_mimic_parent_id]; + } + + if (first_non_mimic_parent_id > 0) + non_mimic_parents_fromRow[(Index)idx_vExtended_j] = + model.joints[first_non_mimic_parent_id].idx_vExtended() + + model.joints[first_non_mimic_parent_id].nvExtended() - 1; + else + non_mimic_parents_fromRow[(Index)idx_vExtended_j] = -1; + + JointIndex first_mimic_parent_id = parent; + while (first_mimic_parent_id > 0 + && !boost::get>( + &model.joints[first_mimic_parent_id])) + { + first_mimic_parent_id = model.parents[first_mimic_parent_id]; + } + + if (first_mimic_parent_id > 0) + mimic_parents_fromRow[(Index)idx_vExtended_j] = + model.joints[first_mimic_parent_id].idx_vExtended() + + model.joints[first_mimic_parent_id].nvExtended() - 1; + else + mimic_parents_fromRow[(Index)idx_vExtended_j] = -1; + nvSubtree_fromRow[(Index)idx_vExtended_j] = nvSubtree[joint]; start_idx_v_fromRow[(size_t)idx_vj] = idx_vj; end_idx_v_fromRow[(size_t)idx_vj] = idx_vj + nvExtended_j - 1; @@ -270,6 +302,8 @@ namespace pinocchio for (int row = 1; row < nvExtended_j; ++row) { parents_fromRow[(size_t)(idx_vExtended_j + row)] = idx_vExtended_j + row - 1; + mimic_parents_fromRow[(size_t)(idx_vExtended_j + row)] = idx_vExtended_j + row - 1; + non_mimic_parents_fromRow[(size_t)(idx_vExtended_j + row)] = idx_vExtended_j + row - 1; nvSubtree_fromRow[(size_t)(idx_vExtended_j + row)] = nvSubtree[joint] - row; start_idx_v_fromRow[(size_t)(idx_vExtended_j + row)] = start_idx_v_fromRow[(size_t)idx_vExtended_j]; @@ -338,6 +372,8 @@ namespace pinocchio && data1.end_idx_v_fromRow == data2.end_idx_v_fromRow && data1.U == data2.U && data1.D == data2.D && data1.Dinv == data2.Dinv && data1.parents_fromRow == data2.parents_fromRow + && data1.mimic_parents_fromRow == data2.mimic_parents_fromRow + && data1.non_mimic_parents_fromRow == data2.non_mimic_parents_fromRow && data1.idx_v_extended_fromRow == data2.idx_v_extended_fromRow && data1.supports_fromRow == data2.supports_fromRow && data1.nvSubtree_fromRow == data2.nvSubtree_fromRow && data1.J == data2.J diff --git a/include/pinocchio/serialization/data.hpp b/include/pinocchio/serialization/data.hpp index ff83e3ad62..bbf12e72dc 100644 --- a/include/pinocchio/serialization/data.hpp +++ b/include/pinocchio/serialization/data.hpp @@ -89,6 +89,8 @@ namespace boost PINOCCHIO_MAKE_DATA_NVP(ar, data, D); PINOCCHIO_MAKE_DATA_NVP(ar, data, Dinv); PINOCCHIO_MAKE_DATA_NVP(ar, data, parents_fromRow); + PINOCCHIO_MAKE_DATA_NVP(ar, data, mimic_parents_fromRow); + PINOCCHIO_MAKE_DATA_NVP(ar, data, non_mimic_parents_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, idx_v_extended_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, supports_fromRow); PINOCCHIO_MAKE_DATA_NVP(ar, data, nvSubtree_fromRow); From d2c257fe8181bf7681f233a3cc08a23f4605c685 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 14:24:14 +0100 Subject: [PATCH 298/310] [EtienneAr feedback] topic/mimic: Fix Jacobian for mimic and make it more efficient --- include/pinocchio/algorithm/jacobian.hxx | 96 +++++++----------------- 1 file changed, 26 insertions(+), 70 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 73dcc1bbd3..69e98a2db3 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -199,11 +199,15 @@ namespace pinocchio const int colRef = nvExtended(model.joints[joint_id]) + idx_vExtended(model.joints[joint_id]) - 1; + const int colRefMimicPass = + boost::get>(&model.joints[joint_id]) + ? colRef + : data.mimic_parents_fromRow[(size_t)idx_vExtended(model.joints[joint_id])]; switch (rf) { case WORLD: { for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; - jExtended = data.parents_fromRow[(size_t)jExtended]) + jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended]) { MotionIn v_in(Jin.col(jExtended)); MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); @@ -212,35 +216,19 @@ namespace pinocchio } // During the backward pass, mimicking joint jacobian might have been overwritten by the // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back - for (size_t i = 0; i < model.mimicking_joints.size(); i++) + for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; + jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) { - const JointIndex mimicking_id = model.mimicking_joints[i]; - const JointIndex mimicked_id = model.mimicked_joints[i]; - const Model::IndexVector & joint_support = model.supports[joint_id]; - if ( - std::find(joint_support.begin(), joint_support.end(), mimicking_id) - == joint_support.end()) - continue; // This mimicking joint does not support the selected joint, skip - if ( - std::find(joint_support.begin(), joint_support.end(), mimicked_id) - == joint_support.end()) - continue; // This mimicked joint does not support the selected joint, skip - for (Eigen::DenseIndex jExtended = idx_vExtended(model.joints[mimicking_id]); - jExtended - < nvExtended(model.joints[mimicking_id]) + idx_vExtended(model.joints[mimicking_id]); - jExtended++) - { - MotionIn v_in(Jin.col(jExtended)); - MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - - v_out += v_in; - } + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += v_in; } break; } case LOCAL_WORLD_ALIGNED: { for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; - jExtended = data.parents_fromRow[(size_t)jExtended]) + jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended]) { MotionIn v_in(Jin.col(jExtended)); MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); @@ -250,36 +238,20 @@ namespace pinocchio } // During the backward pass, mimicking joint jacobian might have been overwritten by the // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back - for (size_t i = 0; i < model.mimicking_joints.size(); i++) + for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; + jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) { - const JointIndex mimicking_id = model.mimicking_joints[i]; - const JointIndex mimicked_id = model.mimicked_joints[i]; - const Model::IndexVector & joint_support = model.supports[joint_id]; - if ( - std::find(joint_support.begin(), joint_support.end(), mimicking_id) - == joint_support.end()) - continue; // This mimicking joint does not support the selected joint, skip - if ( - std::find(joint_support.begin(), joint_support.end(), mimicked_id) - == joint_support.end()) - continue; // This mimicked joint does not support the selected joint, skip - for (Eigen::DenseIndex jExtended = idx_vExtended(model.joints[mimicking_id]); - jExtended - < nvExtended(model.joints[mimicking_id]) + idx_vExtended(model.joints[mimicking_id]); - jExtended++) - { - MotionIn v_in(Jin.col(jExtended)); - MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - - v_out += v_in; - v_out.linear() -= placement.translation().cross(v_in.angular()); - } + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += v_in; + v_out.linear() -= placement.translation().cross(v_in.angular()); } break; } case LOCAL: { for (Eigen::DenseIndex jExtended = colRef; jExtended >= 0; - jExtended = data.parents_fromRow[(size_t)jExtended]) + jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended]) { MotionIn v_in(Jin.col(jExtended)); MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); @@ -288,29 +260,13 @@ namespace pinocchio } // During the backward pass, mimicking joint jacobian might have been overwritten by the // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back - for (size_t i = 0; i < model.mimicking_joints.size(); i++) + for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; + jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) { - const JointIndex mimicking_id = model.mimicking_joints[i]; - const JointIndex mimicked_id = model.mimicked_joints[i]; - const Model::IndexVector & joint_support = model.supports[joint_id]; - if ( - std::find(joint_support.begin(), joint_support.end(), mimicking_id) - == joint_support.end()) - continue; // This mimicking joint does not support the selected joint, skip - if ( - std::find(joint_support.begin(), joint_support.end(), mimicked_id) - == joint_support.end()) - continue; // This mimicked joint does not support the selected joint, skip - for (Eigen::DenseIndex jExtended = idx_vExtended(model.joints[mimicking_id]); - jExtended - < nvExtended(model.joints[mimicking_id]) + idx_vExtended(model.joints[mimicking_id]); - jExtended++) - { - MotionIn v_in(Jin.col(jExtended)); - MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); - - v_out += placement.actInv(v_in); - } + MotionIn v_in(Jin.col(jExtended)); + MotionOut v_out(Jout_.col(data.idx_v_extended_fromRow[jExtended])); + + v_out += placement.actInv(v_in); } break; } From 5d73225b2d1f1d8e5ac24a58da240cc4b6fa3c45 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 14:53:11 +0100 Subject: [PATCH 299/310] [EtienneAr feedback] topic/mimic: Add basic checks in chack-data from mimic_parents_fromRow and non_mimic_parents_fromRow --- include/pinocchio/algorithm/check-data.hxx | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 7030a293ab..117734c92d 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -81,6 +81,8 @@ namespace pinocchio CHECK_DATA((int)data.lastChild.size() == model.njoints); CHECK_DATA((int)data.nvSubtree.size() == model.njoints); CHECK_DATA((int)data.parents_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.mimic_parents_fromRow.size() == model.nvExtended); + CHECK_DATA((int)data.non_mimic_parents_fromRow.size() == model.nvExtended); CHECK_DATA((int)data.idx_v_extended_fromRow.size() == model.nvExtended); CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nvExtended); CHECK_DATA((int)data.idx_v_extended_fromRow.size() == model.nvExtended); @@ -129,11 +131,22 @@ namespace pinocchio if (row == 0) { CHECK_DATA(data.parents_fromRow[(size_t)row] == -1); + CHECK_DATA(data.mimic_parents_fromRow[(size_t)row] == -1); + CHECK_DATA(data.non_mimic_parents_fromRow[(size_t)row] == -1); } else { CHECK_DATA( jparent.idx_vExtended() + jparent.nvExtended() - 1 == data.parents_fromRow[(size_t)row]); + if (boost::get>(&jparent)) + { + CHECK_DATA(data.parents_fromRow[(size_t)row] == data.mimic_parents_fromRow[(size_t)row]); + } + else + { + CHECK_DATA( + data.parents_fromRow[(size_t)row] == data.non_mimic_parents_fromRow[(size_t)row]); + } } } From eab64c6b0b279794b6e983f51d419bfaec216b0d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 16:04:57 +0100 Subject: [PATCH 300/310] [algo/crba] Move joint visitor into joint visitor dedicated file --- include/pinocchio/algorithm/crba.hxx | 23 ------------------- .../multibody/joint/joint-basic-visitors.hxx | 23 +++++++++++++++++++ 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/include/pinocchio/algorithm/crba.hxx b/include/pinocchio/algorithm/crba.hxx index 09da6d71bb..2880f3bee0 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -327,29 +327,6 @@ namespace pinocchio } }; - template - struct UseMotionSubspaceNoMalloc : public boost::static_visitor - { - ForceType F; - - UseMotionSubspaceNoMalloc(ForceType F_) - : F(F_) - { - } - - template - ReturnType operator()(const JointDataBase & jdata) const - { - // assert(jdata.S().nv() <= ReturnType::MaxColsAtCompileTime); - return jdata.S().transpose() * F; - } - - static ReturnType run(const JointDataTpl & jdata, const ForceType & F_) - { - return boost::apply_visitor(UseMotionSubspaceNoMalloc(F_), jdata); - } - }; - // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in // order not to replace the mimicked joint(s) info in the matrix. This step allows for the // mimicking joint(s) contribution to be added. It is done in 3 steps: diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index bee3714503..aab57fe256 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -1035,6 +1035,29 @@ namespace pinocchio jmodel); } + // + template + struct UseMotionSubspaceNoMalloc : public boost::static_visitor + { + ForceType F; + + UseMotionSubspaceNoMalloc(ForceType F_) + : F(F_) + { + } + + template + ReturnType operator()(const JointDataBase & jdata) const + { + return jdata.S().transpose() * F; + } + + static ReturnType run(const JointDataTpl & jdata, const ForceType & F_) + { + return boost::apply_visitor(UseMotionSubspaceNoMalloc(F_), jdata); + } + }; + /// @endcond } // namespace pinocchio From ade95119514e6416755fdd824344e204cdf8789a Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 16:08:46 +0100 Subject: [PATCH 301/310] [joint-visitor] Add comment --- include/pinocchio/multibody/joint/joint-basic-visitors.hxx | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx index aab57fe256..8ef3f0a4dd 100644 --- a/include/pinocchio/multibody/joint/joint-basic-visitors.hxx +++ b/include/pinocchio/multibody/joint/joint-basic-visitors.hxx @@ -1035,7 +1035,8 @@ namespace pinocchio jmodel); } - // + // Visitor to use when visiting another joint, but still in need of visiting another + // joint to take advantage of the specialisations that exists for MotionSubspace * Force template struct UseMotionSubspaceNoMalloc : public boost::static_visitor { From a2987f0ed13c151ef738922e2f5d5ca3232d311d Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 17:19:11 +0100 Subject: [PATCH 302/310] [examples] Add an example on what a mimic can and cannot do --- examples/mimic_dynamics.py | 63 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 examples/mimic_dynamics.py diff --git a/examples/mimic_dynamics.py b/examples/mimic_dynamics.py new file mode 100644 index 0000000000..76e8f3acd1 --- /dev/null +++ b/examples/mimic_dynamics.py @@ -0,0 +1,63 @@ + +from pathlib import Path +import pinocchio +import numpy as np + +# Get URDF model +pinocchio_model_dir = Path(__file__).parent.parent / "models" +model_path = pinocchio_model_dir / "baxter_simple.urdf" +model_full = pinocchio.buildModelFromUrdf(model_path) + +# To use the mimic tag from URDF set mimic flag to true (otherwise the tag will simply be ignored) +model_mimic_from_urdf = pinocchio.buildModelFromUrdf(model_path, mimic=True) + +print(f"{model_full.nq=}") +print(f"{model_mimic_from_urdf.nq=}") + +# Alternatively a "mimic" model can also be made by-hand +model_mimic = pinocchio.transformJointIntoMimic(model_full, 9, 10, -1., 0.) +model_mimic = pinocchio.transformJointIntoMimic(model_mimic, 18, 19, -1., 0.) + +print(f"{(model_mimic_from_urdf == model_mimic)=}") # True + +# Creating manually the G matrix (cf. Featherstone's Rigid Body Dynamics Algorithms, chapter 10 about Gears if you want to know +#more about how joints mimic are handled here) +G = np.zeros([model_mimic.nv, model_full.nv]) +for i in range(model_full.njoints): + mimic_scaling = getattr(model_mimic.joints[i].extract(), "scaling", None) + if(mimic_scaling): + G[model_mimic.joints[i].idx_v, model_full.joints[i].idx_v] = mimic_scaling + else: + # Not a mimic joint + G[model_mimic.joints[i].idx_v, model_full.joints[i].idx_v] = 1 +print("G = ") +print(np.array_str(G, precision=0, suppress_small=True, max_line_width=80)) + +# Random data +q_mimic = pinocchio.neutral(model_mimic) +v_mimic = np.random.random(model_mimic.nv) +a_mimic = np.random.random(model_mimic.nv) + +# Expand configuration using the G matrix for the full model +q_full = G.transpose() @ q_mimic # Possible because offset is 0, otherwise you need to add it to q of joints mimic +v_full = G.transpose() @ v_mimic +a_full = G.transpose() @ a_mimic + +# Supported algorithms works as-is +data_mimic = model_mimic.createData() +data_full = model_full.createData() + +tau_mimic = pinocchio.rnea(model_mimic, data_mimic, q_mimic, v_mimic, a_mimic) +tau_full = pinocchio.rnea(model_full, data_full, q_full, v_full, a_full) +print(f"{np.allclose(tau_mimic, G @ tau_full)=}") + +M_mimic = pinocchio.crba(model_mimic, data_mimic, q_mimic) +M_full = pinocchio.crba(model_full, data_full, q_full) +print(f"{np.allclose(M_mimic, G @ M_full @ G.transpose())=}") + +C_full = pinocchio.nle(model_full, data_full, q_full, v_full) +# For forward dynamics, aba does not support joints mimic. However it's still possible to compute acceleration of the +# mimic model, using the G matrix +a_computed = np.linalg.solve(G @ M_full @ G.transpose(), tau_mimic - G @ C_full) +print(f"{np.allclose(a_mimic,a_computed)=}") + From a52c98820eb05750b7c44890b212e6ca255f463e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 17:27:38 +0100 Subject: [PATCH 303/310] [data] Remove compilation warning --- include/pinocchio/algorithm/check-data.hxx | 4 ++-- include/pinocchio/multibody/data.hxx | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/pinocchio/algorithm/check-data.hxx b/include/pinocchio/algorithm/check-data.hxx index 117734c92d..e00266834e 100644 --- a/include/pinocchio/algorithm/check-data.hxx +++ b/include/pinocchio/algorithm/check-data.hxx @@ -152,11 +152,11 @@ namespace pinocchio if (model.mimicking_joints.size() != 0) { - for (int k = 0; k < model.mimicking_joints.size(); k++) + for (size_t k = 0; k < model.mimicking_joints.size(); k++) { // Check the mimic_subtree_joint const auto & mimicking_sub = model.subtrees[model.mimicking_joints[k]]; - int j = 1; + size_t j = 1; JointIndex id_subtree = 0; for (; j < mimicking_sub.size(); j++) { diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx index 7d139c352f..5de70e1ead 100644 --- a/include/pinocchio/multibody/data.hxx +++ b/include/pinocchio/multibody/data.hxx @@ -228,7 +228,7 @@ namespace pinocchio for (const JointIndex mimicking_id : model.mimicking_joints) { const auto & mimicking_sub = model.subtrees[mimicking_id]; - int j = 1; + size_t j = 1; for (; j < mimicking_sub.size(); j++) { if (model.nvs[mimicking_sub[j]] != 0) From d990bb91060a9100505cbaa70b217864e7631789 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 17:27:47 +0100 Subject: [PATCH 304/310] [algo/crba] Add comment --- 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 2880f3bee0..acf9dd3fa0 100644 --- a/include/pinocchio/algorithm/crba.hxx +++ b/include/pinocchio/algorithm/crba.hxx @@ -322,6 +322,8 @@ namespace pinocchio // Since we don't just copy columns, we need to use ADDTO method, to avoid ecrasing // already filled columns, in case of parallel arms, it is important to not ecrase already // filled data from the other part of the tree + // By using data.mimic_subtree_joint, we could do the same as for a non-mimic joint. + // But to do so, we need to change the either this visitor or data.mimic_subtree_joint forceSet::se3Action(data.liMi[i], iF, jF); } } From ac10ab3690fa2468ed75272f8acfbc966eb25b6c Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 17:28:01 +0100 Subject: [PATCH 305/310] [example] Apply pre-commit --- examples/mimic_dynamics.py | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/examples/mimic_dynamics.py b/examples/mimic_dynamics.py index 76e8f3acd1..f393c34d4c 100644 --- a/examples/mimic_dynamics.py +++ b/examples/mimic_dynamics.py @@ -1,31 +1,33 @@ - from pathlib import Path -import pinocchio + import numpy as np +import pinocchio # Get URDF model pinocchio_model_dir = Path(__file__).parent.parent / "models" model_path = pinocchio_model_dir / "baxter_simple.urdf" model_full = pinocchio.buildModelFromUrdf(model_path) -# To use the mimic tag from URDF set mimic flag to true (otherwise the tag will simply be ignored) +# To use the mimic tag from URDF set mimic flag to true +# (otherwise the tag will simply be ignored) model_mimic_from_urdf = pinocchio.buildModelFromUrdf(model_path, mimic=True) print(f"{model_full.nq=}") print(f"{model_mimic_from_urdf.nq=}") # Alternatively a "mimic" model can also be made by-hand -model_mimic = pinocchio.transformJointIntoMimic(model_full, 9, 10, -1., 0.) -model_mimic = pinocchio.transformJointIntoMimic(model_mimic, 18, 19, -1., 0.) +model_mimic = pinocchio.transformJointIntoMimic(model_full, 9, 10, -1.0, 0.0) +model_mimic = pinocchio.transformJointIntoMimic(model_mimic, 18, 19, -1.0, 0.0) -print(f"{(model_mimic_from_urdf == model_mimic)=}") # True +print(f"{(model_mimic_from_urdf == model_mimic)=}") # True -# Creating manually the G matrix (cf. Featherstone's Rigid Body Dynamics Algorithms, chapter 10 about Gears if you want to know -#more about how joints mimic are handled here) +# Creating manually the G matrix (cf. Featherstone's Rigid Body Dynamics Algorithms, +# chapter 10 about Gears if you want to know +# more about how joints mimic are handled here) G = np.zeros([model_mimic.nv, model_full.nv]) for i in range(model_full.njoints): mimic_scaling = getattr(model_mimic.joints[i].extract(), "scaling", None) - if(mimic_scaling): + if mimic_scaling: G[model_mimic.joints[i].idx_v, model_full.joints[i].idx_v] = mimic_scaling else: # Not a mimic joint @@ -39,7 +41,10 @@ a_mimic = np.random.random(model_mimic.nv) # Expand configuration using the G matrix for the full model -q_full = G.transpose() @ q_mimic # Possible because offset is 0, otherwise you need to add it to q of joints mimic +# Possible because offset is 0, otherwise +# you need to add it to q of joints mimic +q_full = G.transpose() @ q_mimic + v_full = G.transpose() @ v_mimic a_full = G.transpose() @ a_mimic @@ -56,8 +61,8 @@ print(f"{np.allclose(M_mimic, G @ M_full @ G.transpose())=}") C_full = pinocchio.nle(model_full, data_full, q_full, v_full) -# For forward dynamics, aba does not support joints mimic. However it's still possible to compute acceleration of the +# For forward dynamics, aba does not support joints mimic. +# However it's still possible to compute acceleration of the # mimic model, using the G matrix a_computed = np.linalg.solve(G @ M_full @ G.transpose(), tau_mimic - G @ C_full) print(f"{np.allclose(a_mimic,a_computed)=}") - From 4d4da0f2f0c5878b11f5ba01a0500539830f2950 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9gane=20Millan?= <37700908+MegMll@users.noreply.github.com> Date: Wed, 19 Feb 2025 17:29:47 +0100 Subject: [PATCH 306/310] [Joint/joint-mimic] Change order to remove compilation warning Co-authored-by: Joris Vaillant --- 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 2bce4003fd..f739902a1d 100644 --- a/include/pinocchio/multibody/joint/joint-mimic.hpp +++ b/include/pinocchio/multibody/joint/joint-mimic.hpp @@ -108,8 +108,8 @@ namespace pinocchio } explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor) - : m_scaling_factor(scaling_factor) - , m_constraint(0) + : m_constraint(0) + , m_scaling_factor(scaling_factor) { } From 3f0b4d99ef5ea7f817ceb005f4fda23e0392b67e Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Wed, 19 Feb 2025 17:34:52 +0100 Subject: [PATCH 307/310] [unittest] Remove useless lines --- unittest/casadi/joints.cpp | 1 - unittest/joint-mimic.cpp | 5 ----- 2 files changed, 6 deletions(-) diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 9147691a87..86b33c6a61 100644 --- a/unittest/casadi/joints.cpp +++ b/unittest/casadi/joints.cpp @@ -291,7 +291,6 @@ struct TestADOnJoints void operator()(const pinocchio::JointModelBase &) const { JointModel_ jmodel = init::run(); - // jmodel.setIndexes(0, 0, 0, 0); test(jmodel); } diff --git a/unittest/joint-mimic.cpp b/unittest/joint-mimic.cpp index 18863bd9dd..59b82b200e 100644 --- a/unittest/joint-mimic.cpp +++ b/unittest/joint-mimic.cpp @@ -219,11 +219,6 @@ BOOST_AUTO_TEST_CASE(test_joint) typedef boost::variant VariantUnboundedRevolute; - // 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( From 32859a6ff91dd58415cca93405bcaa790cfa0234 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9gane=20Millan?= <37700908+MegMll@users.noreply.github.com> Date: Wed, 19 Feb 2025 17:36:16 +0100 Subject: [PATCH 308/310] [algo/jacobian] Update doc Co-authored-by: Joris Vaillant --- include/pinocchio/algorithm/jacobian.hxx | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 69e98a2db3..18dd14734e 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -258,8 +258,7 @@ namespace pinocchio v_out = placement.actInv(v_in); } - // During the backward pass, mimicking joint jacobian might have been overwritten by the - // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + // Add mimicking joint effect into mimicked column for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) { From 91b6d4ff8fb39b33abacb53fb66938cd7339a77e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9gane=20Millan?= <37700908+MegMll@users.noreply.github.com> Date: Wed, 19 Feb 2025 17:36:37 +0100 Subject: [PATCH 309/310] [algo/jacobian] Update doc Co-authored-by: Joris Vaillant --- include/pinocchio/algorithm/jacobian.hxx | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 18dd14734e..0e5fdec791 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -236,8 +236,7 @@ namespace pinocchio v_out = v_in; v_out.linear() -= placement.translation().cross(v_in.angular()); } - // During the backward pass, mimicking joint jacobian might have been overwritten by the - // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + // Add mimicking joint effect into mimicked column for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) { From 540cda6c3f4f55fba57edd2982f9420985878051 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9gane=20Millan?= <37700908+MegMll@users.noreply.github.com> Date: Wed, 19 Feb 2025 17:36:59 +0100 Subject: [PATCH 310/310] [algo/jacobian] update doc Co-authored-by: Joris Vaillant --- include/pinocchio/algorithm/jacobian.hxx | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx index 0e5fdec791..0c89708162 100644 --- a/include/pinocchio/algorithm/jacobian.hxx +++ b/include/pinocchio/algorithm/jacobian.hxx @@ -214,8 +214,7 @@ namespace pinocchio v_out = v_in; } - // During the backward pass, mimicking joint jacobian might have been overwritten by the - // mimicked joint (since mimicked_id < mimicking_id) Do another pass to accumulate it back + // Add mimicking joint effect into mimicked column for (Eigen::DenseIndex jExtended = colRefMimicPass; jExtended >= 0; jExtended = data.mimic_parents_fromRow[(size_t)jExtended]) {