174 :
public JointModelBase<JointModelCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
194 using Base::idx_vExtended;
197 using Base::nvExtended;
198 using Base::setIndexes;
221 jointPlacements.reserve(size);
236 template<
typename Jo
intModel>
240 , jointPlacements(1, placement)
243 , m_nvExtended(jmodel.nvExtended())
262 , jointPlacements(
other.jointPlacements)
265 , m_nvExtended(
other.m_nvExtended)
284 template<
typename Jo
intModel>
289 jointPlacements.push_back(placement);
293 m_nvExtended += jmodel.nvExtended();
301 JointDataDerived createData()
const
303 typename JointDataDerived::JointDataVector jdata(
joints.size());
307 return JointDataDerived(jdata,
nq(),
nv());
310 const std::vector<bool> hasConfigurationLimit()
const
312 std::vector<bool> vec;
313 for (
size_t i = 0; i <
joints.size(); ++i)
315 const std::vector<bool> & joint_cf_limit =
joints[i].hasConfigurationLimit();
316 vec.insert(vec.end(), joint_cf_limit.begin(), joint_cf_limit.end());
321 const std::vector<bool> hasConfigurationLimitInTangent()
const
323 std::vector<bool> vec;
324 for (
size_t i = 0; i <
joints.size(); ++i)
326 const std::vector<bool> & joint_cf_limit =
joints[i].hasConfigurationLimitInTangent();
327 vec.insert(vec.end(), joint_cf_limit.begin(), joint_cf_limit.end());
332 template<
typename,
int,
template<
typename S,
int O>
class,
typename>
333 friend struct JointCompositeCalcZeroOrderStep;
335 template<
typename ConfigVectorType>
336 void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVectorType> & qs)
const;
338 template<
typename,
int,
template<
typename S,
int O>
class,
typename,
typename>
339 friend struct JointCompositeCalcFirstOrderStep;
341 template<
typename ConfigVectorType,
typename TangentVectorType>
343 JointDataDerived & data,
344 const Eigen::MatrixBase<ConfigVectorType> & qs,
345 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
347 template<
typename TangentVectorType>
349 JointDataDerived & data,
351 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
353 template<
typename VectorLike,
typename Matrix6Like>
355 JointDataDerived & data,
356 const Eigen::MatrixBase<VectorLike> & armature,
357 const Eigen::MatrixBase<Matrix6Like> & I,
358 const bool update_I)
const
360 data.U.noalias() = I * data.S.matrix();
361 data.StU.noalias() = data.S.matrix().transpose() * data.U;
362 data.StU.diagonal() += armature;
364 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
365 data.UDinv.noalias() = data.U * data.Dinv;
368 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
379 int nvExtended_impl()
const
389 Base::setIndexes_impl(
id, q, v,
vExtended);
393 static std::string classname()
395 return std::string(
"JointModelComposite");
397 std::string shortname()
const
402 JointModelCompositeTpl & operator=(
const JointModelCompositeTpl & other)
407 m_nvExtended = other.m_nvExtended;
415 jointPlacements = other.jointPlacements;
422 bool isEqual(
const JointModelCompositeTpl & other)
const
424 return Base::isEqual(other) && internal::comparison_eq(
nq(), other.nq())
425 && internal::comparison_eq(
nv(), other.nv())
426 && internal::comparison_eq(nvExtended(), other.nvExtended())
427 && internal::comparison_eq(
m_idx_q, other.m_idx_q)
428 && internal::comparison_eq(
m_idx_v, other.m_idx_v)
430 && internal::comparison_eq(
m_nqs, other.m_nqs)
431 && internal::comparison_eq(
m_nvs, other.m_nvs)
432 && internal::comparison_eq(
m_nvExtendeds, other.m_nvExtendeds)
433 && internal::comparison_eq(
joints, other.joints)
434 && internal::comparison_eq(jointPlacements, other.jointPlacements)
435 && internal::comparison_eq(
njoints, other.njoints);
439 template<
typename NewScalar>
443 ReturnType res((
size_t)
njoints);
444 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
447 res.m_nvExtended = m_nvExtended;
456 res.joints.resize(
joints.size());
457 res.jointPlacements.resize(jointPlacements.size());
458 for (
size_t k = 0;
k < jointPlacements.size(); ++
k)
475 JointMappedConfigSelector(
const Eigen::MatrixBase<D> &
a)
const
477 return a.segment(Base::i_q,
nq());
481 JointMappedConfigSelector(Eigen::MatrixBase<D> &
a)
const
483 return a.segment(Base::i_q,
nq());
487 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
488 jointConfigSelector(
const Eigen::MatrixBase<D> & a)
const
490 return a.segment(Base::i_q, nq());
493 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
494 jointConfigSelector(Eigen::MatrixBase<D> & a)
const
496 return a.segment(Base::i_q,
nq());
500 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
501 JointMappedVelocitySelector(
const Eigen::MatrixBase<D> & a)
const
503 return a.segment(Base::i_v,
nv());
506 typename SizeDepType<NV>::template SegmentReturn<D>::Type
507 JointMappedVelocitySelector(Eigen::MatrixBase<D> & a)
const
509 return a.segment(Base::i_v,
nv());
513 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
514 jointVelocitySelector(
const Eigen::MatrixBase<D> & a)
const
516 return a.segment(Base::i_v,
nv());
519 typename SizeDepType<NV>::template SegmentReturn<D>::Type
520 jointVelocitySelector(Eigen::MatrixBase<D> & a)
const
522 return a.segment(Base::i_v,
nv());
526 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
527 jointCols(
const Eigen::MatrixBase<D> & A)
const
529 return A.middleCols(Base::i_v,
nv());
532 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
533 jointExtendedModelCols(
const Eigen::MatrixBase<D> & A)
const
535 return A.middleCols(Base::i_vExtended, nvExtended());
538 typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A)
const
540 return A.middleCols(Base::i_v,
nv());
543 typename SizeDepType<NV>::template ColsReturn<D>::Type
544 jointExtendedModelCols(Eigen::MatrixBase<D> & A)
const
546 return A.middleCols(Base::i_vExtended, nvExtended());
550 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
551 jointConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const
553 return a.segment(Base::i_q,
nq());
556 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
557 jointConfigSelector_impl(Eigen::MatrixBase<D> & a)
const
559 return a.segment(Base::i_q,
nq());
563 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
564 JointMappedConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const
566 return a.segment(Base::i_q,
nq());
569 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
570 JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a)
const
572 return a.segment(Base::i_q,
nq());
576 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
577 JointMappedVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const
579 return a.segment(Base::i_v,
nv());
582 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
583 JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const
585 return a.segment(Base::i_v,
nv());
589 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
590 jointVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const
592 return a.segment(Base::i_v,
nv());
595 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
596 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const
598 return a.segment(Base::i_v,
nv());
602 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
603 jointCols_impl(
const Eigen::MatrixBase<D> & A)
const
605 return A.middleCols(Base::i_v,
nv());
608 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
609 jointExtendedModelCols_impl(
const Eigen::MatrixBase<D> & A)
const
611 return A.middleCols(Base::i_vExtended, nvExtended());
614 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
615 jointCols_impl(Eigen::MatrixBase<D> & A)
const
617 return A.middleCols(Base::i_v,
nv());
620 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
621 jointExtendedModelCols_impl(Eigen::MatrixBase<D> & A)
const
623 return A.middleCols(Base::i_vExtended, nvExtended());
626 void disp(std::ostream & os)
const
628 typedef typename JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>::JointModelVector
631 os <<
"JointModelComposite containing following models:\n";
632 for (
typename JointModelVector::const_iterator it =
joints.begin(); it !=
joints.end(); ++it)
633 os <<
" " << it->shortname() << std::endl;
637 friend struct Serialize<JointModelCompositeTpl>;
639 template<
typename,
int,
template<
typename,
int>
class>
640 friend struct JointModelCompositeTpl;
646 int idx_q = this->idx_q();
647 int idx_v = this->idx_v();
648 int idx_vExtended = this->idx_vExtended();
657 for (
size_t i = 0;
i <
joints.size(); ++
i)