17 #ifndef HPP_PINOCCHIO_LIEGROUP_CARTESIAN_PRODUCT_OPERATION_HH
18 #define HPP_PINOCCHIO_LIEGROUP_CARTESIAN_PRODUCT_OPERATION_HH
20 #include <pinocchio/multibody/liegroup/cartesian-product.hpp>
22 #include <hpp/util/exception-factory.hh>
27 template<
typename LieGroup1,
typename LieGroup2>
31 BoundSize = LieGroup1::BoundSize + LieGroup2::BoundSize,
32 NR = LieGroup1::NR + LieGroup2::NR,
33 NT = LieGroup1::NT + LieGroup2::NT
36 typedef ::pinocchio::CartesianProductOperation<LieGroup1, LieGroup2>
Base;
38 template <
class ConfigL_t,
class ConfigR_t>
40 const Eigen::MatrixBase<ConfigL_t> & q0,
41 const Eigen::MatrixBase<ConfigR_t> & q1)
43 return Base::squaredDistance(q0, q1);
46 template <
class ConfigL_t,
class ConfigR_t>
48 const Eigen::MatrixBase<ConfigL_t> & q0,
49 const Eigen::MatrixBase<ConfigR_t> & q1,
50 const typename ConfigL_t::Scalar& w)
52 return LieGroup1().squaredDistance(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), w)
53 + LieGroup2().squaredDistance(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), w);
56 template <
class ConfigIn_t,
class ConfigOut_t>
58 const Eigen::MatrixBase<ConfigIn_t > & bound,
59 const Eigen::MatrixBase<ConfigOut_t> & out)
61 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, Base::ConfigVector_t)
62 ConfigOut_t& oout =
const_cast<Eigen::MatrixBase<ConfigOut_t>&
> (out).derived();
64 if (LieGroup1::BoundSize > 0)
65 LieGroup1::setBound(bound.template head<LieGroup1::BoundSize>(),
66 oout.
template head<LieGroup1::NQ >());
67 if (LieGroup2::BoundSize > 0)
68 LieGroup2::setBound(bound.template tail<LieGroup2::BoundSize>(),
69 oout.
template tail<LieGroup2::NQ >());
70 }
else if (bound.size() == Base::NQ) {
71 LieGroup1::setBound(bound.template head<LieGroup1::NQ>(),
72 oout.
template head<LieGroup1::NQ>());
73 LieGroup2::setBound(bound.template tail<LieGroup2::NQ>(),
74 oout.
template tail<LieGroup2::NQ>());
76 HPP_THROW(std::invalid_argument,
77 "Expected vector of size " << (
int)
BoundSize <<
" or " << (
int)Base::NQ
78 <<
", got size " << bound.size());
82 template <
class JacobianIn_t,
class JacobianOut_t>
84 const Eigen::MatrixBase<JacobianIn_t > & Jin,
85 const Eigen::MatrixBase<JacobianOut_t> & Jout)
87 JacobianOut_t& J =
const_cast<Eigen::MatrixBase<JacobianOut_t>&
> (Jout).derived();
88 if (LieGroup1::NR > 0)
89 LieGroup1::getRotationSubJacobian(Jin.template leftCols<LieGroup1::NV>(),
90 J .template leftCols<LieGroup1::NR>());
91 if (LieGroup2::NR > 0)
92 LieGroup2::getRotationSubJacobian(Jin.template rightCols<LieGroup2::NV>(),
93 J .template rightCols<LieGroup2::NR>());
96 template <
class ConfigIn_t>
99 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , Base::ConfigVector_t);
108 #endif // HPP_PINOCCHIO_LIEGROUP_CARTESIAN_PRODUCT_OPERATION_HH