GCC Code Coverage Report

Directory: ./
File: unittest/joint-motion-subspace.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 173 173 100.0%
Branches: 490 965 50.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4
5 #include "pinocchio/spatial/se3.hpp"
6 #include "pinocchio/spatial/inertia.hpp"
7 #include "pinocchio/multibody/force-set.hpp"
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/algorithm/joint-configuration.hpp"
10
11 #include "utils/macros.hpp"
12
13 #include <iostream>
14
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17
18 using namespace pinocchio;
19
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21
22
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_ForceSet)
23 {
24 using namespace pinocchio;
25
26
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3 amb = SE3::Random();
27
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3 bmc = SE3::Random();
28
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3 amc = amb * bmc;
29
30
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ForceSet F(12);
31
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 4 ForceSet F2(Eigen::Matrix<double, 3, 2>::Zero(), Eigen::Matrix<double, 3, 2>::Zero());
32
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 F.block(10, 2) = F2;
33
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 2 BOOST_CHECK_EQUAL(F.matrix().col(10).norm(), 0.0);
34
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 2 BOOST_CHECK(std::isnan(F.matrix()(0, 9)));
35
36
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 4 ForceSet F3(Eigen::Matrix<double, 3, 12>::Random(), Eigen::Matrix<double, 3, 12>::Random());
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ForceSet F4 = amb.act(F3);
38
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 SE3::Matrix6 aXb = amb;
39
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 42 not taken.
✓ Branch 43 taken 1 times.
 2 BOOST_CHECK((aXb.transpose().inverse() * F3.matrix()).isApprox(F4.matrix(), 1e-12));
40
41
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ForceSet bF = bmc.act(F3);
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ForceSet aF = amb.act(bF);
43
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ForceSet aF2 = amc.act(F3);
44
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 2 BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));
45
46
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ForceSet F36 = amb.act(F3.block(3, 6));
47
13/26
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✗ Branch 45 not taken.
✓ Branch 46 taken 1 times.
 2 BOOST_CHECK(
48 (aXb.transpose().inverse() * F3.matrix().block(0, 3, 6, 6)).isApprox(F36.matrix(), 1e-12));
49
50
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ForceSet F36full(12);
51
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 2 F36full.block(3, 6) = amb.act(F3.block(3, 6));
52
14/28
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 1 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✗ Branch 48 not taken.
✓ Branch 49 taken 1 times.
 2 BOOST_CHECK((aXb.transpose().inverse() * F3.matrix().block(0, 3, 6, 6))
53 .isApprox(F36full.matrix().block(0, 3, 6, 6), 1e-12));
54 2 }
55
56
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_ConstraintRX)
57 {
58 using namespace pinocchio;
59
60
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Inertia Y = Inertia::Random();
61 2 JointDataRX::Constraint_t S;
62
63
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ForceSet F1(1);
64
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 F1.block(0, 1) = Y * S;
65
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
 2 BOOST_CHECK(F1.matrix().isApprox(Y.matrix().col(3), 1e-12));
66
67
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 4 ForceSet F2(Eigen::Matrix<double, 3, 9>::Random(), Eigen::Matrix<double, 3, 9>::Random());
68
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 2 Eigen::MatrixXd StF2 = S.transpose() * F2.block(5, 3).matrix();
69
12/24
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 1 times.
✗ Branch 36 not taken.
✗ Branch 41 not taken.
✓ Branch 42 taken 1 times.
 2 BOOST_CHECK(StF2.isApprox(S.matrix().transpose() * F2.matrix().block(0, 5, 6, 3), 1e-12));
70 2 }
71
72 template<typename JointModel>
73 46 void test_jmodel_nq_against_nq_ref(const JointModelBase<JointModel> & jmodel, const int & nq_ref)
74 {
75
6/12
✓ Branch 3 taken 23 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 23 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 23 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 23 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 23 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 23 times.
 46 BOOST_CHECK(jmodel.nq() == nq_ref);
76 46 }
77
78 template<typename JointModel>
79 6 void test_jmodel_nq_against_nq_ref(const JointModelMimic<JointModel> & jmodel, const int & nq_ref)
80 {
81
6/12
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 3 times.
 6 BOOST_CHECK(jmodel.jmodel().nq() == nq_ref);
82 6 }
83
84 template<typename JointModel, typename ConstraintDerived>
85 46 void test_nv_against_jmodel(
86 const JointModelBase<JointModel> & jmodel,
87 const JointMotionSubspaceBase<ConstraintDerived> & constraint)
88 {
89
7/14
✓ Branch 3 taken 23 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 23 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 23 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 23 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 23 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 23 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 23 times.
 46 BOOST_CHECK(constraint.nv() == jmodel.nv());
90 46 }
91
92 template<typename JointModel, typename ConstraintDerived>
93 6 void test_nv_against_jmodel(
94 const JointModelMimic<JointModel> & jmodel,
95 const JointMotionSubspaceBase<ConstraintDerived> & constraint)
96 {
97
7/14
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 6 BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv());
98 6 }
99
100 template<class JointModel>
101 struct buildModel
102 {
103 46 static Model run(const JointModelBase<JointModel> & jmodel)
104 {
105 46 Model model;
106
4/8
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
 46 model.addJoint(0, jmodel, SE3::Identity(), "joint");
107
108 46 return model;
109 }
110 };
111
112 template<class JointModel>
113 struct buildModel<JointModelMimic<JointModel>>
114 {
115 typedef JointModelMimic<JointModel> JointModel_;
116
117 6 static Model run(const JointModel_ & jmodel)
118 {
119 6 Model model;
120
4/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
 6 model.addJoint(0, jmodel.jmodel(), SE3::Identity(), "joint");
121
4/8
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 6 model.addJoint(0, jmodel, SE3::Identity(), "joint_mimic");
122
123 6 return model;
124 }
125 };
126
127 template<typename JointModel>
128 52 void test_constraint_operations(const JointModelBase<JointModel> & jmodel)
129 {
130 typedef typename traits<JointModel>::JointDerived Joint;
131 typedef typename traits<Joint>::Constraint_t ConstraintType;
132 typedef typename traits<Joint>::JointDataDerived JointData;
133 typedef Eigen::Matrix<typename JointModel::Scalar, 6, Eigen::Dynamic> Matrix6x;
134
135
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 JointData jdata = jmodel.createData();
136 typedef typename JointModel::ConfigVector_t ConfigVector_t;
137
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 ConfigVector_t q;
138
139 // We need to use a model here in order to call the randomConfiguration to init q.
140
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 52 Model model = buildModel<JointModel>::run(jmodel.derived());
141
142
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 52 test_jmodel_nq_against_nq_ref(jmodel.derived(), model.nq);
143
144
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 52 q = randomConfiguration(
145
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 model, ConfigVector_t::Constant(model.nq, -1.), ConfigVector_t::Constant(model.nq, 1.));
146
147 // By calling jmodel.calc, we then have jdata.S which is initialized with non NaN quantities
148
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 jmodel.calc(jdata, q);
149
150
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 26 ConstraintType constraint(jdata.S);
151
152
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 52 test_nv_against_jmodel(jmodel.derived(), constraint);
153
8/16
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 26 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 26 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 26 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 26 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 26 times.
 52 BOOST_CHECK(constraint.cols() == constraint.nv());
154
6/12
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 26 times.
 52 BOOST_CHECK(constraint.rows() == 6);
155
156 typedef typename JointModel::TangentVector_t TangentVector_t;
157
3/6
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
 52 TangentVector_t v = TangentVector_t::Random(constraint.nv());
158
159
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 52 typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
160
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 52 Motion m = constraint * v;
161
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 Motion m_ref = Motion(constraint_mat * v);
162
163
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(m.isApprox(m_ref));
164
165 // Test SE3 action
166 {
167
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 SE3 M = SE3::Random();
168
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 typename ConstraintType::DenseBase S = M.act(constraint);
169
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 typename ConstraintType::DenseBase S_ref(6, constraint.nv());
170
171
3/4
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
✓ Branch 4 taken 26 times.
 136 for (Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
172 {
173 typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
174
4/8
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.
 84 MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
175
176
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 84 m_out = M.act(m_in);
177 }
178
179
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(S.isApprox(S_ref));
180 }
181
182 // Test SE3 action inverse
183 {
184
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 SE3 M = SE3::Random();
185
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 typename ConstraintType::DenseBase S = M.actInv(constraint);
186
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 typename ConstraintType::DenseBase S_ref(6, constraint.nv());
187
188
3/4
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
✓ Branch 4 taken 26 times.
 136 for (Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
189 {
190 typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
191
4/8
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.
 84 MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
192
193
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 84 m_out = M.actInv(m_in);
194 }
195
196
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(S.isApprox(S_ref));
197 }
198
199 // Test SE3 action and SE3 action inverse
200 {
201
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 const SE3 M = SE3::Random();
202
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 const SE3 Minv = M.inverse();
203
204
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 typename ConstraintType::DenseBase S1_vice = M.actInv(constraint);
205
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 typename ConstraintType::DenseBase S2_vice = Minv.act(constraint);
206
207
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(S1_vice.isApprox(S2_vice));
208
209
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 typename ConstraintType::DenseBase S1_versa = M.act(constraint);
210
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 typename ConstraintType::DenseBase S2_versa = Minv.actInv(constraint);
211
212
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(S1_versa.isApprox(S2_versa));
213 }
214
215 // Test Motion action
216 {
217
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 Motion v = Motion::Random();
218
219
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 typename ConstraintType::DenseBase S = v.cross(constraint);
220
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 typename ConstraintType::DenseBase S_ref(6, constraint.nv());
221
222
3/4
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
✓ Branch 4 taken 26 times.
 136 for (Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
223 {
224 typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
225
4/8
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.
 84 MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
226
227
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 84 m_out = v.cross(m_in);
228 }
229
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(S.isApprox(S_ref));
230 }
231
232 // Test transpose operations
233 {
234 52 const Eigen::DenseIndex dim = 20;
235
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 const Matrix6x Fin = Matrix6x::Random(6, dim);
236
6/8
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 24 times.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 19 times.
✗ Branch 8 not taken.
 52 Eigen::MatrixXd Fout = constraint.transpose() * Fin;
237
3/6
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
 52 Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
238
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(Fout.isApprox(Fout_ref));
239
240
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 Force force_in(Force::Random());
241
5/8
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24 times.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 52 Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
242
4/8
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 26 times.
✗ Branch 11 not taken.
 52 Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
243
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(Stf_ref.isApprox(Stf));
244 52 }
245
246 // CRBA operations
247 {
248
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 const Inertia Y = Inertia::Random();
249
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 Eigen::MatrixXd YS = Y * constraint;
250
3/6
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
 52 Eigen::MatrixXd YS_ref = Y.matrix() * constraint_mat;
251
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(YS.isApprox(YS_ref));
252 52 }
253
254 // ABA operations
255 {
256
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 const Inertia Y = Inertia::Random();
257
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 const Inertia::Matrix6 Y_mat = Y.matrix();
258
3/5
✓ Branch 1 taken 25 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 52 Eigen::MatrixXd YS = Y_mat * constraint;
259
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 52 Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
260
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(YS.isApprox(YS_ref));
261 52 }
262
263 // Test constrainst operations
264 {
265
5/8
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24 times.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
 52 Eigen::MatrixXd StS = constraint.transpose() * constraint;
266
3/6
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
 52 Eigen::MatrixXd StS_ref = constraint_mat.transpose() * constraint_mat;
267
7/14
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 26 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 26 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 26 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 26 times.
 52 BOOST_CHECK(StS.isApprox(StS_ref));
268 52 }
269 52 }
270
271 template<typename JointModel_>
272 struct init;
273
274 template<typename JointModel_>
275 struct init
276 {
277 34 static JointModel_ run()
278 {
279 34 JointModel_ jmodel;
280 34 jmodel.setIndexes(0, 0, 0);
281 34 return jmodel;
282 }
283 };
284
285 template<typename Scalar, int Options>
286 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>>
287 {
288 typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> JointModel;
289
290 1 static JointModel run()
291 {
292 typedef typename JointModel::Vector3 Vector3;
293
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
294
295 1 jmodel.setIndexes(0, 0, 0);
296 1 return jmodel;
297 }
298 };
299
300 template<typename Scalar, int Options>
301 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>>
302 {
303 typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModel;
304
305 1 static JointModel run()
306 {
307 typedef typename JointModel::Vector3 Vector3;
308
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
309
310 1 jmodel.setIndexes(0, 0, 0);
311 1 return jmodel;
312 }
313 };
314
315 template<typename Scalar, int Options>
316 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>>
317 {
318 typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> JointModel;
319
320 1 static JointModel run()
321 {
322 typedef typename JointModel::Vector3 Vector3;
323
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
324
325 1 jmodel.setIndexes(0, 0, 0);
326 1 return jmodel;
327 }
328 };
329
330 template<typename Scalar, int Options, template<typename, int> class JointCollection>
331 struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
332 {
333 typedef pinocchio::JointModelTpl<Scalar, Options, JointCollection> JointModel;
334
335 static JointModel run()
336 {
337 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
338 JointModel jmodel((JointModelRX()));
339
340 jmodel.setIndexes(0, 0, 0);
341 return jmodel;
342 }
343 };
344
345 template<typename Scalar, int Options>
346 struct init<pinocchio::JointModelUniversalTpl<Scalar, Options>>
347 {
348 typedef pinocchio::JointModelUniversalTpl<Scalar, Options> JointModel;
349
350 1 static JointModel run()
351 {
352
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(XAxis::vector(), YAxis::vector());
353
354 1 jmodel.setIndexes(0, 0, 0);
355 1 return jmodel;
356 }
357 };
358
359 template<typename Scalar, int Options, template<typename, int> class JointCollection>
360 struct init<pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection>>
361 {
362 typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> JointModel;
363
364 1 static JointModel run()
365 {
366 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
367 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;
368 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 2> JointModelRZ;
369
370
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(JointModelRX(), SE3::Random());
371
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 jmodel.addJoint(JointModelRY(), SE3::Random());
372
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 jmodel.addJoint(JointModelRZ(), SE3::Random());
373
374
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jmodel.setIndexes(0, 0, 0);
375
376 1 return jmodel;
377 }
378 };
379
380 template<typename JointModel_>
381 struct init<pinocchio::JointModelMimic<JointModel_>>
382 {
383 typedef pinocchio::JointModelMimic<JointModel_> JointModel;
384
385 6 static JointModel run()
386 {
387
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel_ jmodel_ref = init<JointModel_>::run();
388
389
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel jmodel(jmodel_ref, 1., 0.);
390
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 jmodel.setIndexes(0, 0, 0);
391
392 12 return jmodel;
393 }
394 };
395
396 template<typename Scalar, int Options, int axis>
397 struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
398 {
399 typedef pinocchio::JointModelHelicalTpl<Scalar, Options, axis> JointModel;
400
401 6 static JointModel run()
402 {
403
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel jmodel(static_cast<Scalar>(0.5));
404
405 6 jmodel.setIndexes(0, 0, 0);
406 6 return jmodel;
407 }
408 };
409
410 template<typename Scalar, int Options>
411 struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
412 {
413 typedef pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> JointModel;
414
415 1 static JointModel run()
416 {
417 typedef typename JointModel::Vector3 Vector3;
418
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
419
420 1 jmodel.setIndexes(0, 0, 0);
421 1 return jmodel;
422 }
423 };
424
425 struct TestJointConstraint
426 {
427
428 template<typename JointModel>
429 52 void operator()(const JointModelBase<JointModel> &) const
430 {
431
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 JointModel jmodel = init<JointModel>::run();
432
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 jmodel.setIndexes(0, 0, 0);
433
434
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 52 test_constraint_operations(jmodel);
435 52 }
436 };
437
438
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)
439 {
440 typedef JointCollectionDefault::JointModelVariant Variant;
441 2 boost::mpl::for_each<Variant::types>(TestJointConstraint());
442 2 }
443
444 BOOST_AUTO_TEST_SUITE_END()
445