pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
motion-zero.hpp
1//
2// Copyright (c) 2015-2019 CNRS INRIA
3// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4//
5
6#ifndef __pinocchio_spatial_motion_zero_hpp__
7#define __pinocchio_spatial_motion_zero_hpp__
8
9namespace pinocchio
10{
11
12 template<typename Scalar, int Options>
13 struct SE3GroupAction<MotionZeroTpl<Scalar, Options>>
14 {
16 };
17
18 template<typename Scalar, int Options, typename MotionDerived>
23
24 template<typename _Scalar, int _Options>
26 {
27 enum
28 {
29 Options = _Options,
30 LINEAR = 0,
31 ANGULAR = 3
32 };
33 typedef _Scalar Scalar;
34 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
35 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
36 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
37 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
38 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
39 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
40 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
41 typedef Matrix6 ActionMatrixType;
42 typedef Matrix4 HomogeneousMatrixType;
43 typedef Vector3 AngularType;
44 typedef const Vector3 ConstAngularType;
45 typedef Vector3 LinearType;
46 typedef const Vector3 ConstLinearType;
47 typedef Motion MotionPlain;
49
50 }; // traits MotionZeroTpl
51
52 template<typename Scalar, int Options>
53 struct MotionZeroTpl : public MotionBase<MotionZeroTpl<Scalar, Options>>
54 {
55 typedef typename traits<MotionZeroTpl>::MotionPlain MotionPlain;
56 typedef typename traits<MotionZeroTpl>::PlainReturnType PlainReturnType;
57
58 static PlainReturnType plain()
59 {
60 return MotionPlain::Zero();
61 }
62
63 template<typename D2>
64 static bool isEqual_impl(const MotionDense<D2> & other)
65 {
66 return other.linear().isZero(0) && other.angular().isZero(0);
67 }
68
69 static bool isEqual_impl(const MotionZeroTpl &)
70 {
71 return true;
72 }
73
74 template<typename D2>
75 static void addTo(const MotionBase<D2> &)
76 {
77 }
78
79 template<typename D2>
80 static void setTo(MotionBase<D2> & other)
81 {
82 other.setZero();
83 }
84
85 template<typename M1>
86 MotionZeroTpl motionAction(const MotionBase<M1> &) const
87 {
88 return MotionZeroTpl();
89 }
90
91 template<typename S2, int O2, typename D2>
92 void se3Action_impl(const SE3Tpl<S2, O2> &, MotionDense<D2> & v) const
93 {
94 v.setZero();
95 }
96
97 template<typename S2, int O2>
98 MotionZeroTpl se3Action_impl(const SE3Tpl<S2, O2> &) const
99 {
100 return MotionZeroTpl();
101 }
102
103 template<typename S2, int O2, typename D2>
104 void se3ActionInverse_impl(const SE3Tpl<S2, O2> &, MotionDense<D2> & v) const
105 {
106 v.setZero();
107 }
108
109 template<typename S2, int O2>
110 MotionZeroTpl se3ActionInverse_impl(const SE3Tpl<S2, O2> &) const
111 {
112 return MotionZeroTpl();
113 }
114
115 }; // struct MotionZeroTpl
116
117 template<typename M1, typename Scalar, int Options>
118 inline const M1 & operator+(const MotionBase<M1> & v, const MotionZeroTpl<Scalar, Options> &)
119 {
120 return v.derived();
121 }
122
123 template<typename Scalar, int Options, typename M1>
124 inline const M1 & operator+(const MotionZeroTpl<Scalar, Options> &, const MotionBase<M1> & v)
125 {
126 return v.derived();
127 }
128
129} // namespace pinocchio
130
131#endif // ifndef __pinocchio_spatial_motion_zero_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72