spatial-axis.hpp
Go to the documentation of this file.
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 };
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>
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>
47  {
48  typename ForceDense<Derived>::ForcePlain fout;
49  cross(fin,fout);
50  return fout;
51  }
52 
53  template<typename Scalar>
55  {
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
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>
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>
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 
143 
147 }
148 
149 #endif // __pinocchio_spatial_axis_hpp__
JointCollectionTpl const DataTpl< Scalar, Options, JointCollectionTpl > const Eigen::MatrixBase< Mat > & min
Definition: cholesky.hpp:71
SpatialAxis< 2 > AxisVZ
axis
SpatialAxis< 1 > AxisVY
SpatialAxis< 3 > AxisWX
Return type of the ation of a Motion onto an object of type D.
SpatialAxis< 0 > AxisVX
int dim
friend MotionTpl< Scalar > operator*(const Scalar &s, const SpatialAxis &)
ConstLinearType linear() const
Definition: motion-base.hpp:22
static traits< Derived >::ForcePlain cross(const ForceDense< Derived > &fin)
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
ConstAngularType angular() const
Definition: motion-base.hpp:21
SE3::Scalar Scalar
Definition: conversions.cpp:13
SpatialAxis< 4 > AxisWY
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
static void cross(const MotionDense< Derived1 > &min, const MotionDense< Derived2 > &mout)
CartesianAxis< _axis%3 > CartesianAxis3
static traits< Derived >::MotionPlain cross(const MotionDense< Derived > &min)
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
MotionTpl< Scalar > operator*(const Scalar &s) const
Main pinocchio namespace.
Definition: timings.cpp:28
SpatialAxis< 5 > AxisWZ
res
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
MotionDerived::MotionPlain motionAction(const MotionDense< MotionDerived > &m) const


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32