Commit ffe8faa7 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #326 from fvalenza/topic/JointComposite

Topic/joint composite
parents 3b7906d9 abfd018a
......@@ -156,6 +156,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint.hpp
multibody/joint/joint-basic-visitors.hpp
multibody/joint/joint-basic-visitors.hxx
multibody/joint/joint-composite.hpp
)
SET(${PROJECT_NAME}_MULTIBODY_HEADERS
......
......@@ -20,7 +20,7 @@
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
namespace se3
{
......
......@@ -225,14 +225,37 @@ namespace se3
ColBlock Jcols = jmodel.jointCols(data.J);
Jcols = data.oMi[i].act(jdata.S());
if( JointModel::NV==1 )
if (JointModel::NV == -1)
{
if( jmodel.nv() ==1 )
{
data.Jcom.col(jmodel.idx_v())
= data.mass[i] * Jcols.template topLeftCorner<3,1>()
- data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
}
else
{
jmodel.jointCols(data.Jcom)
= data.mass[i] * Jcols.template topRows<3>()
- skew(data.com[i]) * Jcols.template bottomRows<3>();
}
}
else
jmodel.jointCols(data.Jcom)
= data.mass[i] * Jcols.template topRows<3>()
- skew(data.com[i]) * Jcols.template bottomRows<3>();
{
if( JointModel::NV ==1 )
{
data.Jcom.col(jmodel.idx_v())
= data.mass[i] * Jcols.template topLeftCorner<3,1>()
- data.com[i].cross(Jcols.template bottomLeftCorner<3,1>()) ;
}
else
{
jmodel.jointCols(data.Jcom)
= data.mass[i] * Jcols.template topRows<3>()
- skew(data.com[i]) * Jcols.template bottomRows<3>();
}
}
if(computeSubtreeComs)
data.com[i] /= data.mass[i];
......
......@@ -161,7 +161,7 @@ namespace se3
const se3::Model & model,
se3::Data & data)
{
typedef typename Data::Matrix6x::NColsBlockXpr<JointModel::NV>::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,11 +169,33 @@ namespace se3
data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
jdata.U() = data.Ycrb[i] * jdata.S();
ColsBlock jF
= data.Ag.middleCols <JointModel::NV> (jmodel.idx_v());
= 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 &
......
......@@ -146,7 +146,6 @@ namespace se3
const Eigen::VectorXd & v,
Eigen::VectorXd & result)
{
jmodel.jointConfigSelector(result) = jmodel.integrate(q, v);
}
......
......@@ -181,6 +181,11 @@ namespace se3
return (m.toActionMatrix()*S).eval();
}
DenseBase se3ActionInverse(const SE3 & m) const
{
return (m.inverse().toActionMatrix()*S).eval();
}
void disp_impl(std::ostream & os) const { os << "S =\n" << S << std::endl;}
protected:
......
......@@ -399,7 +399,9 @@ namespace se3
int idx_v() const { return i_v; }
JointIndex id() const { return i_id; }
void setIndexes(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void setIndexes(JointIndex id, int q, int v) { derived().setIndexes_impl(id, q, v); }
void setIndexes_impl(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
void disp(std::ostream & os) const
{
......
......@@ -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 ); }
......
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terljMj of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_composite_hpp__
#define __se3_joint_composite_hpp__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::SE3)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(se3::Motion)
namespace se3
{
struct JointComposite;
struct JointModelComposite;
struct JointDataComposite;
template<>
struct traits<JointComposite>
{
enum {
NQ = Eigen::Dynamic,
NV = Eigen::Dynamic
};
typedef double Scalar;
typedef JointDataComposite JointDataDerived;
typedef JointModelComposite JointModelDerived;
typedef ConstraintXd Constraint_t;
typedef SE3 Transformation_t;
typedef Motion Motion_t;
typedef Motion Bias_t;
typedef Eigen::Matrix<double,6,Eigen::Dynamic> F_t;
// [ABA]
typedef Eigen::Matrix<double,6,Eigen::Dynamic> U_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> D_t;
typedef Eigen::Matrix<double,6,Eigen::Dynamic> UD_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
};
template<> struct traits<JointDataComposite> { typedef JointComposite JointDerived; };
template<> struct traits<JointModelComposite> { typedef JointComposite JointDerived; };
struct JointDataComposite : public JointDataBase<JointDataComposite>
{
typedef JointComposite Joint;
SE3_JOINT_TYPEDEF;
JointDataVector joints;
int nq_composite,nv_composite;
Constraint_t S;
std::vector<Transformation_t> ljMj;
Transformation_t M;
Motion_t v;
Bias_t c;
// // [ABA] specific data
U_t U;
D_t Dinv;
UD_t UDinv;
// JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
JointDataComposite( JointDataVector & joints, int nq, int nv )
: joints(joints)
, nq_composite(nq)
, nv_composite(nv)
, S(Eigen::MatrixXd::Zero(6, nv_composite))
, ljMj(joints.size())
, M(Transformation_t::Identity())
, v(Motion_t::Zero())
, c(Bias_t::Zero())
, U(Eigen::MatrixXd::Zero(6, nv_composite))
, Dinv(Eigen::MatrixXd::Zero(nv_composite, nv_composite))
, UDinv(Eigen::MatrixXd::Zero(6, nv_composite))
{}
};
struct JointModelComposite : public JointModelBase<JointModelComposite>
{
typedef JointComposite Joint;
SE3_JOINT_TYPEDEF;
SE3_JOINT_USE_INDEXES;
using JointModelBase<JointModelComposite>::id;
using JointModelBase<JointModelComposite>::setIndexes;
std::size_t max_joints;
JointModelVector joints;
int nq_composite,nv_composite;
// Same as JointModelComposite(1)
JointModelComposite() : max_joints(1)
, joints(0)
, nq_composite(0)
, nv_composite(0)
{}
JointModelComposite(std::size_t max_number_of_joints) : max_joints(max_number_of_joints)
, joints(0)
, nq_composite(0)
, nv_composite(0)
{}
template <typename D1>
JointModelComposite(const JointModelBase<D1> & jmodel1) : max_joints(1)
, joints(0)
, nq_composite(jmodel1.nq())
, nv_composite(jmodel1.nv())
{
joints.push_back(JointModel(jmodel1.derived()));
}
template <typename D1, typename D2 >
JointModelComposite(const JointModelBase<D1> & jmodel1, const JointModelBase<D2> & jmodel2)
: max_joints(2)
, joints(0)
, nq_composite(jmodel1.nq() + jmodel2.nq())
, nv_composite(jmodel1.nv() + jmodel2.nv())
{
joints.push_back(JointModel(jmodel1.derived()));
joints.push_back(JointModel(jmodel2.derived()));
}
template <typename D1, typename D2, typename D3 >
JointModelComposite(const JointModelBase<D1> & jmodel1,
const JointModelBase<D2> & jmodel2,
const JointModelBase<D3> & jmodel3)
: max_joints(3)
, joints(0)
, nq_composite(jmodel1.nq() + jmodel2.nq() + jmodel3.nq())
, nv_composite(jmodel1.nv() + jmodel2.nv() + jmodel3.nv())
{
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) {}
template < typename D>
std::size_t addJointModel(const JointModelBase<D> & jmodel)
{
std::size_t nb_joints = joints.size();
if (!isFull())
{
joints.push_back(JointModel(jmodel.derived()));
nq_composite += jmodel.nq();
nv_composite += jmodel.nv();
nb_joints = joints.size();
}
return nb_joints;
}
bool isFull() const
{
return joints.size() == max_joints ;
}
bool isEmpty() const
{
return joints.size() <= 0 ;
}
JointDataDerived createData() const
{
JointDataVector res;
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
res.push_back(::se3::createData(*i));
}
return JointDataDerived(res, nq_composite, nv_composite);
}
void calc (JointDataDerived & data,
const Eigen::VectorXd & qs) const
{
data.M.setIdentity();
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
calc_zero_order(joints[(std::size_t)index], *i, qs);
data.ljMj[(std::size_t)index] = joint_transform(*i);
data.M = data.M * data.ljMj[(std::size_t)index];
}
}
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();
for (JointDataVector::iterator i = data.joints.begin(); i != data.joints.end(); ++i)
{
JointDataVector::iterator::difference_type index = i - data.joints.begin();
calc_first_order(joints[(std::size_t)index], *i, qs, vs);
data.ljMj[(std::size_t)index] = joint_transform(*i);
data.M = data.M * data.ljMj[(std::size_t)index];
data.v = motion(*i);
if (i == data.joints.begin())
{}
else
{
data.v += data.ljMj[(std::size_t)index].actInv(motion(*(i-1)));
}
}
data.c = bias(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();
start_col -= sub_constraint_dimension;
iMcomposite = joint_transform(*(i+0)) * iMcomposite;
data.S.matrix().middleCols(start_col,sub_constraint_dimension) = iMcomposite.actInv(constraint_xd(*(i+0)));
Motion acceleration_d_entrainement_coriolis = Motion::Zero(); // TODO: compute
data.c += iMcomposite.actInv(bias(*i)) + acceleration_d_entrainement_coriolis;
}
}
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();
data.Dinv.setZero();
data.UDinv.setZero();
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);
}
data.U = I * data.S;
Eigen::MatrixXd tmp = data.S.matrix().transpose() * I * data.S.matrix();
data.Dinv = tmp.inverse();
data.UDinv = data.U * data.Dinv;
if (update_I)
I -= data.UDinv * data.U.transpose();
}
Scalar finiteDifferenceIncrement() const
{
using std::sqrt;
return 2.*sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) 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::integrate(*i,qs,vs);
}
return result;
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) 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::interpolate(*i,q0,q1,u);
}
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));
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
result.segment(::se3::idx_q(*i),::se3::nq(*i)) +=
::se3::randomConfiguration(*i,
lower_pos_limit.segment(::se3::idx_q(*i), ::se3::nq(*i)),
upper_pos_limit.segment(::se3::idx_q(*i), ::se3::nq(*i))
);
}
return result;
}
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
TangentVector_t result(Eigen::VectorXd::Zero(nv_composite));
for (JointModelVector::const_iterator i = joints.begin(); i != joints.end(); ++i)
{
result.segment(::se3::idx_v(*i),::se3::nv(*i)) += ::se3::difference(*i,q0,q1);
}
return result;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
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,