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>
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;
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
83 ,
M(Transformation_t::Identity())
86 ,
U(6,0), Dinv(0,0), UDinv(6,0)
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))
124 static std::string
classname() {
return std::string(
"JointDataComposite"); }
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)
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
192 joints.reserve(size); jointPlacements.reserve(size);
193 m_idx_q.reserve(size); m_idx_v.reserve(size);
194 m_nqs.reserve(size); m_nvs.reserve(size);
203 template<
typename Jo
intModel>
206 : joints(1,(JointModelVariant)jmodel.derived())
210 , m_idx_q(1,0), m_nqs(1,jmodel.
nq())
211 , m_idx_v(1,0), m_nvs(1,jmodel.
nv())
222 , joints(other.joints)
223 , jointPlacements(other.jointPlacements)
226 , m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
227 , m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
228 , njoints(other.njoints)
240 template<
typename Jo
intModel>
244 joints.push_back((JointModelVariant)jmodel.
derived());
247 m_nq += jmodel.
nq(); m_nv += jmodel.
nv();
249 updateJointIndexes();
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());
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());
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>
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;
315 data.UDinv.noalias() = data.U * data.Dinv;
329 Base::setIndexes_impl(
id, q, v);
330 updateJointIndexes();
333 static std::string
classname() {
return std::string(
"JointModelComposite"); }
346 jointPlacements = other.jointPlacements;
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);
376 res.m_idx_q = m_idx_q;
377 res.m_idx_v = m_idx_v;
380 res.njoints = 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>();
414 jointCols(
const Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
417 jointCols(Eigen::MatrixBase<D>& A)
const {
return A.middleCols(Base::i_v,
nv()); }
444 template<
typename,
int,
template<
typename,
int>
class>
454 m_idx_q.resize(joints.size());
455 m_idx_v.resize(joints.size());
456 m_nqs.resize(joints.size());
457 m_nvs.resize(joints.size());
459 for(
size_t i = 0;
i < joints.size(); ++
i)
461 JointModelVariant & joint = joints[
i];
467 idx_q += m_nqs[
i]; idx_v += m_nvs[
i];
492 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
493 inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
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__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
static std::string classname()
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
ConstraintTpl< Eigen::Dynamic, Scalar, Options > Constraint_t
JointCollectionTpl< Scalar, Options > JointCollection
int m_nq
Dimensions of the config and tangent space of the composite joint.
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector
SizeDepType< NV >::template SegmentReturn< D >::ConstType jointVelocitySelector(const Eigen::MatrixBase< D > &a) const
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
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...
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
SizeDepType< NV >::template SegmentReturn< D >::Type jointVelocitySelector(Eigen::MatrixBase< D > &a) const
MotionTpl< Scalar, Options > Bias_t
JointCompositeTpl< Scalar, Options, JointCollectionTpl > JointDerived
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
JointDataDerived createData() const
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
bool isEqual(const JointModelCompositeTpl &other) const
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
InertiaTpl< Scalar, Options > Inertia
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
JointModelCompositeTpl & operator=(const JointModelCompositeTpl &other)
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::Type jointConfigSelector_impl(Eigen::MatrixBase< D > &a) const
JointDataVector joints
Vector of joints.
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
static ColsReturn< D >::ConstType middleCols(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size)
void updateJointIndexes()
Update the indexes of the joints contained in the composition according to the position of the joint ...
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::Type jointVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
JointDataCompositeTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR typedef JointCollectionTpl< Scalar, Options > JointCollection
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols(const Eigen::MatrixBase< D > &A) const
JointDataCompositeTpl(const JointDataVector &joint_data, const int, const int nv)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
const std::vector< bool > hasConfigurationLimit() const
JointModel JointModelVariant
virtual bool isEqual(const CollisionGeometry &other) const=0
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
JointCollectionTpl< Scalar, Options > JointCollection
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
static std::string classname()
void setIndexes_impl(JointIndex id, int q, int v)
Update the indexes of subjoints in the stack.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointDataTpl< Scalar, Options, JointCollectionTpl > JointDataVariant
static SegmentReturn< D >::ConstType segment(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size)
JointModelVector joints
Vector of joints contained in the joint composite.
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
SizeDepType< Eigen::Dynamic >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
SizeDepType< Eigen::Dynamic >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
const std::vector< bool > hasConfigurationLimitInTangent() const
JointCompositeTpl< Scalar, Options, JointCollectionTpl > JointDerived
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
MotionTpl< Scalar, Options > Motion_t
JointModelDerived & derived()
std::string shortname() const
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointDataBase< JointDataCompositeTpl > Base
SizeDepType< NV >::template ColsReturn< D >::Type jointCols(Eigen::MatrixBase< D > &A) const
std::string shortname() const
std::vector< int > m_nqs
Dimension of the segment in the config vector.
int njoints
Number of joints contained in the JointModelComposite.
Main pinocchio namespace.
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
MotionTpl< Scalar, Options > Motion
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointModelBase< JointModelCompositeTpl > Base
SizeDepType< NQ >::template SegmentReturn< D >::ConstType jointConfigSelector(const Eigen::MatrixBase< D > &a) const
Common traits structure to fully define base classes for CRTP.
JointModelCompositeTpl()
Default contructor.
JointModelCompositeTpl(const size_t size)
Default contructor with a defined size.
AABB & operator=(const AABB &other)=default
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
JointModelCompositeTpl(const JointModelCompositeTpl &other)
Copy constructor.
std::vector< int > m_idx_v
Index in the tangent vector.
SE3Tpl< Scalar, Options > Transformation_t
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
SizeDepType< NQ >::template SegmentReturn< D >::Type jointConfigSelector(Eigen::MatrixBase< D > &a) const
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > type
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
SE3Tpl< Scalar, Options > SE3
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)