Program Listing for File motion-dense.hpp
↰ Return to documentation for file (include/pinocchio/spatial/motion-dense.hpp
)
//
// Copyright (c) 2017-2020 CNRS INRIA
//
#ifndef __pinocchio_spatial_motion_dense_hpp__
#define __pinocchio_spatial_motion_dense_hpp__
#include "pinocchio/spatial/skew.hpp"
namespace pinocchio
{
template<typename Derived>
struct SE3GroupAction<MotionDense<Derived>>
{
typedef typename SE3GroupAction<Derived>::ReturnType ReturnType;
};
template<typename Derived, typename MotionDerived>
struct MotionAlgebraAction<MotionDense<Derived>, MotionDerived>
{
typedef typename MotionAlgebraAction<Derived, MotionDerived>::ReturnType ReturnType;
};
template<typename Derived>
class MotionDense : public MotionBase<Derived>
{
public:
typedef MotionBase<Derived> Base;
MOTION_TYPEDEF_TPL(Derived);
typedef typename traits<Derived>::MotionRefType MotionRefType;
using Base::angular;
using Base::derived;
using Base::isApprox;
using Base::isZero;
using Base::linear;
Derived & setZero()
{
linear().setZero();
angular().setZero();
return derived();
}
Derived & setRandom()
{
linear().setRandom();
angular().setRandom();
return derived();
}
ActionMatrixType toActionMatrix_impl() const
{
ActionMatrixType X;
X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
skew(angular());
X.template block<3, 3>(LINEAR, ANGULAR) = skew(linear());
X.template block<3, 3>(ANGULAR, LINEAR).setZero();
return X;
}
ActionMatrixType toDualActionMatrix_impl() const
{
ActionMatrixType X;
X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) =
skew(angular());
X.template block<3, 3>(ANGULAR, LINEAR) = skew(linear());
X.template block<3, 3>(LINEAR, ANGULAR).setZero();
return X;
}
HomogeneousMatrixType toHomogeneousMatrix_impl() const
{
HomogeneousMatrixType M;
M.template block<3, 3>(0, 0) = skew(angular());
M.template block<3, 1>(0, 3) = linear();
M.template block<1, 4>(3, 0).setZero();
return M;
}
template<typename D2>
bool isEqual_impl(const MotionDense<D2> & other) const
{
return linear() == other.linear() && angular() == other.angular();
}
template<typename D2>
bool isEqual_impl(const MotionBase<D2> & other) const
{
return other.derived() == derived();
}
// Arithmetic operators
template<typename D2>
Derived & operator=(const MotionDense<D2> & other)
{
return derived().set(other.derived());
}
Derived & operator=(const MotionDense & other)
{
return derived().set(other.derived());
}
template<typename D2>
Derived & set(const MotionDense<D2> & other)
{
linear() = other.linear();
angular() = other.angular();
return derived();
}
template<typename D2>
Derived & operator=(const MotionBase<D2> & other)
{
other.derived().setTo(derived());
return derived();
}
template<typename V6>
Derived & operator=(const Eigen::MatrixBase<V6> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
assert(v.size() == 6);
linear() = v.template segment<3>(LINEAR);
angular() = v.template segment<3>(ANGULAR);
return derived();
}
MotionPlain operator-() const
{
return derived().__opposite__();
}
template<typename M1>
MotionPlain operator+(const MotionDense<M1> & v) const
{
return derived().__plus__(v.derived());
}
template<typename M1>
MotionPlain operator-(const MotionDense<M1> & v) const
{
return derived().__minus__(v.derived());
}
template<typename M1>
Derived & operator+=(const MotionDense<M1> & v)
{
return derived().__pequ__(v.derived());
}
template<typename M1>
Derived & operator+=(const MotionBase<M1> & v)
{
v.derived().addTo(derived());
return derived();
}
template<typename M1>
Derived & operator-=(const MotionDense<M1> & v)
{
return derived().__mequ__(v.derived());
}
MotionPlain __opposite__() const
{
return MotionPlain(-linear(), -angular());
}
template<typename M1>
MotionPlain __plus__(const MotionDense<M1> & v) const
{
return MotionPlain(linear() + v.linear(), angular() + v.angular());
}
template<typename M1>
MotionPlain __minus__(const MotionDense<M1> & v) const
{
return MotionPlain(linear() - v.linear(), angular() - v.angular());
}
template<typename M1>
Derived & __pequ__(const MotionDense<M1> & v)
{
linear() += v.linear();
angular() += v.angular();
return derived();
}
template<typename M1>
Derived & __mequ__(const MotionDense<M1> & v)
{
linear() -= v.linear();
angular() -= v.angular();
return derived();
}
template<typename OtherScalar>
MotionPlain __mult__(const OtherScalar & alpha) const
{
return MotionPlain(alpha * linear(), alpha * angular());
}
template<typename OtherScalar>
MotionPlain __div__(const OtherScalar & alpha) const
{
return derived().__mult__((OtherScalar)(1) / alpha);
}
template<typename F1>
Scalar dot(const ForceBase<F1> & phi) const
{
return phi.linear().dot(linear()) + phi.angular().dot(angular());
}
template<typename D>
typename MotionAlgebraAction<D, Derived>::ReturnType cross_impl(const D & d) const
{
return d.motionAction(derived());
}
template<typename M1, typename M2>
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
{
mout.linear() = v.linear().cross(angular()) + v.angular().cross(linear());
mout.angular() = v.angular().cross(angular());
}
template<typename M1>
MotionPlain motionAction(const MotionDense<M1> & v) const
{
MotionPlain res;
motionAction(v, res);
return res;
}
template<typename M2>
bool isApprox(
const MotionDense<M2> & m2,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return derived().isApprox_impl(m2, prec);
}
template<typename D2>
bool isApprox_impl(
const MotionDense<D2> & m2,
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec);
}
bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
{
return linear().isZero(prec) && angular().isZero(prec);
}
template<typename S2, int O2, typename D2>
void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
{
v.angular().noalias() = m.rotation() * angular();
v.linear().noalias() = m.rotation() * linear() + m.translation().cross(v.angular());
}
template<typename S2, int O2>
typename SE3GroupAction<Derived>::ReturnType se3Action_impl(const SE3Tpl<S2, O2> & m) const
{
typename SE3GroupAction<Derived>::ReturnType res;
se3Action_impl(m, res);
return res;
}
template<typename S2, int O2, typename D2>
void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
{
v.linear().noalias() =
m.rotation().transpose() * (linear() - m.translation().cross(angular()));
v.angular().noalias() = m.rotation().transpose() * angular();
}
template<typename S2, int O2>
typename SE3GroupAction<Derived>::ReturnType
se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
{
typename SE3GroupAction<Derived>::ReturnType res;
se3ActionInverse_impl(m, res);
return res;
}
void disp_impl(std::ostream & os) const
{
os << " v = " << linear().transpose() << std::endl
<< " w = " << angular().transpose() << std::endl;
}
MotionRefType ref()
{
return derived().ref();
}
protected:
MotionDense() {};
MotionDense(const MotionDense &) = delete;
}; // class MotionDense
template<typename M1, typename M2>
typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1, const MotionDense<M2> & v2)
{
return v1.derived().cross(v2.derived());
}
template<typename M1, typename F1>
typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v, const ForceBase<F1> & f)
{
return v.derived().cross(f.derived());
}
template<typename M1>
typename traits<M1>::MotionPlain
operator*(const typename traits<M1>::Scalar alpha, const MotionDense<M1> & v)
{
return v * alpha;
}
} // namespace pinocchio
#endif // ifndef __pinocchio_spatial_motion_dense_hpp__