Directory: | ./ |
---|---|
File: | tests/tasks.cpp |
Date: | 2024-08-26 20:29:39 |
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 |