diff --git a/src/multibody/model.hpp b/src/multibody/model.hpp
index d13ebf78bc9c8d7b8d5d093f182ebca6bce0b3ab..fd8eb8d482bcd803802358bbd523f3693d01b79b 100644
--- a/src/multibody/model.hpp
+++ b/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 = ""
);
///
diff --git a/src/multibody/model.hxx b/src/multibody/model.hxx
index 1a3d3256c00d7805b8f0581f41cd5ae4452e82b8..5c1154f11a8da5e259a44bd11a030211e8e537f6 100644
--- a/src/multibody/model.hxx
+++ b/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)
diff --git a/src/parsers/sample-models.cpp b/src/parsers/sample-models.cpp
index 052cde20be33617bc74dcc4e9e500bf987ed6b92..aae94b3296785531471078dfcd47f645583cd94e 100644
--- a/src/parsers/sample-models.cpp
+++ b/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)
diff --git a/src/parsers/urdf/model.cpp b/src/parsers/urdf/model.cpp
index 9b72c807d488027f3236adf64090bd85529c49df..23acf3e8b116a0cc9bfd817fd06f588bf03588ee 100644
--- a/src/parsers/urdf/model.cpp
+++ b/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);
}