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 const std::vector<bool> hasConfigurationLimit()
const
265 std::vector<bool> vec;
266 for (
size_t i = 0; i <
joints.size(); ++i)
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());
276 const std::vector<bool> hasConfigurationLimitInTangent()
const
278 std::vector<bool> vec;
279 for (
size_t i = 0; i <
joints.size(); ++i)
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());
289 template<
typename,
int,
template<
typename S,
int O>
class,
typename>
290 friend struct JointCompositeCalcZeroOrderStep;
292 template<
typename ConfigVectorType>
293 void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVectorType> & qs)
const;
295 template<
typename,
int,
template<
typename S,
int O>
class,
typename,
typename>
296 friend struct JointCompositeCalcFirstOrderStep;
298 template<
typename ConfigVectorType,
typename TangentVectorType>
299 void calc(JointDataDerived & data,
300 const Eigen::MatrixBase<ConfigVectorType> & qs,
301 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
303 template<
typename Matrix6Like>
304 void calc_aba(JointDataDerived & data,
305 const Eigen::MatrixBase<Matrix6Like> & I,
306 const bool update_I)
const
308 data.U.noalias() = I * data.S.matrix();
309 data.StU.noalias() = data.S.matrix().transpose() * data.U;
314 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
315 data.UDinv.noalias() = data.U * data.Dinv;
318 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
321 int nv_impl()
const {
return m_nv; }
322 int nq_impl()
const {
return m_nq; }
329 Base::setIndexes_impl(
id, q, v);
333 static std::string classname() {
return std::string(
"JointModelComposite"); }
334 std::string shortname()
const {
return classname(); }
336 JointModelCompositeTpl & operator=(
const JointModelCompositeTpl & other)
346 jointPlacements = other.jointPlacements;
353 bool isEqual(
const JointModelCompositeTpl & other)
const
355 return Base::isEqual(other)
356 &&
nq() == other.nq()
357 &&
nv() == other.nv()
360 &&
m_nqs == other.m_nqs
361 &&
m_nvs == other.m_nvs
363 && jointPlacements == other.jointPlacements
368 template<
typename NewScalar>
372 ReturnType res((
size_t)
njoints);
382 res.joints.resize(
joints.size());
383 res.jointPlacements.resize(jointPlacements.size());
384 for(
size_t k = 0; k < jointPlacements.size(); ++k)
386 res.joints[k] =
joints[k].template cast<NewScalar>();
387 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
396 PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) jointPlacements;
400 jointConfigSelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
403 jointConfigSelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
406 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
407 jointVelocitySelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
409 typename SizeDepType<NV>::template SegmentReturn<D>::Type
410 jointVelocitySelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
413 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
414 jointCols(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
416 typename SizeDepType<NV>::template ColsReturn<D>::Type
417 jointCols(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
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()); }
423 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
424 jointConfigSelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
426 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
427 jointVelocitySelector_impl(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
429 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
430 jointVelocitySelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
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()); }
436 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
437 jointCols_impl(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
442 friend struct Serialize<JointModelCompositeTpl>;
444 template<
typename,
int,
template<
typename,
int>
class>
445 friend struct JointModelCompositeTpl;
459 for(
size_t i = 0; i <
joints.size(); ++i)
465 m_nqs[i] = ::pinocchio::nq(joint);
466 m_nvs[i] = ::pinocchio::nv(joint);
492 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
497 os <<
"JointModelComposite containing following models:\n" ;
498 for (
typename JointModelVector::const_iterator it = jmodel.
joints.begin();
499 it != jmodel.
joints.end(); ++it)
500 os <<
" " <<
shortname(*it) << std::endl;
507 #include <boost/type_traits.hpp>
511 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
513 :
public integral_constant<bool,true> {};
515 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
517 :
public integral_constant<bool,true> {};
519 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
521 :
public integral_constant<bool,true> {};
523 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
525 :
public integral_constant<bool,true> {};
531 #include "pinocchio/multibody/joint/joint-composite.hxx"
533 #endif // ifndef __pinocchio_joint_composite_hpp__