pinocchio  UNKNOWN
motion-zero.hpp
1 //
2 // Copyright (c) 2015-2017 CNRS
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 // This file is part of Pinocchio
6 // Pinocchio is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // Pinocchio is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // Pinocchio If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef __se3_motion_zero_hpp__
20 #define __se3_motion_zero_hpp__
21 
22 namespace se3
23 {
24 
25  template<>
26  struct traits<BiasZero>
27  {
28  typedef double Scalar;
29  typedef Eigen::Matrix<double,3,1,0> Vector3;
30  typedef Eigen::Matrix<double,6,1,0> Vector6;
31  typedef Eigen::Matrix<double,3,3,0> Matrix3;
32  typedef Eigen::Matrix<double,6,6,0> Matrix6;
33  typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
34  typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
35  typedef Matrix6 ActionMatrixType;
36  typedef Vector3 AngularType;
37  typedef const Vector3 ConstAngularType;
38  typedef Vector3 LinearType;
39  typedef const Vector3 ConstLinearType;
40  typedef Motion MotionPlain;
41  enum {
42  LINEAR = 0,
43  ANGULAR = 3
44  };
45  }; // traits BiasZero
46 
47  struct BiasZero : public MotionBase<BiasZero>
48  {
49  typedef traits<BiasZero>::MotionPlain MotionPlain;
50  operator MotionPlain () const { return MotionPlain::Zero(); }
51 
52  template<typename D2>
53  static bool isEqual_impl(const MotionDense<D2> & other)
54  { return other.linear().isZero() && other.angular().isZero(); }
55 
56  template<typename D2>
57  static void addTo(const MotionDense<D2> &) {}
58 
59  }; // struct BiasZero
60 
61  template<typename M1>
62  inline const M1 & operator+(const MotionBase<M1> & v,
63  const BiasZero&)
64  { return v.derived(); }
65 
66  template<typename M1>
67  inline const M1 & operator+(const BiasZero&,
68  const MotionBase<M1> & v)
69  { return v.derived(); }
70 
71 } // namespace se3
72 
73 #endif // ifndef __se3_motion_zero_hpp__