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;
83 , U(6,0), Dinv(0,0), UDinv(6,0)
89 : joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
92 , U(6,nv), Dinv(nv,nv), UDinv(6,nv)
117 static std::string classname() {
return std::string(
"JointDataComposite"); }
118 std::string
shortname()
const {
return classname(); }
122 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
123 inline std::ostream & operator <<(std::ostream & os, const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
127 os <<
"JointDataComposite containing following models:\n" ;
128 for (
typename JointDataVector::const_iterator it = jdata.joints.begin();
129 it != jdata.joints.end(); ++it)
130 os <<
" " <<
shortname(*it) << std::endl;
135 template<
typename NewScalar,
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
141 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
143 :
public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
145 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
149 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
151 typedef JointCollectionTpl<Scalar,Options> JointCollection;
163 using Base::setIndexes;
184 joints.reserve(size); jointPlacements.reserve(size);
185 m_idx_q.reserve(size); m_idx_v.reserve(size);
186 m_nqs.reserve(size); m_nvs.reserve(size);
195 template<
typename Jo
intModel>
197 const SE3 & placement = SE3::Identity())
198 : joints(1,(JointModelVariant)jmodel.derived())
199 , jointPlacements(1,placement)
202 , m_idx_q(1,0), m_nqs(1,jmodel.
nq())
203 , m_idx_v(1,0), m_nvs(1,jmodel.
nv())
214 , joints(other.joints)
215 , jointPlacements(other.jointPlacements)
218 , m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
219 , m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
220 , njoints(other.njoints)
232 template<
typename Jo
intModel>
234 const SE3 & placement = SE3::Identity())
236 joints.push_back((JointModelVariant)jmodel.derived());
237 jointPlacements.push_back(placement);
239 m_nq += jmodel.nq(); m_nv += jmodel.nv();
241 updateJointIndexes();
249 typename JointDataDerived::JointDataVector jdata(joints.size());
250 for (
int i = 0; i < (int)joints.size(); ++i)
251 jdata[(
size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
252 return JointDataDerived(jdata,
nq(),
nv());
255 template<
typename,
int,
template<
typename S,
int O>
class,
typename>
256 friend struct JointCompositeCalcZeroOrderStep;
258 template<
typename ConfigVectorType>
259 void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVectorType> & qs)
const;
261 template<
typename,
int,
template<
typename S,
int O>
class,
typename,
typename>
262 friend struct JointCompositeCalcFirstOrderStep;
264 template<
typename ConfigVectorType,
typename TangentVectorType>
265 void calc(JointDataDerived & data,
266 const Eigen::MatrixBase<ConfigVectorType> & qs,
267 const Eigen::MatrixBase<TangentVectorType> & vs)
const;
269 template<
typename Matrix6Like>
270 void calc_aba(JointDataDerived & data,
271 const Eigen::MatrixBase<Matrix6Like> & I,
272 const bool update_I)
const 274 data.U.noalias() = I * data.S.matrix();
275 data.StU.noalias() = data.S.matrix().transpose() * data.U;
280 internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
281 data.UDinv.noalias() = data.U * data.Dinv;
284 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
287 int nv_impl()
const {
return m_nv; }
288 int nq_impl()
const {
return m_nq; }
295 Base::setIndexes_impl(
id, q, v);
296 updateJointIndexes();
299 static std::string classname() {
return std::string(
"JointModelComposite"); }
300 std::string
shortname()
const {
return classname(); }
304 Base::operator=(other);
321 return Base::isEqual(other)
322 &&
nq() == other.nq()
323 &&
nv() == other.nv()
326 && m_nqs == other.
m_nqs 327 && m_nvs == other.
m_nvs 334 template<
typename NewScalar>
338 ReturnType res((
size_t)njoints);
342 res.m_idx_q = m_idx_q;
343 res.m_idx_v = m_idx_v;
346 res.njoints = njoints;
348 res.joints.resize(joints.size());
349 res.jointPlacements.resize(jointPlacements.size());
350 for(
size_t k = 0; k < jointPlacements.size(); ++k)
352 res.joints[k] = joints[k].template cast<NewScalar>();
353 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
366 jointConfigSelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
369 jointConfigSelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
373 jointVelocitySelector(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
376 jointVelocitySelector( Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
380 jointCols(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
383 jointCols(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
387 jointConfigSelector_impl(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
390 jointConfigSelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_q,
nq()); }
393 jointVelocitySelector_impl(
const Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
396 jointVelocitySelector_impl(Eigen::MatrixBase<D>& a)
const {
return a.segment(Base::i_v,
nv()); }
400 jointCols_impl(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
403 jointCols_impl(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
408 friend struct Serialize<JointModelCompositeTpl>;
410 template<
typename,
int,
template<
typename,
int>
class>
411 friend struct JointModelCompositeTpl;
420 m_idx_q.resize(joints.size());
421 m_idx_v.resize(joints.size());
422 m_nqs.resize(joints.size());
423 m_nvs.resize(joints.size());
425 for(
size_t i = 0; i < joints.size(); ++i)
427 JointModelVariant & joint = joints[i];
430 ::pinocchio::setIndexes(joint,i,idx_q,idx_v);
431 m_nqs[i] = ::pinocchio::nq(joint);
432 m_nvs[i] = ::pinocchio::nv(joint);
433 idx_q += m_nqs[i]; idx_v += m_nvs[i];
458 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
459 inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
463 os <<
"JointModelComposite containing following models:\n" ;
464 for (
typename JointModelVector::const_iterator it = jmodel.joints.begin();
465 it != jmodel.joints.end(); ++it)
466 os <<
" " <<
shortname(*it) << std::endl;
473 #include <boost/type_traits.hpp> 477 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
479 :
public integral_constant<bool,true> {};
481 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
483 :
public integral_constant<bool,true> {};
485 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
487 :
public integral_constant<bool,true> {};
489 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
491 :
public integral_constant<bool,true> {};
497 #include "pinocchio/multibody/joint/joint-composite.hxx" 499 #endif // ifndef __pinocchio_joint_composite_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
std::vector< int > m_idx_v
Index in the tangent vector.
void updateJointIndexes()
Update the indexes of the joints contained in the composition according to the position of the joint ...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
JointModelCompositeTpl()
Default contructor.
JointDataVector joints
Vector of joints.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
void setIndexes_impl(JointIndex id, int q, int v)
Update the indexes of subjoints in the stack.
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
JointModelCompositeTpl(const JointModelCompositeTpl &other)
Copy constructor.
container::aligned_vector< Transformation_t > pjMi
Transforms from previous joint to joint i.
container::aligned_vector< Transformation_t > iMlast
Transforms from previous joint to last joint.
std::vector< int > m_nqs
Dimension of the segment in the config vector.
JointModelCompositeTpl(const size_t size)
Default contructor with a defined size.
Main pinocchio namespace.
JointModelVector joints
Vector of joints contained in the joint composite.
container::aligned_vector< SE3 > jointPlacements
Vector of joint placements. Those placements correspond to the origin of the joint relatively to thei...
Common traits structure to fully define base classes for CRTP.
int m_nq
Dimensions of the config and tangent space of the composite joint.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to...
int njoints
Number of joints contained in the JointModelComposite.