Skip to content
GitLab
Unverified Commit aa39638f authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1304 from jcarpent/topic/devel 

Add support of joint friction and dampings
parents 460162f6 82d7e083
Pipeline #11544 passed with stage
in 161 minutes and 52 seconds
Hide whitespace changes
Inline Side-by-side
bindings/python/multibody/joint/expose-joints.cpp
View file @ aa39638f
......@@ -16,11 +16,11 @@ namespace pinocchio
static void exposeVariants()
{
boost::mpl::for_each<JointModelVariant::types>(ModelExposer());
boost::mpl::for_each<JointModelVariant::types>(JointModelExposer());
bp::to_python_converter<pinocchio::JointModelVariant,
JointVariantVisitor<pinocchio::JointModelVariant > >();
boost::mpl::for_each<JointDataVariant::types>(DataExposer());
boost::mpl::for_each<JointDataVariant::types>(JointDataExposer());
bp::to_python_converter<pinocchio::JointDataVariant,
JointVariantVisitor<pinocchio::JointDataVariant > >();
}
......
bindings/python/multibody/joint/joint-derived.hpp
View file @ aa39638f
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_python_joint_dense_hpp__
#define __pinocchio_python_joint_dense_hpp__
#ifndef __pinocchio_python_joint_variants_hpp__
#define __pinocchio_python_joint_variants_hpp__
#include <boost/python.hpp>
#include <eigenpy/exception.hpp>
......@@ -27,27 +27,29 @@ namespace pinocchio
{
cl
// All are add_properties cause ReadOnly
.add_property("id",&JointModelDerivedPythonVisitor::getId)
.add_property("idx_q",&JointModelDerivedPythonVisitor::getIdx_q)
.add_property("idx_v",&JointModelDerivedPythonVisitor::getIdx_v)
.add_property("nq",&JointModelDerivedPythonVisitor::getNq)
.add_property("nv",&JointModelDerivedPythonVisitor::getNv)
.add_property("id",&get_id)
.add_property("idx_q",&get_idx_q)
.add_property("idx_v",&get_idx_v)
.add_property("nq",&get_nq)
.add_property("nv",&get_nv)
.def("setIndexes",&JointModelDerived::setIndexes)
.def("shortname",&JointModelDerived::shortname)
;
}
static JointIndex getId( const JointModelDerived & self ) { return self.id(); }
static int getIdx_q(const JointModelDerived & self) {return self.idx_q();}
static int getIdx_v(const JointModelDerived & self) {return self.idx_v();}
static int getNq(const JointModelDerived & self) {return self.nq();}
static int getNv(const JointModelDerived & self) {return self.nv();}
static JointIndex get_id(const JointModelDerived & self)
{ return self.id(); }
static int get_idx_q(const JointModelDerived & self)
{ return self.idx_q(); }
static int get_idx_v(const JointModelDerived & self)
{ return self.idx_v(); }
static int get_nq(const JointModelDerived & self)
{ return self.nq(); }
static int get_nv(const JointModelDerived & self)
{ return self.nv(); }
static void expose()
{
}
{}
};
......@@ -62,42 +64,37 @@ namespace pinocchio
{
cl
// All are add_properties cause ReadOnly
.add_property("S",&JointDataDerivedPythonVisitor::getS)
.add_property("M",&JointDataDerivedPythonVisitor::getM)
.add_property("v",&JointDataDerivedPythonVisitor::getv)
.add_property("c",&JointDataDerivedPythonVisitor::getc)
.add_property("U",&JointDataDerivedPythonVisitor::getU)
.add_property("Dinv",&JointDataDerivedPythonVisitor::getDinv)
.add_property("UDinv",&JointDataDerivedPythonVisitor::getUDinv)
.add_property("S",&get_S)
.add_property("M",&get_M)
.add_property("v",&get_v)
.add_property("c",&get_c)
.add_property("U",&get_U)
.add_property("Dinv",&get_Dinv)
.add_property("UDinv",&get_UDinv)
.def("shortname",&JointDataDerived::shortname)
;
}
static typename JointDataDerived::Constraint_t getS(const JointDataDerived & self )
static typename JointDataDerived::Constraint_t get_S(const JointDataDerived & self)
{ return self.S_accessor(); }
static typename JointDataDerived::Transformation_t getM(const JointDataDerived & self )
static typename JointDataDerived::Transformation_t get_M(const JointDataDerived & self)
{ return self.M_accessor(); }
static typename JointDataDerived::Motion_t getv(const JointDataDerived & self )
static typename JointDataDerived::Motion_t get_v(const JointDataDerived & self)
{ return self.v_accessor(); }
static typename JointDataDerived::Bias_t getc(const JointDataDerived & self )
static typename JointDataDerived::Bias_t get_c(const JointDataDerived & self)
{ return self.c_accessor(); }
static typename JointDataDerived::U_t getU(const JointDataDerived & self )
static typename JointDataDerived::U_t get_U(const JointDataDerived & self)
{ return self.U_accessor(); }
static typename JointDataDerived::D_t getDinv(const JointDataDerived & self )
static typename JointDataDerived::D_t get_Dinv(const JointDataDerived & self)
{ return self.Dinv_accessor(); }
static typename JointDataDerived::UD_t getUDinv(const JointDataDerived & self )
static typename JointDataDerived::UD_t get_UDinv(const JointDataDerived & self)
{ return self.UDinv_accessor(); }
static void expose()
{
}
{}
};
}} // namespace pinocchio::python
#endif // ifndef __pinocchio_python_joint_dense_hpp__
#endif // ifndef __pinocchio_python_joint_variants_hpp__
bindings/python/multibody/joint/joints-variant.hpp
View file @ aa39638f
......@@ -33,7 +33,7 @@ namespace pinocchio
}
};
struct DataExposer
struct JointDataExposer
{
template<class T>
void operator()(T)
......@@ -49,7 +49,7 @@ namespace pinocchio
}
};
struct ModelExposer
struct JointModelExposer
{
template<class T>
void operator()(T)
......
bindings/python/multibody/model.hpp
View file @ aa39638f
......@@ -97,7 +97,7 @@ namespace pinocchio
protected:
struct addJointVisitor
struct addJointVisitor0
: public boost::static_visitor<Index>
{
Model & m_model;
......@@ -105,10 +105,10 @@ namespace pinocchio
const SE3 & m_joint_placement;
const std::string & m_joint_name;
addJointVisitor(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name)
addJointVisitor0(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name)
: m_model(model)
, m_parent_id(parent_id)
, m_joint_placement(joint_placement)
......@@ -120,21 +120,22 @@ namespace pinocchio
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name);
}
}; // struct addJointVisitor
}; // struct addJointVisitor0
struct addJointWithLimitsVisitor
: public boost::static_visitor<Index>
struct addJointVisitor1
: public addJointVisitor0
{
Model & m_model;
const JointIndex m_parent_id;
const SE3 & m_joint_placement;
const std::string & m_joint_name;
using addJointVisitor0::m_model;
using addJointVisitor0::m_parent_id;
using addJointVisitor0::m_joint_placement;
using addJointVisitor0::m_joint_name;
const VectorXs & m_max_effort;
const VectorXs & m_max_velocity;
const VectorXs & m_min_config;
const VectorXs & m_max_config;
addJointWithLimitsVisitor(Model & model,
addJointVisitor1(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
......@@ -142,10 +143,7 @@ namespace pinocchio
const VectorXs & max_velocity,
const VectorXs & min_config,
const VectorXs & max_config)
: m_model(model)
, m_parent_id(parent_id)
, m_joint_placement(joint_placement)
, m_joint_name(joint_name)
: addJointVisitor0(model,parent_id,joint_placement,joint_name)
, m_max_effort(max_effort)
, m_max_velocity(max_velocity)
, m_min_config(min_config)
......@@ -157,7 +155,45 @@ namespace pinocchio
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name,m_max_effort,m_max_velocity,m_min_config,m_max_config);
}
}; // struct addJointWithLimitsVisitor
}; // struct addJointVisitor1
struct addJointVisitor2
: public addJointVisitor1
{
using addJointVisitor1::m_model;
using addJointVisitor1::m_parent_id;
using addJointVisitor1::m_joint_placement;
using addJointVisitor1::m_joint_name;
using addJointVisitor1::m_max_effort;
using addJointVisitor1::m_max_velocity;
using addJointVisitor1::m_min_config;
using addJointVisitor1::m_max_config;
const VectorXs & m_friction;
const VectorXs & m_damping;
addJointVisitor2(Model & model,
const JointIndex parent_id,
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,
const VectorXs & damping)
: addJointVisitor1(model,parent_id,joint_placement,joint_name,
max_effort,max_velocity,min_config,max_config)
, m_friction(friction)
, m_damping(damping)
{}
template <typename JointModelDerived>
JointIndex operator()(JointModelDerived & jmodel) const
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name,m_max_effort,m_max_velocity,m_min_config,m_max_config,m_friction,m_damping);
}
}; // struct addJointVisitor1
public:
......@@ -191,6 +227,10 @@ namespace pinocchio
"Vector of rotor inertia parameters.")
.def_readwrite("rotorGearRatio",&Model::rotorGearRatio,
"Vector of rotor gear ratio parameters.")
.def_readwrite("friction",&Model::friction,
"Vector of joint friction parameters.")
.def_readwrite("damping",&Model::damping,
"Vector of joint damping parameters.")
.def_readwrite("effortLimit",&Model::effortLimit,
"Joint max effort.")
.def_readwrite("velocityLimit",&Model::velocityLimit,
......@@ -216,13 +256,21 @@ namespace pinocchio
"Motion vector corresponding to the gravity field expressed in the world Frame.")
// Class Methods
.def("addJoint",&ModelPythonVisitor::addJoint,
.def("addJoint",&ModelPythonVisitor::addJoint0,
bp::args("self","parent_id","joint_model","joint_placement","joint_name"),
"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
.def("addJoint",&ModelPythonVisitor::addJointWithLimits,
.def("addJoint",&ModelPythonVisitor::addJoint1,
bp::args("self","parent_id","joint_model","joint_placement","joint_name",
"max_effort","max_velocity","min_config","max_config"),
"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.")
"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name."
"This signature also takes as input effort, velocity limits as well as the bounds on the joint configuration.")
.def("addJoint",&ModelPythonVisitor::addJoint2,
bp::args("self","parent_id","joint_model","joint_placement","joint_name",
"max_effort","max_velocity","min_config","max_config",
"friction","damping"),
"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.\n"
"This signature also takes as input effort, velocity limits as well as the bounds on the joint configuration.\n"
"The user should also provide the friction and damping related to the joint.")
.def("addJointFrame", &Model::addJointFrame,
addJointFrame_overload(bp::args("self","joint_id", "frame_id"),
"Add the joint provided by its joint_id as a frame to the frame tree.\n"
......@@ -255,28 +303,44 @@ namespace pinocchio
;
}
static JointIndex addJoint(Model & model,
JointIndex parent_id,
bp::object jmodel,
const SE3 & joint_placement,
const std::string & joint_name)
static JointIndex addJoint0(Model & model,
JointIndex parent_id,
bp::object jmodel,
const SE3 & joint_placement,
const std::string & joint_name)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointVisitor(model,parent_id,joint_placement,joint_name), jmodel_variant);
return boost::apply_visitor(addJointVisitor0(model,parent_id,joint_placement,joint_name), jmodel_variant);
}
static JointIndex addJointWithLimits(Model & model,
JointIndex parent_id,
bp::object jmodel,
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)
static JointIndex addJoint1(Model & model,
JointIndex parent_id,
bp::object jmodel,
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)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointVisitor1(model,parent_id,joint_placement,joint_name,max_effort,max_velocity,min_config,max_config), jmodel_variant);
}
static JointIndex addJoint2(Model & model,
JointIndex parent_id,
bp::object jmodel,
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,
const VectorXs & damping)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointWithLimitsVisitor(model,parent_id,joint_placement,joint_name,max_effort,max_velocity,min_config,max_config), jmodel_variant);
return boost::apply_visitor(addJointVisitor2(model,parent_id,joint_placement,joint_name,max_effort,max_velocity,min_config,max_config,friction,damping), jmodel_variant);
}
static Data createData(const Model & model) { return Data(model); }
......
src/algorithm/model.hxx
View file @ aa39638f
......@@ -97,44 +97,51 @@ namespace pinocchio
GeometryModel& > ArgsType;
template <typename JointModel>
static void algo (const JointModelBase<JointModel> & jmodel,
static void algo (const JointModelBase<JointModel> & jmodel_in,
const Model & modelAB,
const GeometryModel & geomModelAB,
JointIndex parentId,
JointIndex parent_id,
const typename Model::SE3 & pMi,
Model & model,
GeometryModel & geomModel)
{
// If old parent is universe, use what's provided in the input.
// otherwise, get the parent from modelAB.
if (modelAB.parents[jmodel.id()] > 0)
parentId = model.getJointId(modelAB.names[modelAB.parents[jmodel.id()]]);
const JointIndex joint_id_in = jmodel_in.id();
if (modelAB.parents[joint_id_in] > 0)
parent_id = model.getJointId(modelAB.names[modelAB.parents[joint_id_in]]);
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existJointName(modelAB.names[jmodel.id()]),
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existJointName(modelAB.names[joint_id_in]),
"The two models have conflicting joint names.");
JointIndex jid = model.addJoint (
parentId,
jmodel,
pMi * modelAB.jointPlacements[jmodel.id()],
modelAB.names[jmodel.id()],
jmodel.jointVelocitySelector(modelAB.effortLimit),
jmodel.jointVelocitySelector(modelAB.velocityLimit),
jmodel.jointConfigSelector(modelAB.lowerPositionLimit),
jmodel.jointConfigSelector(modelAB.upperPositionLimit));
assert (jid < model.joints.size());
JointIndex joint_id_out = model.addJoint(parent_id,
jmodel_in,
pMi * modelAB.jointPlacements[joint_id_in],
modelAB.names[joint_id_in],
jmodel_in.jointVelocitySelector(modelAB.effortLimit),
jmodel_in.jointVelocitySelector(modelAB.velocityLimit),
jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit),
jmodel_in.jointConfigSelector(modelAB.upperPositionLimit),
jmodel_in.jointVelocitySelector(modelAB.friction),
jmodel_in.jointVelocitySelector(modelAB.damping));
assert(joint_id_out < model.joints.size());
model.appendBodyToJoint (jid, modelAB.inertias[jmodel.id()]);
model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]);
const typename Model::JointModel & jmodel_out = model.joints[joint_id_out];
jmodel_out.jointVelocitySelector(model.rotorInertia) = jmodel_in.jointVelocitySelector(modelAB.rotorInertia);
jmodel_out.jointVelocitySelector(model.rotorGearRatio) = jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio);
// Add all frames whose parent is this joint.
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid) {
for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)
{
Frame frame = modelAB.frames[fid];
if (frame.parent == jmodel.id())
if (frame.parent == jmodel_in.id())
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existFrame(frame.name, frame.type),
"The two models have conflicting frame names.");
frame.parent = jid;
frame.parent = joint_id_out;
assert (frame.previousFrame > 0 || frame.type == JOINT);
if (frame.previousFrame != 0)
{
......@@ -146,19 +153,19 @@ namespace pinocchio
}
}
// Add all geometries whose parent is this joint.
for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)
for(GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)
{
GeometryObject go = geomModelAB.geometryObjects[gid];
if (go.parentJoint == jmodel.id())
if(go.parentJoint == joint_id_in)
{
go.parentJoint = jid;
assert (go.parentFrame > 0);
if (go.parentFrame != 0)
go.parentJoint = joint_id_out;
assert(go.parentFrame > 0);
if(go.parentFrame != 0)
{
go.parentFrame = model.getFrameId(modelAB.frames[go.parentFrame].name,
modelAB.frames[go.parentFrame].type);
}
geomModel.addGeometryObject (go);
geomModel.addGeometryObject(go);
}
}
}
......@@ -384,11 +391,20 @@ namespace pinocchio
joint_input_model.jointVelocitySelector(input_model.effortLimit),
joint_input_model.jointVelocitySelector(input_model.velocityLimit),
joint_input_model.jointConfigSelector(input_model.lowerPositionLimit),
joint_input_model.jointConfigSelector(input_model.upperPositionLimit));
joint_input_model.jointConfigSelector(input_model.upperPositionLimit),
joint_input_model.jointVelocitySelector(input_model.friction),
joint_input_model.jointVelocitySelector(input_model.damping));
// Append inertia
reduced_model.appendBodyToJoint(reduced_joint_id,
input_model.inertias[joint_id],
SE3::Identity());
// Copy other kinematics and dynamics properties
const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id];
jmodel_out.jointVelocitySelector(reduced_model.rotorInertia)
= joint_input_model.jointVelocitySelector(input_model.rotorInertia);
jmodel_out.jointVelocitySelector(reduced_model.rotorGearRatio)
= joint_input_model.jointVelocitySelector(input_model.rotorGearRatio);
}
}
......
src/multibody/model.hpp
View file @ aa39638f
......@@ -65,6 +65,7 @@ namespace pinocchio
/// \brief Dense vectorized version of a joint configuration vector.
typedef VectorXs ConfigVectorType;
/// \brief Map between a string (key) and a configuration vector
typedef std::map<std::string, ConfigVectorType> ConfigVectorMap;
/// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc).
......@@ -117,11 +118,17 @@ namespace pinocchio
ConfigVectorMap referenceConfigurations;
/// \brief Vector of rotor inertia parameters
VectorXs rotorInertia;
TangentVectorType rotorInertia;
/// \brief Vector of rotor gear ratio parameters
VectorXs rotorGearRatio;
TangentVectorType rotorGearRatio;
/// \brief Vector of joint friction parameters
TangentVectorType friction;
/// \brief Vector of joint damping parameters
TangentVectorType damping;
/// \brief Vector of maximal joint torques
TangentVectorType effortLimit;
......@@ -129,10 +136,10 @@ namespace pinocchio
TangentVectorType velocityLimit;
/// \brief Lower joint configuration limit
VectorXs lowerPositionLimit;
ConfigVectorType lowerPositionLimit;
/// \brief Upper joint configuration limit
VectorXs upperPositionLimit;
ConfigVectorType upperPositionLimit;
/// \brief Vector of operational frames registered on the model.
FrameVector frames;
......@@ -208,6 +215,8 @@ namespace pinocchio
// Eigen Vectors
res.rotorInertia = rotorInertia.template cast<NewScalar>();
res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
res.friction = friction.template cast<NewScalar>();
res.damping = damping.template cast<NewScalar>();
res.effortLimit = effortLimit.template cast<NewScalar>();
res.velocityLimit = velocityLimit.template cast<NewScalar>();
res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
......@@ -287,6 +296,16 @@ namespace pinocchio
res &= other.rotorInertia == rotorInertia;
if(!res) return res;
if(other.friction.size() != friction.size())
return false;
res &= other.friction == friction;
if(!res) return res;
if(other.damping.size() != damping.size())
return false;
res &= other.damping == damping;
if(!res) return res;
if(other.rotorGearRatio.size() != rotorGearRatio.size())
return false;
res &= other.rotorGearRatio == rotorGearRatio;
......@@ -336,12 +355,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.
......@@ -350,15 +368,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,
......@@ -366,30 +393,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.