5 #ifndef __pinocchio_joint_composite_hpp__
6 #define __pinocchio_joint_composite_hpp__
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"
14 #include "pinocchio/serialization/fwd.hpp"
19 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
22 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
25 typedef _Scalar Scalar;
33 typedef JointCollectionTpl<Scalar,Options> JointCollection;
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;
46 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
48 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
49 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
52 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
56 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
60 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
62 :
public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
64 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
69 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
71 typedef JointCollectionTpl<Scalar,Options> JointCollection;
74 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector;
83 , M(Transformation_t::Identity())
86 , U(6,0), Dinv(0,0), UDinv(6,0)
91 JointDataCompositeTpl(
const JointDataVector & joint_data,
const int ,
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())
98 , Dinv(D_t::Zero(
nv,
nv))
99 , UDinv(UD_t::Zero(6,
nv))
100 , StU(D_t::Zero(
nv,
nv))
107 PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast;
110 PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi;
124 static std::string classname() {
return std::string(
"JointDataComposite"); }
125 std::string shortname()
const {
return classname(); }
129 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
130 inline std::ostream & operator <<(std::ostream & os,
const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
132 typedef typename JointDataCompositeTpl<Scalar,Options,JointCollectionTpl>::JointDataVector JointDataVector;
134 os <<
"JointDataComposite containing following models:\n" ;
135 for (
typename JointDataVector::const_iterator it = jdata.joints.begin();
136 it != jdata.joints.end(); ++it)
137 os <<
" " <<
shortname(*it) << std::endl;
142 template<
typename NewScalar,
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
148 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
150 :
public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
152 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
156 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
158 typedef JointCollectionTpl<Scalar,Options> JointCollection;
165 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector;
170 using Base::setIndexes;
191 joints.reserve(size); jointPlacements.reserve(size);
202 template<
typename Jo
intModel>
204 const SE3 & placement = SE3::Identity())
206 , jointPlacements(1,placement)
222 , jointPlacements(other.jointPlacements)
239 template<
typename Jo
intModel>
241 const SE3 & placement = SE3::Identity())
244 jointPlacements.push_back(placement);
246 m_nq += jmodel.nq(); m_nv += jmodel.nv();
254 JointDataDerived createData()
const
256 typename JointDataDerived::JointDataVector jdata(
joints.size());
257 for (
int i = 0; i < (int)
joints.size(); ++i)
258 jdata[(
size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(
joints[(
size_t)i]);
259 return JointDataDerived(jdata,
nq(),
nv());
262 template<
typename,
int,
template<
typename S,
int O>
class,
typename>
263 friend struct JointCompositeCalcZeroOrderStep;
265 template<
typename ConfigVectorType>
266 void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVectorType> & qs)
const;
268 template<
typename,
int,
template<
typename S,
int O>
class,
typename,
typename>
269 friend struct JointCompositeCalcFirstOrderStep;
271 template<
typename ConfigVectorType,
typename TangentVectorType>
272 void calc(JointDataDerived & data,
273 const Eigen::MatrixBase<ConfigVectorType> & qs,
274 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
276 template<
typename Matrix6Like>
277 void calc_aba(JointDataDerived & data,
278 const Eigen::MatrixBase<Matrix6Like> & I,
279 const bool update_I)
const
281 data.U.noalias() = I * data.S.matrix();
282 data.StU.noalias() = data.S.matrix().transpose() * data.U;
287 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
288 data.UDinv.noalias() = data.U * data.Dinv;
291 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
294 int nv_impl()
const {
return m_nv; }
295 int nq_impl()
const {
return m_nq; }
302 Base::setIndexes_impl(
id, q, v);
306 static std::string classname() {
return std::string(
"JointModelComposite"); }
307 std::string shortname()
const {
return classname(); }
309 JointModelCompositeTpl & operator=(
const JointModelCompositeTpl & other)
319 jointPlacements = other.jointPlacements;
326 bool isEqual(
const JointModelCompositeTpl & other)
const
328 return Base::isEqual(other)
329 &&
nq() == other.nq()
330 &&
nv() == other.nv()
333 &&
m_nqs == other.m_nqs
334 &&
m_nvs == other.m_nvs
336 && jointPlacements == other.jointPlacements
341 template<
typename NewScalar>
345 ReturnType res((
size_t)
njoints);
355 res.joints.resize(
joints.size());
356 res.jointPlacements.resize(jointPlacements.size());
357 for(
size_t k = 0; k < jointPlacements.size(); ++k)
359 res.joints[k] =
joints[k].template cast<NewScalar>();
360 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
369 PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) jointPlacements;
373 jointConfigSelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
376 jointConfigSelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
379 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
380 jointVelocitySelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
382 typename SizeDepType<NV>::template SegmentReturn<D>::Type
383 jointVelocitySelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
386 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
387 jointCols(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
389 typename SizeDepType<NV>::template ColsReturn<D>::Type
390 jointCols(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
393 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
394 jointConfigSelector_impl(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
396 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
397 jointConfigSelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
399 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
400 jointVelocitySelector_impl(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
402 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
403 jointVelocitySelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
406 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
407 jointCols_impl(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
409 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
410 jointCols_impl(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
415 friend struct Serialize<JointModelCompositeTpl>;
417 template<
typename,
int,
template<
typename,
int>
class>
418 friend struct JointModelCompositeTpl;
432 for(
size_t i = 0; i <
joints.size(); ++i)
438 m_nqs[i] = ::pinocchio::nq(joint);
439 m_nvs[i] = ::pinocchio::nv(joint);
465 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
470 os <<
"JointModelComposite containing following models:\n" ;
471 for (
typename JointModelVector::const_iterator it = jmodel.
joints.begin();
472 it != jmodel.
joints.end(); ++it)
473 os <<
" " <<
shortname(*it) << std::endl;
480 #include <boost/type_traits.hpp>
484 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
486 :
public integral_constant<bool,true> {};
488 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
490 :
public integral_constant<bool,true> {};
492 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
494 :
public integral_constant<bool,true> {};
496 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
498 :
public integral_constant<bool,true> {};
504 #include "pinocchio/multibody/joint/joint-composite.hxx"
506 #endif // ifndef __pinocchio_joint_composite_hpp__