Skip to content

GitLab

Verified Commit fa693f2f authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: handle friction and damping parameters when adding joint

parent b7628d41
Hide whitespace changes
Inline Side-by-side
src/multibody/model.hpp
View file @ fa693f2f
......@@ -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
View file @ fa693f2f
......@@ -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>
......
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