GCC Code Coverage Report

Directory: ./
File: tests/tasks.cpp
Date: 2024-11-10 01:12:44
Exec Total Coverage
Lines: 233 235 99.1%
Branches: 904 1810 49.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017 CNRS
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17
18 #include <iostream>
19
20 #include <boost/test/unit_test.hpp>
21 #include <boost/utility/binary.hpp>
22
23 #include <tsid/math/utils.hpp>
24 #include <tsid/robots/robot-wrapper.hpp>
25
26 #include <tsid/tasks/task-se3-equality.hpp>
27 #include <tsid/tasks/task-com-equality.hpp>
28 #include <tsid/tasks/task-joint-posture.hpp>
29 #include <tsid/tasks/task-joint-bounds.hpp>
30 #include <tsid/tasks/task-joint-posVelAcc-bounds.hpp>
31
32 #include <tsid/trajectories/trajectory-se3.hpp>
33 #include <tsid/trajectories/trajectory-euclidian.hpp>
34
35 #include <pinocchio/parsers/srdf.hpp>
36 #include <pinocchio/algorithm/joint-configuration.hpp>
37 #include <pinocchio/algorithm/center-of-mass.hpp>
38 #include <Eigen/SVD>
39
40 using namespace tsid;
41 using namespace trajectories;
42 using namespace math;
43 using namespace tasks;
44 using namespace std;
45 using namespace Eigen;
46 using namespace tsid::robots;
47
48 #define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A << ": " << A)
49
50 const string romeo_model_path = TSID_SOURCE_DIR "/models/romeo";
51
52 #ifndef NDEBUG
53 const int max_it = 100;
54 #else
55 const int max_it = 10000;
56 #endif
57
58 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
59
60
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_task_se3_equality) {
61
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "\n\n*********** TEST TASK SE3 EQUALITY ***********\n";
62 2 vector<string> package_dirs;
63
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 package_dirs.push_back(romeo_model_path);
64
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
65 RobotWrapper robot(urdfFileName, package_dirs,
66
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 pinocchio::JointModelFreeFlyer(), false);
67
68
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 4 TaskSE3Equality task("task-se3", robot, "RWristPitch");
69
70
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd Kp = VectorXd::Ones(6);
71
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 VectorXd Kd = 2 * VectorXd::Ones(6);
72
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.Kp(Kp);
73
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.Kd(Kd);
74
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 18 taken 1 times.
✗ Branch 19 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(task.Kp().isApprox(Kp));
75
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 18 taken 1 times.
✗ Branch 19 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(task.Kd().isApprox(Kd));
76
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::SE3 M_ref = pinocchio::SE3::Random();
78
3/6
✓ 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.
 2 TrajectoryBase *traj = new TrajectorySE3Constant("traj_SE3", M_ref);
79
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 TrajectorySample sample;
80
81 2 double t = 0.0;
82 2 const double dt = 0.001;
83
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd Jpinv(robot.nv(), 6);
84 2 double error, error_past = 1e100;
85
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = neutral(robot.model());
86
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 VectorXd v = VectorXd::Zero(robot.nv());
87
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(robot.model());
88
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (int i = 0; i < max_it; i++) {
89
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 robot.computeAllTerms(data, q, v);
90
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 sample = traj->computeNext();
91
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 task.setReference(sample);
92
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 const ConstraintBase &constraint = task.compute(t, q, v, data);
93
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_CHECK(constraint.rows() == 6);
94
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 14 taken 100 times.
✗ Branch 15 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(static_cast<tsid::math::Index>(constraint.cols()) ==
95 static_cast<tsid::math::Index>(robot.nv()));
96
11/22
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 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 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 100 times.
 200 REQUIRE_FINITE(constraint.matrix());
97
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.vector()));
98
99
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
 200 pseudoInverse(constraint.matrix(), Jpinv, 1e-4);
100
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 ConstRefVector dv = Jpinv * constraint.vector();
101
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(Jpinv));
102
10/20
✓ 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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 100 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 100 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 100 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 100 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 100 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 100 times.
 200 BOOST_CHECK(MatrixXd::Identity(6, 6).isApprox(constraint.matrix() * Jpinv));
103
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 100 times.
 200 if (!isFinite(dv)) {
104 ✗ cout << "Jpinv" << Jpinv.transpose() << endl;
105 ✗ cout << "b" << constraint.vector().transpose() << endl;
106 }
107
11/22
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 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 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 100 times.
 200 REQUIRE_FINITE(dv.transpose());
108
109
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 v += dt * dv;
110
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 q = pinocchio::integrate(robot.model(), q, dt * v);
111
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(v));
112
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(q));
113 200 t += dt;
114
115
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 error = task.position_error().norm();
116
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(task.position_error()));
117
6/12
✓ 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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 100 times.
 200 BOOST_CHECK(error <= error_past);
118 200 error_past = error;
119
120
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 99 times.
 200 if (i % 100 == 0)
121
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 cout << "Time " << t << "\t Pos error " << error << "\t Vel error "
122
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 << task.velocity_error().norm() << endl;
123 200 }
124 2 }
125
126
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_task_com_equality) {
127
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "\n\n*********** TEST TASK COM EQUALITY ***********\n";
128 2 vector<string> package_dirs;
129
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 package_dirs.push_back(romeo_model_path);
130
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
131 RobotWrapper robot(urdfFileName, package_dirs,
132
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 pinocchio::JointModelFreeFlyer(), false);
133
134
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(robot.model());
135
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf";
136
137
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::srdf::loadReferenceConfigurations(robot.model(), srdfFileName,
138 false);
139
140 // const unsigned int nv = robot.nv();
141
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = neutral(robot.model());
142
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 std::cout << "q: " << q.transpose() << std::endl;
143
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 q(2) += 0.84;
144
145
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::centerOfMass(robot.model(), data, q);
146
147
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 TaskComEquality task("task-com", robot);
148
149
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd Kp = VectorXd::Ones(3);
150
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 VectorXd Kd = 2.0 * VectorXd::Ones(3);
151
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.Kp(Kp);
152
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.Kd(Kd);
153
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 18 taken 1 times.
✗ Branch 19 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(task.Kp().isApprox(Kp));
154
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 18 taken 1 times.
✗ Branch 19 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(task.Kd().isApprox(Kd));
155
156 math::Vector3 com_ref =
157
3/6
✓ 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.
 2 data.com[0] + pinocchio::SE3::Vector3(0.02, 0.02, 0.02);
158
4/8
✓ 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 11 taken 1 times.
✗ Branch 12 not taken.
 2 TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_com", com_ref);
159
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 TrajectorySample sample;
160
161 2 double t = 0.0;
162 2 const double dt = 0.001;
163
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd Jpinv(robot.nv(), 3);
164 2 double error, error_past = 1e100;
165
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 VectorXd v = VectorXd::Zero(robot.nv());
166
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (int i = 0; i < max_it; i++) {
167
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 robot.computeAllTerms(data, q, v);
168
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 sample = traj->computeNext();
169
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 task.setReference(sample);
170
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 const ConstraintBase &constraint = task.compute(t, q, v, data);
171
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_CHECK(constraint.rows() == 3);
172
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 14 taken 100 times.
✗ Branch 15 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(static_cast<tsid::math::Index>(constraint.cols()) ==
173 static_cast<tsid::math::Index>(robot.nv()));
174
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.matrix()));
175
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.vector()));
176
177
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
 200 pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
178
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 ConstRefVector dv = Jpinv * constraint.vector();
179
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(Jpinv));
180
12/24
✓ 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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✓ Branch 24 taken 100 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 100 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 100 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 100 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 100 times.
✗ Branch 37 not taken.
✗ Branch 41 not taken.
✓ Branch 42 taken 100 times.
 200 BOOST_CHECK(MatrixXd::Identity(constraint.rows(), constraint.rows())
181 .isApprox(constraint.matrix() * Jpinv));
182
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(dv));
183
184
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 v += dt * dv;
185
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 q = pinocchio::integrate(robot.model(), q, dt * v);
186
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(v));
187
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(q));
188 200 t += dt;
189
190
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 error = task.position_error().norm();
191
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(task.position_error()));
192
6/12
✓ 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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 100 times.
 200 BOOST_CHECK((error - error_past) <= 1e-4);
193 200 error_past = error;
194
195
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
 200 if (error < 1e-8) break;
196
197
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 99 times.
 200 if (i % 100 == 0)
198
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 cout << "Time " << t << "\t CoM pos error " << error
199
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 << "\t CoM vel error " << task.velocity_error().norm() << endl;
200
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 }
201 2 }
202
203
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_task_joint_posture) {
204
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "\n\n*********** TEST TASK JOINT POSTURE ***********\n";
205 2 vector<string> package_dirs;
206
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 package_dirs.push_back(romeo_model_path);
207
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
208 RobotWrapper robot(urdfFileName, package_dirs,
209
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 pinocchio::JointModelFreeFlyer(), false);
210
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const unsigned int na = robot.nv() - 6;
211
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "Gonna create task\n";
213
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 TaskJointPosture task("task-posture", robot);
214
215
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 cout << "Gonna set gains\n" << na << endl;
216
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd Kp = VectorXd::Ones(na);
217
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd Kd = 2.0 * Kp;
218
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.Kp(Kp);
219
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.Kd(Kd);
220
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 18 taken 1 times.
✗ Branch 19 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(task.Kp().isApprox(Kp));
221
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 18 taken 1 times.
✗ Branch 19 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(task.Kd().isApprox(Kd));
222
223
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "Gonna create reference trajectory\n";
224
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 ConstRefVector q_ref = math::Vector::Random(na);
225
4/8
✓ 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 11 taken 1 times.
✗ Branch 12 not taken.
 2 TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_joint", q_ref);
226
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 TrajectorySample sample;
227
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "Gonna set up for simulation\n";
229 2 double t = 0.0;
230 2 const double dt = 0.001;
231
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 MatrixXd Jpinv(robot.nv(), na);
232 2 double error, error_past = 1e100;
233
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = neutral(robot.model());
234
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 VectorXd v = VectorXd::Zero(robot.nv());
235
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(robot.model());
236
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (int i = 0; i < max_it; i++) {
237
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 robot.computeAllTerms(data, q, v);
238
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 sample = traj->computeNext();
239
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 task.setReference(sample);
240
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 const ConstraintBase &constraint = task.compute(t, q, v, data);
241
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_CHECK(constraint.rows() == na);
242
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 14 taken 100 times.
✗ Branch 15 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(static_cast<tsid::math::Index>(constraint.cols()) ==
243 static_cast<tsid::math::Index>(robot.nv()));
244
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.matrix()));
245
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.vector()));
246
247
4/8
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
 200 pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
248
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 ConstRefVector dv = Jpinv * constraint.vector();
249
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(Jpinv));
250
10/20
✓ 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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 100 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 100 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 100 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 100 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 100 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 100 times.
 200 BOOST_CHECK(
251 MatrixXd::Identity(na, na).isApprox(constraint.matrix() * Jpinv));
252
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(dv));
253
254
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 v += dt * dv;
255
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 q = pinocchio::integrate(robot.model(), q, dt * v);
256
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(v));
257
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(q));
258 200 t += dt;
259
260
2/4
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
 200 error = task.position_error().norm();
261
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(task.position_error()));
262
6/12
✓ 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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✗ Branch 22 not taken.
✓ Branch 23 taken 100 times.
 200 BOOST_CHECK(error <= error_past);
263 200 error_past = error;
264
265
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 99 times.
 200 if (i % 100 == 0)
266
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 cout << "Time " << t << "\t pos error " << error << "\t vel error "
267
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 << task.velocity_error().norm() << endl;
268 200 }
269 2 }
270
271
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_task_joint_bounds) {
272
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "\n\n*********** TEST TASK JOINT BOUNDS ***********\n";
273 2 vector<string> package_dirs;
274
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 package_dirs.push_back(romeo_model_path);
275
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
276 RobotWrapper robot(urdfFileName, package_dirs,
277
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 pinocchio::JointModelFreeFlyer(), false);
278
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const unsigned int na = robot.nv() - 6;
279 2 const double dt = 0.001;
280
281
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "Gonna create task\n";
282
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 TaskJointBounds task("task-joint-bounds", robot, dt);
283
284
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 cout << "Gonna set limits\n" << na << endl;
285
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd dq_max = VectorXd::Ones(na);
286
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd dq_min = -dq_max;
287
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 task.setVelocityBounds(dq_min, dq_max);
288
289
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 18 taken 1 times.
✗ Branch 19 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(task.getVelocityLowerBounds().isApprox(dq_min));
290
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 18 taken 1 times.
✗ Branch 19 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(task.getVelocityUpperBounds().isApprox(dq_max));
291
292
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "Gonna set up for simulation\n";
293 2 double t = 0.0;
294
295
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = neutral(robot.model());
296
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 VectorXd v = VectorXd::Zero(robot.nv());
297
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(robot.model());
298
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (int i = 0; i < max_it; i++) {
299
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 robot.computeAllTerms(data, q, v);
300
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 const ConstraintBase &constraint = task.compute(t, q, v, data);
301
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 14 taken 100 times.
✗ Branch 15 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(constraint.rows() == (Eigen::Index)robot.nv());
302
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 14 taken 100 times.
✗ Branch 15 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(static_cast<tsid::math::Index>(constraint.cols()) ==
303 static_cast<tsid::math::Index>(robot.nv()));
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.lowerBound()));
305
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.upperBound()));
306
307
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(v));
308
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(q));
309 200 t += dt;
310 }
311 2 }
312
313
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_task_joint_posVelAcc_bounds) {
314
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "\n\n*********** TEST TASK JOINT POS VEL ACC BOUNDS ***********\n";
315 2 vector<string> package_dirs;
316
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 package_dirs.push_back(romeo_model_path);
317
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
318 RobotWrapper robot(urdfFileName, package_dirs,
319
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 pinocchio::JointModelFreeFlyer(), false);
320
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const unsigned int na = robot.nv() - 6;
321 2 const double dt = 0.001;
322
323
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "Gonna create task\n";
324
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 TaskJointPosVelAccBounds task("task-joint-posVelAcc-bounds", robot, dt);
325
326
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 cout << "Gonna set limits\n" << na << endl;
327
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd dq_max = VectorXd::Ones(na);
328
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd dq_min = -dq_max;
329
330
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 task.setPositionBounds(dq_min, dq_max);
331
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.setVelocityBounds(dq_max);
332
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 task.setAccelerationBounds(dq_max);
333
334
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 18 taken 1 times.
✗ Branch 19 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(task.getPositionLowerBounds().isApprox(dq_min));
335
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 18 taken 1 times.
✗ Branch 19 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(task.getPositionUpperBounds().isApprox(dq_max));
336
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 18 taken 1 times.
✗ Branch 19 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(task.getVelocityBounds().isApprox(dq_max));
337
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 18 taken 1 times.
✗ Branch 19 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(task.getAccelerationBounds().isApprox(dq_max));
338
339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 cout << "Gonna set up for simulation\n";
340 2 double t = 0.0;
341
342
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = neutral(robot.model());
343
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 VectorXd v = VectorXd::Zero(robot.nv());
344
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 pinocchio::Data data(robot.model());
345
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
 202 for (int i = 0; i < max_it; i++) {
346
1/2
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
 200 robot.computeAllTerms(data, q, v);
347
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 const ConstraintBase &constraint = task.compute(t, q, v, data);
348
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 14 taken 100 times.
✗ Branch 15 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(constraint.rows() == (Eigen::Index)robot.na());
349
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 14 taken 100 times.
✗ Branch 15 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(static_cast<tsid::math::Index>(constraint.cols()) ==
350 static_cast<tsid::math::Index>(robot.nv()));
351
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.lowerBound()));
352
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 14 taken 100 times.
✗ Branch 15 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_REQUIRE(isFinite(constraint.upperBound()));
353
354
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(v));
355
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 14 taken 100 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 100 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 100 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 100 times.
 200 BOOST_REQUIRE(isFinite(q));
356 200 t += dt;
357 }
358 2 }
359
360 BOOST_AUTO_TEST_SUITE_END()
361