GCC Code Coverage Report

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