pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
spatial-axis.hpp
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_spatial_axis_hpp__
6 #define __pinocchio_spatial_axis_hpp__
7 
8 #include "pinocchio/spatial/fwd.hpp"
9 #include "pinocchio/spatial/cartesian-axis.hpp"
10 #include "pinocchio/spatial/motion.hpp"
11 #include "pinocchio/spatial/force.hpp"
12 
13 namespace pinocchio
14 {
15  template<int axis> struct SpatialAxis;
16 
17  template<int axis, typename MotionDerived>
18  struct MotionAlgebraAction<SpatialAxis<axis>, MotionDerived>
19  { typedef typename MotionDerived::MotionPlain ReturnType; };
20 
21  template<int _axis>
22  struct SpatialAxis //: MotionBase< SpatialAxis<_axis> >
23  {
24  enum { axis = _axis, dim = 6 };
25  typedef CartesianAxis<_axis%3> CartesianAxis3;
26 
27  enum { LINEAR = 0, ANGULAR = 3 };
28 
29  template<typename Derived1, typename Derived2>
30  inline static void cross(const MotionDense<Derived1> & min,
31  const MotionDense<Derived2> & mout);
32 
33  template<typename Derived>
34  static typename traits<Derived>::MotionPlain cross(const MotionDense<Derived> & min)
35  {
37  cross(min,res);
38  return res;
39  }
40 
41  template<typename Derived1, typename Derived2>
42  inline static void cross(const ForceDense<Derived1> & fin,
43  const ForceDense<Derived2> & fout);
44 
45  template<typename Derived>
46  static typename traits<Derived>::ForcePlain cross(const ForceDense<Derived> & fin)
47  {
48  typename ForceDense<Derived>::ForcePlain fout;
49  cross(fin,fout);
50  return fout;
51  }
52 
53  template<typename Scalar>
54  MotionTpl<Scalar> operator*(const Scalar & s) const
55  {
56  typedef MotionTpl<Scalar> ReturnType;
57  ReturnType res;
58  for(Eigen::DenseIndex i = 0; i < dim; ++i)
59  res.toVector()[i] = i == axis ? s : Scalar(0);
60 
61  return res;
62  }
63 
64  template<typename Scalar>
65  friend inline MotionTpl<Scalar>
66  operator*(const Scalar & s, const SpatialAxis &)
67  {
68  return SpatialAxis() * s;
69  }
70 
71  template<typename Derived>
72  friend Derived & operator<<(MotionDense<Derived> & min,
73  const SpatialAxis &)
74  {
75  typedef typename traits<Derived>::Scalar Scalar;
76  min.setZero();
77  min.toVector()[axis] = Scalar(1);
78  return min.derived();
79  }
80 
81  template<typename MotionDerived>
82  typename MotionDerived::MotionPlain
83  motionAction(const MotionDense<MotionDerived> & m) const
84  {
85  typename MotionDerived::MotionPlain res;
86  if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
87  {
88  res.angular().setZero();
89  CartesianAxis3::cross(-m.angular(),res.linear());
90  }
91  else
92  {
93  CartesianAxis3::cross(-m.linear(),res.linear());
94  CartesianAxis3::cross(-m.angular(),res.angular());
95  }
96 
97  return res;
98  }
99 
100  }; // struct SpatialAxis
101 
102  template<int axis>
103  template<typename Derived1, typename Derived2>
104  inline void SpatialAxis<axis>::cross(const MotionDense<Derived1> & min,
105  const MotionDense<Derived2> & mout)
106  {
107  Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,mout);
108 
109  if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
110  {
111  mout_.angular().setZero();
112  CartesianAxis3::cross(min.angular(),mout_.linear());
113  }
114  else
115  {
116  CartesianAxis3::cross(min.linear(),mout_.linear());
117  CartesianAxis3::cross(min.angular(),mout_.angular());
118  }
119  }
120 
121  template<int axis>
122  template<typename Derived1, typename Derived2>
123  inline void SpatialAxis<axis>::cross(const ForceDense<Derived1> & fin,
124  const ForceDense<Derived2> & fout)
125  {
126  Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,fout);
127 
128  if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
129  {
130  fout_.linear().setZero();
131  CartesianAxis3::cross(fin.linear(),fout_.angular());
132  }
133  else
134  {
135  CartesianAxis3::cross(fin.linear(),fout_.linear());
136  CartesianAxis3::cross(fin.angular(),fout_.angular());
137  }
138  }
139 
140  typedef SpatialAxis<0> AxisVX;
141  typedef SpatialAxis<1> AxisVY;
142  typedef SpatialAxis<2> AxisVZ;
143 
144  typedef SpatialAxis<3> AxisWX;
145  typedef SpatialAxis<4> AxisWY;
146  typedef SpatialAxis<5> AxisWZ;
147 }
148 
149 #endif // __pinocchio_spatial_axis_hpp__
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:44
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
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:211
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .