Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/joint/joint-composite.hpp	Lines:	122	153	79.7 %
Date:	2024-04-26 13:14:21	Branches:	108	214	50.5 %


//
// Copyright (c) 2016-2021 CNRS INRIA
//

#ifndef __pinocchio_joint_composite_hpp__
#define __pinocchio_joint_composite_hpp__

#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/spatial/act-on-set.hpp"

#include "pinocchio/serialization/fwd.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct JointCompositeTpl;

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
  {
    typedef _Scalar Scalar;

    enum {
      Options = _Options,
      NQ = Eigen::Dynamic,
      NV = Eigen::Dynamic
    };

    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
    typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
    typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options> Motion_t;
    typedef MotionTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
  };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
  { typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointDataCompositeTpl
  : public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointDataBase<JointDataCompositeTpl> Base;
    typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataVariant;

    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector;

    // JointDataComposite()  {} // can become necessary if we want a vector of JointDataComposite ?

    JointDataCompositeTpl()
    : joints()
    , iMlast(0)
    , pjMi(0)
    , S(0)
    , M(Transformation_t::Identity())
    , v(Motion_t::Zero())
    , c(Motion_t::Zero())
    , U(6,0), Dinv(0,0), UDinv(6,0)
    , StU(0,0)
    {}


    JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
    : joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
    , S(Constraint_t::Zero(nv))
    , M(Transformation_t::Identity())
    , v(Motion_t::Zero())
    , c(Motion_t::Zero())
    , U(U_t::Zero(6,nv))
    , Dinv(D_t::Zero(nv,nv))
    , UDinv(UD_t::Zero(6,nv))
    , StU(D_t::Zero(nv,nv))
    {}

    /// \brief Vector of joints
    JointDataVector joints;

    /// \brief Transforms from previous joint to last joint
    PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast;

    /// \brief Transforms from previous joint to joint i
    PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi;

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    D_t StU;

    static std::string classname() { return std::string("JointDataComposite"); }
    std::string shortname() const { return classname(); }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::ostream & operator <<(std::ostream & os, const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
  {
    typedef typename JointDataCompositeTpl<Scalar,Options,JointCollectionTpl>::JointDataVector JointDataVector;

    os << "JointDataComposite containing following models:\n" ;
    for (typename JointDataVector::const_iterator it = jdata.joints.begin();
         it != jdata.joints.end(); ++it)
      os << "  " << shortname(*it) << std::endl;
    return os;
  }


  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct CastType< NewScalar, JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  {
    typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> type;
  };

  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
  struct JointModelCompositeTpl
  : public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointModelBase<JointModelCompositeTpl> Base;
    typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
    typedef JointModel JointModelVariant;

    typedef SE3Tpl<Scalar,Options> SE3;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef InertiaTpl<Scalar,Options> Inertia;

    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;

    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;
    using Base::nq;
    using Base::nv;

    /// \brief Default contructor
    JointModelCompositeTpl()
    : joints()
    , jointPlacements()
    , m_nq(0)
    , m_nv(0)
    , njoints(0)
    {}

    /// \brief Default contructor with a defined size
    JointModelCompositeTpl(const size_t size)
    : joints()
    , jointPlacements()
    , m_nq(0)
    , m_nv(0)
    , njoints(0)
    {
      joints.reserve(size); jointPlacements.reserve(size);
      m_idx_q.reserve(size); m_idx_v.reserve(size);
      m_nqs.reserve(size); m_nvs.reserve(size);
    }

    ///
    /// \brief Constructor with one joint.
    ///
    /// \param jmodel Model of the first joint.
    /// \param placement Placement of the first joint w.r.t. the joint origin.
    ///
    template<typename JointModel>
    JointModelCompositeTpl(const JointModelBase<JointModel> & jmodel,
                           const SE3 & placement = SE3::Identity())
    : joints(1,(JointModelVariant)jmodel.derived())
    , jointPlacements(1,placement)
    , m_nq(jmodel.nq())
    , m_nv(jmodel.nv())
    , m_idx_q(1,0), m_nqs(1,jmodel.nq())
    , m_idx_v(1,0), m_nvs(1,jmodel.nv())
    , njoints(1)
    {}

    ///
    /// \brief Copy constructor.
    ///
    /// \param other JointModel to copy.
    ///
    JointModelCompositeTpl(const JointModelCompositeTpl & other)
    : Base(other)
    , joints(other.joints)
    , jointPlacements(other.jointPlacements)
    , m_nq(other.m_nq)
    , m_nv(other.m_nv)
    , m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
    , m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
    , njoints(other.njoints)
    {}


    ///
    /// \brief Add a joint to the vector of joints.
    ///
    /// \param jmodel Model of the joint to add.
    /// \param placement Placement of the joint relatively to its predecessor.
    ///
    /// \return A reference to *this
    ///
    template<typename JointModel>
    JointModelDerived & addJoint(const JointModelBase<JointModel> & jmodel,
                                 const SE3 & placement = SE3::Identity())
    {
      joints.push_back((JointModelVariant)jmodel.derived());
      jointPlacements.push_back(placement);

      m_nq += jmodel.nq(); m_nv += jmodel.nv();

      updateJointIndexes();
      njoints++;

      return *this;
    }

    JointDataDerived createData() const
    {
      typename JointDataDerived::JointDataVector jdata(joints.size());
      for (int i = 0; i < (int)joints.size(); ++i)
        jdata[(size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
      return JointDataDerived(jdata,nq(),nv());
    }

    const std::vector<bool> hasConfigurationLimit() const
    {
      std::vector<bool> vec;
      for (size_t i = 0; i < joints.size(); ++i)
        {
          const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimit();
          vec.insert(vec.end(),
                    joint_cf_limit.begin(),
                    joint_cf_limit.end());
        }
      return vec;
    }

    const std::vector<bool> hasConfigurationLimitInTangent() const
    {
      std::vector<bool> vec;
      for (size_t i = 0; i < joints.size(); ++i)
        {
          const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimitInTangent();
          vec.insert(vec.end(),
                    joint_cf_limit.begin(),
                    joint_cf_limit.end());
        }
      return vec;
    }

    template<typename, int, template<typename S, int O> class, typename>
    friend struct JointCompositeCalcZeroOrderStep;

    template<typename ConfigVectorType>
    void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;

    template<typename, int, template<typename S, int O> class, typename, typename>
    friend struct JointCompositeCalcFirstOrderStep;

    template<typename ConfigVectorType, typename TangentVectorType>
    void calc(JointDataDerived & data,
              const Eigen::MatrixBase<ConfigVectorType> & qs,
              const Eigen::MatrixBase<TangentVectorType> & vs) const;

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U.noalias() = I * data.S.matrix();
      data.StU.noalias() = data.S.matrix().transpose() * data.U;

      // compute inverse
//      data.Dinv.setIdentity();
//      data.StU.llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
      data.UDinv.noalias() = data.U * data.Dinv;

      if (update_I)
        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
    }

    int nv_impl() const { return m_nv; }
    int nq_impl() const { return m_nq; }

    /**
     * @brief      Update the indexes of subjoints in the stack
     */
    void setIndexes_impl(JointIndex id, int q, int v)
    {
      Base::setIndexes_impl(id, q, v);
      updateJointIndexes();
    }

    static std::string classname() { return std::string("JointModelComposite"); }
    std::string shortname() const { return classname(); }

    JointModelCompositeTpl & operator=(const JointModelCompositeTpl & other)
    {
      Base::operator=(other);
      m_nq = other.m_nq;
      m_nv = other.m_nv;
      m_idx_q = other.m_idx_q;
      m_idx_v = other.m_idx_v;
      m_nqs = other.m_nqs;
      m_nvs = other.m_nvs;
      joints = other.joints;
      jointPlacements = other.jointPlacements;
      njoints = other.njoints;

      return *this;
    }

    using Base::isEqual;
    bool isEqual(const JointModelCompositeTpl & other) const
    {
      return Base::isEqual(other)
      && nq() == other.nq()
      && nv() == other.nv()
      && m_idx_q == other.m_idx_q
      && m_idx_v == other.m_idx_v
      && m_nqs == other.m_nqs
      && m_nvs == other.m_nvs
      && joints == other.joints
      && jointPlacements == other.jointPlacements
      && njoints == other.njoints;
    }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> cast() const
    {
      typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> ReturnType;
      ReturnType res((size_t)njoints);
      res.setIndexes(id(),idx_q(),idx_v());
      res.m_nq = m_nq;
      res.m_nv = m_nv;
      res.m_idx_q = m_idx_q;
      res.m_idx_v = m_idx_v;
      res.m_nqs = m_nqs;
      res.m_nvs = m_nvs;
      res.njoints = njoints;

      res.joints.resize(joints.size());
      res.jointPlacements.resize(jointPlacements.size());
      for(size_t k = 0; k < jointPlacements.size(); ++k)
      {
        res.joints[k] = joints[k].template cast<NewScalar>();
        res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
      }

      return res;
    }

    /// \brief Vector of joints contained in the joint composite.
    JointModelVector joints;
    /// \brief Vector of joint placements. Those placements correspond to the origin of the joint relatively to their parent.
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;

    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
    jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
    jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }

    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
    jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv());  }
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::Type
    jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv());  }

    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::ConstType
    jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv());  }
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::Type
    jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv());  }

    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
    jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
    jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
    jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
    jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }

    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
    jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
    template<typename D>
    typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
    jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }


  protected:

    friend struct Serialize<JointModelCompositeTpl>;

    template<typename, int, template<typename,int> class>
    friend struct JointModelCompositeTpl;

    /// \brief Update the indexes of the joints contained in the composition according
    /// to the position of the joint composite.
    void updateJointIndexes()
    {
      int idx_q = this->idx_q();
      int idx_v = this->idx_v();

      m_idx_q.resize(joints.size());
      m_idx_v.resize(joints.size());
      m_nqs.resize(joints.size());
      m_nvs.resize(joints.size());

      for(size_t i = 0; i < joints.size(); ++i)
      {
        JointModelVariant & joint = joints[i];

        m_idx_q[i] = idx_q; m_idx_v[i] = idx_v;
        ::pinocchio::setIndexes(joint,i,idx_q,idx_v);
        m_nqs[i] = ::pinocchio::nq(joint);
        m_nvs[i] = ::pinocchio::nv(joint);
        idx_q += m_nqs[i]; idx_v += m_nvs[i];
      }
    }


    /// \brief Dimensions of the config and tangent space of the composite joint.
    int m_nq, m_nv;

    /// Keep information of both the dimension and the position of the joints in the composition.

    /// \brief Index in the config vector
    std::vector<int> m_idx_q;
    /// \brief Dimension of the segment in the config vector
    std::vector<int> m_nqs;
    /// \brief Index in the tangent vector
    std::vector<int> m_idx_v;
    /// \brief Dimension of the segment in the tangent vector
    std::vector<int> m_nvs;

  public:
    /// \brief Number of joints contained in the JointModelComposite
    int njoints;
  };


  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
  {
    typedef typename JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::JointModelVector JointModelVector;

    os << "JointModelComposite containing following models:\n" ;
    for (typename JointModelVector::const_iterator it = jmodel.joints.begin();
         it != jmodel.joints.end(); ++it)
      os << "  " << shortname(*it) << std::endl;

    return os;
  }

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_constructor< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_copy< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_constructor< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
  struct has_nothrow_copy< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
  : public integral_constant<bool,true> {};
}

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/joint/joint-composite.hxx"

#endif // ifndef __pinocchio_joint_composite_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2021 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_joint_composite_hpp__
6			#define __pinocchio_joint_composite_hpp__
7
8			#include "pinocchio/multibody/joint/fwd.hpp"
9			#include "pinocchio/multibody/joint/joint-collection.hpp"
10			#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
11			#include "pinocchio/container/aligned-vector.hpp"
12			#include "pinocchio/spatial/act-on-set.hpp"
13
14			#include "pinocchio/serialization/fwd.hpp"
15
16			namespace pinocchio
17			{
18
19			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
20			struct JointCompositeTpl;
21
22			template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
23			struct traits< JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
24			{
25			typedef _Scalar Scalar;
26
27			enum {
28			Options = _Options,
29			NQ = Eigen::Dynamic,
30			NV = Eigen::Dynamic
31			};
32
33			typedef JointCollectionTpl<Scalar,Options> JointCollection;
34			typedef JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
35			typedef JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
36			typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
37			typedef SE3Tpl<Scalar,Options> Transformation_t;
38			typedef MotionTpl<Scalar,Options> Motion_t;
39			typedef MotionTpl<Scalar,Options> Bias_t;
40
41			// [ABA]
42			typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
43			typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
44			typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
45
46			PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
47
48			typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
49			typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
50			};
51
52			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
53			struct traits< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
54			{ typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
55
56			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
57			struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
58			{ typedef JointCompositeTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
59
60			template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
61			struct JointDataCompositeTpl
62			: public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
63			{
64		36	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65
66			typedef JointDataBase<JointDataCompositeTpl> Base;
67			typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
68			PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
69		464	PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
70
71			typedef JointCollectionTpl<Scalar,Options> JointCollection;
72			typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataVariant;
73
74			typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector;
75
76			// JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
77
78		26	JointDataCompositeTpl()
79			: joints()
80			, iMlast(0)
81			, pjMi(0)
82			, S(0)
83			, M(Transformation_t::Identity())
84			, v(Motion_t::Zero())
85			, c(Motion_t::Zero())
86			, U(6,0), Dinv(0,0), UDinv(6,0)
87	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	26	, StU(0,0)
88		26	{}
89
90
91		45	JointDataCompositeTpl(const JointDataVector & joint_data, const int /nq/, const int nv)
92			: joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
93			, S(Constraint_t::Zero(nv))
94			, M(Transformation_t::Identity())
95			, v(Motion_t::Zero())
96			, c(Motion_t::Zero())
97			, U(U_t::Zero(6,nv))
98			, Dinv(D_t::Zero(nv,nv))
99			, UDinv(UD_t::Zero(6,nv))
100	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	45	, StU(D_t::Zero(nv,nv))
101		45	{}
102
103			/// \brief Vector of joints
104			JointDataVector joints;
105
106			/// \brief Transforms from previous joint to last joint
107			PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast;
108
109			/// \brief Transforms from previous joint to joint i
110			PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi;
111
112			Constraint_t S;
113			Transformation_t M;
114			Motion_t v;
115			Bias_t c;
116
117			// // [ABA] specific data
118			U_t U;
119			D_t Dinv;
120			UD_t UDinv;
121
122			D_t StU;
123
124	✓✗	43	static std::string classname() { return std::string("JointDataComposite"); }
125		2	std::string shortname() const { return classname(); }
126
127			};
128
129			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
130		1	inline std::ostream & operator <<(std::ostream & os, const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
131			{
132			typedef typename JointDataCompositeTpl<Scalar,Options,JointCollectionTpl>::JointDataVector JointDataVector;
133
134		1	os << "JointDataComposite containing following models:\n" ;
135		3	for (typename JointDataVector::const_iterator it = jdata.joints.begin();
136	✓✓	3	it != jdata.joints.end(); ++it)
137	✓✗✓✗ ✓✗✓✗	2	os << " " << shortname(*it) << std::endl;
138		1	return os;
139			}
140
141
142			template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
143			struct CastType< NewScalar, JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
144			{
145			typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> type;
146			};
147
148			template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
149			struct JointModelCompositeTpl
150			: public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
151			{
152		108	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153
154			typedef JointModelBase<JointModelCompositeTpl> Base;
155			typedef JointCompositeTpl<_Scalar,_Options,JointCollectionTpl> JointDerived;
156			PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
157
158			typedef JointCollectionTpl<Scalar,Options> JointCollection;
159			typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
160			typedef JointModel JointModelVariant;
161
162			typedef SE3Tpl<Scalar,Options> SE3;
163			typedef MotionTpl<Scalar,Options> Motion;
164			typedef InertiaTpl<Scalar,Options> Inertia;
165
166			typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
167
168			using Base::id;
169			using Base::idx_q;
170			using Base::idx_v;
171			using Base::setIndexes;
172			using Base::nq;
173			using Base::nv;
174
175			/// \brief Default contructor
176		42	JointModelCompositeTpl()
177			: joints()
178			, jointPlacements()
179			, m_nq(0)
180			, m_nv(0)
181		42	, njoints(0)
182		42	{}
183
184			/// \brief Default contructor with a defined size
185		8	JointModelCompositeTpl(const size_t size)
186			: joints()
187			, jointPlacements()
188			, m_nq(0)
189			, m_nv(0)
190		8	, njoints(0)
191			{
192	✓✗✓✗	8	joints.reserve(size); jointPlacements.reserve(size);
193	✓✗✓✗	8	m_idx_q.reserve(size); m_idx_v.reserve(size);
194	✓✗✓✗	8	m_nqs.reserve(size); m_nvs.reserve(size);
195		8	}
196
197			///
198			/// \brief Constructor with one joint.
199			///
200			/// \param jmodel Model of the first joint.
201			/// \param placement Placement of the first joint w.r.t. the joint origin.
202			///
203			template<typename JointModel>
204		55	JointModelCompositeTpl(const JointModelBase<JointModel> & jmodel,
205			const SE3 & placement = SE3::Identity())
206		55	: joints(1,(JointModelVariant)jmodel.derived())
207			, jointPlacements(1,placement)
208			, m_nq(jmodel.nq())
209			, m_nv(jmodel.nv())
210			, m_idx_q(1,0), m_nqs(1,jmodel.nq())
211			, m_idx_v(1,0), m_nvs(1,jmodel.nv())
212	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	55	, njoints(1)
213		55	{}
214
215			///
216			/// \brief Copy constructor.
217			///
218			/// \param other JointModel to copy.
219			///
220		70	JointModelCompositeTpl(const JointModelCompositeTpl & other)
221			: Base(other)
222		70	, joints(other.joints)
223		70	, jointPlacements(other.jointPlacements)
224		70	, m_nq(other.m_nq)
225		70	, m_nv(other.m_nv)
226		140	, m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
227		140	, m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
228	✓✗✓✗ ✓✗✓✗ ✓✗	70	, njoints(other.njoints)
229		70	{}
230
231
232			///
233			/// \brief Add a joint to the vector of joints.
234			///
235			/// \param jmodel Model of the joint to add.
236			/// \param placement Placement of the joint relatively to its predecessor.
237			///
238			/// \return A reference to *this
239			///
240			template<typename JointModel>
241		65	JointModelDerived & addJoint(const JointModelBase<JointModel> & jmodel,
242			const SE3 & placement = SE3::Identity())
243			{
244	✓✗	65	joints.push_back((JointModelVariant)jmodel.derived());
245		65	jointPlacements.push_back(placement);
246
247		65	m_nq += jmodel.nq(); m_nv += jmodel.nv();
248
249		65	updateJointIndexes();
250		65	njoints++;
251
252		65	return *this;
253			}
254
255		45	JointDataDerived createData() const
256			{
257	✓✗	90	typename JointDataDerived::JointDataVector jdata(joints.size());
258	✓✓	120	for (int i = 0; i < (int)joints.size(); ++i)
259	✓✗✓✗	75	jdata[(size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
260	✓✗✓✗ ✓✗	90	return JointDataDerived(jdata,nq(),nv());
261			}
262
263			const std::vector<bool> hasConfigurationLimit() const
264			{
265			std::vector<bool> vec;
266			for (size_t i = 0; i < joints.size(); ++i)
267			{
268			const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimit();
269			vec.insert(vec.end(),
270			joint_cf_limit.begin(),
271			joint_cf_limit.end());
272			}
273			return vec;
274			}
275
276			const std::vector<bool> hasConfigurationLimitInTangent() const
277			{
278			std::vector<bool> vec;
279			for (size_t i = 0; i < joints.size(); ++i)
280			{
281			const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimitInTangent();
282			vec.insert(vec.end(),
283			joint_cf_limit.begin(),
284			joint_cf_limit.end());
285			}
286			return vec;
287			}
288
289			template<typename, int, template<typename S, int O> class, typename>
290			friend struct JointCompositeCalcZeroOrderStep;
291
292			template<typename ConfigVectorType>
293			void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;
294
295			template<typename, int, template<typename S, int O> class, typename, typename>
296			friend struct JointCompositeCalcFirstOrderStep;
297
298			template<typename ConfigVectorType, typename TangentVectorType>
299			void calc(JointDataDerived & data,
300			const Eigen::MatrixBase<ConfigVectorType> & qs,
301			const Eigen::MatrixBase<TangentVectorType> & vs) const;
302
303			template<typename Matrix6Like>
304		38	void calc_aba(JointDataDerived & data,
305			const Eigen::MatrixBase<Matrix6Like> & I,
306			const bool update_I) const
307			{
308	✓✗✓✗	38	data.U.noalias() = I * data.S.matrix();
309	✓✗✓✗ ✓✗	38	data.StU.noalias() = data.S.matrix().transpose() * data.U;
310
311			// compute inverse
312			// data.Dinv.setIdentity();
313			// data.StU.llt().solveInPlace(data.Dinv);
314		38	internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
315	✓✗✓✗	38	data.UDinv.noalias() = data.U * data.Dinv;
316
317	✓✓	38	if (update_I)
318	✓✗✓✗ ✓✗	36	PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
319		38	}
320
321		412	int nv_impl() const { return m_nv; }
322		232	int nq_impl() const { return m_nq; }
323
324			/**
325			* @brief Update the indexes of subjoints in the stack
326			*/
327		55	void setIndexes_impl(JointIndex id, int q, int v)
328			{
329		55	Base::setIndexes_impl(id, q, v);
330		55	updateJointIndexes();
331		55	}
332
333	✓✗	61	static std::string classname() { return std::string("JointModelComposite"); }
334		20	std::string shortname() const { return classname(); }
335
336			JointModelCompositeTpl & operator=(const JointModelCompositeTpl & other)
337			{
338			Base::operator=(other);
339			m_nq = other.m_nq;
340			m_nv = other.m_nv;
341			m_idx_q = other.m_idx_q;
342			m_idx_v = other.m_idx_v;
343			m_nqs = other.m_nqs;
344			m_nvs = other.m_nvs;
345			joints = other.joints;
346			jointPlacements = other.jointPlacements;
347			njoints = other.njoints;
348
349			return *this;
350			}
351
352			using Base::isEqual;
353		30	bool isEqual(const JointModelCompositeTpl & other) const
354			{
355		30	return Base::isEqual(other)
356	✓✗	29	&& nq() == other.nq()
357	✓✗	29	&& nv() == other.nv()
358	✓✗	29	&& m_idx_q == other.m_idx_q
359	✓✗	29	&& m_idx_v == other.m_idx_v
360	✓✗	29	&& m_nqs == other.m_nqs
361	✓✗	29	&& m_nvs == other.m_nvs
362	✓✗	29	&& joints == other.joints
363	✓✗	29	&& jointPlacements == other.jointPlacements
364	✓✓✓✗	59	&& njoints == other.njoints;
365			}
366
367			/// \returns An expression of *this with the Scalar type casted to NewScalar.
368			template<typename NewScalar>
369		8	JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> cast() const
370			{
371			typedef JointModelCompositeTpl<NewScalar,Options,JointCollectionTpl> ReturnType;
372		8	ReturnType res((size_t)njoints);
373	✓✗✓✗ ✓✗✓✗	8	res.setIndexes(id(),idx_q(),idx_v());
374		8	res.m_nq = m_nq;
375		8	res.m_nv = m_nv;
376	✓✗	8	res.m_idx_q = m_idx_q;
377	✓✗	8	res.m_idx_v = m_idx_v;
378	✓✗	8	res.m_nqs = m_nqs;
379	✓✗	8	res.m_nvs = m_nvs;
380		8	res.njoints = njoints;
381
382	✓✗	8	res.joints.resize(joints.size());
383	✓✗	8	res.jointPlacements.resize(jointPlacements.size());
384	✓✓	24	for(size_t k = 0; k < jointPlacements.size(); ++k)
385			{
386	✓✗✓✗	16	res.joints[k] = joints[k].template cast<NewScalar>();
387	✓✗	16	res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
388			}
389
390		8	return res;
391			}
392
393			/// \brief Vector of joints contained in the joint composite.
394			JointModelVector joints;
395			/// \brief Vector of joint placements. Those placements correspond to the origin of the joint relatively to their parent.
396			PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
397
398			template<typename D>
399			typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
400		18	jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
401			template<typename D>
402			typename SizeDepType<NQ>::template SegmentReturn<D>::Type
403		18	jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
404
405			template<typename D>
406			typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
407		18	jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
408			template<typename D>
409			typename SizeDepType<NV>::template SegmentReturn<D>::Type
410		18	jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
411
412			template<typename D>
413			typename SizeDepType<NV>::template ColsReturn<D>::ConstType
414		18	jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
415			template<typename D>
416			typename SizeDepType<NV>::template ColsReturn<D>::Type
417		18	jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
418
419			template<typename D>
420			typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
421			jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
422			template<typename D>
423			typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
424			jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
425			template<typename D>
426			typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
427		1	jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
428			template<typename D>
429			typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
430			jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
431
432			template<typename D>
433			typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
434			jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
435			template<typename D>
436			typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
437			jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
438
439
440			protected:
441
442			friend struct Serialize<JointModelCompositeTpl>;
443
444			template<typename, int, template<typename,int> class>
445			friend struct JointModelCompositeTpl;
446
447			/// \brief Update the indexes of the joints contained in the composition according
448			/// to the position of the joint composite.
449		94	void updateJointIndexes()
450			{
451		94	int idx_q = this->idx_q();
452		94	int idx_v = this->idx_v();
453
454		94	m_idx_q.resize(joints.size());
455		94	m_idx_v.resize(joints.size());
456		94	m_nqs.resize(joints.size());
457		94	m_nvs.resize(joints.size());
458
459	✓✓	294	for(size_t i = 0; i < joints.size(); ++i)
460			{
461		200	JointModelVariant & joint = joints[i];
462
463		200	m_idx_q[i] = idx_q; m_idx_v[i] = idx_v;
464		200	::pinocchio::setIndexes(joint,i,idx_q,idx_v);
465		200	m_nqs[i] = ::pinocchio::nq(joint);
466		200	m_nvs[i] = ::pinocchio::nv(joint);
467		200	idx_q += m_nqs[i]; idx_v += m_nvs[i];
468			}
469		94	}
470
471
472			/// \brief Dimensions of the config and tangent space of the composite joint.
473			int m_nq, m_nv;
474
475			/// Keep information of both the dimension and the position of the joints in the composition.
476
477			/// \brief Index in the config vector
478			std::vector<int> m_idx_q;
479			/// \brief Dimension of the segment in the config vector
480			std::vector<int> m_nqs;
481			/// \brief Index in the tangent vector
482			std::vector<int> m_idx_v;
483			/// \brief Dimension of the segment in the tangent vector
484			std::vector<int> m_nvs;
485
486			public:
487			/// \brief Number of joints contained in the JointModelComposite
488			int njoints;
489			};
490
491
492			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
493		18	inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
494			{
495			typedef typename JointModelCompositeTpl<Scalar,Options,JointCollectionTpl>::JointModelVector JointModelVector;
496
497		18	os << "JointModelComposite containing following models:\n" ;
498		40	for (typename JointModelVector::const_iterator it = jmodel.joints.begin();
499	✓✓	40	it != jmodel.joints.end(); ++it)
500	✓✗✓✗ ✓✗✓✗	22	os << " " << shortname(*it) << std::endl;
501
502		18	return os;
503			}
504
505			} // namespace pinocchio
506
507			#include <boost/type_traits.hpp>
508
509			namespace boost
510			{
511			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
512			struct has_nothrow_constructor< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
513			: public integral_constant<bool,true> {};
514
515			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
516			struct has_nothrow_copy< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
517			: public integral_constant<bool,true> {};
518
519			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
520			struct has_nothrow_constructor< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
521			: public integral_constant<bool,true> {};
522
523			template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
524			struct has_nothrow_copy< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
525			: public integral_constant<bool,true> {};
526			}
527
528			/* --- Details -------------------------------------------------------------- */
529			/* --- Details -------------------------------------------------------------- */
530			/* --- Details -------------------------------------------------------------- */
531			#include "pinocchio/multibody/joint/joint-composite.hxx"
532
533			#endif // ifndef __pinocchio_joint_composite_hpp__