GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2017-2019 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_force_dense_hpp__ |
||
6 |
#define __pinocchio_force_dense_hpp__ |
||
7 |
|||
8 |
namespace pinocchio |
||
9 |
{ |
||
10 |
|||
11 |
template<typename Derived> |
||
12 |
struct SE3GroupAction< ForceDense<Derived> > |
||
13 |
{ |
||
14 |
typedef typename SE3GroupAction< Derived >::ReturnType ReturnType; |
||
15 |
}; |
||
16 |
|||
17 |
template<typename Derived, typename MotionDerived> |
||
18 |
struct MotionAlgebraAction< ForceDense<Derived>, MotionDerived > |
||
19 |
{ |
||
20 |
typedef typename MotionAlgebraAction< Derived, MotionDerived >::ReturnType ReturnType; |
||
21 |
}; |
||
22 |
|||
23 |
template<typename Derived> |
||
24 |
class ForceDense : public ForceBase<Derived> |
||
25 |
{ |
||
26 |
public: |
||
27 |
typedef ForceBase<Derived> Base; |
||
28 |
FORCE_TYPEDEF_TPL(Derived); |
||
29 |
typedef typename traits<Derived>::ForceRefType ForceRefType; |
||
30 |
|||
31 |
using Base::linear; |
||
32 |
using Base::angular; |
||
33 |
using Base::derived; |
||
34 |
using Base::isApprox; |
||
35 |
using Base::isZero; |
||
36 |
using Base::operator=; |
||
37 |
|||
38 |
✓✗✓✗ |
452 |
Derived & setZero() { linear().setZero(); angular().setZero(); return derived(); } |
39 |
✓✗✓✗ |
116 |
Derived & setRandom() { linear().setRandom(); angular().setRandom(); return derived(); } |
40 |
|||
41 |
template<typename D2> |
||
42 |
1150 |
bool isEqual_impl(const ForceDense<D2> & other) const |
|
43 |
✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✓✗ |
1150 |
{ return linear() == other.linear() && angular() == other.angular(); } |
44 |
|||
45 |
template<typename D2> |
||
46 |
bool isEqual_impl(const ForceBase<D2> & other) const |
||
47 |
{ return other.derived() == derived(); } |
||
48 |
|||
49 |
// Arithmetic operators |
||
50 |
template<typename D2> |
||
51 |
12860 |
Derived & setFrom(const ForceDense<D2> & other) |
|
52 |
{ |
||
53 |
✓✗✓✗ |
12860 |
linear() = other.linear(); |
54 |
✓✗✓✗ |
12860 |
angular() = other.angular(); |
55 |
12860 |
return derived(); |
|
56 |
} |
||
57 |
|||
58 |
template<typename D2> |
||
59 |
12860 |
Derived & operator=(const ForceDense<D2> & other) |
|
60 |
{ |
||
61 |
12860 |
return derived().setFrom(other.derived()); |
|
62 |
} |
||
63 |
|||
64 |
template<typename V6> |
||
65 |
1 |
Derived & operator=(const Eigen::MatrixBase<V6> & v) |
|
66 |
{ |
||
67 |
✗✓ | 1 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); assert(v.size() == 6); |
68 |
✓✗✓✗ |
1 |
linear() = v.template segment<3>(LINEAR); |
69 |
✓✗✓✗ |
1 |
angular() = v.template segment<3>(ANGULAR); |
70 |
1 |
return derived(); |
|
71 |
} |
||
72 |
|||
73 |
1 |
ForcePlain operator-() const { return derived().__opposite__(); } |
|
74 |
template<typename F1> |
||
75 |
14416 |
ForcePlain operator+(const ForceDense<F1> & f) const { return derived().__plus__(f.derived()); } |
|
76 |
template<typename F1> |
||
77 |
204 |
ForcePlain operator-(const ForceDense<F1> & f) const { return derived().__minus__(f.derived()); } |
|
78 |
|||
79 |
template<typename F1> |
||
80 |
145517 |
Derived & operator+=(const ForceDense<F1> & f) { return derived().__pequ__(f.derived()); } |
|
81 |
template<typename F1> |
||
82 |
Derived & operator+=(const ForceBase<F1> & f) |
||
83 |
{ f.derived().addTo(derived()); return derived(); } |
||
84 |
|||
85 |
template<typename M1> |
||
86 |
6701 |
Derived & operator-=(const ForceDense<M1> & v) { return derived().__mequ__(v.derived()); } |
|
87 |
|||
88 |
✓✗✓✗ ✓✗✓✗ |
1 |
ForcePlain __opposite__() const { return ForcePlain(-linear(),-angular()); } |
89 |
|||
90 |
template<typename M1> |
||
91 |
14416 |
ForcePlain __plus__(const ForceDense<M1> & v) const |
|
92 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
14416 |
{ return ForcePlain(linear()+v.linear(), angular()+v.angular()); } |
93 |
|||
94 |
template<typename M1> |
||
95 |
204 |
ForcePlain __minus__(const ForceDense<M1> & v) const |
|
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
204 |
{ return ForcePlain(linear()-v.linear(), angular()-v.angular()); } |
97 |
|||
98 |
template<typename M1> |
||
99 |
145517 |
Derived & __pequ__(const ForceDense<M1> & v) |
|
100 |
✓✗✓✗ ✓✗✓✗ |
145517 |
{ linear() += v.linear(); angular() += v.angular(); return derived(); } |
101 |
|||
102 |
template<typename M1> |
||
103 |
6701 |
Derived & __mequ__(const ForceDense<M1> & v) |
|
104 |
✓✗✓✗ ✓✗✓✗ |
6701 |
{ linear() -= v.linear(); angular() -= v.angular(); return derived(); } |
105 |
|||
106 |
template<typename OtherScalar> |
||
107 |
117 |
ForcePlain __mult__(const OtherScalar & alpha) const |
|
108 |
✓✗✓✗ ✓✗✓✗ |
117 |
{ return ForcePlain(alpha*linear(),alpha*angular()); } |
109 |
|||
110 |
template<typename OtherScalar> |
||
111 |
1 |
ForcePlain __div__(const OtherScalar & alpha) const |
|
112 |
✓✗ | 2 |
{ return derived().__mult__((OtherScalar)(1)/alpha); } |
113 |
|||
114 |
template<typename F1> |
||
115 |
Scalar dot(const MotionDense<F1> & phi) const |
||
116 |
{ return phi.linear().dot(linear()) + phi.angular().dot(angular()); } |
||
117 |
|||
118 |
template<typename M1, typename M2> |
||
119 |
61076 |
void motionAction(const MotionDense<M1> & v, ForceDense<M2> & fout) const |
|
120 |
{ |
||
121 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
61076 |
fout.linear().noalias() = v.angular().cross(linear()); |
122 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
61076 |
fout.angular().noalias() = v.angular().cross(angular())+v.linear().cross(linear()); |
123 |
61076 |
} |
|
124 |
|||
125 |
template<typename M1> |
||
126 |
52929 |
ForcePlain motionAction(const MotionDense<M1> & v) const |
|
127 |
{ |
||
128 |
52929 |
ForcePlain res; |
|
129 |
52929 |
motionAction(v,res); |
|
130 |
52929 |
return res; |
|
131 |
} |
||
132 |
|||
133 |
template<typename M2> |
||
134 |
16 |
bool isApprox(const ForceDense<M2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
|
135 |
16 |
{ return derived().isApprox_impl(f, prec);} |
|
136 |
|||
137 |
template<typename D2> |
||
138 |
234 |
bool isApprox_impl(const ForceDense<D2> & f, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
|
139 |
{ |
||
140 |
✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✓✗ |
234 |
return linear().isApprox(f.linear(), prec) && angular().isApprox(f.angular(), prec); |
141 |
} |
||
142 |
|||
143 |
2 |
bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
|
144 |
{ |
||
145 |
✓✗✓✗ ✓✓✓✗ ✓✗✓✗ |
2 |
return linear().isZero(prec) && angular().isZero(prec); |
146 |
} |
||
147 |
|||
148 |
template<typename S2, int O2, typename D2> |
||
149 |
69332 |
void se3Action_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const |
|
150 |
{ |
||
151 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
69332 |
f.linear().noalias() = m.rotation()*linear(); |
152 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
69332 |
f.angular().noalias() = m.rotation()*angular(); |
153 |
✓✗✓✗ ✓✗ |
69332 |
f.angular() += m.translation().cross(f.linear()); |
154 |
69332 |
} |
|
155 |
|||
156 |
template<typename S2, int O2> |
||
157 |
63424 |
ForcePlain se3Action_impl(const SE3Tpl<S2,O2> & m) const |
|
158 |
{ |
||
159 |
63424 |
ForcePlain res; |
|
160 |
63424 |
se3Action_impl(m,res); |
|
161 |
63424 |
return res; |
|
162 |
} |
||
163 |
|||
164 |
template<typename S2, int O2, typename D2> |
||
165 |
965 |
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, ForceDense<D2> & f) const |
|
166 |
{ |
||
167 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
965 |
f.linear().noalias() = m.rotation().transpose()*linear(); |
168 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
965 |
f.angular().noalias() = m.rotation().transpose()*(angular()-m.translation().cross(linear())); |
169 |
965 |
} |
|
170 |
|||
171 |
template<typename S2, int O2> |
||
172 |
965 |
ForcePlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const |
|
173 |
{ |
||
174 |
965 |
ForcePlain res; |
|
175 |
965 |
se3ActionInverse_impl(m,res); |
|
176 |
965 |
return res; |
|
177 |
} |
||
178 |
|||
179 |
1 |
void disp_impl(std::ostream & os) const |
|
180 |
{ |
||
181 |
os |
||
182 |
✓✗✓✗ ✓✗ |
1 |
<< " f = " << linear().transpose () << std::endl |
183 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
<< "tau = " << angular().transpose () << std::endl; |
184 |
1 |
} |
|
185 |
|||
186 |
/// \returns a ForceRef on this. |
||
187 |
ForceRefType ref() { return derived().ref(); } |
||
188 |
|||
189 |
}; // class ForceDense |
||
190 |
|||
191 |
/// Basic operations specialization |
||
192 |
template<typename F1> |
||
193 |
109 |
typename traits<F1>::ForcePlain operator*(const typename traits<F1>::Scalar alpha, |
|
194 |
const ForceDense<F1> & f) |
||
195 |
109 |
{ return f.derived()*alpha; } |
|
196 |
|||
197 |
} // namespace pinocchio |
||
198 |
|||
199 |
#endif // ifndef __pinocchio_force_dense_hpp__ |
Generated by: GCOVR (Version 4.2) |