Directory: | ./ |
---|---|
File: | tests/tsid-formulation.cpp |
Date: | 2024-08-26 20:29:39 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 380 | 409 | 92.9% |
Branches: | 1730 | 3802 | 45.5% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2017 CNRS, NYU, MPI Tübingen | ||
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/contacts/contact-6d.hpp> | ||
24 | #include <tsid/contacts/contact-point.hpp> | ||
25 | #include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp> | ||
26 | #include <tsid/tasks/task-com-equality.hpp> | ||
27 | #include <tsid/tasks/task-se3-equality.hpp> | ||
28 | #include <tsid/tasks/task-joint-posture.hpp> | ||
29 | #include <tsid/tasks/task-joint-bounds.hpp> | ||
30 | #include <tsid/trajectories/trajectory-euclidian.hpp> | ||
31 | #include <tsid/solvers/solver-HQP-factory.hxx> | ||
32 | #include <tsid/solvers/utils.hpp> | ||
33 | #include <tsid/utils/stop-watch.hpp> | ||
34 | #include <tsid/utils/statistics.hpp> | ||
35 | #include <tsid/math/utils.hpp> | ||
36 | |||
37 | #include <pinocchio/algorithm/joint-configuration.hpp> // integrate | ||
38 | #include <pinocchio/parsers/srdf.hpp> | ||
39 | |||
40 | using namespace tsid; | ||
41 | using namespace tsid::trajectories; | ||
42 | using namespace tsid::math; | ||
43 | using namespace tsid::contacts; | ||
44 | using namespace tsid::tasks; | ||
45 | using namespace tsid::solvers; | ||
46 | using namespace tsid::robots; | ||
47 | using namespace std; | ||
48 | |||
49 | #define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A << ": " << A) | ||
50 | #define CHECK_LESS_THAN(A, B) \ | ||
51 | BOOST_CHECK_MESSAGE(A < B, #A << ": " << A << ">" << B) | ||
52 | |||
53 | #define REQUIRE_TASK_FINITE(task) \ | ||
54 | REQUIRE_FINITE(task.getConstraint().matrix()); \ | ||
55 | REQUIRE_FINITE(task.getConstraint().vector()) | ||
56 | |||
57 | #define REQUIRE_CONTACT_FINITE(contact) \ | ||
58 | REQUIRE_FINITE(contact.getMotionConstraint().matrix()); \ | ||
59 | REQUIRE_FINITE(contact.getMotionConstraint().vector()); \ | ||
60 | REQUIRE_FINITE(contact.getForceConstraint().matrix()); \ | ||
61 | REQUIRE_FINITE(contact.getForceConstraint().lowerBound()); \ | ||
62 | REQUIRE_FINITE(contact.getForceConstraint().upperBound()); \ | ||
63 | REQUIRE_FINITE(contact.getForceRegularizationTask().matrix()); \ | ||
64 | REQUIRE_FINITE(contact.getForceRegularizationTask().vector()) | ||
65 | |||
66 | const string romeo_model_path = TSID_SOURCE_DIR "/models/romeo"; | ||
67 | const string quadruped_model_path = TSID_SOURCE_DIR "/models/quadruped"; | ||
68 | |||
69 | #ifndef NDEBUG | ||
70 | const int max_it = 10; | ||
71 | #else | ||
72 | const int max_it = 10; | ||
73 | #endif | ||
74 | |||
75 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) | ||
76 | |||
77 | class StandardRomeoInvDynCtrl { | ||
78 | public: | ||
79 | static const double lxp; | ||
80 | static const double lxn; | ||
81 | static const double lyp; | ||
82 | static const double lyn; | ||
83 | static const double lz; | ||
84 | static const double mu; | ||
85 | static const double fMin; | ||
86 | static const double fMax; | ||
87 | static const std::string rf_frame_name; | ||
88 | static const std::string lf_frame_name; | ||
89 | static const Vector3 contactNormal; | ||
90 | static const double w_com; | ||
91 | static const double w_posture; | ||
92 | static const double w_forceReg; | ||
93 | static const double kp_contact; | ||
94 | static const double kp_com; | ||
95 | static const double kp_posture; | ||
96 | double t; | ||
97 | |||
98 | std::shared_ptr<RobotWrapper> robot; | ||
99 | std::shared_ptr<InverseDynamicsFormulationAccForce> tsid; | ||
100 | std::shared_ptr<Contact6d> contactRF; | ||
101 | std::shared_ptr<Contact6d> contactLF; | ||
102 | std::shared_ptr<TaskComEquality> comTask; | ||
103 | std::shared_ptr<TaskJointPosture> postureTask; | ||
104 | std::shared_ptr<TaskJointBounds> jointBoundsTask; | ||
105 | Vector q; | ||
106 | Vector v; | ||
107 | pinocchio::SE3 H_rf_ref; | ||
108 | pinocchio::SE3 H_lf_ref; | ||
109 | |||
110 |
4/8✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
|
3 | StandardRomeoInvDynCtrl(double dt) : t(0.) { |
111 | 3 | vector<string> package_dirs; | |
112 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | package_dirs.push_back(romeo_model_path); |
113 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | const string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf"; |
114 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | robot = std::make_shared<RobotWrapper>(urdfFileName, package_dirs, |
115 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | pinocchio::JointModelFreeFlyer()); |
116 | |||
117 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf"; |
118 | |||
119 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | pinocchio::srdf::loadReferenceConfigurations(robot->model(), srdfFileName, |
120 | false); | ||
121 | |||
122 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | const unsigned int nv{static_cast<unsigned int>(robot->nv())}; |
123 |
2/4✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
|
3 | q = neutral(robot->model()); |
124 |
4/8✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
|
3 | std::cout << "q: " << q.transpose() << std::endl; |
125 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | q(2) += 0.84; |
126 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | v = Vector::Zero(nv); |
127 |
8/16✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 3 times.
|
3 | BOOST_REQUIRE(robot->model().existFrame(rf_frame_name)); |
128 |
8/16✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 3 times.
|
3 | BOOST_REQUIRE(robot->model().existFrame(lf_frame_name)); |
129 | |||
130 | // Create the inverse-dynamics formulation | ||
131 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | tsid = std::make_shared<InverseDynamicsFormulationAccForce>("tsid", *robot); |
132 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | tsid->computeProblemData(t, q, v); |
133 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | pinocchio::Data &data = tsid->data(); |
134 | |||
135 | // Add the contact constraints | ||
136 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | Matrix3x contactPoints(3, 4); |
137 |
11/22✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 3 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 3 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 3 times.
✗ Branch 32 not taken.
|
6 | contactPoints << -lxn, -lxn, +lxp, +lxp, -lyn, +lyp, -lyn, +lyp, lz, lz, lz, |
138 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | lz; |
139 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | contactRF = std::make_shared<Contact6d>("contact_rfoot", *robot, |
140 | rf_frame_name, contactPoints, | ||
141 | 3 | contactNormal, mu, fMin, fMax); | |
142 |
4/8✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | contactRF->Kp(kp_contact * Vector::Ones(6)); |
143 |
5/10✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
|
3 | contactRF->Kd(2.0 * contactRF->Kp().cwiseSqrt()); |
144 |
4/8✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
|
3 | H_rf_ref = robot->position(data, robot->model().getJointId(rf_frame_name)); |
145 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | contactRF->setReference(H_rf_ref); |
146 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | tsid->addRigidContact(*contactRF, w_forceReg); |
147 | |||
148 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
6 | contactLF = std::make_shared<Contact6d>("contact_lfoot", *robot, |
149 | lf_frame_name, contactPoints, | ||
150 | 3 | contactNormal, mu, fMin, fMax); | |
151 |
4/8✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | contactLF->Kp(kp_contact * Vector::Ones(6)); |
152 |
5/10✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
|
3 | contactLF->Kd(2.0 * contactLF->Kp().cwiseSqrt()); |
153 |
4/8✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
|
3 | H_lf_ref = robot->position(data, robot->model().getJointId(lf_frame_name)); |
154 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | contactLF->setReference(H_lf_ref); |
155 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | tsid->addRigidContact(*contactLF, w_forceReg); |
156 | |||
157 | // Add the com task | ||
158 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | comTask = std::make_shared<TaskComEquality>("task-com", *robot); |
159 |
4/8✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | comTask->Kp(kp_com * Vector::Ones(3)); |
160 |
5/10✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
|
3 | comTask->Kd(2.0 * comTask->Kp().cwiseSqrt()); |
161 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | tsid->addMotionTask(*comTask, w_com, 1); |
162 | |||
163 | // Add the posture task | ||
164 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | postureTask = std::make_shared<TaskJointPosture>("task-posture", *robot); |
165 |
4/8✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | postureTask->Kp(kp_posture * Vector::Ones(nv - 6)); |
166 |
5/10✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
|
3 | postureTask->Kd(2.0 * postureTask->Kp().cwiseSqrt()); |
167 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | tsid->addMotionTask(*postureTask, w_posture, 1); |
168 | |||
169 | // Add the joint bounds task | ||
170 | jointBoundsTask = | ||
171 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | std::make_shared<TaskJointBounds>("task-joint-bounds", *robot, dt); |
172 |
4/8✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
|
3 | Vector dq_max = 10 * Vector::Ones(robot->na()); |
173 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | Vector dq_min = -dq_max; |
174 |
3/6✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
|
3 | jointBoundsTask->setVelocityBounds(dq_min, dq_max); |
175 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | tsid->addMotionTask(*jointBoundsTask, 1.0, 0); |
176 | 3 | } | |
177 | }; | ||
178 | |||
179 | const double StandardRomeoInvDynCtrl::lxp = 0.14; | ||
180 | const double StandardRomeoInvDynCtrl::lxn = 0.077; | ||
181 | const double StandardRomeoInvDynCtrl::lyp = 0.069; | ||
182 | const double StandardRomeoInvDynCtrl::lyn = 0.069; | ||
183 | const double StandardRomeoInvDynCtrl::lz = 0.105; | ||
184 | const double StandardRomeoInvDynCtrl::mu = 0.3; | ||
185 | const double StandardRomeoInvDynCtrl::fMin = 5.0; | ||
186 | const double StandardRomeoInvDynCtrl::fMax = 1000.0; | ||
187 | const std::string StandardRomeoInvDynCtrl::rf_frame_name = "RAnkleRoll"; | ||
188 | const std::string StandardRomeoInvDynCtrl::lf_frame_name = "LAnkleRoll"; | ||
189 | const Vector3 StandardRomeoInvDynCtrl::contactNormal = Vector3::UnitZ(); | ||
190 | const double StandardRomeoInvDynCtrl::w_com = 1.0; | ||
191 | const double StandardRomeoInvDynCtrl::w_posture = 1e-2; | ||
192 | const double StandardRomeoInvDynCtrl::w_forceReg = 1e-5; | ||
193 | const double StandardRomeoInvDynCtrl::kp_contact = 100.0; | ||
194 | const double StandardRomeoInvDynCtrl::kp_com = 30.0; | ||
195 | const double StandardRomeoInvDynCtrl::kp_posture = 30.0; | ||
196 | |||
197 |
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_invdyn_formulation_acc_force_remove_contact) { |
198 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n*** test_invdyn_formulation_acc_force_remove_contact ***\n"; |
199 | 2 | const double dt = 0.01; | |
200 | 2 | const unsigned int PRINT_N = 10; | |
201 | 2 | const unsigned int REMOVE_CONTACT_N = 100; | |
202 | 2 | const double CONTACT_TRANSITION_TIME = 1.0; | |
203 | 2 | const double kp_RF = 100.0; | |
204 | 2 | const double w_RF = 1e3; | |
205 | 2 | double t = 0.0; | |
206 | |||
207 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | StandardRomeoInvDynCtrl romeo_inv_dyn(dt); |
208 | 2 | RobotWrapper &robot = *(romeo_inv_dyn.robot); | |
209 | 2 | auto tsid = romeo_inv_dyn.tsid; | |
210 | 2 | Contact6d &contactRF = *(romeo_inv_dyn.contactRF); | |
211 | 2 | Contact6d &contactLF = *(romeo_inv_dyn.contactLF); | |
212 | 2 | TaskComEquality &comTask = *(romeo_inv_dyn.comTask); | |
213 | 2 | TaskJointPosture &postureTask = *(romeo_inv_dyn.postureTask); | |
214 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector q = romeo_inv_dyn.q; |
215 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector v = romeo_inv_dyn.v; |
216 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const int nv = robot.model().nv; |
217 | |||
218 | // Add the right foot task | ||
219 | auto rightFootTask = std::make_shared<TaskSE3Equality>( | ||
220 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | "task-right-foot", robot, romeo_inv_dyn.rf_frame_name); |
221 |
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 | rightFootTask->Kp(kp_RF * Vector::Ones(6)); |
222 |
5/10✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
|
2 | rightFootTask->Kd(2.0 * rightFootTask->Kp().cwiseSqrt()); |
223 | 8 | pinocchio::SE3 H_rf_ref = robot.position( | |
224 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 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.
|
2 | tsid->data(), robot.model().getJointId(romeo_inv_dyn.rf_frame_name)); |
225 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2 | tsid->addMotionTask(*rightFootTask, w_RF, 1); |
226 | |||
227 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | rightFootTask->setReference(H_rf_ref); |
228 | |||
229 |
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 | Vector3 com_ref = robot.com(tsid->data()); |
230 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | com_ref(1) += 0.1; |
231 | auto trajCom = | ||
232 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
233 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | TrajectorySample sampleCom(3); |
234 | |||
235 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector q_ref = q.tail(nv - 6); |
236 | auto trajPosture = | ||
237 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<TrajectoryEuclidianConstant>("traj_posture", q_ref); |
238 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | TrajectorySample samplePosture(nv - 6); |
239 | |||
240 | // Create an HQP solver | ||
241 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPBase *solver = SolverHQPFactory::createNewSolver( |
242 | SOLVER_HQP_EIQUADPROG, "solver-eiquadprog"); | ||
243 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
244 | |||
245 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector tau_old(nv - 6); |
246 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
|
22 | for (int i = 0; i < max_it; i++) { |
247 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
|
20 | if (i == REMOVE_CONTACT_N) { |
248 | ✗ | cout << "Start breaking contact right foot\n"; | |
249 | ✗ | tsid->removeRigidContact(contactRF.name(), CONTACT_TRANSITION_TIME); | |
250 | } | ||
251 | |||
252 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
20 | sampleCom = trajCom->computeNext(); |
253 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | comTask.setReference(sampleCom); |
254 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
20 | samplePosture = trajPosture->computeNext(); |
255 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | postureTask.setReference(samplePosture); |
256 | |||
257 |
3/6✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
258 |
5/8✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
20 | if (i == 0) cout << HQPDataToString(HQPData, false) << endl; |
259 | |||
260 |
26/52✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
|
20 | REQUIRE_TASK_FINITE(postureTask); |
261 |
26/52✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
|
20 | REQUIRE_TASK_FINITE(comTask); |
262 |
91/182✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
✓ Branch 93 taken 10 times.
✗ Branch 94 not taken.
✓ Branch 97 taken 10 times.
✗ Branch 98 not taken.
✓ Branch 102 taken 10 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 10 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 10 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 10 times.
✗ Branch 112 not taken.
✓ Branch 114 taken 10 times.
✗ Branch 115 not taken.
✓ Branch 117 taken 10 times.
✗ Branch 118 not taken.
✓ Branch 120 taken 10 times.
✗ Branch 121 not taken.
✓ Branch 123 taken 10 times.
✗ Branch 124 not taken.
✓ Branch 126 taken 10 times.
✗ Branch 127 not taken.
✓ Branch 129 taken 10 times.
✗ Branch 130 not taken.
✗ Branch 136 not taken.
✓ Branch 137 taken 10 times.
✓ Branch 139 taken 10 times.
✗ Branch 140 not taken.
✓ Branch 143 taken 10 times.
✗ Branch 144 not taken.
✓ Branch 148 taken 10 times.
✗ Branch 149 not taken.
✓ Branch 151 taken 10 times.
✗ Branch 152 not taken.
✓ Branch 154 taken 10 times.
✗ Branch 155 not taken.
✓ Branch 157 taken 10 times.
✗ Branch 158 not taken.
✓ Branch 160 taken 10 times.
✗ Branch 161 not taken.
✓ Branch 163 taken 10 times.
✗ Branch 164 not taken.
✓ Branch 166 taken 10 times.
✗ Branch 167 not taken.
✓ Branch 169 taken 10 times.
✗ Branch 170 not taken.
✓ Branch 172 taken 10 times.
✗ Branch 173 not taken.
✓ Branch 175 taken 10 times.
✗ Branch 176 not taken.
✗ Branch 182 not taken.
✓ Branch 183 taken 10 times.
✓ Branch 185 taken 10 times.
✗ Branch 186 not taken.
✓ Branch 189 taken 10 times.
✗ Branch 190 not taken.
✓ Branch 194 taken 10 times.
✗ Branch 195 not taken.
✓ Branch 197 taken 10 times.
✗ Branch 198 not taken.
✓ Branch 200 taken 10 times.
✗ Branch 201 not taken.
✓ Branch 203 taken 10 times.
✗ Branch 204 not taken.
✓ Branch 206 taken 10 times.
✗ Branch 207 not taken.
✓ Branch 209 taken 10 times.
✗ Branch 210 not taken.
✓ Branch 212 taken 10 times.
✗ Branch 213 not taken.
✓ Branch 215 taken 10 times.
✗ Branch 216 not taken.
✓ Branch 218 taken 10 times.
✗ Branch 219 not taken.
✓ Branch 221 taken 10 times.
✗ Branch 222 not taken.
✗ Branch 228 not taken.
✓ Branch 229 taken 10 times.
✓ Branch 231 taken 10 times.
✗ Branch 232 not taken.
✓ Branch 235 taken 10 times.
✗ Branch 236 not taken.
✓ Branch 240 taken 10 times.
✗ Branch 241 not taken.
✓ Branch 243 taken 10 times.
✗ Branch 244 not taken.
✓ Branch 246 taken 10 times.
✗ Branch 247 not taken.
✓ Branch 249 taken 10 times.
✗ Branch 250 not taken.
✓ Branch 252 taken 10 times.
✗ Branch 253 not taken.
✓ Branch 255 taken 10 times.
✗ Branch 256 not taken.
✓ Branch 258 taken 10 times.
✗ Branch 259 not taken.
✓ Branch 261 taken 10 times.
✗ Branch 262 not taken.
✓ Branch 264 taken 10 times.
✗ Branch 265 not taken.
✓ Branch 267 taken 10 times.
✗ Branch 268 not taken.
✗ Branch 274 not taken.
✓ Branch 275 taken 10 times.
✓ Branch 277 taken 10 times.
✗ Branch 278 not taken.
✓ Branch 281 taken 10 times.
✗ Branch 282 not taken.
✓ Branch 286 taken 10 times.
✗ Branch 287 not taken.
✓ Branch 289 taken 10 times.
✗ Branch 290 not taken.
✓ Branch 292 taken 10 times.
✗ Branch 293 not taken.
✓ Branch 295 taken 10 times.
✗ Branch 296 not taken.
✓ Branch 298 taken 10 times.
✗ Branch 299 not taken.
✓ Branch 301 taken 10 times.
✗ Branch 302 not taken.
✓ Branch 304 taken 10 times.
✗ Branch 305 not taken.
✓ Branch 307 taken 10 times.
✗ Branch 308 not taken.
✓ Branch 310 taken 10 times.
✗ Branch 311 not taken.
✓ Branch 313 taken 10 times.
✗ Branch 314 not taken.
✗ Branch 320 not taken.
✓ Branch 321 taken 10 times.
|
20 | REQUIRE_CONTACT_FINITE(contactRF); |
263 |
91/182✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
✓ Branch 93 taken 10 times.
✗ Branch 94 not taken.
✓ Branch 97 taken 10 times.
✗ Branch 98 not taken.
✓ Branch 102 taken 10 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 10 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 10 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 10 times.
✗ Branch 112 not taken.
✓ Branch 114 taken 10 times.
✗ Branch 115 not taken.
✓ Branch 117 taken 10 times.
✗ Branch 118 not taken.
✓ Branch 120 taken 10 times.
✗ Branch 121 not taken.
✓ Branch 123 taken 10 times.
✗ Branch 124 not taken.
✓ Branch 126 taken 10 times.
✗ Branch 127 not taken.
✓ Branch 129 taken 10 times.
✗ Branch 130 not taken.
✗ Branch 136 not taken.
✓ Branch 137 taken 10 times.
✓ Branch 139 taken 10 times.
✗ Branch 140 not taken.
✓ Branch 143 taken 10 times.
✗ Branch 144 not taken.
✓ Branch 148 taken 10 times.
✗ Branch 149 not taken.
✓ Branch 151 taken 10 times.
✗ Branch 152 not taken.
✓ Branch 154 taken 10 times.
✗ Branch 155 not taken.
✓ Branch 157 taken 10 times.
✗ Branch 158 not taken.
✓ Branch 160 taken 10 times.
✗ Branch 161 not taken.
✓ Branch 163 taken 10 times.
✗ Branch 164 not taken.
✓ Branch 166 taken 10 times.
✗ Branch 167 not taken.
✓ Branch 169 taken 10 times.
✗ Branch 170 not taken.
✓ Branch 172 taken 10 times.
✗ Branch 173 not taken.
✓ Branch 175 taken 10 times.
✗ Branch 176 not taken.
✗ Branch 182 not taken.
✓ Branch 183 taken 10 times.
✓ Branch 185 taken 10 times.
✗ Branch 186 not taken.
✓ Branch 189 taken 10 times.
✗ Branch 190 not taken.
✓ Branch 194 taken 10 times.
✗ Branch 195 not taken.
✓ Branch 197 taken 10 times.
✗ Branch 198 not taken.
✓ Branch 200 taken 10 times.
✗ Branch 201 not taken.
✓ Branch 203 taken 10 times.
✗ Branch 204 not taken.
✓ Branch 206 taken 10 times.
✗ Branch 207 not taken.
✓ Branch 209 taken 10 times.
✗ Branch 210 not taken.
✓ Branch 212 taken 10 times.
✗ Branch 213 not taken.
✓ Branch 215 taken 10 times.
✗ Branch 216 not taken.
✓ Branch 218 taken 10 times.
✗ Branch 219 not taken.
✓ Branch 221 taken 10 times.
✗ Branch 222 not taken.
✗ Branch 228 not taken.
✓ Branch 229 taken 10 times.
✓ Branch 231 taken 10 times.
✗ Branch 232 not taken.
✓ Branch 235 taken 10 times.
✗ Branch 236 not taken.
✓ Branch 240 taken 10 times.
✗ Branch 241 not taken.
✓ Branch 243 taken 10 times.
✗ Branch 244 not taken.
✓ Branch 246 taken 10 times.
✗ Branch 247 not taken.
✓ Branch 249 taken 10 times.
✗ Branch 250 not taken.
✓ Branch 252 taken 10 times.
✗ Branch 253 not taken.
✓ Branch 255 taken 10 times.
✗ Branch 256 not taken.
✓ Branch 258 taken 10 times.
✗ Branch 259 not taken.
✓ Branch 261 taken 10 times.
✗ Branch 262 not taken.
✓ Branch 264 taken 10 times.
✗ Branch 265 not taken.
✓ Branch 267 taken 10 times.
✗ Branch 268 not taken.
✗ Branch 274 not taken.
✓ Branch 275 taken 10 times.
✓ Branch 277 taken 10 times.
✗ Branch 278 not taken.
✓ Branch 281 taken 10 times.
✗ Branch 282 not taken.
✓ Branch 286 taken 10 times.
✗ Branch 287 not taken.
✓ Branch 289 taken 10 times.
✗ Branch 290 not taken.
✓ Branch 292 taken 10 times.
✗ Branch 293 not taken.
✓ Branch 295 taken 10 times.
✗ Branch 296 not taken.
✓ Branch 298 taken 10 times.
✗ Branch 299 not taken.
✓ Branch 301 taken 10 times.
✗ Branch 302 not taken.
✓ Branch 304 taken 10 times.
✗ Branch 305 not taken.
✓ Branch 307 taken 10 times.
✗ Branch 308 not taken.
✓ Branch 310 taken 10 times.
✗ Branch 311 not taken.
✓ Branch 313 taken 10 times.
✗ Branch 314 not taken.
✗ Branch 320 not taken.
✓ Branch 321 taken 10 times.
|
20 | REQUIRE_CONTACT_FINITE(contactLF); |
264 | |||
265 |
16/32✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 10 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 10 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 10 times.
✗ Branch 47 not taken.
✗ Branch 55 not taken.
✓ Branch 56 taken 10 times.
|
20 | CHECK_LESS_THAN(contactRF.getMotionTask().position_error().norm(), 1e-3); |
266 |
16/32✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 10 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 10 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 10 times.
✗ Branch 47 not taken.
✗ Branch 55 not taken.
✓ Branch 56 taken 10 times.
|
20 | CHECK_LESS_THAN(contactLF.getMotionTask().position_error().norm(), 1e-3); |
267 | |||
268 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | const HQPOutput &sol = solver->solve(HQPData); |
269 | |||
270 |
8/16✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 10 times.
|
20 | BOOST_CHECK_MESSAGE(sol.status == HQP_STATUS_OPTIMAL, |
271 | "Status " + toString(sol.status)); | ||
272 | |||
273 |
1/2✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
|
20 | const Vector &tau = tsid->getActuatorForces(sol); |
274 |
1/2✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
|
20 | const Vector &dv = tsid->getAccelerations(sol); |
275 | |||
276 |
2/2✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
|
20 | if (i > 0) { |
277 |
14/28✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 9 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 9 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 9 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 9 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 9 times.
✗ Branch 41 not taken.
✗ Branch 49 not taken.
✓ Branch 50 taken 9 times.
|
18 | CHECK_LESS_THAN((tau - tau_old).norm(), 2e1); |
278 |
3/6✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 9 times.
|
18 | if ((tau - tau_old).norm() > 2e1) // || (i>=197 && i<=200)) |
279 | { | ||
280 | // contactRF.computeMotionTask(t, q, v, tsid->data()); | ||
281 | // rightFootTask->compute(t, q, v, tsid->data()); | ||
282 | ✗ | cout << "Time " << i << endl; | |
283 | ✗ | cout << "tau:\n" | |
284 | ✗ | << tau.transpose() << "\ntauOld:\n" | |
285 | ✗ | << tau_old.transpose() << "\n"; | |
286 | // cout << "RF contact task des acc: | ||
287 | // "<<contactRF.getMotionTask().getDesiredAcceleration().transpose()<<endl; | ||
288 | // cout << "RF contact task acc: | ||
289 | // "<<contactRF.getMotionTask().getAcceleration(dv).transpose()<<endl; | ||
290 | // cout << "RF motion task des acc: | ||
291 | // "<<rightFootTask->getDesiredAcceleration().transpose()<<endl; | ||
292 | ✗ | cout << endl; | |
293 | } | ||
294 | } | ||
295 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | tau_old = tau; |
296 | |||
297 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9 times.
|
20 | if (i % PRINT_N == 0) { |
298 |
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 << "Time " << i << endl; |
299 | |||
300 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::Matrix<double, 12, 1> f; |
301 |
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 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | if (tsid->getContactForces(contactRF.name(), sol, f)) |
302 | 4 | cout << " " << contactRF.name() | |
303 |
8/16✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
|
2 | << " force: " << contactRF.getNormalForce(f) << " \t"; |
304 | |||
305 |
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 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | if (tsid->getContactForces(contactLF.name(), sol, f)) |
306 | 4 | cout << " " << contactLF.name() | |
307 |
8/16✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
|
2 | << " force: " << contactLF.getNormalForce(f) << " \t"; |
308 | |||
309 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | cout << comTask.name() << " err: " << comTask.position_error().norm() |
310 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << " \t"; |
311 |
7/14✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | cout << "v=" << v.norm() << "\t dv=" << dv.norm() << endl; |
312 | } | ||
313 | |||
314 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | v += dt * dv; |
315 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | q = pinocchio::integrate(robot.model(), q, dt * v); |
316 | 20 | t += dt; | |
317 | |||
318 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(dv.transpose()); |
319 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(v.transpose()); |
320 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(q.transpose()); |
321 |
12/24✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✗ Branch 43 not taken.
✓ Branch 44 taken 10 times.
|
20 | CHECK_LESS_THAN(dv.norm(), 1e6); |
322 |
12/24✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✗ Branch 43 not taken.
✓ Branch 44 taken 10 times.
|
20 | CHECK_LESS_THAN(v.norm(), 1e6); |
323 | } | ||
324 | |||
325 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver; |
326 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n### TEST FINISHED ###\n"; |
327 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ 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 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
|
2 | PRINT_VECTOR(v); |
328 |
5/10✓ 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.
|
2 | cout << "Final CoM position: " << robot.com(tsid->data()).transpose() |
329 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << endl; |
330 |
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 << "Desired CoM position: " << com_ref.transpose() << endl; |
331 | 2 | } | |
332 | |||
333 |
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_invdyn_formulation_acc_force) { |
334 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n*** test_invdyn_formulation_acc_force ***\n"; |
335 | |||
336 | 2 | const double dt = 0.001; | |
337 | 2 | const unsigned int PRINT_N = 100; | |
338 | 2 | double t = 0.0; | |
339 | |||
340 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | StandardRomeoInvDynCtrl romeo_inv_dyn(dt); |
341 | 2 | RobotWrapper &robot = *(romeo_inv_dyn.robot); | |
342 | 2 | auto tsid = romeo_inv_dyn.tsid; | |
343 | 2 | Contact6d &contactRF = *(romeo_inv_dyn.contactRF); | |
344 | 2 | Contact6d &contactLF = *(romeo_inv_dyn.contactLF); | |
345 | 2 | TaskComEquality &comTask = *(romeo_inv_dyn.comTask); | |
346 | 2 | TaskJointPosture &postureTask = *(romeo_inv_dyn.postureTask); | |
347 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector q = romeo_inv_dyn.q; |
348 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector v = romeo_inv_dyn.v; |
349 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const int nv = robot.model().nv; |
350 | |||
351 |
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 | Vector3 com_ref = robot.com(tsid->data()); |
352 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | com_ref(1) += 0.1; |
353 | auto trajCom = | ||
354 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
355 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | TrajectorySample sampleCom(3); |
356 | |||
357 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector q_ref = q.tail(nv - 6); |
358 | auto trajPosture = | ||
359 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<TrajectoryEuclidianConstant>("traj_posture", q_ref); |
360 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | TrajectorySample samplePosture(nv - 6); |
361 | |||
362 | // Create an HQP solver | ||
363 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPBase *solver = SolverHQPFactory::createNewSolver( |
364 | SOLVER_HQP_EIQUADPROG, "solver-eiquadprog"); | ||
365 | |||
366 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
367 |
4/8✓ 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.
|
2 | cout << "nVar " << tsid->nVar() << endl; |
368 |
4/8✓ 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.
|
2 | cout << "nEq " << tsid->nEq() << endl; |
369 |
4/8✓ 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.
|
2 | cout << "nIn " << tsid->nIn() << endl; |
370 |
5/10✓ 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.
|
2 | cout << "Initial CoM position: " << robot.com(tsid->data()).transpose() |
371 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << endl; |
372 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | cout << "Initial RF position: " << romeo_inv_dyn.H_rf_ref << endl; |
373 |
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 << "Initial LF position: " << romeo_inv_dyn.H_lf_ref << endl; |
374 | |||
375 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector dv = Vector::Zero(nv); |
376 |
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 | Vector f_RF(12), f_LF(12), f(24); |
377 | 2 | vector<ContactBase *> contacts; | |
378 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contacts.push_back(&contactRF); |
379 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contacts.push_back(&contactLF); |
380 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Matrix Jc(24, nv); |
381 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
|
22 | for (int i = 0; i < max_it; i++) { |
382 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
20 | sampleCom = trajCom->computeNext(); |
383 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | comTask.setReference(sampleCom); |
384 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
20 | samplePosture = trajPosture->computeNext(); |
385 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | postureTask.setReference(samplePosture); |
386 | |||
387 |
3/6✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
388 |
5/8✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
20 | if (i == 0) cout << HQPDataToString(HQPData, false) << endl; |
389 | |||
390 |
26/52✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
|
20 | REQUIRE_TASK_FINITE(postureTask); |
391 |
26/52✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
|
20 | REQUIRE_TASK_FINITE(comTask); |
392 |
91/182✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
✓ Branch 93 taken 10 times.
✗ Branch 94 not taken.
✓ Branch 97 taken 10 times.
✗ Branch 98 not taken.
✓ Branch 102 taken 10 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 10 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 10 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 10 times.
✗ Branch 112 not taken.
✓ Branch 114 taken 10 times.
✗ Branch 115 not taken.
✓ Branch 117 taken 10 times.
✗ Branch 118 not taken.
✓ Branch 120 taken 10 times.
✗ Branch 121 not taken.
✓ Branch 123 taken 10 times.
✗ Branch 124 not taken.
✓ Branch 126 taken 10 times.
✗ Branch 127 not taken.
✓ Branch 129 taken 10 times.
✗ Branch 130 not taken.
✗ Branch 136 not taken.
✓ Branch 137 taken 10 times.
✓ Branch 139 taken 10 times.
✗ Branch 140 not taken.
✓ Branch 143 taken 10 times.
✗ Branch 144 not taken.
✓ Branch 148 taken 10 times.
✗ Branch 149 not taken.
✓ Branch 151 taken 10 times.
✗ Branch 152 not taken.
✓ Branch 154 taken 10 times.
✗ Branch 155 not taken.
✓ Branch 157 taken 10 times.
✗ Branch 158 not taken.
✓ Branch 160 taken 10 times.
✗ Branch 161 not taken.
✓ Branch 163 taken 10 times.
✗ Branch 164 not taken.
✓ Branch 166 taken 10 times.
✗ Branch 167 not taken.
✓ Branch 169 taken 10 times.
✗ Branch 170 not taken.
✓ Branch 172 taken 10 times.
✗ Branch 173 not taken.
✓ Branch 175 taken 10 times.
✗ Branch 176 not taken.
✗ Branch 182 not taken.
✓ Branch 183 taken 10 times.
✓ Branch 185 taken 10 times.
✗ Branch 186 not taken.
✓ Branch 189 taken 10 times.
✗ Branch 190 not taken.
✓ Branch 194 taken 10 times.
✗ Branch 195 not taken.
✓ Branch 197 taken 10 times.
✗ Branch 198 not taken.
✓ Branch 200 taken 10 times.
✗ Branch 201 not taken.
✓ Branch 203 taken 10 times.
✗ Branch 204 not taken.
✓ Branch 206 taken 10 times.
✗ Branch 207 not taken.
✓ Branch 209 taken 10 times.
✗ Branch 210 not taken.
✓ Branch 212 taken 10 times.
✗ Branch 213 not taken.
✓ Branch 215 taken 10 times.
✗ Branch 216 not taken.
✓ Branch 218 taken 10 times.
✗ Branch 219 not taken.
✓ Branch 221 taken 10 times.
✗ Branch 222 not taken.
✗ Branch 228 not taken.
✓ Branch 229 taken 10 times.
✓ Branch 231 taken 10 times.
✗ Branch 232 not taken.
✓ Branch 235 taken 10 times.
✗ Branch 236 not taken.
✓ Branch 240 taken 10 times.
✗ Branch 241 not taken.
✓ Branch 243 taken 10 times.
✗ Branch 244 not taken.
✓ Branch 246 taken 10 times.
✗ Branch 247 not taken.
✓ Branch 249 taken 10 times.
✗ Branch 250 not taken.
✓ Branch 252 taken 10 times.
✗ Branch 253 not taken.
✓ Branch 255 taken 10 times.
✗ Branch 256 not taken.
✓ Branch 258 taken 10 times.
✗ Branch 259 not taken.
✓ Branch 261 taken 10 times.
✗ Branch 262 not taken.
✓ Branch 264 taken 10 times.
✗ Branch 265 not taken.
✓ Branch 267 taken 10 times.
✗ Branch 268 not taken.
✗ Branch 274 not taken.
✓ Branch 275 taken 10 times.
✓ Branch 277 taken 10 times.
✗ Branch 278 not taken.
✓ Branch 281 taken 10 times.
✗ Branch 282 not taken.
✓ Branch 286 taken 10 times.
✗ Branch 287 not taken.
✓ Branch 289 taken 10 times.
✗ Branch 290 not taken.
✓ Branch 292 taken 10 times.
✗ Branch 293 not taken.
✓ Branch 295 taken 10 times.
✗ Branch 296 not taken.
✓ Branch 298 taken 10 times.
✗ Branch 299 not taken.
✓ Branch 301 taken 10 times.
✗ Branch 302 not taken.
✓ Branch 304 taken 10 times.
✗ Branch 305 not taken.
✓ Branch 307 taken 10 times.
✗ Branch 308 not taken.
✓ Branch 310 taken 10 times.
✗ Branch 311 not taken.
✓ Branch 313 taken 10 times.
✗ Branch 314 not taken.
✗ Branch 320 not taken.
✓ Branch 321 taken 10 times.
|
20 | REQUIRE_CONTACT_FINITE(contactRF); |
393 |
91/182✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✗ Branch 44 not taken.
✓ Branch 45 taken 10 times.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 51 taken 10 times.
✗ Branch 52 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 10 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 10 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 10 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 10 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 10 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 10 times.
✗ Branch 84 not taken.
✗ Branch 90 not taken.
✓ Branch 91 taken 10 times.
✓ Branch 93 taken 10 times.
✗ Branch 94 not taken.
✓ Branch 97 taken 10 times.
✗ Branch 98 not taken.
✓ Branch 102 taken 10 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 10 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 10 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 10 times.
✗ Branch 112 not taken.
✓ Branch 114 taken 10 times.
✗ Branch 115 not taken.
✓ Branch 117 taken 10 times.
✗ Branch 118 not taken.
✓ Branch 120 taken 10 times.
✗ Branch 121 not taken.
✓ Branch 123 taken 10 times.
✗ Branch 124 not taken.
✓ Branch 126 taken 10 times.
✗ Branch 127 not taken.
✓ Branch 129 taken 10 times.
✗ Branch 130 not taken.
✗ Branch 136 not taken.
✓ Branch 137 taken 10 times.
✓ Branch 139 taken 10 times.
✗ Branch 140 not taken.
✓ Branch 143 taken 10 times.
✗ Branch 144 not taken.
✓ Branch 148 taken 10 times.
✗ Branch 149 not taken.
✓ Branch 151 taken 10 times.
✗ Branch 152 not taken.
✓ Branch 154 taken 10 times.
✗ Branch 155 not taken.
✓ Branch 157 taken 10 times.
✗ Branch 158 not taken.
✓ Branch 160 taken 10 times.
✗ Branch 161 not taken.
✓ Branch 163 taken 10 times.
✗ Branch 164 not taken.
✓ Branch 166 taken 10 times.
✗ Branch 167 not taken.
✓ Branch 169 taken 10 times.
✗ Branch 170 not taken.
✓ Branch 172 taken 10 times.
✗ Branch 173 not taken.
✓ Branch 175 taken 10 times.
✗ Branch 176 not taken.
✗ Branch 182 not taken.
✓ Branch 183 taken 10 times.
✓ Branch 185 taken 10 times.
✗ Branch 186 not taken.
✓ Branch 189 taken 10 times.
✗ Branch 190 not taken.
✓ Branch 194 taken 10 times.
✗ Branch 195 not taken.
✓ Branch 197 taken 10 times.
✗ Branch 198 not taken.
✓ Branch 200 taken 10 times.
✗ Branch 201 not taken.
✓ Branch 203 taken 10 times.
✗ Branch 204 not taken.
✓ Branch 206 taken 10 times.
✗ Branch 207 not taken.
✓ Branch 209 taken 10 times.
✗ Branch 210 not taken.
✓ Branch 212 taken 10 times.
✗ Branch 213 not taken.
✓ Branch 215 taken 10 times.
✗ Branch 216 not taken.
✓ Branch 218 taken 10 times.
✗ Branch 219 not taken.
✓ Branch 221 taken 10 times.
✗ Branch 222 not taken.
✗ Branch 228 not taken.
✓ Branch 229 taken 10 times.
✓ Branch 231 taken 10 times.
✗ Branch 232 not taken.
✓ Branch 235 taken 10 times.
✗ Branch 236 not taken.
✓ Branch 240 taken 10 times.
✗ Branch 241 not taken.
✓ Branch 243 taken 10 times.
✗ Branch 244 not taken.
✓ Branch 246 taken 10 times.
✗ Branch 247 not taken.
✓ Branch 249 taken 10 times.
✗ Branch 250 not taken.
✓ Branch 252 taken 10 times.
✗ Branch 253 not taken.
✓ Branch 255 taken 10 times.
✗ Branch 256 not taken.
✓ Branch 258 taken 10 times.
✗ Branch 259 not taken.
✓ Branch 261 taken 10 times.
✗ Branch 262 not taken.
✓ Branch 264 taken 10 times.
✗ Branch 265 not taken.
✓ Branch 267 taken 10 times.
✗ Branch 268 not taken.
✗ Branch 274 not taken.
✓ Branch 275 taken 10 times.
✓ Branch 277 taken 10 times.
✗ Branch 278 not taken.
✓ Branch 281 taken 10 times.
✗ Branch 282 not taken.
✓ Branch 286 taken 10 times.
✗ Branch 287 not taken.
✓ Branch 289 taken 10 times.
✗ Branch 290 not taken.
✓ Branch 292 taken 10 times.
✗ Branch 293 not taken.
✓ Branch 295 taken 10 times.
✗ Branch 296 not taken.
✓ Branch 298 taken 10 times.
✗ Branch 299 not taken.
✓ Branch 301 taken 10 times.
✗ Branch 302 not taken.
✓ Branch 304 taken 10 times.
✗ Branch 305 not taken.
✓ Branch 307 taken 10 times.
✗ Branch 308 not taken.
✓ Branch 310 taken 10 times.
✗ Branch 311 not taken.
✓ Branch 313 taken 10 times.
✗ Branch 314 not taken.
✗ Branch 320 not taken.
✓ Branch 321 taken 10 times.
|
20 | REQUIRE_CONTACT_FINITE(contactLF); |
394 | |||
395 |
16/32✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 10 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 10 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 10 times.
✗ Branch 47 not taken.
✗ Branch 55 not taken.
✓ Branch 56 taken 10 times.
|
20 | CHECK_LESS_THAN(contactRF.getMotionTask().position_error().norm(), 1e-3); |
396 |
16/32✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 10 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 10 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 10 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 10 times.
✗ Branch 47 not taken.
✗ Branch 55 not taken.
✓ Branch 56 taken 10 times.
|
20 | CHECK_LESS_THAN(contactLF.getMotionTask().position_error().norm(), 1e-3); |
397 | |||
398 |
4/8✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 10 times.
|
20 | if (contactRF.getMotionTask().position_error().norm() > 1e-2) { |
399 | ✗ | PRINT_VECTOR(v); | |
400 | ✗ | PRINT_VECTOR(dv); | |
401 | ✗ | Vector rf_pos = contactRF.getMotionTask().position(); | |
402 | ✗ | Vector rf_pos_ref = contactRF.getMotionTask().position_ref(); | |
403 | ✗ | pinocchio::SE3 M_rf, M_rf_ref; | |
404 | ✗ | vectorToSE3(rf_pos, M_rf); | |
405 | ✗ | vectorToSE3(rf_pos_ref, M_rf_ref); | |
406 | ✗ | cout << "RF pos: " << rf_pos.transpose() << endl; | |
407 | ✗ | cout << "RF pos ref: " << rf_pos_ref.transpose() << endl; | |
408 | } | ||
409 | |||
410 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9 times.
|
20 | if (i % PRINT_N == 0) { |
411 |
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 << "Time " << i << endl; |
412 | 4 | cout << " " << contactRF.name() | |
413 |
8/16✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
|
2 | << " err: " << contactRF.getMotionTask().position_error().norm() |
414 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << " \t"; |
415 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << contactLF.name() |
416 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | << " err: " << contactLF.getMotionTask().position_error().norm() |
417 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << " \t"; |
418 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | cout << comTask.name() << " err: " << comTask.position_error().norm() |
419 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << " \t"; |
420 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << postureTask.name() |
421 |
6/12✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | << " err: " << postureTask.position_error().norm() << " \t"; |
422 |
7/14✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | cout << "v=" << v.norm() << "\t dv=" << dv.norm() << endl; |
423 | // PRINT_VECTOR(postureTask.getConstraint().vector()); | ||
424 | // PRINT_VECTOR(postureTask.position_error()); | ||
425 |
11/22✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 1 times.
✗ Branch 33 not taken.
|
2 | if (i < 20) PRINT_VECTOR(dv); |
426 | } | ||
427 | |||
428 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | const HQPOutput &sol = solver->solve(HQPData); |
429 | |||
430 |
8/16✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 10 times.
|
20 | BOOST_CHECK_MESSAGE(sol.status == HQP_STATUS_OPTIMAL, |
431 | "Status " + toString(sol.status)); | ||
432 | |||
433 | 20 | for (ConstraintLevel::const_iterator it = HQPData[0].begin(); | |
434 |
2/2✓ Branch 4 taken 60 times.
✓ Branch 5 taken 10 times.
|
140 | it != HQPData[0].end(); it++) { |
435 | 120 | auto constr = it->second; | |
436 |
3/6✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 60 times.
|
120 | if (constr->checkConstraint(sol.x) == false) { |
437 | ✗ | if (constr->isEquality()) { | |
438 | ✗ | BOOST_CHECK_MESSAGE( | |
439 | false, | ||
440 | "Equality " + constr->name() + " violated: " + | ||
441 | toString( | ||
442 | (constr->matrix() * sol.x - constr->vector()).norm())); | ||
443 | ✗ | } else if (constr->isInequality()) { | |
444 | ✗ | BOOST_CHECK_MESSAGE( | |
445 | false, | ||
446 | "Inequality " + constr->name() + " violated: " + | ||
447 | toString((constr->matrix() * sol.x - constr->lowerBound()) | ||
448 | .minCoeff()) + | ||
449 | "\n" + | ||
450 | toString((constr->upperBound() - constr->matrix() * sol.x) | ||
451 | .minCoeff())); | ||
452 | ✗ | } else if (constr->isBound()) { | |
453 | ✗ | BOOST_CHECK_MESSAGE( | |
454 | false, "Bound " + constr->name() + " violated: " + | ||
455 | toString((sol.x - constr->lowerBound()).minCoeff()) + | ||
456 | "\n" + | ||
457 | toString((constr->upperBound() - sol.x).minCoeff())); | ||
458 | } | ||
459 | } | ||
460 | 120 | } | |
461 | |||
462 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | dv = sol.x.head(nv); |
463 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | f_RF = sol.x.segment<12>(nv); |
464 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | f_LF = sol.x.segment<12>(nv + 12); |
465 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | f = sol.x.tail(24); |
466 | |||
467 |
9/18✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 10 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 10 times.
|
20 | BOOST_CHECK(contactRF.getMotionConstraint().checkConstraint(dv)); |
468 |
9/18✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 10 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 10 times.
|
20 | BOOST_CHECK(contactRF.getForceConstraint().checkConstraint(f_RF)); |
469 |
9/18✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 10 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 10 times.
|
20 | BOOST_CHECK(contactLF.getMotionConstraint().checkConstraint(dv)); |
470 |
9/18✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 10 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 10 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 10 times.
|
20 | BOOST_CHECK(contactLF.getForceConstraint().checkConstraint(f_LF)); |
471 | |||
472 | // unsigned int index = 0; | ||
473 | // for(vector<ContactBase*>::iterator it=contacts.begin(); | ||
474 | // it!=contacts.end(); it++) | ||
475 | // { | ||
476 | // unsigned int m = (*it)->n_force(); | ||
477 | // const Matrix & T = (*it)->getForceGeneratorMatrix(); // 6x12 | ||
478 | // Jc.middleRows(index, m) = | ||
479 | // T.transpose()*(*it)->getMotionConstraint().matrix(); index += m; | ||
480 | // } | ||
481 | // const Matrix & M_u = robot.mass(data).topRows<6>(); | ||
482 | // const Vector & h_u = robot.nonLinearEffects(data).head<6>(); | ||
483 | // const Matrix & J_u = Jc.leftCols<6>(); | ||
484 | // CHECK_LESS_THAN((M_u*dv + h_u - J_u.transpose()*f).norm(), 1e-6); | ||
485 | |||
486 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | v += dt * dv; |
487 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | q = pinocchio::integrate(robot.model(), q, dt * v); |
488 | 20 | t += dt; | |
489 | |||
490 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(dv.transpose()); |
491 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(v.transpose()); |
492 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(q.transpose()); |
493 |
12/24✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✗ Branch 43 not taken.
✓ Branch 44 taken 10 times.
|
20 | CHECK_LESS_THAN(dv.norm(), 1e6); |
494 |
12/24✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✗ Branch 43 not taken.
✓ Branch 44 taken 10 times.
|
20 | CHECK_LESS_THAN(v.norm(), 1e6); |
495 | } | ||
496 | |||
497 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver; |
498 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n### TEST FINISHED ###\n"; |
499 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ 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 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
|
2 | PRINT_VECTOR(v); |
500 |
5/10✓ 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.
|
2 | cout << "Final CoM position: " << robot.com(tsid->data()).transpose() |
501 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << endl; |
502 |
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 << "Desired CoM position: " << com_ref.transpose() << endl; |
503 | 2 | } | |
504 | |||
505 |
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_contact_point_invdyn_formulation_acc_force) { |
506 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n*** test_contact_point_invdyn_formulation_acc_force ***\n"; |
507 | |||
508 | 2 | const double mu = 0.3; | |
509 | 2 | const double fMin = 0.0; | |
510 | 2 | const double fMax = 10.0; | |
511 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string frameName = "base_link"; |
512 | 2 | const double dt = 1e-3; | |
513 | |||
514 | 2 | double t = 0.; | |
515 | |||
516 | 2 | double w_com = 1.0; // weight of center of mass task | |
517 | 2 | double w_forceReg = 1e-5; // weight of force regularization task | |
518 | 2 | double kp_contact = 100.0; // proportional gain of contact constraint | |
519 | 2 | double kp_com = 1.0; // proportional gain of center of mass task | |
520 | |||
521 | 2 | vector<string> package_dirs; | |
522 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | package_dirs.push_back(quadruped_model_path); |
523 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | string urdfFileName = package_dirs[0] + "/urdf/quadruped.urdf"; |
524 | RobotWrapper robot(urdfFileName, package_dirs, | ||
525 |
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.
|
4 | pinocchio::JointModelFreeFlyer(), false); |
526 | |||
527 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
|
2 | BOOST_REQUIRE(robot.model().existFrame(frameName)); |
528 | |||
529 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector q = neutral(robot.model()); |
530 |
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 | Vector v = Vector::Zero(robot.nv()); |
531 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const unsigned int nv = robot.nv(); |
532 | |||
533 | // Create initial posture. | ||
534 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q(0) = 0.1; |
535 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q(2) = 0.5; |
536 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q(6) = 1.; |
537 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
10 | for (int i = 0; i < 4; i++) { |
538 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | q(7 + 2 * i) = -0.4; |
539 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | q(8 + 2 * i) = 0.8; |
540 | } | ||
541 | |||
542 | // Create the inverse-dynamics formulation | ||
543 | auto tsid = | ||
544 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<InverseDynamicsFormulationAccForce>("tsid", robot); |
545 |
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 | tsid->computeProblemData(t, q, v); |
546 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | pinocchio::Data &data = tsid->data(); |
547 | |||
548 | // Place the robot onto the ground. | ||
549 | |||
550 | pinocchio::SE3 fl_contact = | ||
551 |
5/10✓ 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 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | robot.framePosition(data, robot.model().getFrameId("FL_contact")); |
552 |
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 | q[2] -= fl_contact.translation()(2); |
553 | |||
554 |
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 | tsid->computeProblemData(t, q, v); |
555 | |||
556 | // Add task for the COM. | ||
557 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | auto comTask = std::make_shared<TaskComEquality>("task-com", robot); |
558 |
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 | comTask->Kp(kp_com * Vector::Ones(3)); |
559 |
5/10✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
|
2 | comTask->Kd(2.0 * comTask->Kp().cwiseSqrt()); |
560 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
2 | tsid->addMotionTask(*comTask, w_com, 1); |
561 | |||
562 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector3 com_ref = robot.com(data); |
563 | auto trajCom = | ||
564 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
565 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | TrajectorySample sampleCom(3); |
566 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | sampleCom = trajCom->computeNext(); |
567 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | comTask->setReference(sampleCom); |
568 | |||
569 | // Add contact constraints. | ||
570 | std::string contactFrames[] = {"BL_contact", "BR_contact", "FL_contact", | ||
571 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
|
12 | "FR_contact"}; |
572 | |||
573 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector3 contactNormal = Vector3::UnitZ(); |
574 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<std::shared_ptr<ContactPoint>> contacts(4); |
575 | |||
576 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
10 | for (int i = 0; i < 4; i++) { |
577 | ✗ | auto cp = std::make_shared<ContactPoint>("contact_" + contactFrames[i], | |
578 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | robot, contactFrames[i], |
579 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | contactNormal, mu, fMin, fMax); |
580 |
4/8✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
|
8 | cp->Kp(kp_contact * Vector::Ones(3)); |
581 |
4/8✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
|
8 | cp->Kd(2.0 * sqrt(kp_contact) * Vector::Ones(3)); |
582 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
16 | cp->setReference( |
583 |
3/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
|
16 | robot.framePosition(data, robot.model().getFrameId(contactFrames[i]))); |
584 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
8 | cp->useLocalFrame(false); |
585 |
1/2✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
|
8 | tsid->addRigidContact(*cp, w_forceReg, 1.0, 1); |
586 | |||
587 | 8 | contacts[i] = cp; | |
588 | 8 | } | |
589 | |||
590 | // Create an HQP solver | ||
591 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPBase *solver = SolverHQPFactory::createNewSolver( |
592 | SOLVER_HQP_EIQUADPROG, "solver-eiquadprog"); | ||
593 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
594 | |||
595 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector dv = Vector::Zero(nv); |
596 | const HQPOutput *sol; | ||
597 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
|
22 | for (int i = 0; i < max_it; i++) { |
598 |
3/6✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
599 | |||
600 |
26/52✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 10 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 10 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 10 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 10 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 10 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 10 times.
✗ Branch 40 not taken.
✗ Branch 46 not taken.
✓ Branch 47 taken 10 times.
✓ Branch 49 taken 10 times.
✗ Branch 50 not taken.
✓ Branch 53 taken 10 times.
✗ Branch 54 not taken.
✓ Branch 58 taken 10 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 10 times.
✗ Branch 62 not taken.
✓ Branch 65 taken 10 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 10 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 10 times.
✗ Branch 72 not taken.
✓ Branch 75 taken 10 times.
✗ Branch 76 not taken.
✓ Branch 78 taken 10 times.
✗ Branch 79 not taken.
✓ Branch 81 taken 10 times.
✗ Branch 82 not taken.
✓ Branch 84 taken 10 times.
✗ Branch 85 not taken.
✓ Branch 87 taken 10 times.
✗ Branch 88 not taken.
✗ Branch 94 not taken.
✓ Branch 95 taken 10 times.
|
20 | REQUIRE_TASK_FINITE((*comTask)); |
601 | |||
602 |
2/2✓ Branch 1 taken 40 times.
✓ Branch 2 taken 10 times.
|
100 | for (unsigned int i = 0; i < contacts.size(); i++) { |
603 |
91/182✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 40 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 40 times.
✗ Branch 14 not taken.
✓ Branch 18 taken 40 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 40 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 40 times.
✗ Branch 25 not taken.
✓ Branch 29 taken 40 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 40 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 40 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 40 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 40 times.
✗ Branch 42 not taken.
✗ Branch 48 not taken.
✓ Branch 49 taken 40 times.
✓ Branch 51 taken 40 times.
✗ Branch 52 not taken.
✓ Branch 55 taken 40 times.
✗ Branch 56 not taken.
✓ Branch 60 taken 40 times.
✗ Branch 61 not taken.
✓ Branch 63 taken 40 times.
✗ Branch 64 not taken.
✓ Branch 68 taken 40 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 40 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 40 times.
✗ Branch 75 not taken.
✓ Branch 79 taken 40 times.
✗ Branch 80 not taken.
✓ Branch 82 taken 40 times.
✗ Branch 83 not taken.
✓ Branch 85 taken 40 times.
✗ Branch 86 not taken.
✓ Branch 88 taken 40 times.
✗ Branch 89 not taken.
✓ Branch 91 taken 40 times.
✗ Branch 92 not taken.
✗ Branch 98 not taken.
✓ Branch 99 taken 40 times.
✓ Branch 101 taken 40 times.
✗ Branch 102 not taken.
✓ Branch 105 taken 40 times.
✗ Branch 106 not taken.
✓ Branch 110 taken 40 times.
✗ Branch 111 not taken.
✓ Branch 113 taken 40 times.
✗ Branch 114 not taken.
✓ Branch 118 taken 40 times.
✗ Branch 119 not taken.
✓ Branch 121 taken 40 times.
✗ Branch 122 not taken.
✓ Branch 124 taken 40 times.
✗ Branch 125 not taken.
✓ Branch 129 taken 40 times.
✗ Branch 130 not taken.
✓ Branch 132 taken 40 times.
✗ Branch 133 not taken.
✓ Branch 135 taken 40 times.
✗ Branch 136 not taken.
✓ Branch 138 taken 40 times.
✗ Branch 139 not taken.
✓ Branch 141 taken 40 times.
✗ Branch 142 not taken.
✗ Branch 148 not taken.
✓ Branch 149 taken 40 times.
✓ Branch 151 taken 40 times.
✗ Branch 152 not taken.
✓ Branch 155 taken 40 times.
✗ Branch 156 not taken.
✓ Branch 160 taken 40 times.
✗ Branch 161 not taken.
✓ Branch 163 taken 40 times.
✗ Branch 164 not taken.
✓ Branch 168 taken 40 times.
✗ Branch 169 not taken.
✓ Branch 171 taken 40 times.
✗ Branch 172 not taken.
✓ Branch 174 taken 40 times.
✗ Branch 175 not taken.
✓ Branch 179 taken 40 times.
✗ Branch 180 not taken.
✓ Branch 182 taken 40 times.
✗ Branch 183 not taken.
✓ Branch 185 taken 40 times.
✗ Branch 186 not taken.
✓ Branch 188 taken 40 times.
✗ Branch 189 not taken.
✓ Branch 191 taken 40 times.
✗ Branch 192 not taken.
✗ Branch 198 not taken.
✓ Branch 199 taken 40 times.
✓ Branch 201 taken 40 times.
✗ Branch 202 not taken.
✓ Branch 205 taken 40 times.
✗ Branch 206 not taken.
✓ Branch 210 taken 40 times.
✗ Branch 211 not taken.
✓ Branch 213 taken 40 times.
✗ Branch 214 not taken.
✓ Branch 218 taken 40 times.
✗ Branch 219 not taken.
✓ Branch 221 taken 40 times.
✗ Branch 222 not taken.
✓ Branch 224 taken 40 times.
✗ Branch 225 not taken.
✓ Branch 229 taken 40 times.
✗ Branch 230 not taken.
✓ Branch 232 taken 40 times.
✗ Branch 233 not taken.
✓ Branch 235 taken 40 times.
✗ Branch 236 not taken.
✓ Branch 238 taken 40 times.
✗ Branch 239 not taken.
✓ Branch 241 taken 40 times.
✗ Branch 242 not taken.
✗ Branch 248 not taken.
✓ Branch 249 taken 40 times.
✓ Branch 251 taken 40 times.
✗ Branch 252 not taken.
✓ Branch 255 taken 40 times.
✗ Branch 256 not taken.
✓ Branch 260 taken 40 times.
✗ Branch 261 not taken.
✓ Branch 263 taken 40 times.
✗ Branch 264 not taken.
✓ Branch 268 taken 40 times.
✗ Branch 269 not taken.
✓ Branch 271 taken 40 times.
✗ Branch 272 not taken.
✓ Branch 274 taken 40 times.
✗ Branch 275 not taken.
✓ Branch 279 taken 40 times.
✗ Branch 280 not taken.
✓ Branch 282 taken 40 times.
✗ Branch 283 not taken.
✓ Branch 285 taken 40 times.
✗ Branch 286 not taken.
✓ Branch 288 taken 40 times.
✗ Branch 289 not taken.
✓ Branch 291 taken 40 times.
✗ Branch 292 not taken.
✗ Branch 298 not taken.
✓ Branch 299 taken 40 times.
✓ Branch 301 taken 40 times.
✗ Branch 302 not taken.
✓ Branch 305 taken 40 times.
✗ Branch 306 not taken.
✓ Branch 310 taken 40 times.
✗ Branch 311 not taken.
✓ Branch 313 taken 40 times.
✗ Branch 314 not taken.
✓ Branch 318 taken 40 times.
✗ Branch 319 not taken.
✓ Branch 321 taken 40 times.
✗ Branch 322 not taken.
✓ Branch 324 taken 40 times.
✗ Branch 325 not taken.
✓ Branch 329 taken 40 times.
✗ Branch 330 not taken.
✓ Branch 332 taken 40 times.
✗ Branch 333 not taken.
✓ Branch 335 taken 40 times.
✗ Branch 336 not taken.
✓ Branch 338 taken 40 times.
✗ Branch 339 not taken.
✓ Branch 341 taken 40 times.
✗ Branch 342 not taken.
✗ Branch 348 not taken.
✓ Branch 349 taken 40 times.
|
80 | REQUIRE_CONTACT_FINITE((*(contacts[i]))); |
604 | } | ||
605 | |||
606 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | sol = &(solver->solve(HQPData)); |
607 | |||
608 |
8/16✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 10 times.
|
20 | BOOST_CHECK_MESSAGE(sol->status == HQP_STATUS_OPTIMAL, |
609 | "Status " + toString(sol->status)); | ||
610 | |||
611 | 20 | for (ConstraintLevel::const_iterator it = HQPData[0].begin(); | |
612 |
2/2✓ Branch 4 taken 50 times.
✓ Branch 5 taken 10 times.
|
120 | it != HQPData[0].end(); it++) { |
613 | 100 | auto constr = it->second; | |
614 |
3/6✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 50 times.
|
100 | if (constr->checkConstraint(sol->x) == false) { |
615 | ✗ | if (constr->isEquality()) { | |
616 | ✗ | BOOST_CHECK_MESSAGE( | |
617 | false, | ||
618 | "Equality " + constr->name() + " violated: " + | ||
619 | toString( | ||
620 | (constr->matrix() * sol->x - constr->vector()).norm())); | ||
621 | ✗ | } else if (constr->isInequality()) { | |
622 | ✗ | BOOST_CHECK_MESSAGE( | |
623 | false, | ||
624 | "Inequality " + constr->name() + " violated: " + | ||
625 | toString((constr->matrix() * sol->x - constr->lowerBound()) | ||
626 | .minCoeff()) + | ||
627 | "\n" + | ||
628 | toString((constr->upperBound() - constr->matrix() * sol->x) | ||
629 | .minCoeff())); | ||
630 | ✗ | } else if (constr->isBound()) { | |
631 | ✗ | BOOST_CHECK_MESSAGE( | |
632 | false, "Bound " + constr->name() + " violated: " + | ||
633 | toString((sol->x - constr->lowerBound()).minCoeff()) + | ||
634 | "\n" + | ||
635 | toString((constr->upperBound() - sol->x).minCoeff())); | ||
636 | } | ||
637 | } | ||
638 | 100 | } | |
639 | |||
640 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | dv = sol->x.head(nv); |
641 | |||
642 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | v += dt * dv; |
643 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | q = pinocchio::integrate(robot.model(), q, dt * v); |
644 | 20 | t += dt; | |
645 | |||
646 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(dv.transpose()); |
647 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(v.transpose()); |
648 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(q.transpose()); |
649 |
12/24✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✗ Branch 43 not taken.
✓ Branch 44 taken 10 times.
|
20 | CHECK_LESS_THAN(dv.norm(), 1e6); |
650 |
12/24✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 10 times.
✗ Branch 35 not taken.
✗ Branch 43 not taken.
✓ Branch 44 taken 10 times.
|
20 | CHECK_LESS_THAN(v.norm(), 1e6); |
651 | } | ||
652 | |||
653 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
|
10 | for (int i = 0; i < 4; i++) { |
654 |
1/2✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
|
8 | Eigen::Matrix<double, 3, 1> f; |
655 |
3/6✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
8 | tsid->getContactForces(contacts[i]->name(), *sol, f); |
656 |
6/12✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
|
8 | cout << contacts[i]->name() << " force:" << f.transpose() << endl; |
657 | } | ||
658 | |||
659 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver; |
660 | |||
661 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n### TEST FINISHED ###\n"; |
662 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ 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 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
|
2 | PRINT_VECTOR(v); |
663 |
5/10✓ 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.
|
2 | cout << "Final CoM position: " << robot.com(tsid->data()).transpose() |
664 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << endl; |
665 |
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 << "Desired CoM position: " << com_ref.transpose() << endl; |
666 |
2/4✓ Branch 2 taken 4 times.
✓ Branch 3 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
|
14 | } |
667 | |||
668 | #define PROFILE_CONTROL_CYCLE "Control cycle" | ||
669 | #define PROFILE_PROBLEM_FORMULATION "Problem formulation" | ||
670 | #define PROFILE_HQP "HQP" | ||
671 | #define PROFILE_HQP_FAST "HQP_FAST" | ||
672 | #define PROFILE_HQP_RT "HQP_RT" | ||
673 | |||
674 |
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_invdyn_formulation_acc_force_computation_time) { |
675 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n*** test_invdyn_formulation_acc_force_computation_time ***\n"; |
676 | |||
677 | 2 | const double dt = 0.001; | |
678 | 2 | double t = 0.0; | |
679 | |||
680 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | StandardRomeoInvDynCtrl romeo_inv_dyn(dt); |
681 | 2 | RobotWrapper &robot = *(romeo_inv_dyn.robot); | |
682 | 2 | auto tsid = romeo_inv_dyn.tsid; | |
683 | 2 | TaskComEquality &comTask = *(romeo_inv_dyn.comTask); | |
684 | 2 | TaskJointPosture &postureTask = *(romeo_inv_dyn.postureTask); | |
685 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector q = romeo_inv_dyn.q; |
686 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector v = romeo_inv_dyn.v; |
687 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const int nv = robot.model().nv; |
688 | |||
689 |
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 | Vector3 com_ref = robot.com(tsid->data()); |
690 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | com_ref(1) += 0.1; |
691 | auto trajCom = | ||
692 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
693 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | TrajectorySample sampleCom(3); |
694 | |||
695 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector q_ref = q.tail(nv - 6); |
696 | auto trajPosture = | ||
697 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<TrajectoryEuclidianConstant>("traj_posture", q_ref); |
698 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | TrajectorySample samplePosture(nv - 6); |
699 | |||
700 | // Create an HQP solver | ||
701 | SolverHQPBase *solver = | ||
702 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG, "eiquadprog"); |
703 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPBase *solver_fast = SolverHQPFactory::createNewSolver( |
704 | SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); | ||
705 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPBase *solver_rt = SolverHQPFactory::createNewSolver<61, 18, 71>( |
706 | SOLVER_HQP_EIQUADPROG_RT, "eiquadprog-rt"); | ||
707 | |||
708 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
709 |
4/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | solver_fast->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
710 | |||
711 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector dv = Vector::Zero(nv); |
712 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
|
22 | for (int i = 0; i < max_it; i++) { |
713 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().start(PROFILE_CONTROL_CYCLE); |
714 | |||
715 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
20 | sampleCom = trajCom->computeNext(); |
716 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | comTask.setReference(sampleCom); |
717 |
2/4✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
|
20 | samplePosture = trajPosture->computeNext(); |
718 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | postureTask.setReference(samplePosture); |
719 | |||
720 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().start(PROFILE_PROBLEM_FORMULATION); |
721 |
3/6✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
722 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().stop(PROFILE_PROBLEM_FORMULATION); |
723 | |||
724 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().start(PROFILE_HQP); |
725 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | const HQPOutput &sol = solver->solve(HQPData); |
726 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().stop(PROFILE_HQP); |
727 | |||
728 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().stop(PROFILE_CONTROL_CYCLE); |
729 | |||
730 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().start(PROFILE_HQP_FAST); |
731 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | const HQPOutput &sol_fast = solver_fast->solve(HQPData); |
732 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().stop(PROFILE_HQP_FAST); |
733 | |||
734 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().start(PROFILE_HQP_RT); |
735 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
20 | solver_rt->solve(HQPData); |
736 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getProfiler().stop(PROFILE_HQP_RT); |
737 | |||
738 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
40 | getStatistics().store("active inequalities", |
739 | 20 | static_cast<double>(sol_fast.activeSet.size())); | |
740 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10 times.
✗ Branch 9 not taken.
|
20 | getStatistics().store("solver iterations", sol_fast.iterations); |
741 | |||
742 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | dv = sol.x.head(nv); |
743 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
20 | v += dt * dv; |
744 |
3/6✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
|
20 | q = pinocchio::integrate(robot.model(), q, dt * v); |
745 | 20 | t += dt; | |
746 | |||
747 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(dv.transpose()); |
748 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(v.transpose()); |
749 |
11/22✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 10 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 10 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 10 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 10 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 10 times.
|
20 | REQUIRE_FINITE(q.transpose()); |
750 | } | ||
751 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver; |
752 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver_fast; |
753 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver_rt; |
754 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | cout << "\n### TEST FINISHED ###\n"; |
755 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getProfiler().report_all(3, cout); |
756 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getStatistics().report_all(1, cout); |
757 | 2 | } | |
758 | |||
759 | BOOST_AUTO_TEST_SUITE_END() | ||
760 |