pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
cartesian-axis.hpp
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_cartesian_axis_hpp__
6 #define __pinocchio_cartesian_axis_hpp__
7 
8 #include "pinocchio/fwd.hpp"
9 
10 namespace pinocchio
11 {
12 
13  template<int _axis>
14  struct CartesianAxis
15  {
16  enum
17  {
18  axis = _axis,
19  dim = 3
20  };
21 
22  typedef Eigen::Matrix<double, 3, 1> Vector3;
23 
24  template<typename V3_in, typename V3_out>
25  inline static void
26  cross(const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout);
27 
28  template<typename V3>
29  static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) cross(const Eigen::MatrixBase<V3> & vin)
30  {
31  typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
32  cross(vin, res);
33  return res;
34  }
35 
36  template<typename Scalar, typename V3_in, typename V3_out>
37  inline static void alphaCross(
38  const Scalar & s,
39  const Eigen::MatrixBase<V3_in> & vin,
40  const Eigen::MatrixBase<V3_out> & vout);
41 
42  template<typename Scalar, typename V3>
43  static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3)
44  alphaCross(const Scalar & s, const Eigen::MatrixBase<V3> & vin)
45  {
46  typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
47  alphaCross(s, vin, res);
48  return res;
49  }
50 
51  template<typename Scalar>
52  Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s) const
53  {
54  typedef Eigen::Matrix<Scalar, dim, 1> ReturnType;
55  ReturnType res;
56  for (Eigen::DenseIndex i = 0; i < dim; ++i)
57  res[i] = i == axis ? s : Scalar(0);
58 
59  return res;
60  }
61 
62  template<typename Scalar>
63  friend inline Eigen::Matrix<Scalar, dim, 1> operator*(const Scalar & s, const CartesianAxis &)
64  {
65  return CartesianAxis() * s;
66  }
67 
68  template<typename Vector3Like>
69  static void setTo(const Eigen::MatrixBase<Vector3Like> v3)
70  {
71  Vector3Like & v3_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Like, v3);
72  typedef typename Vector3Like::Scalar Scalar;
73 
74  for (Eigen::DenseIndex i = 0; i < dim; ++i)
75  v3_[i] = i == axis ? Scalar(1) : Scalar(0);
76  }
77 
78  template<typename Scalar>
79  static Eigen::Matrix<Scalar, 3, 1> vector()
80  {
81  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
82  return Vector3::Unit(axis);
83  }
84 
85  static Vector3 vector()
86  {
87  return vector<double>();
88  }
89 
90  }; // struct CartesianAxis
91 
92  template<>
93  template<typename V3_in, typename V3_out>
94  inline void CartesianAxis<0>::cross(
95  const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
96  {
97  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
98  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
99  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
100  vout_[0] = 0.;
101  vout_[1] = -vin[2];
102  vout_[2] = vin[1];
103  }
104 
105  template<>
106  template<typename V3_in, typename V3_out>
107  inline void CartesianAxis<1>::cross(
108  const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
109  {
110  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
111  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
112  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
113  vout_[0] = vin[2];
114  vout_[1] = 0.;
115  vout_[2] = -vin[0];
116  }
117 
118  template<>
119  template<typename V3_in, typename V3_out>
120  inline void CartesianAxis<2>::cross(
121  const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
122  {
123  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
124  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
125  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
126  vout_[0] = -vin[1];
127  vout_[1] = vin[0];
128  vout_[2] = 0.;
129  }
130 
131  template<>
132  template<typename Scalar, typename V3_in, typename V3_out>
133  inline void CartesianAxis<0>::alphaCross(
134  const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
135  {
136  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
137  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
138  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
139  vout_[0] = 0.;
140  vout_[1] = -s * vin[2];
141  vout_[2] = s * vin[1];
142  }
143 
144  template<>
145  template<typename Scalar, typename V3_in, typename V3_out>
146  inline void CartesianAxis<1>::alphaCross(
147  const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
148  {
149  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
150  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
151  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
152  vout_[0] = s * vin[2];
153  vout_[1] = 0.;
154  vout_[2] = -s * vin[0];
155  }
156 
157  template<>
158  template<typename Scalar, typename V3_in, typename V3_out>
159  inline void CartesianAxis<2>::alphaCross(
160  const Scalar & s, const Eigen::MatrixBase<V3_in> & vin, const Eigen::MatrixBase<V3_out> & vout)
161  {
162  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in, 3)
163  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out, 3)
164  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out, vout);
165  vout_[0] = -s * vin[1];
166  vout_[1] = s * vin[0];
167  vout_[2] = 0.;
168  }
169 
170  typedef CartesianAxis<0> XAxis;
171  typedef XAxis AxisX;
172 
173  typedef CartesianAxis<1> YAxis;
174  typedef YAxis AxisY;
175 
176  typedef CartesianAxis<2> ZAxis;
177  typedef ZAxis AxisZ;
178 
179 } // namespace pinocchio
180 
181 #endif // __pinocchio_cartesian_axis_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::cross
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition: skew.hpp:228