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