Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/spatial-axis.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 14 | 38 | 36.8% |
Branches: | 4 | 54 | 7.4% |
Line | Branch | Exec | Source |
---|---|---|---|
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> | ||
16 | struct SpatialAxis; | ||
17 | |||
18 | template<int axis, typename MotionDerived> | ||
19 | struct MotionAlgebraAction<SpatialAxis<axis>, MotionDerived> | ||
20 | { | ||
21 | typedef typename MotionDerived::MotionPlain ReturnType; | ||
22 | }; | ||
23 | |||
24 | template<int _axis> | ||
25 | struct SpatialAxis //: MotionBase< SpatialAxis<_axis> > | ||
26 | { | ||
27 | enum | ||
28 | { | ||
29 | axis = _axis, | ||
30 | dim = 6 | ||
31 | }; | ||
32 | typedef CartesianAxis<_axis % 3> CartesianAxis3; | ||
33 | |||
34 | enum | ||
35 | { | ||
36 | LINEAR = 0, | ||
37 | ANGULAR = 3 | ||
38 | }; | ||
39 | |||
40 | template<typename Derived1, typename Derived2> | ||
41 | inline static void cross(const MotionDense<Derived1> & min, const MotionDense<Derived2> & mout); | ||
42 | |||
43 | template<typename Derived> | ||
44 | ✗ | static typename traits<Derived>::MotionPlain cross(const MotionDense<Derived> & min) | |
45 | { | ||
46 | ✗ | typename MotionDense<Derived>::MotionPlain res; | |
47 | ✗ | cross(min, res); | |
48 | ✗ | return res; | |
49 | } | ||
50 | |||
51 | template<typename Derived1, typename Derived2> | ||
52 | inline static void cross(const ForceDense<Derived1> & fin, const ForceDense<Derived2> & fout); | ||
53 | |||
54 | template<typename Derived> | ||
55 | ✗ | static typename traits<Derived>::ForcePlain cross(const ForceDense<Derived> & fin) | |
56 | { | ||
57 | ✗ | typename ForceDense<Derived>::ForcePlain fout; | |
58 | ✗ | cross(fin, fout); | |
59 | ✗ | return fout; | |
60 | } | ||
61 | |||
62 | template<typename Scalar> | ||
63 | 112 | MotionTpl<Scalar> operator*(const Scalar & s) const | |
64 | { | ||
65 | typedef MotionTpl<Scalar> ReturnType; | ||
66 | 112 | ReturnType res; | |
67 |
2/2✓ Branch 0 taken 336 times.
✓ Branch 1 taken 56 times.
|
784 | for (Eigen::DenseIndex i = 0; i < dim; ++i) |
68 |
2/2✓ Branch 0 taken 56 times.
✓ Branch 1 taken 280 times.
|
672 | res.toVector()[i] = i == axis ? s : Scalar(0); |
69 | |||
70 | 112 | return res; | |
71 | } | ||
72 | |||
73 | template<typename Scalar> | ||
74 | ✗ | friend inline MotionTpl<Scalar> operator*(const Scalar & s, const SpatialAxis &) | |
75 | { | ||
76 | ✗ | return SpatialAxis() * s; | |
77 | } | ||
78 | |||
79 | template<typename Derived> | ||
80 | 27 | friend Derived & operator<<(MotionDense<Derived> & min, const SpatialAxis &) | |
81 | { | ||
82 | typedef typename traits<Derived>::Scalar Scalar; | ||
83 | 27 | min.setZero(); | |
84 | 27 | min.toVector()[axis] = Scalar(1); | |
85 | 27 | return min.derived(); | |
86 | } | ||
87 | |||
88 | template<typename MotionDerived> | ||
89 | 56 | typename MotionDerived::MotionPlain motionAction(const MotionDense<MotionDerived> & m) const | |
90 | { | ||
91 | 56 | typename MotionDerived::MotionPlain res; | |
92 | if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3)) | ||
93 | { | ||
94 | ✗ | res.angular().setZero(); | |
95 | ✗ | CartesianAxis3::cross(-m.angular(), res.linear()); | |
96 | } | ||
97 | else | ||
98 | { | ||
99 |
0/6✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
56 | CartesianAxis3::cross(-m.linear(), res.linear()); |
100 |
0/6✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
56 | CartesianAxis3::cross(-m.angular(), res.angular()); |
101 | } | ||
102 | |||
103 | 56 | return res; | |
104 | } | ||
105 | |||
106 | }; // struct SpatialAxis | ||
107 | |||
108 | template<int axis> | ||
109 | template<typename Derived1, typename Derived2> | ||
110 | inline void | ||
111 | ✗ | SpatialAxis<axis>::cross(const MotionDense<Derived1> & min, const MotionDense<Derived2> & mout) | |
112 | { | ||
113 | ✗ | Derived2 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2, mout); | |
114 | |||
115 | if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3)) | ||
116 | { | ||
117 | ✗ | mout_.angular().setZero(); | |
118 | ✗ | CartesianAxis3::cross(min.angular(), mout_.linear()); | |
119 | } | ||
120 | else | ||
121 | { | ||
122 | ✗ | CartesianAxis3::cross(min.linear(), mout_.linear()); | |
123 | ✗ | CartesianAxis3::cross(min.angular(), mout_.angular()); | |
124 | } | ||
125 | } | ||
126 | |||
127 | template<int axis> | ||
128 | template<typename Derived1, typename Derived2> | ||
129 | inline void | ||
130 | ✗ | SpatialAxis<axis>::cross(const ForceDense<Derived1> & fin, const ForceDense<Derived2> & fout) | |
131 | { | ||
132 | ✗ | Derived2 & fout_ = PINOCCHIO_EIGEN_CONST_CAST(Derived2, fout); | |
133 | |||
134 | if ((LINEAR == 0 && axis < 3) || (LINEAR == 3 && axis >= 3)) | ||
135 | { | ||
136 | ✗ | fout_.linear().setZero(); | |
137 | ✗ | CartesianAxis3::cross(fin.linear(), fout_.angular()); | |
138 | } | ||
139 | else | ||
140 | { | ||
141 | ✗ | CartesianAxis3::cross(fin.linear(), fout_.linear()); | |
142 | ✗ | CartesianAxis3::cross(fin.angular(), fout_.angular()); | |
143 | } | ||
144 | } | ||
145 | |||
146 | typedef SpatialAxis<0> AxisVX; | ||
147 | typedef SpatialAxis<1> AxisVY; | ||
148 | typedef SpatialAxis<2> AxisVZ; | ||
149 | |||
150 | typedef SpatialAxis<3> AxisWX; | ||
151 | typedef SpatialAxis<4> AxisWY; | ||
152 | typedef SpatialAxis<5> AxisWZ; | ||
153 | } // namespace pinocchio | ||
154 | |||
155 | #endif // __pinocchio_spatial_axis_hpp__ | ||
156 |