pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
cartesian-axis.hpp
1 //
2 // Copyright (c) 2017-2018 CNRS
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>
15  {
16  enum { axis = _axis, dim = 3 };
17 
18  template<typename V3_in, typename V3_out>
19  inline static void cross(const Eigen::MatrixBase<V3_in> & vin,
20  const Eigen::MatrixBase<V3_out> & vout);
21 
22  template<typename V3>
23  static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) cross(const Eigen::MatrixBase<V3> & vin)
24  {
25  typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
26  cross(vin,res);
27  return res;
28  }
29 
30  template<typename Scalar, typename V3_in, typename V3_out>
31  inline static void alphaCross(const Scalar & s,
32  const Eigen::MatrixBase<V3_in> & vin,
33  const Eigen::MatrixBase<V3_out> & vout);
34 
35  template<typename Scalar, typename V3>
36  static typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) alphaCross(const Scalar & s,
37  const Eigen::MatrixBase<V3> & vin)
38  {
39  typename PINOCCHIO_EIGEN_PLAIN_TYPE(V3) res;
40  alphaCross(s,vin,res);
41  return res;
42  }
43 
44  template<typename Scalar>
45  Eigen::Matrix<Scalar,dim,1> operator*(const Scalar & s) const
46  {
47  typedef Eigen::Matrix<Scalar,dim,1> ReturnType;
48  ReturnType res;
49  for(Eigen::DenseIndex i = 0; i < dim; ++i)
50  res[i] = i == axis ? s : Scalar(0);
51 
52  return res;
53  }
54 
55  template<typename Scalar>
56  friend inline Eigen::Matrix<Scalar,dim,1>
57  operator*(const Scalar & s, const CartesianAxis &)
58  {
59  return CartesianAxis() * s;
60  }
61 
62  template<typename Vector3Like>
63  static void setTo(const Eigen::MatrixBase<Vector3Like> v3)
64  {
65  Vector3Like & v3_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Like,v3);
66  typedef typename Vector3Like::Scalar Scalar;
67 
68  for(Eigen::DenseIndex i = 0; i < dim; ++i)
69  v3_[i] = i == axis ? Scalar(1) : Scalar(0);
70  }
71 
72  }; // struct CartesianAxis
73 
74  template<>
75  template<typename V3_in, typename V3_out>
76  inline void CartesianAxis<0>::cross(const Eigen::MatrixBase<V3_in> & vin,
77  const Eigen::MatrixBase<V3_out> & vout)
78  {
79  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
80  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
81  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
82  vout_[0] = 0.; vout_[1] = -vin[2]; vout_[2] = vin[1];
83  }
84 
85  template<>
86  template<typename V3_in, typename V3_out>
87  inline void CartesianAxis<1>::cross(const Eigen::MatrixBase<V3_in> & vin,
88  const Eigen::MatrixBase<V3_out> & vout)
89  {
90  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
91  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
92  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
93  vout_[0] = vin[2]; vout_[1] = 0.; vout_[2] = -vin[0];
94  }
95 
96  template<>
97  template<typename V3_in, typename V3_out>
98  inline void CartesianAxis<2>::cross(const Eigen::MatrixBase<V3_in> & vin,
99  const Eigen::MatrixBase<V3_out> & vout)
100  {
101  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
102  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
103  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
104  vout_[0] = -vin[1]; vout_[1] = vin[0]; vout_[2] = 0.;
105  }
106 
107  template<>
108  template<typename Scalar, typename V3_in, typename V3_out>
109  inline void CartesianAxis<0>::alphaCross(const Scalar & s,
110  const Eigen::MatrixBase<V3_in> & vin,
111  const Eigen::MatrixBase<V3_out> & vout)
112  {
113  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
114  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
115  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
116  vout_[0] = 0.; vout_[1] = -s*vin[2]; vout_[2] = s*vin[1];
117  }
118 
119  template<>
120  template<typename Scalar, typename V3_in, typename V3_out>
121  inline void CartesianAxis<1>::alphaCross(const Scalar & s,
122  const Eigen::MatrixBase<V3_in> & vin,
123  const Eigen::MatrixBase<V3_out> & vout)
124  {
125  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
126  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
127  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
128  vout_[0] = s*vin[2]; vout_[1] = 0.; vout_[2] = -s*vin[0];
129  }
130 
131  template<>
132  template<typename Scalar, typename V3_in, typename V3_out>
133  inline void CartesianAxis<2>::alphaCross(const Scalar & s,
134  const Eigen::MatrixBase<V3_in> & vin,
135  const Eigen::MatrixBase<V3_out> & vout)
136  {
137  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_in,3)
138  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3_out,3)
139  V3_out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3_out,vout);
140  vout_[0] = -s*vin[1]; vout_[1] = s*vin[0]; vout_[2] = 0.;
141  }
142 
143  typedef CartesianAxis<0> AxisX;
144  typedef CartesianAxis<1> AxisY;
145  typedef CartesianAxis<2> AxisZ;
146 
147 }
148 
149 #endif // __pinocchio_cartesian_axis_hpp__
Main pinocchio namespace.
Definition: treeview.dox:24