GCC Code Coverage Report

Directory:	./
File:	tests/tsid-formulation.cpp
Date:	2025-05-10 01:12:46

	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_udv + 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