[C++] Removed default parameters for bounds when adding joint and created an...

[C++] Removed default parameters for bounds when adding joint and created an overloaded method instead. Switched the order for addJoint method (put the default valued argument at last position)

src/multibody/model.hpp

@@ -138,7 +138,7 @@ namespace se3
     ~Model() {} // std::cout << "Destroy model" << std::endl; }
     
     ///
-    /// \brief Add a joint to the kinematic tree.
+    /// \brief Add a joint to the kinematic tree with given bounds.
     ///
     /// \remark This method does not add a Frame of same name to the vector of frames.
     ///         Use Model::addJointFrame.
@@ -150,10 +150,10 @@ namespace se3
     /// \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.
-    /// \param[in] max_effort Maximal joint torque. (Default set to infinity).
-    /// \param[in] max_velocity Maximal joint velocity. (Default set to infinity).
-    /// \param[in] min_config Lower joint configuration. (Default set to infinity).
-    /// \param[in] max_config Upper joint configuration. (Default set to infinity).
+    /// \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.
     ///
@@ -161,11 +161,33 @@ namespace se3
     ///
     template<typename JointModelDerived>
     JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
-                        const std::string & joint_name = "",
-                        const Eigen::VectorXd & max_effort = Eigen::VectorXd::Constant(JointModelDerived::NV,std::numeric_limits<double>::max()),
-                        const Eigen::VectorXd & max_velocity = Eigen::VectorXd::Constant(JointModelDerived::NV,std::numeric_limits<double>::max()),
-                        const Eigen::VectorXd & min_config = Eigen::VectorXd::Constant(JointModelDerived::NQ,std::numeric_limits<double>::min()),
-                        const Eigen::VectorXd & max_config = Eigen::VectorXd::Constant(JointModelDerived::NQ,std::numeric_limits<double>::max())
+                        const Eigen::VectorXd & max_effort,
+                        const Eigen::VectorXd & max_velocity,
+                        const Eigen::VectorXd & min_config,
+                        const Eigen::VectorXd & max_config,
+                        const std::string & joint_name = ""
+                        );
+
+    ///
+    /// \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.
+    ///
+    /// \return The index of the new joint.
+    ///
+    /// \sa Model::appendBodyToJoint
+    ///
+    template<typename JointModelDerived>
+    JointIndex addJoint(const JointIndex parent, const JointModelBase<JointModelDerived> & joint_model, const SE3 & joint_placement,
+                        const std::string & joint_name = ""
                         );
 
     ///

src/multibody/model.hxx

@@ -56,11 +56,11 @@ namespace se3
   Model::JointIndex Model::addJoint(const Model::JointIndex parent,
                                     const JointModelBase<JointModelDerived> & joint_model,
                                     const SE3 & joint_placement,
-                                    const std::string & joint_name,
                                     const Eigen::VectorXd & max_effort,
                                     const Eigen::VectorXd & max_velocity,
                                     const Eigen::VectorXd & min_config,
-                                    const Eigen::VectorXd & max_config
+                                    const Eigen::VectorXd & max_config,
+                                    const std::string & joint_name
                                     )
   {
     typedef JointModelDerived D;
@@ -110,6 +110,33 @@ namespace se3
     return idx;
   }
 
+  template<typename JointModelDerived>
+  Model::JointIndex Model::addJoint(const Model::JointIndex parent,
+                                    const JointModelBase<JointModelDerived> & joint_model,
+                                    const SE3 & joint_placement,
+                                    const std::string & joint_name
+                                    )
+  {
+    typedef JointModelDerived D;
+    Eigen::VectorXd max_effort, max_velocity, min_config, max_config;
+
+    if (D::NV == Eigen::Dynamic)
+    {
+      max_effort = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
+      max_velocity = Eigen::VectorXd::Constant(joint_model.nv(), std::numeric_limits<double>::max());
+      min_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());
+      max_config = Eigen::VectorXd::Constant(joint_model.nq(), std::numeric_limits<double>::max());
+    }
+    else
+    {
+      max_effort = Eigen::VectorXd::Constant(D::NV, std::numeric_limits<double>::max());
+      max_velocity = Eigen::VectorXd::Constant(D::NV, std::numeric_limits<double>::max());
+      min_config = Eigen::VectorXd::Constant(D::NQ, std::numeric_limits<double>::max());
+      max_config = Eigen::VectorXd::Constant(D::NQ, std::numeric_limits<double>::max());
+    }
+    return addJoint(parent, joint_model, joint_placement, max_effort, max_velocity, min_config, max_config, joint_name);
+  }
+  
   inline int Model::addJointFrame (const JointIndex& jidx,
                                          int         fidx)
   {
@@ -123,6 +150,7 @@ namespace se3
     return addFrame(Frame(names[jidx],jidx,fidx,SE3::Identity(),JOINT));
   }
 
+
   inline void Model::appendBodyToJoint(const Model::JointIndex joint_index,
                                        const Inertia & Y,
                                        const SE3 & body_placement)

src/parsers/sample-models.cpp

@@ -39,18 +39,22 @@ namespace se3
       Model::JointIndex idx;
       
       if (setRandomLimits)
+      {
         idx = model.addJoint(model.getJointId(parent_name),joint,
-                             placement, name + "_joint",
-                             TV::Random() + TV::Constant(1),
-                             TV::Random() + TV::Constant(1),
-                             CV::Random() - CV::Constant(1),
-                             CV::Random() + CV::Constant(1));
+                           SE3::Random(),
+                           TV::Random() + TV::Constant(1),
+                           TV::Random() + TV::Constant(1),
+                           CV::Random() - CV::Constant(1),
+                           CV::Random() + CV::Constant(1),
+                           name + "_joint");
+        model.addJointFrame(idx);
+      
+        model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
+        model.addBodyFrame(name + "_body", idx);
+      }
       else
         idx = model.addJoint(model.getJointId(parent_name),joint,
                              placement, name + "_joint");
-      model.addJointFrame(idx);
-      model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
-      model.addBodyFrame(name + "_body", idx);
     }
 
     void humanoid2d(Model & model)

src/parsers/urdf/model.cpp

@@ -98,8 +98,8 @@ namespace se3
         
         idx = model.addJoint(frame.parent,jmodel,
                              frame.placement * joint_placement,
-                             joint_name,
-                             max_effort,max_velocity,min_config,max_config);
+                             max_effort,max_velocity,min_config,max_config,
+                             joint_name);
         FrameIndex jointFrameId = model.addJointFrame(idx, parentFrameId);
         appendBodyToJoint(model, jointFrameId, Y, SE3::Identity(), body_name);
       }