Commit 2b24ae72 authored by Valenza Florian's avatar Valenza Florian
Browse files

[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)
parent 62e30593
......@@ -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 = ""
);
///
......
......@@ -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)
......
......@@ -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)
......
......@@ -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);
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment