5 #ifndef __pinocchio_spatial_axis_hpp__
6 #define __pinocchio_spatial_axis_hpp__
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"
17 template<
int axis,
typename MotionDerived>
19 {
typedef typename MotionDerived::MotionPlain ReturnType; };
24 enum { axis = _axis, dim = 6 };
25 typedef CartesianAxis<_axis%3> CartesianAxis3;
27 enum { LINEAR = 0, ANGULAR = 3 };
29 template<
typename Derived1,
typename Derived2>
30 inline static void cross(
const MotionDense<Derived1> & min,
31 const MotionDense<Derived2> & mout);
33 template<
typename Derived>
34 static typename traits<Derived>::MotionPlain cross(
const MotionDense<Derived> & min)
36 typename MotionDense<Derived>::MotionPlain res;
41 template<
typename Derived1,
typename Derived2>
42 inline static void cross(
const ForceDense<Derived1> & fin,
43 const ForceDense<Derived2> & fout);
45 template<
typename Derived>
46 static typename traits<Derived>::ForcePlain cross(
const ForceDense<Derived> & fin)
48 typename ForceDense<Derived>::ForcePlain fout;
53 template<
typename Scalar>
54 MotionTpl<Scalar> operator*(
const Scalar & s)
const
56 typedef MotionTpl<Scalar> ReturnType;
58 for(Eigen::DenseIndex i = 0; i < dim; ++i)
59 res.toVector()[i] = i == axis ? s : Scalar(0);
64 template<
typename Scalar>
65 friend inline MotionTpl<Scalar>
66 operator*(
const Scalar & s,
const SpatialAxis &)
68 return SpatialAxis() * s;
71 template<
typename Derived>
72 friend Derived & operator<<(MotionDense<Derived> & min,
75 typedef typename traits<Derived>::Scalar Scalar;
77 min.toVector()[axis] = Scalar(1);
81 template<
typename MotionDerived>
82 typename MotionDerived::MotionPlain
83 motionAction(
const MotionDense<MotionDerived> & m)
const
85 typename MotionDerived::MotionPlain res;
86 if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
88 res.angular().setZero();
89 CartesianAxis3::cross(-m.angular(),res.linear());
93 CartesianAxis3::cross(-m.linear(),res.linear());
94 CartesianAxis3::cross(-m.angular(),res.angular());
103 template<
typename Derived1,
typename Derived2>
104 inline void SpatialAxis<axis>::cross(
const MotionDense<Derived1> & min,
105 const MotionDense<Derived2> & mout)
107 Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,mout);
109 if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
111 mout_.angular().setZero();
112 CartesianAxis3::cross(min.angular(),mout_.linear());
116 CartesianAxis3::cross(min.linear(),mout_.linear());
117 CartesianAxis3::cross(min.angular(),mout_.angular());
122 template<
typename Derived1,
typename Derived2>
123 inline void SpatialAxis<axis>::cross(
const ForceDense<Derived1> & fin,
124 const ForceDense<Derived2> & fout)
126 Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,fout);
128 if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3))
130 fout_.linear().setZero();
131 CartesianAxis3::cross(fin.linear(),fout_.angular());
135 CartesianAxis3::cross(fin.linear(),fout_.linear());
136 CartesianAxis3::cross(fin.angular(),fout_.angular());
140 typedef SpatialAxis<0> AxisVX;
141 typedef SpatialAxis<1> AxisVY;
142 typedef SpatialAxis<2> AxisVZ;
144 typedef SpatialAxis<3> AxisWX;
145 typedef SpatialAxis<4> AxisWY;
146 typedef SpatialAxis<5> AxisWZ;
149 #endif // __pinocchio_spatial_axis_hpp__