GCC Code Coverage Report

Directory: ./
File: tests/test-jacobians.cc
Date: 2025-05-05 12:19:30
Exec Total Coverage
Lines: 185 185 100.0%
Branches: 602 1190 50.6%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/pinocchio/configuration.hh>
30 #include <hpp/pinocchio/device.hh>
31 #include <hpp/pinocchio/joint.hh>
32 #include <hpp/pinocchio/simple-device.hh>
33 #include <pinocchio/algorithm/joint-configuration.hpp>
34 #include <pinocchio/parsers/sample-models.hpp>
35
36 #include "hpp/constraints/configuration-constraint.hh"
37 #include "hpp/constraints/convex-shape-contact.hh"
38 #include "hpp/constraints/differentiable-function-set.hh"
39 #include "hpp/constraints/generic-transformation.hh"
40 #include "hpp/constraints/static-stability.hh"
41 #include "hpp/constraints/symbolic-function.hh"
42 #include "hpp/constraints/tools.hh"
43
44 #define BOOST_TEST_MODULE hpp_constraints
45 #include <math.h>
46 #include <stdlib.h>
47
48 #include <../tests/convex-shape-contact-function.hh>
49 #include <../tests/util.hh>
50 #include <limits>
51
52 using hpp::pinocchio::BodyPtr_t;
53 using hpp::pinocchio::Configuration_t;
54 using hpp::pinocchio::ConfigurationPtr_t;
55 using hpp::pinocchio::Device;
56 using hpp::pinocchio::DevicePtr_t;
57 using hpp::pinocchio::JACOBIAN;
58 using hpp::pinocchio::JOINT_POSITION;
59 using hpp::pinocchio::JointPtr_t;
60 using hpp::pinocchio::JointVector_t;
61 using hpp::pinocchio::SE3;
62
63 using std::numeric_limits;
64
65 typedef std::vector<bool> BoolVector_t;
66
67 using namespace hpp::constraints;
68
69 const static size_t NUMBER_JACOBIAN_CALCULUS = 5;
70 const static double HESSIAN_MAXIMUM_COEF = 1e1;
71 const static double DQ_MAX = 1e-2;
72 const static size_t MAX_NB_ERROR = 5;
73
74 static matrix3_t I3 = matrix3_t::Identity();
75 static vector3_t zero = vector3_t::Identity();
76 static SE3 MId = SE3::Identity();
77 3 SE3 toSE3(const matrix3_t& R) { return SE3(R, zero); }
78 2 SE3 toSE3(const vector3_t& t) { return SE3(I3, t); }
79
80 3 DevicePtr_t createRobot() {
81 using namespace hpp::pinocchio;
82 DevicePtr_t robot =
83
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 humanoidSimple("test", true, Computation_t(JOINT_POSITION | JACOBIAN));
84
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 robot->rootJoint()->lowerBound(0, -1);
85
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 robot->rootJoint()->lowerBound(1, -1);
86
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 robot->rootJoint()->lowerBound(2, -1);
87
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 robot->rootJoint()->upperBound(0, 1);
88
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 robot->rootJoint()->upperBound(1, 1);
89
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 robot->rootJoint()->upperBound(2, 1);
90 3 return robot;
91 }
92
93
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(triangle) {
94 /// First test ConvexShape class (as a triangle)
95
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 vector3_t x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
96
3/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✓ Branch 4 taken 1 times.
 20 vector3_t p[9];
97
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 p[0] = vector3_t(0, 0, 0);
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 p[1] = vector3_t(1, 0, 0);
99
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 p[2] = vector3_t(0, 1, 0);
100
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 p[3] = vector3_t(1, 1, 1);
101
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 p[4] = vector3_t(0.2, 0.2, 0);
102
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 p[5] = vector3_t(1, 1, 0);
103
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 p[6] = vector3_t(-1, -1, 1);
104
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 ConvexShape t(p[0], p[1], p[2]);
105
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 ConvexShapeData d;
106
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 d.updateToCurrentTransform(t);
107
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 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 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_MESSAGE((d.normal_ - z).isZero(), "Norm of triangle is wrong");
108
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 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 34 not taken.
✓ Branch 35 taken 1 times.
 2 BOOST_CHECK_MESSAGE((d.center_ - (x + y) / 3).isZero(),
109 "Center of triangle is wrong");
110
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_MESSAGE(std::abs(t.planeXaxis().dot(z)) < 1e-8,
111 "X axis of triangle is wrong");
112
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 2 BOOST_CHECK_MESSAGE(std::abs(t.planeYaxis().dot(z)) < 1e-8,
113 "Y axis of triangle is wrong");
114
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 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_MESSAGE(std::abs(t.planeYaxis().dot(t.planeXaxis())) < 1e-8,
115 "X and Y axes are not orthogonal");
116
11/22
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 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 37 not taken.
✓ Branch 38 taken 1 times.
 2 BOOST_CHECK_MESSAGE((d.intersection(p[6], y - z) + x).isZero(),
117 "Wrong intersection of triangle and line is wrong");
118
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_MESSAGE(d.isInside(t, p[4]), "This point is inside");
119
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 1 times.
 2 BOOST_CHECK_MESSAGE(!d.isInside(t, p[5]), "This point is outside");
120
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_MESSAGE(std::abs(d.distance(t, p[0])) < 1e-8,
121 "Distance to triangle is wrong");
122
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_MESSAGE(std::abs(d.distance(t, p[1])) < 1e-8,
123 "Distance to triangle is wrong");
124
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_MESSAGE(std::abs(d.distance(t, p[2])) < 1e-8,
125 "Distance to triangle is wrong");
126
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_MESSAGE(std::abs(d.distance(t, p[4]) + 0.2) < 1e-8,
127 "Distance to triangle is wrong");
128
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK_MESSAGE(std::abs(d.distance(t, p[5]) - 0.5 * std::sqrt(2)) < 1e-8,
129 "Distance to triangle is wrong");
130
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 2 BOOST_CHECK_MESSAGE(
131 std::abs(d.distance(t, d.intersection(p[6], z)) - std::sqrt(2)) < 1e-8,
132 "Distance to triangle is wrong");
133 2 }
134
135 template <bool forward>
136 105 void checkJacobianDiffIsZero(const std::string& name, const matrix_t& diff,
137 const value_type& eps) {
138 size_type row, col;
139
2/4
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
 105 value_type maxError = diff.cwiseAbs().maxCoeff(&row, &col);
140
17/34
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 105 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 105 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 105 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 105 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 105 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 105 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 105 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 105 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 105 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 105 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 105 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 105 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 105 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 105 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 105 times.
✗ Branch 50 not taken.
✗ Branch 65 not taken.
✓ Branch 66 taken 105 times.
 105 BOOST_CHECK_MESSAGE(
141 maxError < /* HESSIAN_MAXIMUM_COEF * */ eps,
142 "Jacobian of " << name << " seems wrong ("
143 << (forward ? "forward" : "central") << "). DOF " << col
144 << " at row " << row << ": " << maxError << " > "
145 << /* HESSIAN_MAXIMUM_COEF << " * " << */ eps);
146 105 }
147
148
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(jacobian) {
149
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 DevicePtr_t device = createRobot();
150
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
151
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint");
152
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Configuration_t goal = device->currentConfiguration();
153
6/12
✓ 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 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
154
155
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Configuration_t q1;
156
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 randomConfig(device, q1);
157
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 device->currentConfiguration(q1);
158
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->computeForwardKinematics(JOINT_POSITION);
159
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf1(ee1->currentTransformation());
160
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 Transform3s tf2(ee2->currentTransformation());
161
162 /// Create the constraints
163 typedef std::list<DifferentiableFunctionPtr_t> DFs;
164
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<bool> mask011(3, true);
165
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 mask011[0] = false;
166 2 DFs functions;
167
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
168
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 4 ConfigurationConstraint::create("Configuration", device, goal));
169 // /*
170
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
171
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 4 Orientation::create("Orientation", device, ee2, toSE3(tf2.rotation())));
172
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 6 functions.push_back(OrientationSO3::create("OrientationSO3", device, ee2,
173
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 toSE3(tf2.rotation())));
174
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 4 functions.push_back(Orientation::create("Orientation with mask (0,1,1)",
175
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 device, ee1, toSE3(tf1.rotation()),
176 mask011));
177
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
178
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 4 Position::create("Position", device, ee1, toSE3(tf1.translation()), tf2));
179
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 4 functions.push_back(Position::create("Position with mask (0,1,1)", device,
180
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 4 ee1, toSE3(tf1.translation()), tf2,
181 mask011));
182
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeOrientation::create("RelativeOrientation", device,
183 ee1, ee2, tf1, tf2));
184
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeOrientationSO3::create(
185 "RelativeOrientationSO3", device, ee1, ee2, tf1, tf2));
186
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativeOrientation::create(
187
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 4 "(Relative)Orientation", device, hpp::pinocchio::Joint::create(device, 0),
188 ee2, tf1, tf2));
189
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 6 functions.push_back(RelativeOrientationSO3::create(
190 "(Relative)OrientationSO3", device,
191
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 4 hpp::pinocchio::Joint::create(device, 0), ee2, tf1, tf2));
192
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
193
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 RelativeOrientation::create("RelativeOrientation with mask (0,1,1)",
194 device, ee1, ee2, tf1, tf2, mask011));
195
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
196
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 RelativePosition::create("RelativePosition", device, ee1, ee2, tf1, tf2));
197
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
198
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 RelativePosition::create("RelativePosition with mask (0,1,1)", device,
199 ee1, ee2, tf1, tf2, mask011));
200
201
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 randomConfig(device, q1);
202
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 device->currentConfiguration(q1);
203
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->computeForwardKinematics(JOINT_POSITION);
204
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 tf1 = ee1->currentTransformation();
205
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 tf2 = ee2->currentTransformation();
206
207
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
208
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 Transformation::create("Transformation", device, ee1, tf1));
209
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(
210
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 TransformationR3xSO3::create("TransformationR3xSO3", device, ee1, tf1));
211
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformation::create(
212 "RelativeTransformation", device, ee1, ee2, tf1, tf2));
213
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 functions.push_back(RelativeTransformationR3xSO3::create(
214 "RelativeTransformationR3xSO3", device, ee1, ee2, tf1, tf2));
215
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 functions.push_back(createConvexShapeContact_triangles(
216 device, ee1, "ConvexShapeContact triangle"));
217
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 functions.push_back(createConvexShapeContact_punctual(
218 device, ee1, "ConvexShapeContact punctual"));
219
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 functions.push_back(createConvexShapeContact_convex(
220 device, ee1, "ConvexShapeContact convex"));
221
222 // DifferentiableFunctionSet
223 DifferentiableFunctionSetPtr_t stack =
224
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 DifferentiableFunctionSet::create("Stack");
225
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 2 stack->add(Position::create("Position", device, ee1, MId, MId));
226
3/6
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 4 stack->add(RelativeOrientation::create("RelativeOrientation", device, ee1,
227 ee2, MId,
228
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 4 BoolVector_t{false, true, true}));
229
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 functions.push_back(stack);
230 //*/
231
232
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 std::vector<Configuration_t> cfgs(NUMBER_JACOBIAN_CALCULUS);
233
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 12 for (size_t i = 0; i < NUMBER_JACOBIAN_CALCULUS; i++)
234
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 10 randomConfig(device, cfgs[i]);
235
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 matrix_t jacobian, fdCentral, fdForward, errorJacobian;
236
2/2
✓ Branch 4 taken 21 times.
✓ Branch 5 taken 1 times.
 44 for (DFs::iterator fit = functions.begin(); fit != functions.end(); ++fit) {
237 42 DifferentiableFunction& f = **fit;
238
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 jacobian.resize(f.outputDerivativeSize(), f.inputDerivativeSize());
239
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 fdForward.resize(f.outputDerivativeSize(), f.inputDerivativeSize());
240
1/2
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
 42 fdCentral.resize(f.outputDerivativeSize(), f.inputDerivativeSize());
241
242
2/2
✓ Branch 0 taken 105 times.
✓ Branch 1 taken 21 times.
 252 for (size_t i = 0; i < NUMBER_JACOBIAN_CALCULUS; i++) {
243
1/2
✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
 210 q1 = cfgs[i];
244
1/2
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
 210 jacobian.setZero();
245
3/6
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 105 times.
✗ Branch 8 not taken.
 210 f.jacobian(jacobian, q1);
246
247 210 const value_type eps = std::sqrt(Eigen::NumTraits<value_type>::epsilon());
248
249 // fdForward.setZero(); f.finiteDifferenceForward(fdForward, q1, device,
250 // eps);
251
1/2
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
 210 fdCentral.setZero();
252
3/6
✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 105 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 105 times.
✗ Branch 9 not taken.
 210 f.finiteDifferenceCentral(fdCentral, q1, device, eps);
253
254 // Forward: check the error
255 // errorJacobian = jacobian - fdForward;
256 // checkJacobianDiffIsZero<true> (f.name(), errorJacobian, eps);
257
258 // Central: check the error
259
2/4
✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
 210 errorJacobian = jacobian - fdCentral;
260
1/2
✓ Branch 2 taken 105 times.
✗ Branch 3 not taken.
 210 checkJacobianDiffIsZero<false>(f.name(), errorJacobian, sqrt(eps));
261 }
262 }
263 2 }
264
265
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(SymbolicCalculus_position) {
266
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 DevicePtr_t device = createRobot();
267
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
268
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint");
269
6/12
✓ 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 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
270
271 /// Create the constraints
272 typedef DifferentiableFunctionPtr_t DFptr;
273
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 DFptr pos = Position::create("Position", device, ee1, MId, MId);
274 Traits<PointInJoint>::Ptr_t pij =
275
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 PointInJoint::create(ee1, vector3_t(0, 0, 0));
276 Traits<PointInJoint>::Ptr_t pij2 =
277
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 PointInJoint::create(ee2, vector3_t(0, 0, 0));
278 Traits<CalculusBaseAbstract<> >::Ptr_t relpos_sb_ptr =
279
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 JointTranspose(ee1) * (pij2 - pij);
280
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 4 DFptr relpos = RelativePosition::create("RelPos", device, ee1, ee2, MId, MId);
281
282
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Configuration_t q1, q2;
283
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 randomConfig(device, q2);
284
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 matrix_t jacobian = matrix_t(pos->outputSize(), device->numberDof());
285
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (int i = 0; i < 100; i++) {
286
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 randomConfig(device, q1);
287
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 device->currentConfiguration(q1);
288
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 200 device->computeForwardKinematics(JOINT_POSITION | JACOBIAN);
289
290
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 200 pij->invalidate();
291
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 200 relpos_sb_ptr->invalidate();
292
293 /// Position
294
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 LiegroupElement value = (*pos)(q1);
295
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 pij->computeValue(q1);
296
8/16
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 100 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 100 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 100 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 100 times.
 200 BOOST_CHECK(pij->value().isApprox(value.vector()));
297
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 jacobian.setZero();
298
3/6
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
 200 pos->jacobian(jacobian, q1);
299
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 pij->computeJacobian(q1);
300
8/16
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 100 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 100 times.
 200 BOOST_CHECK(pij->jacobian().isApprox(jacobian));
301 // Relative position
302
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 value = (*relpos)(q1);
303
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 relpos_sb_ptr->computeValue(q1);
304
8/16
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 100 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 100 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 100 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 100 times.
 200 BOOST_CHECK(relpos_sb_ptr->value().isApprox(value.vector()));
305
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 jacobian.setZero();
306
3/6
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
 200 relpos->jacobian(jacobian, q1);
307
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 relpos_sb_ptr->computeJacobian(q1);
308
8/16
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 100 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 100 times.
 200 BOOST_CHECK(relpos_sb_ptr->jacobian().isApprox(jacobian));
309 200 }
310 2 }
311
312
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(SymbolicCalculus_jointframe) {
313
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 DevicePtr_t device = createRobot();
314
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 device->setDimensionExtraConfigSpace(3);
315
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 8 for (int i = 0; i < 3; ++i) {
316
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 device->extraConfigSpace().lower(i) = -1;
317
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 device->extraConfigSpace().upper(i) = 1;
318 }
319
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 JointPtr_t ee1 = device->getJointByName("lleg5_joint"),
320
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 ee2 = device->getJointByName("rleg5_joint");
321
6/12
✓ 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 22 not taken.
✓ Branch 23 taken 1 times.
 2 BOOST_REQUIRE(device);
322
323 /// Create the constraints
324 typedef DifferentiableFunctionPtr_t DFptr;
325
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 4 DFptr trans = Transformation::create("Transform", device, ee1, MId);
326
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Traits<JointFrame>::Ptr_t jf = JointFrame::create(ee1);
327 DFptr sf =
328
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 4 SymbolicFunction<JointFrame>::create("SymbolicFunctionTest", device, jf);
329
330
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 Configuration_t q1, q2;
331
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 randomConfig(device, q2);
332
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 matrix_t jacobian1 = matrix_t(trans->outputSize(), device->numberDof());
333
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 matrix_t jacobian2 = matrix_t(trans->outputSize(), device->numberDof());
334
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (int i = 0; i < 100; i++) {
335
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 randomConfig(device, q1);
336
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 device->currentConfiguration(q1);
337
1/2
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
 200 device->computeForwardKinematics(JOINT_POSITION | JACOBIAN);
338
339
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 LiegroupElement value1 = (*trans)(q1);
340
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 400 LiegroupElement value2 = (*sf)(q1);
341
7/14
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 100 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 100 times.
 200 BOOST_CHECK(value1.vector().isApprox(value2.vector()));
342
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 jacobian1.setZero();
343
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 jacobian2.setZero();
344
3/6
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
 200 trans->jacobian(jacobian1, q1);
345
3/6
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
 200 sf->jacobian(jacobian2, q1);
346
7/14
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 100 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 100 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 100 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 100 times.
 200 BOOST_CHECK(jacobian1.isApprox(jacobian2));
347 200 }
348 2 }
349