core: handle friction and damping parameters when adding joint

src/multibody/model.hpp

@@ -354,12 +354,11 @@ namespace pinocchio
     ///
     bool operator!=(const ModelTpl & other) const
     { return !(*this == other); }
-    
+
     ///
-    /// \brief Add a joint to the kinematic tree with given bounds.
+    /// \brief Add a joint to the kinematic tree with infinite bounds.
     ///
-    /// \remark This method does not add a Frame of same name to the vector of frames.
-    ///         Use Model::addJointFrame.
+    /// \remark This method also adds a Frame of same name to the vector of frames.
     /// \remark The inertia supported by the joint is set to Zero.
     ///
     /// \tparam JointModelDerived The type of the joint model.
@@ -368,15 +367,24 @@ namespace pinocchio
     /// \param[in] joint_model The joint model.
     /// \param[in] joint_placement Placement of the joint inside its parent joint.
     /// \param[in] joint_name Name of the joint. If empty, the name is random.
+    ///
+    /// \return The index of the new joint.
+    ///
+    /// \sa Model::appendBodyToJoint
+    ///
+    JointIndex addJoint(const JointIndex parent,
+                        const JointModel & joint_model,
+                        const SE3 & joint_placement,
+                        const std::string & joint_name);
+    
+    ///
+    /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &)
+    ///
     /// \param[in] max_effort Maximal joint torque.
     /// \param[in] max_velocity Maximal joint velocity.
     /// \param[in] min_config Lower joint configuration.
     /// \param[in] max_config Upper joint configuration.
     ///
-    /// \return The index of the new joint.
-    ///
-    /// \sa Model::appendBodyToJoint, Model::addJointFrame
-    ///
     JointIndex addJoint(const JointIndex parent,
                         const JointModel & joint_model,
                         const SE3 & joint_placement,
@@ -384,30 +392,24 @@ namespace pinocchio
                         const VectorXs & max_effort,
                         const VectorXs & max_velocity,
                         const VectorXs & min_config,
-                        const VectorXs & max_config
-                        );
-
-    ///
-    /// \brief Add a joint to the kinematic tree with infinite bounds.
-    ///
-    /// \remark This method also adds a Frame of same name to the vector of frames.
-    /// \remark The inertia supported by the joint is set to Zero.
-    ///
-    /// \tparam JointModelDerived The type of the joint model.
-    ///
-    /// \param[in] parent Index of the parent joint.
-    /// \param[in] joint_model The joint model.
-    /// \param[in] joint_placement Placement of the joint inside its parent joint.
-    /// \param[in] joint_name Name of the joint. If empty, the name is random.
+                        const VectorXs & max_config);
+    
     ///
-    /// \return The index of the new joint.
+    /// \copydoc ModelTpl::addJoint(const JointIndex,const JointModel &,const SE3 &,const std::string &,const VectorXs &,const VectorXs &,const VectorXs &,const VectorXs &)
     ///
-    /// \sa Model::appendBodyToJoint
+    /// \param[in] friction Joint friction parameters.
+    /// \param[in] damping Joint damping parameters.
     ///
     JointIndex addJoint(const JointIndex parent,
                         const JointModel & joint_model,
                         const SE3 & joint_placement,
-                        const std::string & joint_name);
+                        const std::string & joint_name,
+                        const VectorXs & max_effort,
+                        const VectorXs & max_velocity,
+                        const VectorXs & min_config,
+                        const VectorXs & max_config,
+                        const VectorXs & friction,
+                        const VectorXs & damping);
 
     ///
     /// \brief Add a joint to the frame tree.

src/multibody/model.hxx

@@ -63,7 +63,9 @@ namespace pinocchio
                                                         const VectorXs & max_effort,
                                                         const VectorXs & max_velocity,
                                                         const VectorXs & min_config,
-                                                        const VectorXs & max_config
+                                                        const VectorXs & max_config,
+                                                        const VectorXs & joint_friction,
+                                                        const VectorXs & joint_damping
                                                         )
   {
     assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size())
@@ -71,11 +73,13 @@ namespace pinocchio
     assert((joint_model.nq()>=0) && (joint_model.nv()>=0));
     assert(joint_model.nq() >= joint_model.nv());
 
-    assert(max_effort.size() == joint_model.nv()
-           && max_velocity.size() == joint_model.nv()
-           && min_config.size() == joint_model.nq()
-           && max_config.size() == joint_model.nq());
-    
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(max_effort.size(),joint_model.nv(),"The joint maximum effort vector is not of right size");
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(max_velocity.size(),joint_model.nv(),"The joint maximum velocity vector is not of right size");
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(min_config.size(),joint_model.nq(),"The joint lower configuration bound is not of right size");
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(max_config.size(),joint_model.nq(),"The joint upper configuration bound is not of right size");
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_friction.size(),joint_model.nv(),"The joint friction vector is not of right size");
+    PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_damping.size(),joint_model.nv(),"The joint damping vector is not of right size");
+
     JointIndex idx = (JointIndex)(njoints++);
     
     joints         .push_back(JointModel(joint_model.derived()));
@@ -114,6 +118,10 @@ namespace pinocchio
       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.
@@ -127,6 +135,26 @@ namespace pinocchio
     
     return idx;
   }
+    
+  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
+  typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
+  ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent,
+                                                        const JointModel & joint_model,
+                                                        const SE3 & joint_placement,
+                                                        const std::string & joint_name,
+                                                        const VectorXs & max_effort,
+                                                        const VectorXs & max_velocity,
+                                                        const VectorXs & min_config,
+                                                        const VectorXs & max_config)
+  {
+    const VectorXs friction = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
+    const VectorXs damping = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
+    
+    return addJoint(parent, joint_model,
+                    joint_placement, joint_name,
+                    max_effort, max_velocity, min_config, max_config,
+                    friction, damping);
+  }
 
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
   typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex
@@ -135,14 +163,13 @@ namespace pinocchio
                                                         const SE3 & joint_placement,
                                                         const std::string & joint_name)
   {
-    VectorXs max_effort, max_velocity, min_config, max_config;
-
-    max_effort = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
-    max_velocity = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
-    min_config = VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max());
-    max_config = VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max());
+    const VectorXs max_effort = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
+    const VectorXs max_velocity = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
+    const VectorXs min_config = VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max());
+    const VectorXs max_config = VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max());
 
-    return addJoint(parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config, max_config);
+    return addJoint(parent, joint_model, joint_placement, joint_name,
+                    max_effort, max_velocity, min_config, max_config);
   }
   
   template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>