GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
✓✗✓✗ ✓✗✓✗ |
3 |
StandardRomeoInvDynCtrl(double dt) : t(0.) { |
111 |
6 |
vector<string> package_dirs; |
|
112 |
✓✗ | 3 |
package_dirs.push_back(romeo_model_path); |
113 |
✓✗ | 6 |
const string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf"; |
114 |
✓✗ | 6 |
robot = std::make_shared<RobotWrapper>(urdfFileName, package_dirs, |
115 |
✓✗ | 6 |
pinocchio::JointModelFreeFlyer()); |
116 |
|||
117 |
✓✗ | 6 |
const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf"; |
118 |
|||
119 |
✓✗✓✗ |
3 |
pinocchio::srdf::loadReferenceConfigurations(robot->model(), srdfFileName, |
120 |
false); |
||
121 |
|||
122 |
✓✗ | 3 |
const unsigned int nv{static_cast<unsigned int>(robot->nv())}; |
123 |
✓✗✓✗ |
3 |
q = neutral(robot->model()); |
124 |
✓✗✓✗ ✓✗✓✗ |
3 |
std::cout << "q: " << q.transpose() << std::endl; |
125 |
✓✗ | 3 |
q(2) += 0.84; |
126 |
✓✗✓✗ |
3 |
v = Vector::Zero(nv); |
127 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_REQUIRE(robot->model().existFrame(rf_frame_name)); |
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_REQUIRE(robot->model().existFrame(lf_frame_name)); |
129 |
|||
130 |
// Create the inverse-dynamics formulation |
||
131 |
✓✗ | 3 |
tsid = std::make_shared<InverseDynamicsFormulationAccForce>("tsid", *robot); |
132 |
✓✗✓✗ ✓✗ |
3 |
tsid->computeProblemData(t, q, v); |
133 |
✓✗ | 3 |
pinocchio::Data &data = tsid->data(); |
134 |
|||
135 |
// Add the contact constraints |
||
136 |
✓✗ | 6 |
Matrix3x contactPoints(3, 4); |
137 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
6 |
contactPoints << -lxn, -lxn, +lxp, +lxp, -lyn, +lyp, -lyn, +lyp, lz, lz, lz, |
138 |
✓✗ | 3 |
lz; |
139 |
✓✗ | 6 |
contactRF = std::make_shared<Contact6d>("contact_rfoot", *robot, |
140 |
rf_frame_name, contactPoints, |
||
141 |
3 |
contactNormal, mu, fMin, fMax); |
|
142 |
✓✗✓✗ ✓✗✓✗ |
3 |
contactRF->Kp(kp_contact * Vector::Ones(6)); |
143 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
contactRF->Kd(2.0 * contactRF->Kp().cwiseSqrt()); |
144 |
✓✗✓✗ ✓✗✓✗ |
3 |
H_rf_ref = robot->position(data, robot->model().getJointId(rf_frame_name)); |
145 |
✓✗ | 3 |
contactRF->setReference(H_rf_ref); |
146 |
✓✗ | 3 |
tsid->addRigidContact(*contactRF, w_forceReg); |
147 |
|||
148 |
✓✗ | 6 |
contactLF = std::make_shared<Contact6d>("contact_lfoot", *robot, |
149 |
lf_frame_name, contactPoints, |
||
150 |
3 |
contactNormal, mu, fMin, fMax); |
|
151 |
✓✗✓✗ ✓✗✓✗ |
3 |
contactLF->Kp(kp_contact * Vector::Ones(6)); |
152 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
contactLF->Kd(2.0 * contactLF->Kp().cwiseSqrt()); |
153 |
✓✗✓✗ ✓✗✓✗ |
3 |
H_lf_ref = robot->position(data, robot->model().getJointId(lf_frame_name)); |
154 |
✓✗ | 3 |
contactLF->setReference(H_lf_ref); |
155 |
✓✗ | 3 |
tsid->addRigidContact(*contactLF, w_forceReg); |
156 |
|||
157 |
// Add the com task |
||
158 |
✓✗ | 3 |
comTask = std::make_shared<TaskComEquality>("task-com", *robot); |
159 |
✓✗✓✗ ✓✗✓✗ |
3 |
comTask->Kp(kp_com * Vector::Ones(3)); |
160 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
comTask->Kd(2.0 * comTask->Kp().cwiseSqrt()); |
161 |
✓✗ | 3 |
tsid->addMotionTask(*comTask, w_com, 1); |
162 |
|||
163 |
// Add the posture task |
||
164 |
✓✗ | 3 |
postureTask = std::make_shared<TaskJointPosture>("task-posture", *robot); |
165 |
✓✗✓✗ ✓✗✓✗ |
3 |
postureTask->Kp(kp_posture * Vector::Ones(nv - 6)); |
166 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
postureTask->Kd(2.0 * postureTask->Kp().cwiseSqrt()); |
167 |
✓✗ | 3 |
tsid->addMotionTask(*postureTask, w_posture, 1); |
168 |
|||
169 |
// Add the joint bounds task |
||
170 |
jointBoundsTask = |
||
171 |
✓✗ | 3 |
std::make_shared<TaskJointBounds>("task-joint-bounds", *robot, dt); |
172 |
✓✗✓✗ ✓✗✓✗ |
6 |
Vector dq_max = 10 * Vector::Ones(robot->na()); |
173 |
✓✗✓✗ |
6 |
Vector dq_min = -dq_max; |
174 |
✓✗✓✗ ✓✗ |
3 |
jointBoundsTask->setVelocityBounds(dq_min, dq_max); |
175 |
✓✗ | 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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_invdyn_formulation_acc_force_remove_contact) { |
198 |
✓✗ | 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 |
✓✗ | 4 |
StandardRomeoInvDynCtrl romeo_inv_dyn(dt); |
208 |
2 |
RobotWrapper &robot = *(romeo_inv_dyn.robot); |
|
209 |
4 |
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 |
✓✗ | 4 |
Vector q = romeo_inv_dyn.q; |
215 |
✓✗ | 4 |
Vector v = romeo_inv_dyn.v; |
216 |
✓✗ | 2 |
const int nv = robot.model().nv; |
217 |
|||
218 |
// Add the right foot task |
||
219 |
auto rightFootTask = std::make_shared<TaskSE3Equality>( |
||
220 |
✓✗ | 4 |
"task-right-foot", robot, romeo_inv_dyn.rf_frame_name); |
221 |
✓✗✓✗ ✓✗✓✗ |
2 |
rightFootTask->Kp(kp_RF * Vector::Ones(6)); |
222 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
rightFootTask->Kd(2.0 * rightFootTask->Kp().cwiseSqrt()); |
223 |
pinocchio::SE3 H_rf_ref = robot.position( |
||
224 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
tsid->data(), robot.model().getJointId(romeo_inv_dyn.rf_frame_name)); |
225 |
✓✗ | 2 |
tsid->addMotionTask(*rightFootTask, w_RF, 1); |
226 |
|||
227 |
✓✗ | 2 |
rightFootTask->setReference(H_rf_ref); |
228 |
|||
229 |
✓✗✓✗ ✓✗ |
2 |
Vector3 com_ref = robot.com(tsid->data()); |
230 |
✓✗ | 2 |
com_ref(1) += 0.1; |
231 |
auto trajCom = |
||
232 |
✓✗ | 4 |
std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
233 |
✓✗ | 4 |
TrajectorySample sampleCom(3); |
234 |
|||
235 |
✓✗✓✗ |
4 |
Vector q_ref = q.tail(nv - 6); |
236 |
auto trajPosture = |
||
237 |
✓✗ | 4 |
std::make_shared<TrajectoryEuclidianConstant>("traj_posture", q_ref); |
238 |
✓✗ | 4 |
TrajectorySample samplePosture(nv - 6); |
239 |
|||
240 |
// Create an HQP solver |
||
241 |
✓✗✓✗ |
2 |
SolverHQPBase *solver = SolverHQPFactory::createNewSolver( |
242 |
SOLVER_HQP_EIQUADPROG, "solver-eiquadprog"); |
||
243 |
✓✗✓✗ ✓✗✓✗ |
2 |
solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
244 |
|||
245 |
✓✗ | 2 |
Vector tau_old(nv - 6); |
246 |
✓✓ | 22 |
for (int i = 0; i < max_it; i++) { |
247 |
✗✓ | 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 |
✓✗✓✗ |
20 |
sampleCom = trajCom->computeNext(); |
253 |
✓✗ | 20 |
comTask.setReference(sampleCom); |
254 |
✓✗✓✗ |
20 |
samplePosture = trajPosture->computeNext(); |
255 |
✓✗ | 20 |
postureTask.setReference(samplePosture); |
256 |
|||
257 |
✓✗✓✗ ✓✗ |
20 |
const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
258 |
✓✓✓✗ ✓✗✓✗ |
20 |
if (i == 0) cout << HQPDataToString(HQPData, false) << endl; |
259 |
|||
260 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
REQUIRE_TASK_FINITE(postureTask); |
261 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
REQUIRE_TASK_FINITE(comTask); |
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_CONTACT_FINITE(contactRF); |
263 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_CONTACT_FINITE(contactLF); |
264 |
|||
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(contactRF.getMotionTask().position_error().norm(), 1e-3); |
266 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(contactLF.getMotionTask().position_error().norm(), 1e-3); |
267 |
|||
268 |
✓✗ | 20 |
const HQPOutput &sol = solver->solve(HQPData); |
269 |
|||
270 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
BOOST_CHECK_MESSAGE(sol.status == HQP_STATUS_OPTIMAL, |
271 |
"Status " + toString(sol.status)); |
||
272 |
|||
273 |
✓✗ | 20 |
const Vector &tau = tsid->getActuatorForces(sol); |
274 |
✓✗ | 20 |
const Vector &dv = tsid->getAccelerations(sol); |
275 |
|||
276 |
✓✓ | 20 |
if (i > 0) { |
277 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
18 |
CHECK_LESS_THAN((tau - tau_old).norm(), 2e1); |
278 |
✓✗✓✗ ✗✓ |
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 |
✓✗ | 20 |
tau_old = tau; |
296 |
|||
297 |
✓✓ | 20 |
if (i % PRINT_N == 0) { |
298 |
✓✗✓✗ ✓✗ |
2 |
cout << "Time " << i << endl; |
299 |
|||
300 |
✓✗ | 2 |
Eigen::Matrix<double, 12, 1> f; |
301 |
✓✗✓✗ ✓✗✓✗ |
2 |
if (tsid->getContactForces(contactRF.name(), sol, f)) |
302 |
4 |
cout << " " << contactRF.name() |
|
303 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
<< " force: " << contactRF.getNormalForce(f) << " \t"; |
304 |
|||
305 |
✓✗✓✗ ✓✗✓✗ |
2 |
if (tsid->getContactForces(contactLF.name(), sol, f)) |
306 |
4 |
cout << " " << contactLF.name() |
|
307 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
<< " force: " << contactLF.getNormalForce(f) << " \t"; |
308 |
|||
309 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
cout << comTask.name() << " err: " << comTask.position_error().norm() |
310 |
✓✗ | 2 |
<< " \t"; |
311 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
cout << "v=" << v.norm() << "\t dv=" << dv.norm() << endl; |
312 |
} |
||
313 |
|||
314 |
✓✗✓✗ |
20 |
v += dt * dv; |
315 |
✓✗✓✗ ✓✗ |
20 |
q = pinocchio::integrate(robot.model(), q, dt * v); |
316 |
20 |
t += dt; |
|
317 |
|||
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(dv.transpose()); |
319 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(v.transpose()); |
320 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(q.transpose()); |
321 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(dv.norm(), 1e6); |
322 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(v.norm(), 1e6); |
323 |
} |
||
324 |
|||
325 |
✓✗ | 2 |
delete solver; |
326 |
✓✗ | 2 |
cout << "\n### TEST FINISHED ###\n"; |
327 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
PRINT_VECTOR(v); |
328 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
cout << "Final CoM position: " << robot.com(tsid->data()).transpose() |
329 |
✓✗ | 2 |
<< endl; |
330 |
✓✗✓✗ ✓✗✓✗ |
2 |
cout << "Desired CoM position: " << com_ref.transpose() << endl; |
331 |
2 |
} |
|
332 |
|||
333 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_invdyn_formulation_acc_force) { |
334 |
✓✗ | 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 |
✓✗ | 4 |
StandardRomeoInvDynCtrl romeo_inv_dyn(dt); |
341 |
2 |
RobotWrapper &robot = *(romeo_inv_dyn.robot); |
|
342 |
4 |
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 |
✓✗ | 4 |
Vector q = romeo_inv_dyn.q; |
348 |
✓✗ | 4 |
Vector v = romeo_inv_dyn.v; |
349 |
✓✗ | 2 |
const int nv = robot.model().nv; |
350 |
|||
351 |
✓✗✓✗ ✓✗ |
2 |
Vector3 com_ref = robot.com(tsid->data()); |
352 |
✓✗ | 2 |
com_ref(1) += 0.1; |
353 |
auto trajCom = |
||
354 |
✓✗ | 4 |
std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
355 |
✓✗ | 4 |
TrajectorySample sampleCom(3); |
356 |
|||
357 |
✓✗✓✗ |
4 |
Vector q_ref = q.tail(nv - 6); |
358 |
auto trajPosture = |
||
359 |
✓✗ | 4 |
std::make_shared<TrajectoryEuclidianConstant>("traj_posture", q_ref); |
360 |
✓✗ | 4 |
TrajectorySample samplePosture(nv - 6); |
361 |
|||
362 |
// Create an HQP solver |
||
363 |
✓✗✓✗ |
2 |
SolverHQPBase *solver = SolverHQPFactory::createNewSolver( |
364 |
SOLVER_HQP_EIQUADPROG, "solver-eiquadprog"); |
||
365 |
|||
366 |
✓✗✓✗ ✓✗✓✗ |
2 |
solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
367 |
✓✗✓✗ ✓✗✓✗ |
2 |
cout << "nVar " << tsid->nVar() << endl; |
368 |
✓✗✓✗ ✓✗✓✗ |
2 |
cout << "nEq " << tsid->nEq() << endl; |
369 |
✓✗✓✗ ✓✗✓✗ |
2 |
cout << "nIn " << tsid->nIn() << endl; |
370 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
cout << "Initial CoM position: " << robot.com(tsid->data()).transpose() |
371 |
✓✗ | 2 |
<< endl; |
372 |
✓✗✓✗ ✓✗ |
2 |
cout << "Initial RF position: " << romeo_inv_dyn.H_rf_ref << endl; |
373 |
✓✗✓✗ ✓✗ |
2 |
cout << "Initial LF position: " << romeo_inv_dyn.H_lf_ref << endl; |
374 |
|||
375 |
✓✗✓✗ |
4 |
Vector dv = Vector::Zero(nv); |
376 |
✓✗✓✗ ✓✗ |
4 |
Vector f_RF(12), f_LF(12), f(24); |
377 |
4 |
vector<ContactBase *> contacts; |
|
378 |
✓✗ | 2 |
contacts.push_back(&contactRF); |
379 |
✓✗ | 2 |
contacts.push_back(&contactLF); |
380 |
✓✗ | 2 |
Matrix Jc(24, nv); |
381 |
✓✓ | 22 |
for (int i = 0; i < max_it; i++) { |
382 |
✓✗✓✗ |
20 |
sampleCom = trajCom->computeNext(); |
383 |
✓✗ | 20 |
comTask.setReference(sampleCom); |
384 |
✓✗✓✗ |
20 |
samplePosture = trajPosture->computeNext(); |
385 |
✓✗ | 20 |
postureTask.setReference(samplePosture); |
386 |
|||
387 |
✓✗✓✗ ✓✗ |
20 |
const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
388 |
✓✓✓✗ ✓✗✓✗ |
20 |
if (i == 0) cout << HQPDataToString(HQPData, false) << endl; |
389 |
|||
390 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
REQUIRE_TASK_FINITE(postureTask); |
391 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
REQUIRE_TASK_FINITE(comTask); |
392 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_CONTACT_FINITE(contactRF); |
393 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_CONTACT_FINITE(contactLF); |
394 |
|||
395 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(contactRF.getMotionTask().position_error().norm(), 1e-3); |
396 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(contactLF.getMotionTask().position_error().norm(), 1e-3); |
397 |
|||
398 |
✓✗✓✗ ✓✗✗✓ |
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 |
✓✓ | 20 |
if (i % PRINT_N == 0) { |
411 |
✓✗✓✗ ✓✗ |
2 |
cout << "Time " << i << endl; |
412 |
4 |
cout << " " << contactRF.name() |
|
413 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
<< " err: " << contactRF.getMotionTask().position_error().norm() |
414 |
✓✗ | 2 |
<< " \t"; |
415 |
✓✗ | 2 |
cout << contactLF.name() |
416 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
<< " err: " << contactLF.getMotionTask().position_error().norm() |
417 |
✓✗ | 2 |
<< " \t"; |
418 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
cout << comTask.name() << " err: " << comTask.position_error().norm() |
419 |
✓✗ | 2 |
<< " \t"; |
420 |
✓✗ | 2 |
cout << postureTask.name() |
421 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
<< " err: " << postureTask.position_error().norm() << " \t"; |
422 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
cout << "v=" << v.norm() << "\t dv=" << dv.norm() << endl; |
423 |
// PRINT_VECTOR(postureTask.getConstraint().vector()); |
||
424 |
// PRINT_VECTOR(postureTask.position_error()); |
||
425 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
if (i < 20) PRINT_VECTOR(dv); |
426 |
} |
||
427 |
|||
428 |
✓✗ | 20 |
const HQPOutput &sol = solver->solve(HQPData); |
429 |
|||
430 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
BOOST_CHECK_MESSAGE(sol.status == HQP_STATUS_OPTIMAL, |
431 |
"Status " + toString(sol.status)); |
||
432 |
|||
433 |
140 |
for (ConstraintLevel::const_iterator it = HQPData[0].begin(); |
|
434 |
✓✓ | 260 |
it != HQPData[0].end(); it++) { |
435 |
240 |
auto constr = it->second; |
|
436 |
✓✗✓✗ ✗✓ |
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 |
} |
||
461 |
|||
462 |
✓✗✓✗ |
20 |
dv = sol.x.head(nv); |
463 |
✓✗✓✗ |
20 |
f_RF = sol.x.segment<12>(nv); |
464 |
✓✗✓✗ |
20 |
f_LF = sol.x.segment<12>(nv + 12); |
465 |
✓✗✓✗ |
20 |
f = sol.x.tail(24); |
466 |
|||
467 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(contactRF.getMotionConstraint().checkConstraint(dv)); |
468 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(contactRF.getForceConstraint().checkConstraint(f_RF)); |
469 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(contactLF.getMotionConstraint().checkConstraint(dv)); |
470 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
BOOST_CHECK(contactLF.getForceConstraint().checkConstraint(f_LF)); |
471 |
|||
472 |
// unsigned int index = 0; |
||
473 |
// for(vector<ContactBase*>::iterator it=contacts.begin(); |
||
474 |
// it!=contacts.end(); it++) |
||
475 |
// { |
||
476 |
// unsigned int m = (*it)->n_force(); |
||
477 |
// const Matrix & T = (*it)->getForceGeneratorMatrix(); // 6x12 |
||
478 |
// Jc.middleRows(index, m) = |
||
479 |
// T.transpose()*(*it)->getMotionConstraint().matrix(); index += m; |
||
480 |
// } |
||
481 |
// const Matrix & M_u = robot.mass(data).topRows<6>(); |
||
482 |
// const Vector & h_u = robot.nonLinearEffects(data).head<6>(); |
||
483 |
// const Matrix & J_u = Jc.leftCols<6>(); |
||
484 |
// CHECK_LESS_THAN((M_u*dv + h_u - J_u.transpose()*f).norm(), 1e-6); |
||
485 |
|||
486 |
✓✗✓✗ |
20 |
v += dt * dv; |
487 |
✓✗✓✗ ✓✗ |
20 |
q = pinocchio::integrate(robot.model(), q, dt * v); |
488 |
20 |
t += dt; |
|
489 |
|||
490 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(dv.transpose()); |
491 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(v.transpose()); |
492 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(q.transpose()); |
493 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(dv.norm(), 1e6); |
494 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(v.norm(), 1e6); |
495 |
} |
||
496 |
|||
497 |
✓✗ | 2 |
delete solver; |
498 |
✓✗ | 2 |
cout << "\n### TEST FINISHED ###\n"; |
499 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
PRINT_VECTOR(v); |
500 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
cout << "Final CoM position: " << robot.com(tsid->data()).transpose() |
501 |
✓✗ | 2 |
<< endl; |
502 |
✓✗✓✗ ✓✗✓✗ |
2 |
cout << "Desired CoM position: " << com_ref.transpose() << endl; |
503 |
2 |
} |
|
504 |
|||
505 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_contact_point_invdyn_formulation_acc_force) { |
506 |
✓✗ | 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 |
✓✗ | 4 |
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 |
4 |
vector<string> package_dirs; |
|
522 |
✓✗ | 2 |
package_dirs.push_back(quadruped_model_path); |
523 |
✓✗ | 4 |
string urdfFileName = package_dirs[0] + "/urdf/quadruped.urdf"; |
524 |
RobotWrapper robot(urdfFileName, package_dirs, |
||
525 |
✓✗✓✗ ✓✗ |
6 |
pinocchio::JointModelFreeFlyer(), false); |
526 |
|||
527 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_REQUIRE(robot.model().existFrame(frameName)); |
528 |
|||
529 |
✓✗✓✗ |
4 |
Vector q = neutral(robot.model()); |
530 |
✓✗✓✗ ✓✗ |
4 |
Vector v = Vector::Zero(robot.nv()); |
531 |
✓✗ | 2 |
const unsigned int nv = robot.nv(); |
532 |
|||
533 |
// Create initial posture. |
||
534 |
✓✗ | 2 |
q(0) = 0.1; |
535 |
✓✗ | 2 |
q(2) = 0.5; |
536 |
✓✗ | 2 |
q(6) = 1.; |
537 |
✓✓ | 10 |
for (int i = 0; i < 4; i++) { |
538 |
✓✗ | 8 |
q(7 + 2 * i) = -0.4; |
539 |
✓✗ | 8 |
q(8 + 2 * i) = 0.8; |
540 |
} |
||
541 |
|||
542 |
// Create the inverse-dynamics formulation |
||
543 |
auto tsid = |
||
544 |
✓✗ | 4 |
std::make_shared<InverseDynamicsFormulationAccForce>("tsid", robot); |
545 |
✓✗✓✗ ✓✗ |
2 |
tsid->computeProblemData(t, q, v); |
546 |
✓✗ | 2 |
pinocchio::Data &data = tsid->data(); |
547 |
|||
548 |
// Place the robot onto the ground. |
||
549 |
|||
550 |
pinocchio::SE3 fl_contact = |
||
551 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
robot.framePosition(data, robot.model().getFrameId("FL_contact")); |
552 |
✓✗✓✗ ✓✗ |
2 |
q[2] -= fl_contact.translation()(2); |
553 |
|||
554 |
✓✗✓✗ ✓✗ |
2 |
tsid->computeProblemData(t, q, v); |
555 |
|||
556 |
// Add task for the COM. |
||
557 |
✓✗ | 4 |
auto comTask = std::make_shared<TaskComEquality>("task-com", robot); |
558 |
✓✗✓✗ ✓✗✓✗ |
2 |
comTask->Kp(kp_com * Vector::Ones(3)); |
559 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
comTask->Kd(2.0 * comTask->Kp().cwiseSqrt()); |
560 |
✓✗ | 2 |
tsid->addMotionTask(*comTask, w_com, 1); |
561 |
|||
562 |
✓✗✓✗ |
2 |
Vector3 com_ref = robot.com(data); |
563 |
auto trajCom = |
||
564 |
✓✗ | 4 |
std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
565 |
✓✗ | 4 |
TrajectorySample sampleCom(3); |
566 |
✓✗✓✗ |
2 |
sampleCom = trajCom->computeNext(); |
567 |
✓✗ | 2 |
comTask->setReference(sampleCom); |
568 |
|||
569 |
// Add contact constraints. |
||
570 |
std::string contactFrames[] = {"BL_contact", "BR_contact", "FL_contact", |
||
571 |
✓✗✓✗ ✓✗✓✗ ✓✓✗✗ |
12 |
"FR_contact"}; |
572 |
|||
573 |
✓✗✓✗ |
2 |
Vector3 contactNormal = Vector3::UnitZ(); |
574 |
✓✗ | 4 |
std::vector<std::shared_ptr<ContactPoint>> contacts(4); |
575 |
|||
576 |
✓✓ | 10 |
for (int i = 0; i < 4; i++) { |
577 |
auto cp = std::make_shared<ContactPoint>("contact_" + contactFrames[i], |
||
578 |
✓✗ | 8 |
robot, contactFrames[i], |
579 |
✓✗ | 16 |
contactNormal, mu, fMin, fMax); |
580 |
✓✗✓✗ ✓✗✓✗ |
8 |
cp->Kp(kp_contact * Vector::Ones(3)); |
581 |
✓✗✓✗ ✓✗✓✗ |
8 |
cp->Kd(2.0 * sqrt(kp_contact) * Vector::Ones(3)); |
582 |
✓✗ | 16 |
cp->setReference( |
583 |
✓✗✓✗ ✓✗ |
16 |
robot.framePosition(data, robot.model().getFrameId(contactFrames[i]))); |
584 |
✓✗ | 8 |
cp->useLocalFrame(false); |
585 |
✓✗ | 8 |
tsid->addRigidContact(*cp, w_forceReg, 1.0, 1); |
586 |
|||
587 |
8 |
contacts[i] = cp; |
|
588 |
} |
||
589 |
|||
590 |
// Create an HQP solver |
||
591 |
✓✗✓✗ |
2 |
SolverHQPBase *solver = SolverHQPFactory::createNewSolver( |
592 |
SOLVER_HQP_EIQUADPROG, "solver-eiquadprog"); |
||
593 |
✓✗✓✗ ✓✗✓✗ |
2 |
solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
594 |
|||
595 |
✓✗✓✗ |
2 |
Vector dv = Vector::Zero(nv); |
596 |
const HQPOutput *sol; |
||
597 |
✓✓ | 22 |
for (int i = 0; i < max_it; i++) { |
598 |
✓✗✓✗ ✓✗ |
20 |
const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
599 |
|||
600 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
REQUIRE_TASK_FINITE((*comTask)); |
601 |
|||
602 |
✓✓ | 100 |
for (unsigned int i = 0; i < contacts.size(); i++) { |
603 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
80 |
REQUIRE_CONTACT_FINITE((*(contacts[i]))); |
604 |
} |
||
605 |
|||
606 |
✓✗ | 20 |
sol = &(solver->solve(HQPData)); |
607 |
|||
608 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
BOOST_CHECK_MESSAGE(sol->status == HQP_STATUS_OPTIMAL, |
609 |
"Status " + toString(sol->status)); |
||
610 |
|||
611 |
120 |
for (ConstraintLevel::const_iterator it = HQPData[0].begin(); |
|
612 |
✓✓ | 220 |
it != HQPData[0].end(); it++) { |
613 |
200 |
auto constr = it->second; |
|
614 |
✓✗✓✗ ✗✓ |
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 |
} |
||
639 |
|||
640 |
✓✗✓✗ |
20 |
dv = sol->x.head(nv); |
641 |
|||
642 |
✓✗✓✗ |
20 |
v += dt * dv; |
643 |
✓✗✓✗ ✓✗ |
20 |
q = pinocchio::integrate(robot.model(), q, dt * v); |
644 |
20 |
t += dt; |
|
645 |
|||
646 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(dv.transpose()); |
647 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(v.transpose()); |
648 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(q.transpose()); |
649 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(dv.norm(), 1e6); |
650 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
20 |
CHECK_LESS_THAN(v.norm(), 1e6); |
651 |
} |
||
652 |
|||
653 |
✓✓ | 10 |
for (int i = 0; i < 4; i++) { |
654 |
✓✗ | 8 |
Eigen::Matrix<double, 3, 1> f; |
655 |
✓✗✓✗ ✓✗ |
8 |
tsid->getContactForces(contacts[i]->name(), *sol, f); |
656 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
8 |
cout << contacts[i]->name() << " force:" << f.transpose() << endl; |
657 |
} |
||
658 |
|||
659 |
✓✗ | 2 |
delete solver; |
660 |
|||
661 |
✓✗ | 2 |
cout << "\n### TEST FINISHED ###\n"; |
662 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
PRINT_VECTOR(v); |
663 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
cout << "Final CoM position: " << robot.com(tsid->data()).transpose() |
664 |
✓✗ | 2 |
<< endl; |
665 |
✓✗✓✗ ✓✗✓✗ |
2 |
cout << "Desired CoM position: " << com_ref.transpose() << endl; |
666 |
2 |
} |
|
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_invdyn_formulation_acc_force_computation_time) { |
675 |
✓✗ | 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 |
✓✗ | 4 |
StandardRomeoInvDynCtrl romeo_inv_dyn(dt); |
681 |
2 |
RobotWrapper &robot = *(romeo_inv_dyn.robot); |
|
682 |
4 |
auto tsid = romeo_inv_dyn.tsid; |
|
683 |
2 |
TaskComEquality &comTask = *(romeo_inv_dyn.comTask); |
|
684 |
2 |
TaskJointPosture &postureTask = *(romeo_inv_dyn.postureTask); |
|
685 |
✓✗ | 4 |
Vector q = romeo_inv_dyn.q; |
686 |
✓✗ | 4 |
Vector v = romeo_inv_dyn.v; |
687 |
✓✗ | 2 |
const int nv = robot.model().nv; |
688 |
|||
689 |
✓✗✓✗ ✓✗ |
2 |
Vector3 com_ref = robot.com(tsid->data()); |
690 |
✓✗ | 2 |
com_ref(1) += 0.1; |
691 |
auto trajCom = |
||
692 |
✓✗ | 4 |
std::make_shared<TrajectoryEuclidianConstant>("traj_com", com_ref); |
693 |
✓✗ | 4 |
TrajectorySample sampleCom(3); |
694 |
|||
695 |
✓✗✓✗ |
4 |
Vector q_ref = q.tail(nv - 6); |
696 |
auto trajPosture = |
||
697 |
✓✗ | 4 |
std::make_shared<TrajectoryEuclidianConstant>("traj_posture", q_ref); |
698 |
✓✗ | 4 |
TrajectorySample samplePosture(nv - 6); |
699 |
|||
700 |
// Create an HQP solver |
||
701 |
SolverHQPBase *solver = |
||
702 |
✓✗✓✗ |
2 |
SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG, "eiquadprog"); |
703 |
✓✗✓✗ |
2 |
SolverHQPBase *solver_fast = SolverHQPFactory::createNewSolver( |
704 |
SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog-fast"); |
||
705 |
✓✗✓✗ |
2 |
SolverHQPBase *solver_rt = SolverHQPFactory::createNewSolver<61, 18, 71>( |
706 |
SOLVER_HQP_EIQUADPROG_RT, "eiquadprog-rt"); |
||
707 |
|||
708 |
✓✗✓✗ ✓✗✓✗ |
2 |
solver->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
709 |
✓✗✓✗ ✓✗✓✗ |
2 |
solver_fast->resize(tsid->nVar(), tsid->nEq(), tsid->nIn()); |
710 |
|||
711 |
✓✗✓✗ |
4 |
Vector dv = Vector::Zero(nv); |
712 |
✓✓ | 22 |
for (int i = 0; i < max_it; i++) { |
713 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().start(PROFILE_CONTROL_CYCLE); |
714 |
|||
715 |
✓✗✓✗ |
20 |
sampleCom = trajCom->computeNext(); |
716 |
✓✗ | 20 |
comTask.setReference(sampleCom); |
717 |
✓✗✓✗ |
20 |
samplePosture = trajPosture->computeNext(); |
718 |
✓✗ | 20 |
postureTask.setReference(samplePosture); |
719 |
|||
720 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().start(PROFILE_PROBLEM_FORMULATION); |
721 |
✓✗✓✗ ✓✗ |
20 |
const HQPData &HQPData = tsid->computeProblemData(t, q, v); |
722 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().stop(PROFILE_PROBLEM_FORMULATION); |
723 |
|||
724 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().start(PROFILE_HQP); |
725 |
✓✗ | 20 |
const HQPOutput &sol = solver->solve(HQPData); |
726 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().stop(PROFILE_HQP); |
727 |
|||
728 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().stop(PROFILE_CONTROL_CYCLE); |
729 |
|||
730 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().start(PROFILE_HQP_FAST); |
731 |
✓✗ | 20 |
const HQPOutput &sol_fast = solver_fast->solve(HQPData); |
732 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().stop(PROFILE_HQP_FAST); |
733 |
|||
734 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().start(PROFILE_HQP_RT); |
735 |
✓✗ | 20 |
solver_rt->solve(HQPData); |
736 |
✓✗✓✗ ✓✗ |
20 |
getProfiler().stop(PROFILE_HQP_RT); |
737 |
|||
738 |
✓✗✓✗ ✓✗ |
40 |
getStatistics().store("active inequalities", |
739 |
✓✗ | 20 |
static_cast<double>(sol_fast.activeSet.size())); |
740 |
✓✗✓✗ ✓✗ |
20 |
getStatistics().store("solver iterations", sol_fast.iterations); |
741 |
|||
742 |
✓✗✓✗ |
20 |
dv = sol.x.head(nv); |
743 |
✓✗✓✗ |
20 |
v += dt * dv; |
744 |
✓✗✓✗ ✓✗ |
20 |
q = pinocchio::integrate(robot.model(), q, dt * v); |
745 |
20 |
t += dt; |
|
746 |
|||
747 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(dv.transpose()); |
748 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(v.transpose()); |
749 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
20 |
REQUIRE_FINITE(q.transpose()); |
750 |
} |
||
751 |
✓✗ | 2 |
delete solver; |
752 |
✓✗ | 2 |
delete solver_fast; |
753 |
✓✗ | 2 |
delete solver_rt; |
754 |
✓✗ | 2 |
cout << "\n### TEST FINISHED ###\n"; |
755 |
✓✗✓✗ |
2 |
getProfiler().report_all(3, cout); |
756 |
✓✗✓✗ |
2 |
getStatistics().report_all(1, cout); |
757 |
2 |
} |
|
758 |
|||
759 |
BOOST_AUTO_TEST_SUITE_END() |
Generated by: GCOVR (Version 4.2) |