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;
166 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
JointModel) JointModelVector;
171 using Base::setIndexes;
192 joints.reserve(size); jointPlacements.reserve(size);
203 template<
typename Jo
intModel>
205 const SE3 & placement = SE3::Identity())
207 , jointPlacements(1,placement)
223 , jointPlacements(other.jointPlacements)
240 template<
typename Jo
intModel>
242 const SE3 & placement = SE3::Identity())
245 jointPlacements.push_back(placement);
247 m_nq += jmodel.nq(); m_nv += jmodel.nv();
255 JointDataDerived createData()
const
257 typename JointDataDerived::JointDataVector jdata(
joints.size());
258 for (
int i = 0; i < (int)
joints.size(); ++i)
259 jdata[(
size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(
joints[(
size_t)i]);
260 return JointDataDerived(jdata,
nq(),
nv());
263 template<
typename,
int,
template<
typename S,
int O>
class,
typename>
264 friend struct JointCompositeCalcZeroOrderStep;
266 template<
typename ConfigVectorType>
267 void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVectorType> & qs)
const;
269 template<
typename,
int,
template<
typename S,
int O>
class,
typename,
typename>
270 friend struct JointCompositeCalcFirstOrderStep;
272 template<
typename ConfigVectorType,
typename TangentVectorType>
273 void calc(JointDataDerived & data,
274 const Eigen::MatrixBase<ConfigVectorType> & qs,
275 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
277 template<
typename Matrix6Like>
278 void calc_aba(JointDataDerived & data,
279 const Eigen::MatrixBase<Matrix6Like> & I,
280 const bool update_I)
const
282 data.U.noalias() = I * data.S.matrix();
283 data.StU.noalias() = data.S.matrix().transpose() * data.U;
288 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
289 data.UDinv.noalias() = data.U * data.Dinv;
292 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
295 int nv_impl()
const {
return m_nv; }
296 int nq_impl()
const {
return m_nq; }
303 Base::setIndexes_impl(
id, q, v);
307 static std::string classname() {
return std::string(
"JointModelComposite"); }
308 std::string shortname()
const {
return classname(); }
310 JointModelCompositeTpl & operator=(
const JointModelCompositeTpl & other)
320 jointPlacements = other.jointPlacements;
327 bool isEqual(
const JointModelCompositeTpl & other)
const
329 std::cout <<
"JointModelCompositeTpl::isEqual" << std::endl;
330 return Base::isEqual(other)
331 &&
nq() == other.nq()
332 &&
nv() == other.nv()
335 &&
m_nqs == other.m_nqs
336 &&
m_nvs == other.m_nvs
338 && jointPlacements == other.jointPlacements
343 template<
typename NewScalar>
347 ReturnType res((
size_t)
njoints);
357 res.joints.resize(
joints.size());
358 res.jointPlacements.resize(jointPlacements.size());
359 for(
size_t k = 0; k < jointPlacements.size(); ++k)
361 res.joints[k] =
joints[k].template cast<NewScalar>();
362 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
371 PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) jointPlacements;
375 jointConfigSelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
378 jointConfigSelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
381 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
382 jointVelocitySelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
384 typename SizeDepType<NV>::template SegmentReturn<D>::Type
385 jointVelocitySelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
388 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
389 jointCols(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
391 typename SizeDepType<NV>::template ColsReturn<D>::Type
392 jointCols(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
395 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
396 jointConfigSelector_impl(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
398 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
399 jointConfigSelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
401 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
402 jointVelocitySelector_impl(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
404 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
405 jointVelocitySelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
408 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
409 jointCols_impl(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
411 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
412 jointCols_impl(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
417 friend struct Serialize<JointModelCompositeTpl>;
419 template<
typename,
int,
template<
typename,
int>
class>
420 friend struct JointModelCompositeTpl;
434 for(
size_t i = 0; i <
joints.size(); ++i)
440 m_nqs[i] = ::pinocchio::nq(joint);
441 m_nvs[i] = ::pinocchio::nv(joint);
467 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
472 os <<
"JointModelComposite containing following models:\n" ;
473 for (
typename JointModelVector::const_iterator it = jmodel.
joints.begin();
474 it != jmodel.
joints.end(); ++it)
475 os <<
" " <<
shortname(*it) << std::endl;
482 #include <boost/type_traits.hpp>
486 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
488 :
public integral_constant<bool,true> {};
490 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
492 :
public integral_constant<bool,true> {};
494 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
496 :
public integral_constant<bool,true> {};
498 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
500 :
public integral_constant<bool,true> {};
506 #include "pinocchio/multibody/joint/joint-composite.hxx"
508 #endif // ifndef __pinocchio_joint_composite_hpp__