GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2019 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/joint/joint-generic.hpp" |
||
6 |
#include "pinocchio/multibody/liegroup/liegroup.hpp" |
||
7 |
|||
8 |
#include <boost/test/unit_test.hpp> |
||
9 |
#include <boost/utility/binary.hpp> |
||
10 |
|||
11 |
using namespace pinocchio; |
||
12 |
|||
13 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
14 |
|||
15 |
typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x; |
||
16 |
|||
17 |
template<typename JointModel> |
||
18 |
22 |
void test_constraint_mimic(const JointModelBase<JointModel> & jmodel) |
|
19 |
{ |
||
20 |
typedef typename traits<JointModel>::JointDerived Joint; |
||
21 |
typedef typename traits<Joint>::Constraint_t ConstraintType; |
||
22 |
typedef typename traits<Joint>::JointDataDerived JointData; |
||
23 |
typedef ScaledConstraint<ConstraintType> ScaledConstraintType; |
||
24 |
|||
25 |
✓✗ | 22 |
JointData jdata = jmodel.createData(); |
26 |
|||
27 |
22 |
const double scaling_factor = 2.; |
|
28 |
✓✗✓✗ |
4 |
ConstraintType constraint_ref(jdata.S), constraint_ref_shared(jdata.S); |
29 |
✓✗ | 22 |
ScaledConstraintType scaled_constraint(constraint_ref_shared,scaling_factor); |
30 |
|||
31 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
22 |
BOOST_CHECK(constraint_ref.nv() == scaled_constraint.nv()); |
32 |
|||
33 |
typedef typename JointModel::TangentVector_t TangentVector_t; |
||
34 |
✓✗✓✗ |
22 |
TangentVector_t v = TangentVector_t::Random(); |
35 |
|||
36 |
✓✗✓✗ |
22 |
Motion m = scaled_constraint * v; |
37 |
✓✗✓✗ ✓✗ |
22 |
Motion m_ref = scaling_factor * (Motion)(constraint_ref * v); |
38 |
|||
39 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(m.isApprox(m_ref)); |
40 |
|||
41 |
{ |
||
42 |
✓✗ | 22 |
SE3 M = SE3::Random(); |
43 |
✓✗ | 22 |
typename ScaledConstraintType::DenseBase S = M.act(scaled_constraint); |
44 |
✓✗✓✗ ✓✗ |
22 |
typename ScaledConstraintType::DenseBase S_ref = scaling_factor * M.act(constraint_ref); |
45 |
|||
46 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(S.isApprox(S_ref)); |
47 |
} |
||
48 |
|||
49 |
{ |
||
50 |
✓✗ | 22 |
typename ScaledConstraintType::DenseBase S = scaled_constraint.matrix(); |
51 |
✓✗✓✗ ✓✗ |
22 |
typename ScaledConstraintType::DenseBase S_ref = scaling_factor * constraint_ref.matrix(); |
52 |
|||
53 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(S.isApprox(S_ref)); |
54 |
} |
||
55 |
|||
56 |
{ |
||
57 |
✓✗ | 22 |
Motion v = Motion::Random(); |
58 |
✓✗ | 22 |
typename ScaledConstraintType::DenseBase S = v.cross(scaled_constraint); |
59 |
✓✗✓✗ ✓✗ |
22 |
typename ScaledConstraintType::DenseBase S_ref = scaling_factor * v.cross(constraint_ref); |
60 |
|||
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(S.isApprox(S_ref)); |
62 |
} |
||
63 |
|||
64 |
// Test transpose operations |
||
65 |
{ |
||
66 |
22 |
const Eigen::DenseIndex dim = 20; |
|
67 |
✓✗✓✗ |
44 |
const Matrix6x Fin = Matrix6x::Random(6,dim); |
68 |
✓✗✓✗ ✓✗ |
44 |
Eigen::MatrixXd Fout = scaled_constraint.transpose() * Fin; |
69 |
✓✗✓✗ ✓✗✓✗ |
44 |
Eigen::MatrixXd Fout_ref = scaling_factor * (constraint_ref.transpose() * Fin); |
70 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(Fout.isApprox(Fout_ref)); |
71 |
|||
72 |
✓✗ | 22 |
Force force_in(Force::Random()); |
73 |
✓✗✓✗ ✓✗ |
44 |
Eigen::MatrixXd Stf = (scaled_constraint.transpose() * force_in); |
74 |
✓✗✓✗ ✓✗✓✗ |
44 |
Eigen::MatrixXd Stf_ref = scaling_factor * (constraint_ref.transpose() * force_in); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(Stf_ref.isApprox(Stf)); |
76 |
} |
||
77 |
|||
78 |
// CRBA Y*S |
||
79 |
{ |
||
80 |
✓✗ | 22 |
Inertia Y = Inertia::Random(); |
81 |
✓✗✓✗ |
44 |
Eigen::MatrixXd YS = Y * scaled_constraint; |
82 |
✓✗✓✗ ✓✗ |
44 |
Eigen::MatrixXd YS_ref = scaling_factor * (Y * constraint_ref); |
83 |
|||
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(YS.isApprox(YS_ref)); |
85 |
} |
||
86 |
|||
87 |
22 |
} |
|
88 |
|||
89 |
struct TestJointConstraint |
||
90 |
{ |
||
91 |
|||
92 |
template <typename JointModel> |
||
93 |
18 |
void operator()(const JointModelBase<JointModel> &) const |
|
94 |
{ |
||
95 |
✓✗ | 18 |
JointModel jmodel; |
96 |
✓✗ | 18 |
jmodel.setIndexes(0,0,0); |
97 |
|||
98 |
✓✗ | 18 |
test_constraint_mimic(jmodel); |
99 |
18 |
} |
|
100 |
|||
101 |
1 |
void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const |
|
102 |
{ |
||
103 |
✓✗ | 1 |
JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); |
104 |
✓✗ | 1 |
jmodel.setIndexes(0,0,0); |
105 |
|||
106 |
✓✗ | 1 |
test_constraint_mimic(jmodel); |
107 |
1 |
} |
|
108 |
|||
109 |
1 |
void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const |
|
110 |
{ |
||
111 |
✓✗ | 1 |
JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); |
112 |
✓✗ | 1 |
jmodel.setIndexes(0,0,0); |
113 |
|||
114 |
✓✗ | 1 |
test_constraint_mimic(jmodel); |
115 |
1 |
} |
|
116 |
|||
117 |
}; |
||
118 |
|||
119 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_constraint) |
120 |
{ |
||
121 |
using namespace pinocchio; |
||
122 |
typedef boost::variant< |
||
123 |
JointModelRX, JointModelRY, JointModelRZ |
||
124 |
, JointModelRevoluteUnaligned |
||
125 |
, JointModelPX, JointModelPY, JointModelPZ |
||
126 |
, JointModelPrismaticUnaligned |
||
127 |
, JointModelRUBX, JointModelRUBY, JointModelRUBZ |
||
128 |
> Variant; |
||
129 |
|||
130 |
✓✗ | 2 |
boost::mpl::for_each<Variant::types>(TestJointConstraint()); |
131 |
2 |
} |
|
132 |
|||
133 |
template<typename JointModel> |
||
134 |
22 |
void test_joint_mimic(const JointModelBase<JointModel> & jmodel) |
|
135 |
{ |
||
136 |
typedef typename traits<JointModel>::JointDerived Joint; |
||
137 |
typedef typename traits<Joint>::JointDataDerived JointData; |
||
138 |
|||
139 |
✓✗ | 22 |
JointData jdata = jmodel.createData(); |
140 |
|||
141 |
22 |
const double scaling_factor = 1.; |
|
142 |
22 |
const double offset = 0.; |
|
143 |
|||
144 |
typedef JointMimic<Joint> JointMimicType; |
||
145 |
typedef typename traits<JointMimicType>::JointModelDerived JointModelMimicType; |
||
146 |
typedef typename traits<JointMimicType>::JointDataDerived JointDataMimicType; |
||
147 |
|||
148 |
// test constructor |
||
149 |
✓✗ | 22 |
JointModelMimicType jmodel_mimic(jmodel.derived(),scaling_factor,offset); |
150 |
✓✗ | 22 |
JointDataMimicType jdata_mimic = jmodel_mimic.createData(); |
151 |
|||
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(jmodel_mimic.nq() == 0); |
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(jmodel_mimic.nv() == 0); |
154 |
|||
155 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
22 |
BOOST_CHECK(jmodel_mimic.idx_q() == jmodel.idx_q()); |
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
22 |
BOOST_CHECK(jmodel_mimic.idx_v() == jmodel.idx_v()); |
157 |
|||
158 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(jmodel_mimic.idx_q() == 0); |
159 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(jmodel_mimic.idx_v() == 0); |
160 |
|||
161 |
typedef typename JointModelMimicType::ConfigVector_t ConfigVectorType; |
||
162 |
typedef typename LieGroup<JointModel>::type LieGroupType; |
||
163 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
22 |
ConfigVectorType q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones()); |
164 |
|||
165 |
✓✗ | 22 |
jmodel.calc(jdata,q0); |
166 |
✓✗ | 22 |
jmodel_mimic.calc(jdata_mimic,q0); |
167 |
|||
168 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
22 |
BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); |
169 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); |
170 |
|||
171 |
typedef typename JointModelMimicType::TangentVector_t TangentVectorType; |
||
172 |
|||
173 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
22 |
q0 = LieGroupType().randomConfiguration(-ConfigVectorType::Ones(),ConfigVectorType::Ones()); |
174 |
✓✗✓✗ |
22 |
TangentVectorType v0 = TangentVectorType::Random(); |
175 |
✓✗ | 22 |
jmodel.calc(jdata,q0,v0); |
176 |
✓✗ | 22 |
jmodel_mimic.calc(jdata_mimic,q0,v0); |
177 |
|||
178 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
22 |
BOOST_CHECK(((SE3)jdata.M).isApprox((SE3)jdata_mimic.M())); |
179 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
22 |
BOOST_CHECK(jdata.S.matrix().isApprox(jdata_mimic.S.matrix())); |
180 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
22 |
BOOST_CHECK(((Motion)jdata.v).isApprox((Motion)jdata_mimic.v())); |
181 |
22 |
} |
|
182 |
|||
183 |
struct TestJointMimic |
||
184 |
{ |
||
185 |
|||
186 |
template <typename JointModel> |
||
187 |
18 |
void operator()(const JointModelBase<JointModel> &) const |
|
188 |
{ |
||
189 |
✓✗ | 18 |
JointModel jmodel; |
190 |
✓✗ | 18 |
jmodel.setIndexes(0,0,0); |
191 |
|||
192 |
✓✗ | 18 |
test_joint_mimic(jmodel); |
193 |
18 |
} |
|
194 |
|||
195 |
1 |
void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const |
|
196 |
{ |
||
197 |
✓✗ | 1 |
JointModelRevoluteUnaligned jmodel(1.5, 1., 0.); |
198 |
✓✗ | 1 |
jmodel.setIndexes(0,0,0); |
199 |
|||
200 |
✓✗ | 1 |
test_joint_mimic(jmodel); |
201 |
1 |
} |
|
202 |
|||
203 |
1 |
void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const |
|
204 |
{ |
||
205 |
✓✗ | 1 |
JointModelPrismaticUnaligned jmodel(1.5, 1., 0.); |
206 |
✓✗ | 1 |
jmodel.setIndexes(0,0,0); |
207 |
|||
208 |
✓✗ | 1 |
test_joint_mimic(jmodel); |
209 |
1 |
} |
|
210 |
|||
211 |
}; |
||
212 |
|||
213 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_joint) |
214 |
{ |
||
215 |
using namespace pinocchio; |
||
216 |
typedef boost::variant< |
||
217 |
JointModelRX, JointModelRY, JointModelRZ |
||
218 |
, JointModelRevoluteUnaligned |
||
219 |
, JointModelPX, JointModelPY, JointModelPZ |
||
220 |
, JointModelPrismaticUnaligned |
||
221 |
, JointModelRUBX, JointModelRUBY, JointModelRUBZ |
||
222 |
> Variant; |
||
223 |
|||
224 |
✓✗ | 2 |
boost::mpl::for_each<Variant::types>(TestJointMimic()); |
225 |
2 |
} |
|
226 |
|||
227 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_transform_linear_affine) |
228 |
{ |
||
229 |
typedef JointModelRX::ConfigVector_t ConfigVectorType; |
||
230 |
2 |
double scaling = 1., offset = 0.; |
|
231 |
|||
232 |
✓✗✓✗ |
2 |
ConfigVectorType q0 = ConfigVectorType::Random(); |
233 |
✓✗ | 2 |
ConfigVectorType q1; |
234 |
✓✗ | 2 |
LinearAffineTransform::run(q0,scaling,offset,q1); |
235 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(q0 == q1); |
236 |
|||
237 |
2 |
offset = 2.; |
|
238 |
✓✗✓✗ |
2 |
LinearAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1); |
239 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(q1 == ConfigVectorType::Constant(offset)); |
240 |
2 |
} |
|
241 |
|||
242 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_transform_linear_revolute) |
243 |
{ |
||
244 |
typedef JointModelRUBX::ConfigVector_t ConfigVectorType; |
||
245 |
2 |
double scaling = 1., offset = 0.; |
|
246 |
|||
247 |
✓✗✓✗ |
2 |
ConfigVectorType q0 = ConfigVectorType::Random().normalized(); |
248 |
✓✗ | 2 |
ConfigVectorType q1; |
249 |
✓✗ | 2 |
UnboundedRevoluteAffineTransform::run(q0,scaling,offset,q1); |
250 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(q0.isApprox(q1)); |
251 |
|||
252 |
2 |
offset = 2.; |
|
253 |
✓✗✓✗ |
2 |
UnboundedRevoluteAffineTransform::run(ConfigVectorType::Zero(),scaling,offset,q1); |
254 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(q1 == ConfigVectorType(math::cos(offset),math::sin(offset))); |
255 |
2 |
} |
|
256 |
|||
257 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_joint_generic_cast) |
258 |
{ |
||
259 |
✓✗ | 2 |
JointModelRX jmodel_ref; |
260 |
✓✗ | 2 |
jmodel_ref.setIndexes(1,2,3); |
261 |
|||
262 |
✓✗ | 2 |
JointModelMimic<JointModelRX> jmodel(jmodel_ref,2.,1.); |
263 |
✓✗ | 2 |
jmodel.setIndexes(1,-1,-1); |
264 |
|||
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(jmodel.id() == jmodel_ref.id()); |
266 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(jmodel.idx_q() == jmodel_ref.idx_q()); |
267 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(jmodel.idx_v() == jmodel_ref.idx_v()); |
268 |
|||
269 |
✓✗ | 4 |
JointModel jmodel_generic(jmodel); |
270 |
✓✗ | 2 |
jmodel_generic.setIndexes(1,-2,-2); |
271 |
|||
272 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(jmodel_generic.id() == jmodel_ref.id()); |
273 |
2 |
} |
|
274 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |