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 |
|
12 |
static typename traits<Derived>::MotionPlain cross(const MotionDense<Derived> & min) |
35 |
|
|
{ |
36 |
|
12 |
typename MotionDense<Derived>::MotionPlain res; |
37 |
|
12 |
cross(min,res); |
38 |
|
12 |
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 |
|
12 |
static typename traits<Derived>::ForcePlain cross(const ForceDense<Derived> & fin) |
47 |
|
|
{ |
48 |
|
12 |
typename ForceDense<Derived>::ForcePlain fout; |
49 |
|
12 |
cross(fin,fout); |
50 |
|
12 |
return fout; |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
template<typename Scalar> |
54 |
|
66 |
MotionTpl<Scalar> operator*(const Scalar & s) const |
55 |
|
|
{ |
56 |
|
|
typedef MotionTpl<Scalar> ReturnType; |
57 |
|
66 |
ReturnType res; |
58 |
✓✓ |
462 |
for(Eigen::DenseIndex i = 0; i < dim; ++i) |
59 |
✓✓ |
396 |
res.toVector()[i] = i == axis ? s : Scalar(0); |
60 |
|
|
|
61 |
|
66 |
return res; |
62 |
|
|
} |
63 |
|
|
|
64 |
|
|
template<typename Scalar> |
65 |
|
|
friend inline MotionTpl<Scalar> |
66 |
|
12 |
operator*(const Scalar & s, const SpatialAxis &) |
67 |
|
|
{ |
68 |
✓✗ |
12 |
return SpatialAxis() * s; |
69 |
|
|
} |
70 |
|
|
|
71 |
|
|
template<typename Derived> |
72 |
|
1068 |
friend Derived & operator<<(MotionDense<Derived> & min, |
73 |
|
|
const SpatialAxis &) |
74 |
|
|
{ |
75 |
|
|
typedef typename traits<Derived>::Scalar Scalar; |
76 |
|
1068 |
min.setZero(); |
77 |
|
1068 |
min.toVector()[axis] = Scalar(1); |
78 |
|
1068 |
return min.derived(); |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
template<typename MotionDerived> |
82 |
|
|
typename MotionDerived::MotionPlain |
83 |
|
110 |
motionAction(const MotionDense<MotionDerived> & m) const |
84 |
|
|
{ |
85 |
|
110 |
typename MotionDerived::MotionPlain res; |
86 |
|
|
if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3)) |
87 |
|
|
{ |
88 |
✓✗ |
18 |
res.angular().setZero(); |
89 |
✓✗✓✗ ✓✗ |
18 |
CartesianAxis3::cross(-m.angular(),res.linear()); |
90 |
|
|
} |
91 |
|
|
else |
92 |
|
|
{ |
93 |
✓✗✓✗ ✓✗ |
92 |
CartesianAxis3::cross(-m.linear(),res.linear()); |
94 |
✓✗✓✗ ✓✗ |
92 |
CartesianAxis3::cross(-m.angular(),res.angular()); |
95 |
|
|
} |
96 |
|
|
|
97 |
|
110 |
return res; |
98 |
|
|
} |
99 |
|
|
|
100 |
|
|
}; // struct SpatialAxis |
101 |
|
|
|
102 |
|
|
template<int axis> |
103 |
|
|
template<typename Derived1, typename Derived2> |
104 |
|
12 |
inline void SpatialAxis<axis>::cross(const MotionDense<Derived1> & min, |
105 |
|
|
const MotionDense<Derived2> & mout) |
106 |
|
|
{ |
107 |
|
12 |
Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,mout); |
108 |
|
|
|
109 |
|
|
if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3)) |
110 |
|
|
{ |
111 |
✓✗ |
6 |
mout_.angular().setZero(); |
112 |
✓✗✓✗
|
6 |
CartesianAxis3::cross(min.angular(),mout_.linear()); |
113 |
|
|
} |
114 |
|
|
else |
115 |
|
|
{ |
116 |
✓✗✓✗
|
6 |
CartesianAxis3::cross(min.linear(),mout_.linear()); |
117 |
✓✗✓✗
|
6 |
CartesianAxis3::cross(min.angular(),mout_.angular()); |
118 |
|
|
} |
119 |
|
12 |
} |
120 |
|
|
|
121 |
|
|
template<int axis> |
122 |
|
|
template<typename Derived1, typename Derived2> |
123 |
|
24 |
inline void SpatialAxis<axis>::cross(const ForceDense<Derived1> & fin, |
124 |
|
|
const ForceDense<Derived2> & fout) |
125 |
|
|
{ |
126 |
|
24 |
Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2,fout); |
127 |
|
|
|
128 |
|
|
if((LINEAR == 0 && axis<3) || (LINEAR == 3 && axis >= 3)) |
129 |
|
|
{ |
130 |
✓✗ |
12 |
fout_.linear().setZero(); |
131 |
✓✗✓✗
|
12 |
CartesianAxis3::cross(fin.linear(),fout_.angular()); |
132 |
|
|
} |
133 |
|
|
else |
134 |
|
|
{ |
135 |
✓✗✓✗
|
12 |
CartesianAxis3::cross(fin.linear(),fout_.linear()); |
136 |
✓✗✓✗
|
12 |
CartesianAxis3::cross(fin.angular(),fout_.angular()); |
137 |
|
|
} |
138 |
|
24 |
} |
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__ |