GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include <iostream> |
||
6 |
|||
7 |
#include "pinocchio/spatial/force.hpp" |
||
8 |
#include "pinocchio/spatial/motion.hpp" |
||
9 |
#include "pinocchio/spatial/se3.hpp" |
||
10 |
#include "pinocchio/spatial/inertia.hpp" |
||
11 |
#include "pinocchio/spatial/act-on-set.hpp" |
||
12 |
#include "pinocchio/spatial/explog.hpp" |
||
13 |
#include "pinocchio/spatial/skew.hpp" |
||
14 |
#include "pinocchio/spatial/cartesian-axis.hpp" |
||
15 |
#include "pinocchio/spatial/spatial-axis.hpp" |
||
16 |
|||
17 |
#include <boost/test/unit_test.hpp> |
||
18 |
#include <boost/utility/binary.hpp> |
||
19 |
|||
20 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
21 |
|||
22 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_SE3 ) |
23 |
{ |
||
24 |
using namespace pinocchio; |
||
25 |
typedef SE3::HomogeneousMatrixType HomogeneousMatrixType; |
||
26 |
typedef SE3::ActionMatrixType ActionMatrixType; |
||
27 |
typedef SE3::Vector3 Vector3; |
||
28 |
typedef Eigen::Matrix<double,4,1> Vector4; |
||
29 |
|||
30 |
✓✗ | 2 |
const SE3 identity = SE3::Identity(); |
31 |
|||
32 |
typedef SE3::Quaternion Quaternion; |
||
33 |
typedef SE3::Vector4 Vector4; |
||
34 |
✓✗✓✗ ✓✗ |
2 |
Quaternion quat(Vector4::Random().normalized()); |
35 |
|||
36 |
✓✗✓✗ |
2 |
SE3 m_from_quat(quat,Vector3::Random()); |
37 |
|||
38 |
✓✗ | 2 |
SE3 amb = SE3::Random(); |
39 |
✓✗ | 2 |
SE3 bmc = SE3::Random(); |
40 |
✓✗ | 2 |
SE3 amc = amb*bmc; |
41 |
|||
42 |
✓✗ | 2 |
HomogeneousMatrixType aMb = amb; |
43 |
✓✗ | 2 |
HomogeneousMatrixType bMc = bmc; |
44 |
|||
45 |
// Test internal product |
||
46 |
✓✗ | 2 |
HomogeneousMatrixType aMc = amc; |
47 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(aMc.isApprox(aMb*bMc)); |
48 |
|||
49 |
✓✗✓✗ |
2 |
HomogeneousMatrixType bMa = amb.inverse(); |
50 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(bMa.isApprox(aMb.inverse())); |
51 |
|||
52 |
// Test point action |
||
53 |
✓✗✓✗ |
2 |
Vector3 p = Vector3::Random(); |
54 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vector4 p4; p4.head(3) = p; p4[3] = 1; |
55 |
|||
56 |
✓✗✓✗ ✓✗ |
2 |
Vector3 Mp = (aMb*p4).head(3); |
57 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(amb.act(p).isApprox(Mp)); |
58 |
|||
59 |
✓✗✓✗ ✓✗✓✗ |
2 |
Vector3 Mip = (aMb.inverse()*p4).head(3); |
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(amb.actInv(p).isApprox(Mip)); |
61 |
|||
62 |
// Test action matrix |
||
63 |
✓✗ | 2 |
ActionMatrixType aXb = amb; |
64 |
✓✗ | 2 |
ActionMatrixType bXc = bmc; |
65 |
✓✗ | 2 |
ActionMatrixType aXc = amc; |
66 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(aXc.isApprox(aXb*bXc)); |
67 |
|||
68 |
✓✗✓✗ |
2 |
ActionMatrixType bXa = amb.inverse(); |
69 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(bXa.isApprox(aXb.inverse())); |
70 |
|||
71 |
✓✗ | 2 |
ActionMatrixType X_identity = identity.toActionMatrix(); |
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(X_identity.isIdentity()); |
73 |
|||
74 |
✓✗ | 2 |
ActionMatrixType X_identity_inverse = identity.toActionMatrixInverse(); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(X_identity_inverse.isIdentity()); |
76 |
|||
77 |
// Test dual action matrix |
||
78 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix())); |
79 |
|||
80 |
// Test isIdentity |
||
81 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(identity.isIdentity()); |
82 |
|||
83 |
// Test isApprox |
||
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(identity.isApprox(identity)); |
85 |
|||
86 |
// Test cast |
||
87 |
typedef SE3Tpl<float> SE3f; |
||
88 |
✓✗✓✗ ✓✗ |
2 |
SE3f::Matrix3 rot_float(amb.rotation().cast<float>()); |
89 |
✓✗ | 2 |
SE3f amb_float = amb.cast<float>(); |
90 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(amb_float.isApprox(amb.cast<float>())); |
91 |
|||
92 |
// Test actInv |
||
93 |
✓✗ | 2 |
const SE3 M = SE3::Random(); |
94 |
✓✗ | 2 |
const SE3 Minv = M.inverse(); |
95 |
|||
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Minv.actInv(Minv).isIdentity()); |
97 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(M.actInv(identity).isApprox(Minv)); |
98 |
|||
99 |
// Test normalization |
||
100 |
{ |
||
101 |
2 |
const double prec = Eigen::NumTraits<double>::dummy_precision(); |
|
102 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
103 |
✓✗✓✗ ✓✗✓✗ |
2 |
M.rotation() += prec * SE3::Matrix3::Random(); |
104 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!M.isNormalized()); |
105 |
|||
106 |
✓✗ | 2 |
SE3 M_normalized = M.normalized(); |
107 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M_normalized.isNormalized()); |
108 |
|||
109 |
✓✗ | 2 |
M.normalize(); |
110 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.isNormalized()); |
111 |
} |
||
112 |
|||
113 |
2 |
} |
|
114 |
|||
115 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_Motion ) |
116 |
{ |
||
117 |
using namespace pinocchio; |
||
118 |
typedef SE3::ActionMatrixType ActionMatrixType; |
||
119 |
typedef Motion::Vector6 Vector6; |
||
120 |
|||
121 |
✓✗ | 2 |
SE3 amb = SE3::Random(); |
122 |
✓✗ | 2 |
SE3 bmc = SE3::Random(); |
123 |
✓✗ | 2 |
SE3 amc = amb*bmc; |
124 |
|||
125 |
✓✗ | 2 |
Motion bv = Motion::Random(); |
126 |
✓✗ | 2 |
Motion bv2 = Motion::Random(); |
127 |
|||
128 |
typedef MotionBase<Motion> Base; |
||
129 |
|||
130 |
✓✗ | 2 |
Vector6 bv_vec = bv; |
131 |
✓✗ | 2 |
Vector6 bv2_vec = bv2; |
132 |
|||
133 |
// std::stringstream |
||
134 |
✓✗ | 4 |
std::stringstream ss; |
135 |
✓✗✓✗ |
2 |
ss << bv << std::endl; |
136 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!ss.str().empty()); |
137 |
|||
138 |
// Test .+. |
||
139 |
✓✗✓✗ |
2 |
Vector6 bvPbv2_vec = bv+bv2; |
140 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(bvPbv2_vec.isApprox(bv_vec+bv2_vec)); |
141 |
|||
142 |
✓✗ | 2 |
Motion bplus = static_cast<Base &>(bv) + static_cast<Base &>(bv2); |
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((bv+bv2).isApprox(bplus)); |
144 |
|||
145 |
✓✗✓✗ |
2 |
Motion v_not_zero(Vector6::Ones()); |
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!v_not_zero.isZero()); |
147 |
|||
148 |
✓✗✓✗ |
2 |
Motion v_zero(Vector6::Zero()); |
149 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_zero.isZero()); |
150 |
|||
151 |
// Test == and != |
||
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bv == bv); |
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!(bv != bv)); |
154 |
|||
155 |
// Test -. |
||
156 |
✓✗✓✗ |
2 |
Vector6 Mbv_vec = -bv; |
157 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK( Mbv_vec.isApprox(-bv_vec)); |
158 |
|||
159 |
// Test .+=. |
||
160 |
✓✗✓✗ |
2 |
Motion bv3 = bv; bv3 += bv2; |
161 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK( bv3.toVector().isApprox(bv_vec+bv2_vec)); |
162 |
|||
163 |
// Test .=V6 |
||
164 |
✓✗ | 2 |
bv3 = bv2_vec; |
165 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK( bv3.toVector().isApprox(bv2_vec)); |
166 |
|||
167 |
// Test scalar*M6 |
||
168 |
✓✗ | 2 |
Motion twicebv(2.*bv); |
169 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(twicebv.isApprox(Motion(2.*bv.toVector()))); |
170 |
|||
171 |
// Test M6*scalar |
||
172 |
✓✗ | 2 |
Motion bvtwice(bv*2.); |
173 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bvtwice.isApprox(twicebv)); |
174 |
|||
175 |
// Test M6/scalar |
||
176 |
✓✗ | 2 |
Motion bvdividedbytwo(bvtwice/2.); |
177 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bvdividedbytwo.isApprox(bv)); |
178 |
|||
179 |
// Test constructor from V6 |
||
180 |
✓✗ | 2 |
Motion bv4(bv2_vec); |
181 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK( bv4.toVector().isApprox(bv2_vec)); |
182 |
|||
183 |
// Test action |
||
184 |
✓✗ | 2 |
ActionMatrixType aXb = amb; |
185 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(amb.act(bv).toVector().isApprox(aXb*bv_vec)); |
186 |
|||
187 |
// Test action inverse |
||
188 |
✓✗ | 2 |
ActionMatrixType bXc = bmc; |
189 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec)); |
190 |
|||
191 |
// Test double action |
||
192 |
✓✗ | 2 |
Motion cv = Motion::Random(); |
193 |
✓✗ | 2 |
bv = bmc.act(cv); |
194 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(amb.act(bv).toVector().isApprox(amc.act(cv).toVector())); |
195 |
|||
196 |
// Simple test for cross product vxv |
||
197 |
✓✗ | 2 |
Motion vxv = bv.cross(bv); |
198 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(vxv.toVector().tail(3).norm(), 1e-3); //previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3)); |
199 |
|||
200 |
// Test Action Matrix |
||
201 |
✓✗ | 2 |
Motion v2xv = bv2.cross(bv); |
202 |
✓✗ | 2 |
Motion::ActionMatrixType actv2 = bv2.toActionMatrix(); |
203 |
|||
204 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v2xv.toVector().isApprox(actv2*bv.toVector())); |
205 |
|||
206 |
// Test Dual Action Matrix |
||
207 |
✓✗✓✗ |
2 |
Force f(bv.toVector()); |
208 |
✓✗ | 2 |
Force v2xf = bv2.cross(f); |
209 |
✓✗ | 2 |
Motion::ActionMatrixType dualactv2 = bv2.toDualActionMatrix(); |
210 |
|||
211 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v2xf.toVector().isApprox(dualactv2*f.toVector())); |
212 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dualactv2.isApprox(-actv2.transpose())); |
213 |
|||
214 |
// Simple test for cross product vxf |
||
215 |
✓✗ | 2 |
Force vxf = bv.cross(f); |
216 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear()))); |
217 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(vxf.angular().norm(), 1e-3);//previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3)); |
218 |
|||
219 |
// Test frame change for vxf |
||
220 |
✓✗ | 2 |
Motion av = Motion::Random(); |
221 |
✓✗ | 2 |
Force af = Force::Random(); |
222 |
✓✗ | 2 |
bv = amb.actInv(av); |
223 |
✓✗ | 2 |
Force bf = amb.actInv(af); |
224 |
✓✗ | 2 |
Force avxf = av.cross(af); |
225 |
✓✗ | 2 |
Force bvxf = bv.cross(bf); |
226 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(avxf.toVector().isApprox(amb.act(bvxf).toVector())); |
227 |
|||
228 |
// Test frame change for vxv |
||
229 |
✓✗ | 2 |
av = Motion::Random(); |
230 |
✓✗ | 2 |
Motion aw = Motion::Random(); |
231 |
✓✗ | 2 |
bv = amb.actInv(av); |
232 |
✓✗ | 2 |
Motion bw = amb.actInv(aw); |
233 |
✓✗ | 2 |
Motion avxw = av.cross(aw); |
234 |
✓✗ | 2 |
Motion bvxw = bv.cross(bw); |
235 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(avxw.toVector().isApprox(amb.act(bvxw).toVector())); |
236 |
|||
237 |
// Test isApprox |
||
238 |
✓✗✓✗ |
2 |
bv.toVector().setOnes(); |
239 |
2 |
const double eps = 1e-6; |
|
240 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bv == bv); |
241 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bv.isApprox(bv)); |
242 |
✓✗ | 2 |
Motion bv_approx(bv); |
243 |
✓✗✓✗ |
2 |
bv_approx.linear()[0] += eps; |
244 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bv_approx.isApprox(bv,eps)); |
245 |
|||
246 |
// Test ref() method |
||
247 |
{ |
||
248 |
✓✗ | 2 |
Motion a(Motion::Random()); |
249 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(a.ref().isApprox(a)); |
250 |
|||
251 |
✓✗ | 2 |
const Motion b(a); |
252 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(b.isApprox(a.ref())); |
253 |
} |
||
254 |
|||
255 |
// Test cast |
||
256 |
{ |
||
257 |
typedef MotionTpl<float> Motionf; |
||
258 |
✓✗ | 2 |
Motion a(Motion::Random()); |
259 |
✓✗ | 2 |
Motionf a_float = a.cast<float>(); |
260 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(a_float.isApprox(a.cast<float>())); |
261 |
} |
||
262 |
2 |
} |
|
263 |
|||
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (test_motion_ref) |
265 |
{ |
||
266 |
using namespace pinocchio; |
||
267 |
typedef Motion::Vector6 Vector6; |
||
268 |
|||
269 |
typedef MotionRef<Vector6> MotionV6; |
||
270 |
|||
271 |
✓✗ | 2 |
Motion v_ref(Motion::Random()); |
272 |
✓✗✓✗ |
2 |
MotionV6 v(v_ref.toVector()); |
273 |
|||
274 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v_ref.isApprox(v)); |
275 |
|||
276 |
✓✗ | 2 |
MotionV6::MotionPlain v2(v*2.); |
277 |
✓✗ | 2 |
Motion v2_ref(v_ref*2.); |
278 |
|||
279 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v2_ref.isApprox(v2)); |
280 |
|||
281 |
✓✗ | 2 |
v2 = v_ref + v; |
282 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v2_ref.isApprox(v2)); |
283 |
|||
284 |
✓✗ | 2 |
v = v2; |
285 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v2.isApprox(v)); |
286 |
|||
287 |
✓✗ | 2 |
v2 = v - v; |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v2.isApprox(Motion::Zero())); |
289 |
|||
290 |
✓✗ | 2 |
SE3 M(SE3::Identity()); |
291 |
✓✗ | 2 |
v2 = M.act(v); |
292 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v2.isApprox(v)); |
293 |
|||
294 |
✓✗ | 2 |
v2 = M.actInv(v); |
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v2.isApprox(v)); |
296 |
|||
297 |
✓✗ | 2 |
Motion v3(Motion::Random()); |
298 |
✓✗ | 2 |
v_ref.setRandom(); |
299 |
✓✗ | 2 |
v = v_ref; |
300 |
✓✗ | 2 |
v2 = v.cross(v3); |
301 |
✓✗ | 2 |
v2_ref = v_ref.cross(v3); |
302 |
|||
303 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v2.isApprox(v2_ref)); |
304 |
|||
305 |
✓✗ | 2 |
v.setRandom(); |
306 |
✓✗ | 2 |
v.setZero(); |
307 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v.isApprox(Motion::Zero())); |
308 |
|||
309 |
// Test ref() method |
||
310 |
{ |
||
311 |
✓✗✓✗ |
2 |
Vector6 v6(Vector6::Random()); |
312 |
✓✗ | 2 |
MotionV6 a(v6); |
313 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a.ref().isApprox(a)); |
314 |
|||
315 |
✓✗ | 2 |
const Motion b(a); |
316 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(b.isApprox(a.ref())); |
317 |
} |
||
318 |
|||
319 |
2 |
} |
|
320 |
|||
321 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_motion_zero) |
322 |
{ |
||
323 |
using namespace pinocchio; |
||
324 |
✓✗ | 2 |
Motion v((MotionZero())); |
325 |
|||
326 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(v.toVector().isZero()); |
327 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(MotionZero() == Motion::Zero()); |
328 |
|||
329 |
// SE3.act |
||
330 |
✓✗ | 2 |
SE3 m(SE3::Random()); |
331 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(m.act(MotionZero()) == Motion::Zero()); |
332 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(m.actInv(MotionZero()) == Motion::Zero()); |
333 |
|||
334 |
// Motion.cross |
||
335 |
✓✗ | 2 |
Motion v2(Motion::Random()); |
336 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v2.cross(MotionZero()) == Motion::Zero()); |
337 |
2 |
} |
|
338 |
|||
339 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_Force ) |
340 |
{ |
||
341 |
using namespace pinocchio; |
||
342 |
typedef SE3::ActionMatrixType ActionMatrixType; |
||
343 |
typedef Force::Vector6 Vector6; |
||
344 |
|||
345 |
✓✗ | 2 |
SE3 amb = SE3::Random(); |
346 |
✓✗ | 2 |
SE3 bmc = SE3::Random(); |
347 |
✓✗ | 2 |
SE3 amc = amb*bmc; |
348 |
|||
349 |
✓✗ | 2 |
Force bf = Force::Random(); |
350 |
✓✗ | 2 |
Force bf2 = Force::Random(); |
351 |
|||
352 |
✓✗ | 2 |
Vector6 bf_vec = bf; |
353 |
✓✗ | 2 |
Vector6 bf2_vec = bf2; |
354 |
|||
355 |
// std::stringstream |
||
356 |
✓✗ | 4 |
std::stringstream ss; |
357 |
✓✗✓✗ |
2 |
ss << bf << std::endl; |
358 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!ss.str().empty()); |
359 |
|||
360 |
// Test .+. |
||
361 |
✓✗✓✗ |
2 |
Vector6 bfPbf2_vec = bf+bf2; |
362 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(bfPbf2_vec.isApprox(bf_vec+bf2_vec)); |
363 |
|||
364 |
// Test -. |
||
365 |
✓✗✓✗ |
2 |
Vector6 Mbf_vec = -bf; |
366 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Mbf_vec.isApprox(-bf_vec)); |
367 |
|||
368 |
// Test .+=. |
||
369 |
✓✗✓✗ |
2 |
Force bf3 = bf; bf3 += bf2; |
370 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bf3.toVector().isApprox(bf_vec+bf2_vec)); |
371 |
|||
372 |
// Test .= V6 |
||
373 |
✓✗ | 2 |
bf3 = bf2_vec; |
374 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(bf3.toVector().isApprox(bf2_vec)); |
375 |
|||
376 |
// Test constructor from V6 |
||
377 |
✓✗ | 2 |
Force bf4(bf2_vec); |
378 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(bf4.toVector().isApprox(bf2_vec)); |
379 |
|||
380 |
// Test action |
||
381 |
✓✗ | 2 |
ActionMatrixType aXb = amb; |
382 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec)); |
383 |
|||
384 |
// Test action inverse |
||
385 |
✓✗ | 2 |
ActionMatrixType bXc = bmc; |
386 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec)); |
387 |
|||
388 |
// Test double action |
||
389 |
✓✗ | 2 |
Force cf = Force::Random(); |
390 |
✓✗✓✗ |
2 |
bf = bmc.act(cf); |
391 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(amb.act(bf).toVector().isApprox(amc.act(cf).toVector())); |
392 |
|||
393 |
// Simple test for cross product |
||
394 |
// Force vxv = bf.cross(bf); |
||
395 |
// ensure that (vxv.toVector().isMuchSmallerThan(bf.toVector())); |
||
396 |
|||
397 |
✓✗✓✗ |
2 |
Force f_not_zero(Vector6::Ones()); |
398 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!f_not_zero.isZero()); |
399 |
|||
400 |
✓✗✓✗ |
2 |
Force f_zero(Vector6::Zero()); |
401 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f_zero.isZero()); |
402 |
|||
403 |
// Test isApprox |
||
404 |
|||
405 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bf == bf); |
406 |
✓✗ | 2 |
bf.setRandom(); |
407 |
✓✗ | 2 |
bf2.setZero(); |
408 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bf == bf); |
409 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bf != bf2); |
410 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(bf.isApprox(bf)); |
411 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!bf.isApprox(bf2)); |
412 |
|||
413 |
2 |
const double eps = 1e-6; |
|
414 |
✓✗ | 2 |
Force bf_approx(bf); |
415 |
✓✗✓✗ |
2 |
bf_approx.linear()[0] += 2.*eps; |
416 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!bf_approx.isApprox(bf,eps)); |
417 |
|||
418 |
// Test ref() method |
||
419 |
{ |
||
420 |
✓✗ | 2 |
Force a(Force::Random()); |
421 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(a.ref().isApprox(a)); |
422 |
|||
423 |
✓✗ | 2 |
const Force b(a); |
424 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(b.isApprox(a.ref())); |
425 |
} |
||
426 |
|||
427 |
// Test cast |
||
428 |
{ |
||
429 |
typedef ForceTpl<float> Forcef; |
||
430 |
✓✗ | 2 |
Force a(Force::Random()); |
431 |
✓✗ | 2 |
Forcef a_float = a.cast<float>(); |
432 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(a_float.isApprox(a.cast<float>())); |
433 |
} |
||
434 |
|||
435 |
// Test scalar multiplication |
||
436 |
2 |
const double alpha = 1.5; |
|
437 |
✓✗ | 2 |
Force b(Force::Random()); |
438 |
✓✗ | 2 |
Force alpha_f = alpha * b; |
439 |
✓✗ | 2 |
Force f_alpha = b * alpha; |
440 |
|||
441 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(alpha_f == f_alpha); |
442 |
2 |
} |
|
443 |
|||
444 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (test_force_ref) |
445 |
{ |
||
446 |
using namespace pinocchio; |
||
447 |
typedef Force::Vector6 Vector6; |
||
448 |
|||
449 |
typedef ForceRef<Vector6> ForceV6; |
||
450 |
|||
451 |
✓✗ | 2 |
Force f_ref(Force::Random()); |
452 |
✓✗✓✗ |
2 |
ForceV6 f(f_ref.toVector()); |
453 |
|||
454 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f_ref.isApprox(f)); |
455 |
|||
456 |
✓✗ | 2 |
ForceV6::ForcePlain f2(f*2.); |
457 |
✓✗ | 2 |
Force f2_ref(f_ref*2.); |
458 |
|||
459 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f2_ref.isApprox(f2)); |
460 |
|||
461 |
✓✗✓✗ |
2 |
f2 = f_ref + f; |
462 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f2_ref.isApprox(f2)); |
463 |
|||
464 |
✓✗ | 2 |
f = f2; |
465 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f2.isApprox(f)); |
466 |
|||
467 |
✓✗✓✗ |
2 |
f2 = f - f; |
468 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(f2.isApprox(Force::Zero())); |
469 |
|||
470 |
✓✗ | 2 |
SE3 M(SE3::Identity()); |
471 |
✓✗✓✗ |
2 |
f2 = M.act(f); |
472 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f2.isApprox(f)); |
473 |
|||
474 |
✓✗✓✗ |
2 |
f2 = M.actInv(f); |
475 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f2.isApprox(f)); |
476 |
|||
477 |
✓✗ | 2 |
Motion v(Motion::Random()); |
478 |
✓✗ | 2 |
f_ref.setRandom(); |
479 |
✓✗ | 2 |
f = f_ref; |
480 |
✓✗✓✗ |
2 |
f2 = v.cross(f); |
481 |
✓✗✓✗ |
2 |
f2_ref = v.cross(f_ref); |
482 |
|||
483 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f2.isApprox(f2_ref)); |
484 |
|||
485 |
✓✗ | 2 |
f.setRandom(); |
486 |
✓✗ | 2 |
f.setZero(); |
487 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(f.isApprox(Force::Zero())); |
488 |
|||
489 |
// Test ref() method |
||
490 |
{ |
||
491 |
✓✗✓✗ |
2 |
Vector6 v6(Vector6::Random()); |
492 |
✓✗ | 2 |
ForceV6 a(v6); |
493 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(a.ref().isApprox(a)); |
494 |
|||
495 |
✓✗ | 2 |
const Force b(a); |
496 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(b.isApprox(a.ref())); |
497 |
} |
||
498 |
2 |
} |
|
499 |
|||
500 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_Inertia ) |
501 |
{ |
||
502 |
using namespace pinocchio; |
||
503 |
typedef Inertia::Matrix6 Matrix6; |
||
504 |
|||
505 |
✓✗ | 2 |
Inertia aI = Inertia::Random(); |
506 |
✓✗ | 2 |
Matrix6 matI = aI; |
507 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_EQUAL(matI(0,0), aI.mass()); |
508 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_EQUAL(matI(1,1), aI.mass()); |
509 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_EQUAL(matI(2,2), aI.mass()); // 1,1 before unifying |
510 |
|||
511 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL((matI-matI.transpose()).norm(),matI.norm()); //previously ensure that( (matI-matI.transpose()).isMuchSmallerThan(matI) ); |
512 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL((matI.topRightCorner<3,3>()*aI.lever()).norm(), |
513 |
aI.lever().norm()); //previously ensure that( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) ); |
||
514 |
|||
515 |
✓✗ | 2 |
Inertia I1 = Inertia::Identity(); |
516 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(I1.matrix().isApprox(Matrix6::Identity())); |
517 |
|||
518 |
// Test motion-to-force map |
||
519 |
✓✗ | 2 |
Motion v = Motion::Random(); |
520 |
✓✗ | 2 |
Force f = I1 * v; |
521 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(f.toVector().isApprox(v.toVector())); |
522 |
|||
523 |
// Test Inertia group application |
||
524 |
✓✗ | 2 |
SE3 bma = SE3::Random(); |
525 |
✓✗ | 2 |
Inertia bI = bma.act(aI); |
526 |
✓✗ | 2 |
Matrix6 bXa = bma; |
527 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((bma.rotation()*aI.inertia().matrix()*bma.rotation().transpose()) |
528 |
.isApprox(bI.inertia().matrix())); |
||
529 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((bXa.transpose().inverse() * aI.matrix() * bXa.inverse()) |
530 |
.isApprox(bI.matrix())); |
||
531 |
|||
532 |
// Test inverse action |
||
533 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((bXa.transpose() * bI.matrix() * bXa) |
534 |
.isApprox(bma.actInv(bI).matrix())); |
||
535 |
|||
536 |
// Test vxIv cross product |
||
537 |
✓✗ | 2 |
v = Motion::Random(); |
538 |
✓✗✓✗ |
2 |
f = aI*v; |
539 |
✓✗ | 2 |
Force vxf = v.cross(f); |
540 |
✓✗ | 2 |
Force vxIv = aI.vxiv(v); |
541 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector())); |
542 |
|||
543 |
// Test operator+ |
||
544 |
✓✗✓✗ |
2 |
I1 = Inertia::Random(); |
545 |
✓✗ | 2 |
Inertia I2 = Inertia::Random(); |
546 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox((I1+I2).matrix())); |
547 |
|||
548 |
// operator += |
||
549 |
✓✗ | 2 |
Inertia I12 = I1; |
550 |
✓✗ | 2 |
I12 += I2; |
551 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox(I12.matrix())); |
552 |
|||
553 |
// Test operator vtiv |
||
554 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector(); |
555 |
✓✗ | 2 |
double kinetic = aI.vtiv(v); |
556 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12); |
557 |
|||
558 |
// Test constructor (Matrix6) |
||
559 |
✓✗✓✗ |
2 |
Inertia I1_bis(I1.matrix()); |
560 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix())); |
561 |
|||
562 |
// Test Inertia from ellipsoid |
||
563 |
2 |
const double sphere_mass = 5.; |
|
564 |
2 |
const double sphere_radius = 2.; |
|
565 |
✓✗✓✗ |
2 |
I1 = Inertia::FromSphere(sphere_mass, sphere_radius); |
566 |
2 |
const double L_sphere = 2./5. * sphere_mass * sphere_radius * sphere_radius; |
|
567 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(I1.mass() - sphere_mass, 1e-12); |
568 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(I1.lever().isZero()); |
569 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(L_sphere, 0., L_sphere , 0., 0., L_sphere).matrix())); |
570 |
|||
571 |
// Test Inertia from ellipsoid |
||
572 |
✓✗✓✗ |
2 |
I1 = Inertia::FromEllipsoid(2., 3., 4., 5.); |
573 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12); |
574 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12); |
575 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3( |
576 |
16.4, 0., 13.6, 0., 0., 10.).matrix())); |
||
577 |
|||
578 |
// Test Inertia from Cylinder |
||
579 |
✓✗✓✗ |
2 |
I1 = Inertia::FromCylinder(2., 4., 6.); |
580 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12); |
581 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12); |
582 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3( |
583 |
14., 0., 14., 0., 0., 16.).matrix())); |
||
584 |
|||
585 |
// Test Inertia from Box |
||
586 |
✓✗✓✗ |
2 |
I1 = Inertia::FromBox(2., 6., 12., 18.); |
587 |
✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12); |
588 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12); |
589 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3( |
590 |
78., 0., 60., 0., 0., 30.).matrix())); |
||
591 |
|||
592 |
// Copy operator |
||
593 |
✓✗ | 2 |
Inertia aI_copy(aI); |
594 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aI_copy == aI); |
595 |
|||
596 |
// Test isZero |
||
597 |
✓✗ | 2 |
Inertia I_not_zero = Inertia::Identity(); |
598 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(!I_not_zero.isZero()); |
599 |
|||
600 |
✓✗ | 2 |
Inertia I_zero = Inertia::Zero(); |
601 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(I_zero.isZero()); |
602 |
|||
603 |
// Test isApprox |
||
604 |
2 |
const double eps = 1e-6; |
|
605 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aI == aI); |
606 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aI.isApprox(aI)); |
607 |
✓✗ | 2 |
Inertia aI_approx(aI); |
608 |
2 |
aI_approx.mass() += eps/2.; |
|
609 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aI_approx.isApprox(aI,eps)); |
610 |
|||
611 |
// Test Variation |
||
612 |
✓✗ | 2 |
Inertia::Matrix6 aIvariation = aI.variation(v); |
613 |
|||
614 |
✓✗ | 2 |
Motion::ActionMatrixType vAction = v.toActionMatrix(); |
615 |
✓✗ | 2 |
Motion::ActionMatrixType vDualAction = v.toDualActionMatrix(); |
616 |
|||
617 |
✓✗ | 2 |
Inertia::Matrix6 aImatrix = aI.matrix(); |
618 |
✓✗✓✗ ✓✗✓✗ |
2 |
Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction; |
619 |
|||
620 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aIvariation.isApprox(aIvariation_ref)); |
621 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(vxIv.isApprox(Force(aIvariation*v.toVector()))); |
622 |
|||
623 |
// Test vxI operator |
||
624 |
{ |
||
625 |
typedef Inertia::Matrix6 Matrix6; |
||
626 |
✓✗ | 2 |
Inertia I(Inertia::Random()); |
627 |
✓✗ | 2 |
Motion v(Motion::Random()); |
628 |
|||
629 |
✓✗✓✗ ✓✗✓✗ |
2 |
const Matrix6 M_ref(v.toDualActionMatrix()*I.matrix()); |
630 |
✓✗✓✗ |
2 |
Matrix6 M; Inertia::vxi(v,I,M); |
631 |
|||
632 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.isApprox(M_ref)); |
633 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(I.vxi(v).isApprox(M_ref)); |
634 |
} |
||
635 |
|||
636 |
// Test Ivx operator |
||
637 |
{ |
||
638 |
typedef Inertia::Matrix6 Matrix6; |
||
639 |
✓✗ | 2 |
Inertia I(Inertia::Random()); |
640 |
✓✗ | 2 |
Motion v(Motion::Random()); |
641 |
|||
642 |
✓✗✓✗ ✓✗✓✗ |
2 |
const Matrix6 M_ref(I.matrix()*v.toActionMatrix()); |
643 |
✓✗✓✗ |
2 |
Matrix6 M; Inertia::ivx(v,I,M); |
644 |
|||
645 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.isApprox(M_ref)); |
646 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(I.ivx(v).isApprox(M_ref)); |
647 |
} |
||
648 |
|||
649 |
// Test variation against vxI - Ivx operator |
||
650 |
{ |
||
651 |
typedef Inertia::Matrix6 Matrix6; |
||
652 |
✓✗ | 2 |
Inertia I(Inertia::Random()); |
653 |
✓✗ | 2 |
Motion v(Motion::Random()); |
654 |
|||
655 |
✓✗ | 2 |
Matrix6 Ivariation = I.variation(v); |
656 |
|||
657 |
✓✗✓✗ |
2 |
Matrix6 M1; Inertia::vxi(v,I,M1); |
658 |
✓✗✓✗ |
2 |
Matrix6 M2; Inertia::ivx(v,I,M2); |
659 |
✓✗✓✗ |
2 |
Matrix6 M3(M1-M2); |
660 |
|||
661 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M3.isApprox(Ivariation)); |
662 |
} |
||
663 |
|||
664 |
// Test dynamic parameters |
||
665 |
{ |
||
666 |
✓✗ | 2 |
Inertia I(Inertia::Random()); |
667 |
|||
668 |
✓✗ | 2 |
Inertia::Vector10 v = I.toDynamicParameters(); |
669 |
|||
670 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK_CLOSE(v[0], I.mass(), 1e-12); |
671 |
|||
672 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.segment<3>(1).isApprox(I.mass()*I.lever())); |
673 |
|||
674 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
Eigen::Matrix3d I_o = I.inertia() + I.mass()*skew(I.lever()).transpose()*skew(I.lever()); |
675 |
✓✗ | 2 |
Eigen::Matrix3d I_ov; |
676 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
I_ov << v[4], v[5], v[7], |
677 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
v[5], v[6], v[8], |
678 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
v[7], v[8], v[9]; |
679 |
|||
680 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(I_o.isApprox(I_ov)); |
681 |
|||
682 |
✓✗ | 2 |
Inertia I2 = Inertia::FromDynamicParameters(v); |
683 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(I2.isApprox(I)); |
684 |
|||
685 |
} |
||
686 |
|||
687 |
// Test disp |
||
688 |
✓✗✓✗ |
2 |
std::cout << aI << std::endl; |
689 |
|||
690 |
2 |
} |
|
691 |
|||
692 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(cast_inertia) |
693 |
{ |
||
694 |
using namespace pinocchio; |
||
695 |
✓✗ | 2 |
Inertia Y(Inertia::Random()); |
696 |
|||
697 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(Y.cast<double>() == Y); |
698 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Y.cast<long double>().cast<double>() == Y); |
699 |
2 |
} |
|
700 |
|||
701 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_ActOnSet ) |
702 |
{ |
||
703 |
using namespace pinocchio; |
||
704 |
2 |
const int N = 20; |
|
705 |
typedef Eigen::Matrix<double,6,N> Matrix6N; |
||
706 |
✓✗ | 2 |
SE3 jMi = SE3::Random(); |
707 |
✓✗ | 2 |
Motion v = Motion::Random(); |
708 |
|||
709 |
// Forcet SET |
||
710 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
Matrix6N iF = Matrix6N::Random(),jF,jFinv,jF_ref,jFinv_ref; |
711 |
|||
712 |
// forceSet::se3Action |
||
713 |
✓✗ | 2 |
forceSet::se3Action(jMi,iF,jF); |
714 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
715 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_CHECK(jMi.act(Force(iF.col(k))).toVector().isApprox(jF.col(k))); |
716 |
|||
717 |
✓✗✓✗ ✓✗ |
2 |
jF_ref = jMi.toDualActionMatrix()*iF; |
718 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF_ref.isApprox(jF)); |
719 |
|||
720 |
✓✗✓✗ |
2 |
forceSet::se3ActionInverse(jMi.inverse(),iF,jFinv); |
721 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jFinv.isApprox(jF)); |
722 |
|||
723 |
✓✗✓✗ |
2 |
Matrix6N iF2 = Matrix6N::Random(); |
724 |
✓✗✓✗ ✓✗ |
2 |
jF_ref += jMi.toDualActionMatrix() * iF2; |
725 |
|||
726 |
✓✗ | 2 |
forceSet::se3Action<ADDTO>(jMi,iF2,jF); |
727 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
728 |
|||
729 |
✓✗✓✗ |
2 |
Matrix6N iF3 = Matrix6N::Random(); |
730 |
✓✗✓✗ ✓✗ |
2 |
jF_ref -= jMi.toDualActionMatrix() * iF3; |
731 |
|||
732 |
✓✗ | 2 |
forceSet::se3Action<RMTO>(jMi,iF3,jF); |
733 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
734 |
|||
735 |
// forceSet::se3ActionInverse |
||
736 |
✓✗ | 2 |
forceSet::se3ActionInverse(jMi,iF,jFinv); |
737 |
✓✗✓✗ ✓✗✓✗ |
2 |
jFinv_ref = jMi.inverse().toDualActionMatrix() * iF; |
738 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jFinv_ref.isApprox(jFinv)); |
739 |
|||
740 |
✓✗✓✗ ✓✗✓✗ |
2 |
jFinv_ref += jMi.inverse().toDualActionMatrix() * iF2; |
741 |
✓✗ | 2 |
forceSet::se3ActionInverse<ADDTO>(jMi,iF2,jFinv); |
742 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jFinv.isApprox(jFinv_ref)); |
743 |
|||
744 |
✓✗✓✗ ✓✗✓✗ |
2 |
jFinv_ref -= jMi.inverse().toDualActionMatrix() * iF3; |
745 |
✓✗ | 2 |
forceSet::se3ActionInverse<RMTO>(jMi,iF3,jFinv); |
746 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jFinv.isApprox(jFinv_ref)); |
747 |
|||
748 |
// forceSet::motionAction |
||
749 |
✓✗ | 2 |
forceSet::motionAction(v,iF,jF); |
750 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
751 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_CHECK(v.cross(Force(iF.col(k))).toVector().isApprox(jF.col(k))); |
752 |
|||
753 |
✓✗✓✗ ✓✗ |
2 |
jF_ref = v.toDualActionMatrix() * iF; |
754 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
755 |
|||
756 |
✓✗✓✗ ✓✗ |
2 |
jF_ref += v.toDualActionMatrix() * iF2; |
757 |
✓✗ | 2 |
forceSet::motionAction<ADDTO>(v,iF2,jF); |
758 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
759 |
|||
760 |
✓✗✓✗ ✓✗ |
2 |
jF_ref -= v.toDualActionMatrix() * iF3; |
761 |
✓✗ | 2 |
forceSet::motionAction<RMTO>(v,iF3,jF); |
762 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
763 |
|||
764 |
// Motion SET |
||
765 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
Matrix6N iV = Matrix6N::Random(),jV,jV_ref,jVinv,jVinv_ref; |
766 |
|||
767 |
// motionSet::se3Action |
||
768 |
✓✗ | 2 |
motionSet::se3Action(jMi,iV,jV); |
769 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
770 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_CHECK(jMi.act(Motion(iV.col(k))).toVector().isApprox(jV.col(k))); |
771 |
|||
772 |
✓✗✓✗ ✓✗ |
2 |
jV_ref = jMi.toActionMatrix()*iV; |
773 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
774 |
|||
775 |
✓✗✓✗ |
2 |
motionSet::se3ActionInverse(jMi.inverse(),iV,jVinv); |
776 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jVinv.isApprox(jV)); |
777 |
|||
778 |
✓✗✓✗ |
2 |
Matrix6N iV2 = Matrix6N::Random(); |
779 |
✓✗✓✗ ✓✗ |
2 |
jV_ref += jMi.toActionMatrix()*iV2; |
780 |
✓✗ | 2 |
motionSet::se3Action<ADDTO>(jMi,iV2,jV); |
781 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
782 |
|||
783 |
✓✗✓✗ |
2 |
Matrix6N iV3 = Matrix6N::Random(); |
784 |
✓✗✓✗ ✓✗ |
2 |
jV_ref -= jMi.toActionMatrix()*iV3; |
785 |
✓✗ | 2 |
motionSet::se3Action<RMTO>(jMi,iV3,jV); |
786 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
787 |
|||
788 |
// motionSet::se3ActionInverse |
||
789 |
✓✗ | 2 |
motionSet::se3ActionInverse(jMi,iV,jVinv); |
790 |
✓✗✓✗ ✓✗✓✗ |
2 |
jVinv_ref = jMi.inverse().toActionMatrix() * iV; |
791 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jVinv.isApprox(jVinv_ref)); |
792 |
|||
793 |
✓✗✓✗ ✓✗✓✗ |
2 |
jVinv_ref += jMi.inverse().toActionMatrix()*iV2; |
794 |
✓✗ | 2 |
motionSet::se3ActionInverse<ADDTO>(jMi,iV2,jVinv); |
795 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jVinv.isApprox(jVinv_ref)); |
796 |
|||
797 |
✓✗✓✗ ✓✗✓✗ |
2 |
jVinv_ref -= jMi.inverse().toActionMatrix()*iV3; |
798 |
✓✗ | 2 |
motionSet::se3ActionInverse<RMTO>(jMi,iV3,jVinv); |
799 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jVinv.isApprox(jVinv_ref)); |
800 |
|||
801 |
// motionSet::motionAction |
||
802 |
✓✗ | 2 |
motionSet::motionAction(v,iV,jV); |
803 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
804 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_CHECK(v.cross(Motion(iV.col(k))).toVector().isApprox(jV.col(k))); |
805 |
|||
806 |
✓✗✓✗ ✓✗ |
2 |
jV_ref = v.toActionMatrix()*iV; |
807 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
808 |
|||
809 |
✓✗✓✗ ✓✗ |
2 |
jV_ref += v.toActionMatrix()*iV2; |
810 |
✓✗ | 2 |
motionSet::motionAction<ADDTO>(v,iV2,jV); |
811 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
812 |
|||
813 |
✓✗✓✗ ✓✗ |
2 |
jV_ref -= v.toActionMatrix()*iV3; |
814 |
✓✗ | 2 |
motionSet::motionAction<RMTO>(v,iV3,jV); |
815 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
816 |
|||
817 |
// motionSet::inertiaAction |
||
818 |
✓✗ | 2 |
const Inertia I(Inertia::Random()); |
819 |
✓✗ | 2 |
motionSet::inertiaAction(I,iV,jV); |
820 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
821 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_CHECK((I*(Motion(iV.col(k)))).toVector().isApprox(jV.col(k))); |
822 |
|||
823 |
✓✗✓✗ ✓✗ |
2 |
jV_ref = I.matrix()*iV; |
824 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
825 |
|||
826 |
✓✗✓✗ ✓✗ |
2 |
jV_ref += I.matrix()*iV2; |
827 |
✓✗ | 2 |
motionSet::inertiaAction<ADDTO>(I,iV2,jV); |
828 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
829 |
|||
830 |
✓✗✓✗ ✓✗ |
2 |
jV_ref -= I.matrix()*iV3; |
831 |
✓✗ | 2 |
motionSet::inertiaAction<RMTO>(I,iV3,jV); |
832 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jV.isApprox(jV_ref)); |
833 |
|||
834 |
// motionSet::act |
||
835 |
✓✗ | 2 |
Force f = Force::Random(); |
836 |
✓✗ | 2 |
motionSet::act(iV,f,jF); |
837 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
838 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
40 |
BOOST_CHECK(Motion(iV.col(k)).cross(f).toVector().isApprox(jF.col(k))); |
839 |
|||
840 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
841 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
40 |
jF_ref.col(k) = Force(Motion(iV.col(k)).cross(f)).toVector(); |
842 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
843 |
|||
844 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
845 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
40 |
jF_ref.col(k) += Force(Motion(iV2.col(k)).cross(f)).toVector(); |
846 |
✓✗ | 2 |
motionSet::act<ADDTO>(iV2,f,jF); |
847 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
848 |
|||
849 |
✓✓ | 42 |
for( int k=0;k<N;++k ) |
850 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
40 |
jF_ref.col(k) -= Force(Motion(iV3.col(k)).cross(f)).toVector(); |
851 |
✓✗ | 2 |
motionSet::act<RMTO>(iV3,f,jF); |
852 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(jF.isApprox(jF_ref)); |
853 |
2 |
} |
|
854 |
|||
855 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_skew) |
856 |
{ |
||
857 |
using namespace pinocchio; |
||
858 |
typedef SE3::Vector3 Vector3; |
||
859 |
typedef Motion::Vector6 Vector6; |
||
860 |
|||
861 |
✓✗✓✗ |
2 |
Vector3 v3(Vector3::Random()); |
862 |
✓✗✓✗ |
2 |
Vector6 v6(Vector6::Random()); |
863 |
|||
864 |
✓✗✓✗ |
2 |
Vector3 res1 = unSkew(skew(v3)); |
865 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(res1.isApprox(v3)); |
866 |
|||
867 |
✓✗✓✗ ✓✗ |
2 |
Vector3 res2 = unSkew(skew(v6.head<3>())); |
868 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(res2.isApprox(v6.head<3>())); |
869 |
|||
870 |
✓✗✓✗ ✓✗ |
2 |
Vector3 res3 = skew(v3)*v3; |
871 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(res3.isZero()); |
872 |
|||
873 |
✓✗✓✗ |
2 |
Vector3 rhs(Vector3::Random()); |
874 |
✓✗✓✗ ✓✗ |
2 |
Vector3 res41 = skew(v3)*rhs; |
875 |
✓✗ | 2 |
Vector3 res42 = v3.cross(rhs); |
876 |
|||
877 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(res41.isApprox(res42)); |
878 |
|||
879 |
2 |
} |
|
880 |
|||
881 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_addSkew) |
882 |
{ |
||
883 |
using namespace pinocchio; |
||
884 |
typedef SE3::Vector3 Vector3; |
||
885 |
typedef SE3::Matrix3 Matrix3; |
||
886 |
|||
887 |
✓✗✓✗ |
2 |
Vector3 v(Vector3::Random()); |
888 |
✓✗✓✗ |
2 |
Matrix3 M(Matrix3::Random()); |
889 |
✓✗ | 2 |
Matrix3 Mcopy(M); |
890 |
|||
891 |
✓✗ | 2 |
addSkew(v,M); |
892 |
✓✗✓✗ ✓✗ |
2 |
Matrix3 Mref = Mcopy + skew(v); |
893 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.isApprox(Mref)); |
894 |
|||
895 |
✓✗✓✗ ✓✗ |
2 |
Mref += skew(-v); |
896 |
✓✗✓✗ |
2 |
addSkew(-v,M); |
897 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.isApprox(Mcopy)); |
898 |
|||
899 |
✓✗ | 2 |
M.setZero(); |
900 |
✓✗ | 2 |
addSkew(v,M); |
901 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(M.isApprox(skew(v))); |
902 |
2 |
} |
|
903 |
|||
904 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_skew_square) |
905 |
{ |
||
906 |
using namespace pinocchio; |
||
907 |
typedef SE3::Vector3 Vector3; |
||
908 |
typedef SE3::Matrix3 Matrix3; |
||
909 |
|||
910 |
✓✗✓✗ |
2 |
Vector3 u(Vector3::Random()); |
911 |
✓✗✓✗ |
2 |
Vector3 v(Vector3::Random()); |
912 |
|||
913 |
✓✗✓✗ ✓✗✓✗ |
2 |
Matrix3 ref = skew(u) * skew(v); |
914 |
|||
915 |
✓✗ | 2 |
Matrix3 res = skewSquare(u,v); |
916 |
|||
917 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(res.isApprox(ref)); |
918 |
2 |
} |
|
919 |
|||
920 |
template<int axis> |
||
921 |
struct test_scalar_multiplication_cartesian_axis |
||
922 |
{ |
||
923 |
typedef pinocchio::CartesianAxis<axis> Axis; |
||
924 |
typedef double Scalar; |
||
925 |
typedef Eigen::Matrix<Scalar,3,1> Vector3; |
||
926 |
|||
927 |
6 |
static void run() |
|
928 |
{ |
||
929 |
6 |
const Scalar alpha = static_cast <Scalar> (rand()) / static_cast <Scalar> (RAND_MAX); |
|
930 |
✓✗ | 6 |
const Vector3 r1 = Axis() * alpha; |
931 |
✓✗ | 6 |
const Vector3 r2 = alpha * Axis(); |
932 |
|||
933 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
6 |
BOOST_CHECK(r1.isApprox(r2)); |
934 |
|||
935 |
✓✓ | 24 |
for(int k = 0; k < Axis::dim; ++k) |
936 |
{ |
||
937 |
✓✓ | 18 |
if(k==axis) |
938 |
{ |
||
939 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
6 |
BOOST_CHECK(r1[k] == alpha); |
940 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
6 |
BOOST_CHECK(r2[k] == alpha); |
941 |
} |
||
942 |
else |
||
943 |
{ |
||
944 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(r1[k] == Scalar(0)); |
945 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(r2[k] == Scalar(0)); |
946 |
} |
||
947 |
} |
||
948 |
6 |
} |
|
949 |
}; |
||
950 |
|||
951 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_cartesian_axis) |
952 |
{ |
||
953 |
using namespace Eigen; |
||
954 |
using namespace pinocchio; |
||
955 |
✓✗✓✗ |
2 |
Vector3d v(Vector3d::Random()); |
956 |
2 |
const double alpha = 3; |
|
957 |
✓✗✓✗ |
2 |
Vector3d v2(alpha*v); |
958 |
|||
959 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(AxisX::cross(v).isApprox(Vector3d::Unit(0).cross(v))); |
960 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(AxisY::cross(v).isApprox(Vector3d::Unit(1).cross(v))); |
961 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(AxisZ::cross(v).isApprox(Vector3d::Unit(2).cross(v))); |
962 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(AxisX::alphaCross(alpha,v).isApprox(Vector3d::Unit(0).cross(v2))); |
963 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(AxisY::alphaCross(alpha,v).isApprox(Vector3d::Unit(1).cross(v2))); |
964 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(AxisZ::alphaCross(alpha,v).isApprox(Vector3d::Unit(2).cross(v2))); |
965 |
|||
966 |
✓✗ | 2 |
test_scalar_multiplication_cartesian_axis<0>::run(); |
967 |
✓✗ | 2 |
test_scalar_multiplication_cartesian_axis<1>::run(); |
968 |
✓✗ | 2 |
test_scalar_multiplication_cartesian_axis<2>::run(); |
969 |
2 |
} |
|
970 |
|||
971 |
template<int axis> |
||
972 |
struct test_scalar_multiplication |
||
973 |
{ |
||
974 |
typedef pinocchio::SpatialAxis<axis> Axis; |
||
975 |
typedef double Scalar; |
||
976 |
typedef pinocchio::MotionTpl<Scalar> Motion; |
||
977 |
|||
978 |
12 |
static void run() |
|
979 |
{ |
||
980 |
12 |
const Scalar alpha = static_cast <Scalar> (rand()) / static_cast <Scalar> (RAND_MAX); |
|
981 |
✓✗ | 12 |
const Motion r1 = Axis() * alpha; |
982 |
✓✗ | 12 |
const Motion r2 = alpha * Axis(); |
983 |
|||
984 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
12 |
BOOST_CHECK(r1.isApprox(r2)); |
985 |
|||
986 |
✓✓ | 84 |
for(int k = 0; k < Axis::dim; ++k) |
987 |
{ |
||
988 |
✓✓ | 72 |
if(k==axis) |
989 |
{ |
||
990 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(r1.toVector()[k] == alpha); |
991 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
12 |
BOOST_CHECK(r2.toVector()[k] == alpha); |
992 |
} |
||
993 |
else |
||
994 |
{ |
||
995 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
60 |
BOOST_CHECK(r1.toVector()[k] == Scalar(0)); |
996 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
60 |
BOOST_CHECK(r2.toVector()[k] == Scalar(0)); |
997 |
} |
||
998 |
} |
||
999 |
12 |
} |
|
1000 |
}; |
||
1001 |
|||
1002 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_spatial_axis) |
1003 |
{ |
||
1004 |
using namespace pinocchio; |
||
1005 |
|||
1006 |
✓✗ | 2 |
Motion v(Motion::Random()); |
1007 |
✓✗ | 2 |
Force f(Force::Random()); |
1008 |
|||
1009 |
✓✗ | 2 |
Motion vaxis; |
1010 |
✓✗ | 2 |
vaxis << AxisVX(); |
1011 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisVX::cross(v).isApprox(vaxis.cross(v))); |
1012 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(AxisVX()).isApprox(v.cross(vaxis))); |
1013 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisVX::cross(f).isApprox(vaxis.cross(f))); |
1014 |
|||
1015 |
✓✗ | 2 |
vaxis << AxisVY(); |
1016 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisVY::cross(v).isApprox(vaxis.cross(v))); |
1017 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(AxisVY()).isApprox(v.cross(vaxis))); |
1018 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisVY::cross(f).isApprox(vaxis.cross(f))); |
1019 |
|||
1020 |
✓✗ | 2 |
vaxis << AxisVZ(); |
1021 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisVZ::cross(v).isApprox(vaxis.cross(v))); |
1022 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(AxisVZ()).isApprox(v.cross(vaxis))); |
1023 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisVZ::cross(f).isApprox(vaxis.cross(f))); |
1024 |
|||
1025 |
✓✗ | 2 |
vaxis << AxisWX(); |
1026 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisWX::cross(v).isApprox(vaxis.cross(v))); |
1027 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(AxisWX()).isApprox(v.cross(vaxis))); |
1028 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisWX::cross(f).isApprox(vaxis.cross(f))); |
1029 |
|||
1030 |
✓✗ | 2 |
vaxis << AxisWY(); |
1031 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisWY::cross(v).isApprox(vaxis.cross(v))); |
1032 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(AxisWY()).isApprox(v.cross(vaxis))); |
1033 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisWY::cross(f).isApprox(vaxis.cross(f))); |
1034 |
|||
1035 |
✓✗ | 2 |
vaxis << AxisWZ(); |
1036 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisWZ::cross(v).isApprox(vaxis.cross(v))); |
1037 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(v.cross(AxisWZ()).isApprox(v.cross(vaxis))); |
1038 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(AxisWZ::cross(f).isApprox(vaxis.cross(f))); |
1039 |
|||
1040 |
// Test operation Axis * Scalar |
||
1041 |
✓✗ | 2 |
test_scalar_multiplication<0>::run(); |
1042 |
✓✗ | 2 |
test_scalar_multiplication<1>::run(); |
1043 |
✓✗ | 2 |
test_scalar_multiplication<2>::run(); |
1044 |
✓✗ | 2 |
test_scalar_multiplication<3>::run(); |
1045 |
✓✗ | 2 |
test_scalar_multiplication<4>::run(); |
1046 |
✓✗ | 2 |
test_scalar_multiplication<5>::run(); |
1047 |
|||
1048 |
// Operations of Constraint on forces Sxf |
||
1049 |
typedef Motion::ActionMatrixType ActionMatrixType; |
||
1050 |
typedef ActionMatrixType::ColXpr ColType; |
||
1051 |
typedef ForceRef<ColType> ForceRefOnColType; |
||
1052 |
typedef MotionRef<ColType> MotionRefOnColType; |
||
1053 |
✓✗✓✗ |
2 |
ActionMatrixType Sxf,Sxf_ref; |
1054 |
✓✗✓✗ |
2 |
ActionMatrixType S(ActionMatrixType::Identity()); |
1055 |
|||
1056 |
✓✗✓✗ ✓✗ |
2 |
SpatialAxis<0>::cross(f,ForceRefOnColType(Sxf.col(0))); |
1057 |
✓✗✓✗ ✓✗ |
2 |
SpatialAxis<1>::cross(f,ForceRefOnColType(Sxf.col(1))); |
1058 |
✓✗✓✗ ✓✗ |
2 |
SpatialAxis<2>::cross(f,ForceRefOnColType(Sxf.col(2))); |
1059 |
✓✗✓✗ ✓✗ |
2 |
SpatialAxis<3>::cross(f,ForceRefOnColType(Sxf.col(3))); |
1060 |
✓✗✓✗ ✓✗ |
2 |
SpatialAxis<4>::cross(f,ForceRefOnColType(Sxf.col(4))); |
1061 |
✓✗✓✗ ✓✗ |
2 |
SpatialAxis<5>::cross(f,ForceRefOnColType(Sxf.col(5))); |
1062 |
|||
1063 |
✓✓ | 14 |
for(int k = 0; k < 6; ++k) |
1064 |
{ |
||
1065 |
✓✗✓✗ |
12 |
MotionRefOnColType Scol(S.col(k)); |
1066 |
✓✗✓✗ ✓✗✓✗ |
12 |
ForceRefOnColType(Sxf_ref.col(k)) = Scol.cross(f); |
1067 |
} |
||
1068 |
|||
1069 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Sxf.isApprox(Sxf_ref)); |
1070 |
2 |
} |
|
1071 |
|||
1072 |
BOOST_AUTO_TEST_SUITE_END () |
Generated by: GCOVR (Version 4.2) |