Commit 62e30593 authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++] Reworked inclusion tree for joints, added visitors + minor fixes

parent e068c552
......@@ -161,7 +161,7 @@ namespace se3
const se3::Model & model,
se3::Data & data)
{
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColsBlock;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Data::Matrix6x>::Type ColsBlock;
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
......@@ -169,19 +169,33 @@ namespace se3
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
jdata.U() = data.Ycrb[i] * jdata.S();
if (JointModel::NV == -1)
{
ColsBlock jF
= data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
}
else
{
ColsBlock jF
ColsBlock jF
= data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
}
forceSet::se3Action(data.oMi[i],jdata.U(),jF);
}
static void algo(const se3::JointModelBase<JointModelComposite> & jmodel,
se3::JointDataBase<JointDataComposite> & jdata,
const se3::Model & model,
se3::Data & data)
{
typedef SizeDepType<JointModel::NV>::ColsReturn<Data::Matrix6x>::Type ColsBlock;
const Model::JointIndex & i = (Model::JointIndex) jmodel.id();
const Model::Index & parent = model.parents[i];
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
jdata.U() = data.Ycrb[i] * jdata.S();
ColsBlock jF
= data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
forceSet::se3Action(data.oMi[i],jdata.U(),jF);
}
}; // struct CcrbaBackwardStep
inline const Data::Matrix6x &
......
......@@ -19,9 +19,9 @@
#define __se3_joint_basic_visitors_hpp__
#include <Eigen/StdVector>
#include <boost/variant.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
namespace se3
{
......@@ -108,6 +108,16 @@ namespace se3
const double u);
/**
* @brief Visit a JointModelVariant through JointRandomVisitor to
* generate a random configuration vector
*
* @param[in] jmodel The JointModelVariant
*
* @return The joint randomconfiguration
*/
inline Eigen::VectorXd random(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointRandomConfigurationVisitor to
* generate a configuration vector uniformly sampled among provided limits
......@@ -343,7 +353,8 @@ namespace se3
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
// Included later
// #include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
#endif // ifndef __se3_joint_basic_visitors_hpp__
......@@ -19,7 +19,8 @@
#define __se3_joint_basic_visitors_hxx__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/visitor.hpp"
namespace se3
......@@ -192,6 +193,25 @@ namespace se3
return JointInterpolateVisitor::run(jmodel, q0, q1, u);
}
/**
* @brief JointRandomVisitor visitor
*/
class JointRandomVisitor: public boost::static_visitor<Eigen::VectorXd>
{
public:
template<typename D>
Eigen::VectorXd operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.random(); }
static Eigen::VectorXd run(const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointRandomVisitor(), jmodel ); }
};
inline Eigen::VectorXd random(const JointModelVariant & jmodel)
{
return JointRandomVisitor::run(jmodel);
}
/**
* @brief JointRandomConfigurationVisitor visitor
*/
......@@ -348,8 +368,8 @@ namespace se3
{
public:
template<typename D>
int operator()(const JointModelBase<D> & ) const
{ return D::NV; }
int operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.nv(); }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointNvVisitor(), jmodel ); }
......@@ -364,8 +384,8 @@ namespace se3
{
public:
template<typename D>
int operator()(const JointModelBase<D> & ) const
{ return D::NQ; }
int operator()(const JointModelBase<D> & jmodel) const
{ return jmodel.nq(); }
static int run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( JointNqVisitor(), jmodel ); }
......
......@@ -19,7 +19,8 @@
#define __se3_joint_composite_hpp__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
// #include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
namespace se3
{
......@@ -36,8 +37,9 @@ namespace se3
NQ = Eigen::Dynamic,
NV = Eigen::Dynamic
};
typedef JointDataComposite JointData;
typedef JointModelComposite JointModel;
typedef double Scalar;
typedef JointDataComposite JointDataDerived;
typedef JointModelComposite JointModelDerived;
typedef ConstraintXd Constraint_t;
typedef SE3 Transformation_t;
typedef Motion Motion_t;
......@@ -52,8 +54,8 @@ namespace se3
typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
};
template<> struct traits<JointDataComposite> { typedef JointComposite Joint; };
template<> struct traits<JointModelComposite> { typedef JointComposite Joint; };
template<> struct traits<JointDataComposite> { typedef JointComposite JointDerived; };
template<> struct traits<JointModelComposite> { typedef JointComposite JointDerived; };
struct JointDataComposite : public JointDataBase<JointDataComposite>
{
......@@ -121,7 +123,7 @@ namespace se3
, nq_composite(jmodel1.nq())
, nv_composite(jmodel1.nv())
{
joints.push_back(jmodel1.derived());
joints.push_back(JointModel(jmodel1.derived()));
}
template <typename D1, typename D2 >
......@@ -131,8 +133,8 @@ namespace se3
, nq_composite(jmodel1.nq() + jmodel2.nq())
, nv_composite(jmodel1.nv() + jmodel2.nv())
{
joints.push_back(jmodel1.derived());
joints.push_back(jmodel2.derived());
joints.push_back(JointModel(jmodel1.derived()));
joints.push_back(JointModel(jmodel2.derived()));
}
template <typename D1, typename D2, typename D3 >
......@@ -144,9 +146,9 @@ namespace se3
, nq_composite(jmodel1.nq() + jmodel2.nq() + jmodel3.nq())
, nv_composite(jmodel1.nv() + jmodel2.nv() + jmodel3.nv())
{
joints.push_back(jmodel1.derived());
joints.push_back(jmodel2.derived());
joints.push_back(jmodel3.derived());
joints.push_back(JointModel(jmodel1.derived()));
joints.push_back(JointModel(jmodel2.derived()));
joints.push_back(JointModel(jmodel3.derived()));
}
// JointModelComposite( const JointModelVector & models ) : max_joints(models.size()) , joints(models) {}
......@@ -157,7 +159,7 @@ namespace se3
std::size_t nb_joints = joints.size();
if (!isFull())
{
joints.push_back(jmodel.derived());
joints.push_back(JointModel(jmodel.derived()));
nq_composite += jmodel.nq();
nv_composite += jmodel.nv();
nb_joints = joints.size();
......@@ -174,18 +176,18 @@ namespace se3
{
return joints.size() <= 0 ;
}
JointData createData() const
JointDataDerived createData() const
{
JointDataVector res;
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
res.push_back(::se3::createData(*i));
}
return JointData(res, nq_composite, nv_composite);
return JointDataDerived(res, nq_composite, nv_composite);
}
void calc (JointData & data,
void calc (JointDataDerived & data,
const Eigen::VectorXd & qs) const
{
data.M.setIdentity();
......@@ -197,24 +199,34 @@ namespace se3
}
}
void calc (JointData & data,
void calc (JointDataDerived & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
data.M.setIdentity();
data.v.setZero();
data.c.setZero();
data.S.matrix().setZero();
std::cout << "Size of joint stack: " << data.joints.size() << std::endl;
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
std::cout << "i = " << index << std::endl;
std::cout << "Index in joint composite : \t" << index << std::endl;
std::cout << "nq of contained joint : \t" << ::se3::nq(joints[(std::size_t)index]) << std::endl;
calc_first_order(joints[(std::size_t)index], *i, qs, vs);
data.M = data.M * joint_transform(*i);
}
SE3 iMcomposite(SE3::Identity());
data.v = motion(data.joints[joints.size()-1]);
data.c = bias(data.joints[joints.size()-1]);
// data.S = constraint_xd(data.joints[joints.size()-1])
int start_col = nv_composite;
int sub_constraint_dimension = (int)constraint_xd(data.joints[joints.size()-1]).matrix().cols();
start_col -= sub_constraint_dimension;
data.S.matrix().middleCols(start_col,sub_constraint_dimension) = constraint_xd(data.joints[joints.size()-1]).matrix();
SE3 iMcomposite(SE3::Identity());
for (JointDataVector::reverse_iterator i = data.joints.rbegin()+1; i != data.joints.rend(); ++i)
{
sub_constraint_dimension = (int)constraint_xd(*i).matrix().cols();
......@@ -224,13 +236,13 @@ namespace se3
data.v += iMcomposite.actInv(motion(*i));
data.S.matrix().middleCols(start_col,sub_constraint_dimension) = iMcomposite.actInv(constraint_xd(*i));
// Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
// data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
}
}
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{
// calc has been called previously in abaforwardstep1 so data.M, data.v are up to date
data.U.setZero();
......@@ -239,7 +251,7 @@ namespace se3
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
::se3::calc_aba(joints[(std::size_t)index], *i, I, false);
::se3::calc_aba(joints[(std::size_t)index], *i, I, false); // HERE MEMORY ACCES VIOLATION WHEN VISITING A JOINT(JOINTMODELCOMPOSITE). SEE UNITTEST JOINT
}
data.U = I * data.S;
Eigen::MatrixXd tmp = data.S.matrix().transpose() * I * data.S.matrix();
......@@ -270,6 +282,16 @@ namespace se3
return result;
}
ConfigVector_t random_impl() const
{
ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::random(*i);
}
return result;
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
{
ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
......@@ -299,6 +321,34 @@ namespace se3
return difference_impl(q0,q1).norm();
}
void normalize_impl(Eigen::VectorXd & q) const
{
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
::se3::normalize(*i,q);
}
}
ConfigVector_t neutralConfiguration_impl() const
{
ConfigVector_t result(Eigen::VectorXd::Zero(nq_composite));
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
result.segment(::se3::idx_q(*i),::se3::nq(*i)) += ::se3::neutralConfiguration(*i);
}
return result;
}
bool isSameConfiguration_impl(const Eigen::VectorXd& q1, const Eigen::VectorXd& q2, const Scalar & = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
if ( !::se3::isSameConfiguration(*i, q1, q2) )
return false;
}
return true;
}
int nv_impl() const { return nv_composite; }
int nq_impl() const { return nq_composite; }
......@@ -320,10 +370,8 @@ namespace se3
}
}
static const std::string shortname()
{
return std::string("JointModelComposite");
}
static std::string classname() { return std::string("JointModelComposite"); }
std::string shortname() const { return classname(); }
template <class D>
bool operator == (const JointModelBase<D> &) const
......@@ -338,8 +386,22 @@ namespace se3
&& jmodel.idx_v() == idx_v();
}
// see http://en.cppreference.com/w/cpp/language/copy_assignment#Copy-and-swap_idiom
void swap(JointModelComposite & other) {
max_joints = other.max_joints;
nq_composite = other.nq_composite;
nv_composite = other.nv_composite;
joints.swap(other.joints);
}
JointModelComposite& operator=(JointModelComposite other)
{
swap(other);
return *this;
}
template<typename D>
template<typename D>
typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
template<typename D>
......@@ -360,25 +422,25 @@ template<typename D>
typename SizeDepType<NV>::template ColsReturn<D>::Type
jointCols(Eigen::MatrixBase<D>& A) const { return A.segment(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_q,nq_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
template<typename D>
typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(i_v,nv_composite); }
};
......
......@@ -482,7 +482,7 @@ namespace se3
const Scalar & q_0 = q0[idx_q()];
const Scalar & q_1 = q1[idx_q()];
ConfigVector_t result;
TangentVector_t result;
result << (q_1 - q_0);
return result;
}
......
......@@ -31,7 +31,6 @@
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
#include <Eigen/StdVector>
#include <boost/variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
......
......@@ -20,7 +20,7 @@
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
namespace se3
{
......
......@@ -26,7 +26,7 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/frame.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
// #include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/deprecated.hh"
#include "pinocchio/tools/string-generator.hpp"
......
......@@ -85,12 +85,12 @@ namespace se3
nq += joint_model.nq();
nv += joint_model.nv();
if (D::NV == -1)
if (D::NV == Eigen::Dynamic)
{
effortLimit.conservativeResize(nv);effortLimit.bottomRows(j.nv()) = max_effort;
velocityLimit.conservativeResize(nv);velocityLimit.bottomRows(j.nv()) = max_velocity;
lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows(j.nq()) = min_config;
upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows(j.nq()) = max_config;
effortLimit.conservativeResize(nv);effortLimit.bottomRows(joint_model.nv()) = max_effort;
velocityLimit.conservativeResize(nv);velocityLimit.bottomRows(joint_model.nv()) = max_velocity;
lowerPositionLimit.conservativeResize(nq);lowerPositionLimit.bottomRows(joint_model.nq()) = min_config;
upperPositionLimit.conservativeResize(nq);upperPositionLimit.bottomRows(joint_model.nq()) = max_config;
}
else
{
......
......@@ -61,6 +61,16 @@ namespace se3
static_cast<const Visitor*>(this)->args));
}
// void operator() (const JointModelBase<JointModelComposite> & jmodel) const
// {
// JointDataVariant& jdataSpec = static_cast<const Visitor*>(this)->jdata;
// bf::invoke(&Visitor::template algo<JointModelComposite>,
// bf::append2(jmodel,
// boost::ref(boost::get<JointDataComposite>(jdataSpec)),
// static_cast<const Visitor*>(this)->args));
// }
template<typename ArgsTmp>
static void run(const JointModelVariant & jmodel,
JointDataVariant & jdata,
......
......@@ -135,6 +135,21 @@ namespace se3
appendBodyToJoint(model, (FrameIndex)fid, Y, SE3::Identity(), body_name);
}
///
/// \brief Handle the case of JointModelComposite which is dynamic.
///
void addJointAndBody(Model & model, const JointModelBase< JointModelComposite > & jmodel, const Model::JointIndex parent_id,
const SE3 & joint_placement, const std::string & joint_name,
const Inertia & Y, const std::string & body_name)
{
Model::JointIndex idx;
idx = model.addJoint(parent_id,jmodel,
joint_placement,joint_name);
model.appendBodyToJoint(idx,Y,SE3::Identity(),body_name);
}
///
/// \brief Recursive procedure for reading the URDF tree.
/// The function returns an exception as soon as a necessary Inertia or Joint information are missing.
......
This diff is collapsed.
......@@ -15,6 +15,7 @@
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
#define BOOST_TEST_DYN_LINK
......@@ -32,8 +33,10 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
q1 = jmodel.random();
q2 = jmodel.random();
q1 = jmodel.random();
q2 = jmodel.random();
std::cout << "Running calc from composite" << std::endl;
jmodel.calc(jdata, q1, q1_dot);
jmodel.calc_aba(jdata, Ia, update_I);
......@@ -42,7 +45,7 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::JointModel jma(jmodel);
se3::JointData jda(jdata);
std::cout << "Running calc from Joint" << std::endl;
jma.calc(jda, q1, q1_dot);
jma.calc_aba(jda, Ia, update_I);
......@@ -93,6 +96,21 @@ struct TestJoint{
// JointModelDense will be removed soon
}
void operator()(const se3::JointModelComposite & ) const
{
// se3::JointModelComposite jmodel(2);
// jmodel.addJointModel(se3::JointModelRX());
// jmodel.addJointModel(se3::JointModelRY());
se3::JointModelComposite jmodel((se3::JointModelRX()), (se3::JointModelRY()));
jmodel.setIndexes(0,0,0);
jmodel.updateComponentsIndexes();
se3::JointModelComposite::JointDataDerived jdata = jmodel.createData();
test_joint_methods(jmodel, jdata);
}
void operator()(const se3::JointModelRevoluteUnaligned & ) const
{
se3::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
......
Supports Markdown
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