diff --git a/src/multibody/joint/joint-base.hpp b/src/multibody/joint/joint-base.hpp index 9a251fad2c0f3e954f09d9c871bf1cce8bbb557d..923b5dff96347ae275d52ec76cd4ac5d21f8231b 100644 --- a/src/multibody/joint/joint-base.hpp +++ b/src/multibody/joint/joint-base.hpp @@ -328,6 +328,15 @@ namespace se3 double distance(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const { return derived().distance_impl(q0, q1); } + /** + * @brief Get neutral configuration of joint + * + * @return The joint's neutral configuration + */ + ConfigVector_t neutralConfiguration() const + { return derived().neutralConfiguration_impl(); } + + 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. diff --git a/src/multibody/joint/joint-basic-visitors.hpp b/src/multibody/joint/joint-basic-visitors.hpp index 1fb040797dc05acdb8e67aefade4243e0d72009d..71e7c95ea95877d94e0d830a20ef9bd7ed773bfd 100644 --- a/src/multibody/joint/joint-basic-visitors.hpp +++ b/src/multibody/joint/joint-basic-visitors.hpp @@ -146,6 +146,16 @@ namespace se3 const Eigen::VectorXd & q1); + /** + * @brief Visit a JointModelVariant through JointNeutralfigurationVisitor to + * get the neutral configuration vector of the joint + * + * @param[in] jmodel The JointModelVariant + * + * @return The joint's neutral configuration + */ + inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel); + /** * @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of * the joint tangent space diff --git a/src/multibody/joint/joint-basic-visitors.hxx b/src/multibody/joint/joint-basic-visitors.hxx index fe490d7cee75df105802fc91359bfa2058654bed..81baa60da86e762b7434fc0219522aec0e025686 100644 --- a/src/multibody/joint/joint-basic-visitors.hxx +++ b/src/multibody/joint/joint-basic-visitors.hxx @@ -212,6 +212,27 @@ namespace se3 return JointRandomConfigurationVisitor::run(jmodel, lower_pos_limit, upper_pos_limit); } + + /** + * @brief JointNeutralConfigurationVisitor visitor + */ + class JointNeutralConfigurationVisitor: public boost::static_visitor<Eigen::VectorXd> + { + public: + + template<typename D> + Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const + { return jmodel.neutralConfiguration(); } + + static Eigen::VectorXd run(const JointModelVariant & jmodel) + { return boost::apply_visitor( JointNeutralConfigurationVisitor(), jmodel ); } + }; + inline Eigen::VectorXd neutralConfiguration(const JointModelVariant & jmodel) + { + return JointNeutralConfigurationVisitor::run(jmodel); + } + + /** * @brief JointDifferenceVisitor visitor */ diff --git a/src/multibody/joint/joint-dense.hpp b/src/multibody/joint/joint-dense.hpp index cc53c0070b672c78f425fa191e9c0f8852af25f2..0aa8dddca842f7fad8b46b5ab82a3c8516b24380 100644 --- a/src/multibody/joint/joint-dense.hpp +++ b/src/multibody/joint/joint-dense.hpp @@ -190,6 +190,13 @@ namespace se3 return result; } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t result; + assert(false && "JointModelDense is read-only, should not perform any calc"); + return result; + } + JointModelDense() {} JointModelDense(JointIndex idx, int idx_q, int idx_v) { diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp index 69c4b8d0f2ea804d3ba4d28cddedb4b74112e793..1bd55f38769bb5832358f79b1b46417d1063f9e2 100644 --- a/src/multibody/joint/joint-free-flyer.hpp +++ b/src/multibody/joint/joint-free-flyer.hpp @@ -366,6 +366,14 @@ namespace se3 return difference_impl(q0,q1).norm(); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0, 0, 0, // translation part + 0, 0, 0, 1; // quaternion part + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint-planar.hpp b/src/multibody/joint/joint-planar.hpp index a7835c05d33aee34504fde485e8331f4d1ad8974..54ed3755393cc142482d8075a5e41da538903811 100644 --- a/src/multibody/joint/joint-planar.hpp +++ b/src/multibody/joint/joint-planar.hpp @@ -428,6 +428,13 @@ namespace se3 return (q_1-q_0).norm(); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0, 0, 0; + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint-prismatic-unaligned.hpp b/src/multibody/joint/joint-prismatic-unaligned.hpp index 45f7cb5c9c559ce2cf8cf0a63f76d382185ed84f..1ea16047ecf5e37ac28a329f7461fd558263f61a 100644 --- a/src/multibody/joint/joint-prismatic-unaligned.hpp +++ b/src/multibody/joint/joint-prismatic-unaligned.hpp @@ -433,6 +433,12 @@ namespace se3 return (q_1-q_0); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0; + return q; + } JointModelDense<NQ, NV> toDense_impl() const { diff --git a/src/multibody/joint/joint-prismatic.hpp b/src/multibody/joint/joint-prismatic.hpp index f6e1ee47eb9caa64a01f6c2202919ef5ced7c39d..c8106901833f8ad512c18688742983c5bafad30d 100644 --- a/src/multibody/joint/joint-prismatic.hpp +++ b/src/multibody/joint/joint-prismatic.hpp @@ -489,6 +489,13 @@ namespace se3 return (q_1-q_0); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0; + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint-revolute-unaligned.hpp b/src/multibody/joint/joint-revolute-unaligned.hpp index 808323bade45ad98fd3e410f7cefa020d0ca2a3d..3351142b99f6149d464cb62f2f06be1c995808d9 100644 --- a/src/multibody/joint/joint-revolute-unaligned.hpp +++ b/src/multibody/joint/joint-revolute-unaligned.hpp @@ -438,6 +438,13 @@ namespace se3 return (q_1-q_0); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0; + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint-revolute.hpp b/src/multibody/joint/joint-revolute.hpp index 52a216e68b627093ed2132a542a13ccd4c8b85b8..6c5f478f2a4dac724fbd48bd2c69f7940094e501 100644 --- a/src/multibody/joint/joint-revolute.hpp +++ b/src/multibody/joint/joint-revolute.hpp @@ -523,6 +523,13 @@ namespace se3 return (q_1-q_0); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0; + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint-spherical-ZYX.hpp b/src/multibody/joint/joint-spherical-ZYX.hpp index e57ca6db051ad13e9c6036beaebba36abccbce68..75f37db9c7abb35f1684ba8616eb7e2597e74cb2 100644 --- a/src/multibody/joint/joint-spherical-ZYX.hpp +++ b/src/multibody/joint/joint-spherical-ZYX.hpp @@ -455,6 +455,13 @@ namespace se3 return (q_1 - q_0).norm(); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0, 0, 0; + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp index c3acc3fad6dcb30d4a29075057410d70a495d873..c7aea07372e6d7f5ac4ce6d81892dbd7c53655ca 100644 --- a/src/multibody/joint/joint-spherical.hpp +++ b/src/multibody/joint/joint-spherical.hpp @@ -392,6 +392,13 @@ namespace se3 return difference_impl(q0, q1).norm(); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0, 0, 0, 1; + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint-translation.hpp b/src/multibody/joint/joint-translation.hpp index 61f0292a2bfb4a9286409c2246fe0077b3fd981a..17fb662c8d65de05036655cc5894686b2a0476e5 100644 --- a/src/multibody/joint/joint-translation.hpp +++ b/src/multibody/joint/joint-translation.hpp @@ -367,6 +367,13 @@ namespace se3 return (q_1 - q_0).norm(); } + ConfigVector_t neutralConfiguration_impl() const + { + ConfigVector_t q; + q << 0,0,0; + return q; + } + JointModelDense<NQ, NV> toDense_impl() const { return JointModelDense<NQ, NV>( id(), diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp index fca32b2b175e7879bfab40a2c27f42e4240c9873..1740fc2a72e9e67acb5da7bf12efdf0d73810054 100644 --- a/src/multibody/joint/joint.hpp +++ b/src/multibody/joint/joint.hpp @@ -146,6 +146,11 @@ namespace se3 return ::se3::distance(*this, q0, q1); } + ConfigVector_t neutralConfiguration_impl() const + { + return ::se3::neutralConfiguration(*this); + } + JointModel() : JointModelBoostVariant() {} JointModel( const JointModelVariant & model_variant ) : JointModelBoostVariant(model_variant) {}