tasks.cpp
Go to the documentation of this file.
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 BOOST_AUTO_TEST_CASE(test_task_se3_equality) {
61  cout << "\n\n*********** TEST TASK SE3 EQUALITY ***********\n";
62  vector<string> package_dirs;
63  package_dirs.push_back(romeo_model_path);
64  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
65  RobotWrapper robot(urdfFileName, package_dirs,
66  pinocchio::JointModelFreeFlyer(), false);
67 
68  TaskSE3Equality task("task-se3", robot, "RWristPitch");
69 
70  VectorXd Kp = VectorXd::Ones(6);
71  VectorXd Kd = 2 * VectorXd::Ones(6);
72  task.Kp(Kp);
73  task.Kd(Kd);
74  BOOST_CHECK(task.Kp().isApprox(Kp));
75  BOOST_CHECK(task.Kd().isApprox(Kd));
76 
77  pinocchio::SE3 M_ref = pinocchio::SE3::Random();
78  TrajectoryBase *traj = new TrajectorySE3Constant("traj_SE3", M_ref);
79  TrajectorySample sample;
80 
81  double t = 0.0;
82  const double dt = 0.001;
83  MatrixXd Jpinv(robot.nv(), 6);
84  double error, error_past = 1e100;
85  VectorXd q = neutral(robot.model());
86  VectorXd v = VectorXd::Zero(robot.nv());
87  pinocchio::Data data(robot.model());
88  for (int i = 0; i < max_it; i++) {
89  robot.computeAllTerms(data, q, v);
90  sample = traj->computeNext();
91  task.setReference(sample);
92  const ConstraintBase &constraint = task.compute(t, q, v, data);
93  BOOST_CHECK(constraint.rows() == 6);
94  BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols()) ==
95  static_cast<tsid::math::Index>(robot.nv()));
96  REQUIRE_FINITE(constraint.matrix());
97  BOOST_REQUIRE(isFinite(constraint.vector()));
98 
99  pseudoInverse(constraint.matrix(), Jpinv, 1e-4);
100  ConstRefVector dv = Jpinv * constraint.vector();
101  BOOST_REQUIRE(isFinite(Jpinv));
102  BOOST_CHECK(MatrixXd::Identity(6, 6).isApprox(constraint.matrix() * Jpinv));
103  if (!isFinite(dv)) {
104  cout << "Jpinv" << Jpinv.transpose() << endl;
105  cout << "b" << constraint.vector().transpose() << endl;
106  }
107  REQUIRE_FINITE(dv.transpose());
108 
109  v += dt * dv;
110  q = pinocchio::integrate(robot.model(), q, dt * v);
111  BOOST_REQUIRE(isFinite(v));
112  BOOST_REQUIRE(isFinite(q));
113  t += dt;
114 
115  error = task.position_error().norm();
116  BOOST_REQUIRE(isFinite(task.position_error()));
117  BOOST_CHECK(error <= error_past);
118  error_past = error;
119 
120  if (i % 100 == 0)
121  cout << "Time " << t << "\t Pos error " << error << "\t Vel error "
122  << task.velocity_error().norm() << endl;
123  }
124 }
125 
126 BOOST_AUTO_TEST_CASE(test_task_com_equality) {
127  cout << "\n\n*********** TEST TASK COM EQUALITY ***********\n";
128  vector<string> package_dirs;
129  package_dirs.push_back(romeo_model_path);
130  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
131  RobotWrapper robot(urdfFileName, package_dirs,
132  pinocchio::JointModelFreeFlyer(), false);
133 
134  pinocchio::Data data(robot.model());
135  const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf";
136 
137  pinocchio::srdf::loadReferenceConfigurations(robot.model(), srdfFileName,
138  false);
139 
140  // const unsigned int nv = robot.nv();
141  VectorXd q = neutral(robot.model());
142  std::cout << "q: " << q.transpose() << std::endl;
143  q(2) += 0.84;
144 
145  pinocchio::centerOfMass(robot.model(), data, q);
146 
147  TaskComEquality task("task-com", robot);
148 
149  VectorXd Kp = VectorXd::Ones(3);
150  VectorXd Kd = 2.0 * VectorXd::Ones(3);
151  task.Kp(Kp);
152  task.Kd(Kd);
153  BOOST_CHECK(task.Kp().isApprox(Kp));
154  BOOST_CHECK(task.Kd().isApprox(Kd));
155 
156  math::Vector3 com_ref =
157  data.com[0] + pinocchio::SE3::Vector3(0.02, 0.02, 0.02);
158  TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_com", com_ref);
159  TrajectorySample sample;
160 
161  double t = 0.0;
162  const double dt = 0.001;
163  MatrixXd Jpinv(robot.nv(), 3);
164  double error, error_past = 1e100;
165  VectorXd v = VectorXd::Zero(robot.nv());
166  for (int i = 0; i < max_it; i++) {
167  robot.computeAllTerms(data, q, v);
168  sample = traj->computeNext();
169  task.setReference(sample);
170  const ConstraintBase &constraint = task.compute(t, q, v, data);
171  BOOST_CHECK(constraint.rows() == 3);
172  BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols()) ==
173  static_cast<tsid::math::Index>(robot.nv()));
174  BOOST_REQUIRE(isFinite(constraint.matrix()));
175  BOOST_REQUIRE(isFinite(constraint.vector()));
176 
177  pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
178  ConstRefVector dv = Jpinv * constraint.vector();
179  BOOST_REQUIRE(isFinite(Jpinv));
180  BOOST_CHECK(MatrixXd::Identity(constraint.rows(), constraint.rows())
181  .isApprox(constraint.matrix() * Jpinv));
182  BOOST_REQUIRE(isFinite(dv));
183 
184  v += dt * dv;
185  q = pinocchio::integrate(robot.model(), q, dt * v);
186  BOOST_REQUIRE(isFinite(v));
187  BOOST_REQUIRE(isFinite(q));
188  t += dt;
189 
190  error = task.position_error().norm();
191  BOOST_REQUIRE(isFinite(task.position_error()));
192  BOOST_CHECK((error - error_past) <= 1e-4);
193  error_past = error;
194 
195  if (error < 1e-8) break;
196 
197  if (i % 100 == 0)
198  cout << "Time " << t << "\t CoM pos error " << error
199  << "\t CoM vel error " << task.velocity_error().norm() << endl;
200  }
201 }
202 
203 BOOST_AUTO_TEST_CASE(test_task_joint_posture) {
204  cout << "\n\n*********** TEST TASK JOINT POSTURE ***********\n";
205  vector<string> package_dirs;
206  package_dirs.push_back(romeo_model_path);
207  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
208  RobotWrapper robot(urdfFileName, package_dirs,
209  pinocchio::JointModelFreeFlyer(), false);
210  const unsigned int na = robot.nv() - 6;
211 
212  cout << "Gonna create task\n";
213  TaskJointPosture task("task-posture", robot);
214 
215  cout << "Gonna set gains\n" << na << endl;
216  VectorXd Kp = VectorXd::Ones(na);
217  VectorXd Kd = 2.0 * Kp;
218  task.Kp(Kp);
219  task.Kd(Kd);
220  BOOST_CHECK(task.Kp().isApprox(Kp));
221  BOOST_CHECK(task.Kd().isApprox(Kd));
222 
223  cout << "Gonna create reference trajectory\n";
224  ConstRefVector q_ref = math::Vector::Random(na);
225  TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_joint", q_ref);
226  TrajectorySample sample;
227 
228  cout << "Gonna set up for simulation\n";
229  double t = 0.0;
230  const double dt = 0.001;
231  MatrixXd Jpinv(robot.nv(), na);
232  double error, error_past = 1e100;
233  VectorXd q = neutral(robot.model());
234  VectorXd v = VectorXd::Zero(robot.nv());
235  pinocchio::Data data(robot.model());
236  for (int i = 0; i < max_it; i++) {
237  robot.computeAllTerms(data, q, v);
238  sample = traj->computeNext();
239  task.setReference(sample);
240  const ConstraintBase &constraint = task.compute(t, q, v, data);
241  BOOST_CHECK(constraint.rows() == na);
242  BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols()) ==
243  static_cast<tsid::math::Index>(robot.nv()));
244  BOOST_REQUIRE(isFinite(constraint.matrix()));
245  BOOST_REQUIRE(isFinite(constraint.vector()));
246 
247  pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
248  ConstRefVector dv = Jpinv * constraint.vector();
249  BOOST_REQUIRE(isFinite(Jpinv));
250  BOOST_CHECK(
251  MatrixXd::Identity(na, na).isApprox(constraint.matrix() * Jpinv));
252  BOOST_REQUIRE(isFinite(dv));
253 
254  v += dt * dv;
255  q = pinocchio::integrate(robot.model(), q, dt * v);
256  BOOST_REQUIRE(isFinite(v));
257  BOOST_REQUIRE(isFinite(q));
258  t += dt;
259 
260  error = task.position_error().norm();
261  BOOST_REQUIRE(isFinite(task.position_error()));
262  BOOST_CHECK(error <= error_past);
263  error_past = error;
264 
265  if (i % 100 == 0)
266  cout << "Time " << t << "\t pos error " << error << "\t vel error "
267  << task.velocity_error().norm() << endl;
268  }
269 }
270 
271 BOOST_AUTO_TEST_CASE(test_task_joint_bounds) {
272  cout << "\n\n*********** TEST TASK JOINT BOUNDS ***********\n";
273  vector<string> package_dirs;
274  package_dirs.push_back(romeo_model_path);
275  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
276  RobotWrapper robot(urdfFileName, package_dirs,
277  pinocchio::JointModelFreeFlyer(), false);
278  const unsigned int na = robot.nv() - 6;
279  const double dt = 0.001;
280 
281  cout << "Gonna create task\n";
282  TaskJointBounds task("task-joint-bounds", robot, dt);
283 
284  cout << "Gonna set limits\n" << na << endl;
285  VectorXd dq_max = VectorXd::Ones(na);
286  VectorXd dq_min = -dq_max;
287  task.setVelocityBounds(dq_min, dq_max);
288 
289  BOOST_CHECK(task.getVelocityLowerBounds().isApprox(dq_min));
290  BOOST_CHECK(task.getVelocityUpperBounds().isApprox(dq_max));
291 
292  cout << "Gonna set up for simulation\n";
293  double t = 0.0;
294 
295  VectorXd q = neutral(robot.model());
296  VectorXd v = VectorXd::Zero(robot.nv());
297  pinocchio::Data data(robot.model());
298  for (int i = 0; i < max_it; i++) {
299  robot.computeAllTerms(data, q, v);
300  const ConstraintBase &constraint = task.compute(t, q, v, data);
301  BOOST_CHECK(constraint.rows() == (Eigen::Index)robot.nv());
302  BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols()) ==
303  static_cast<tsid::math::Index>(robot.nv()));
304  BOOST_REQUIRE(isFinite(constraint.lowerBound()));
305  BOOST_REQUIRE(isFinite(constraint.upperBound()));
306 
307  BOOST_REQUIRE(isFinite(v));
308  BOOST_REQUIRE(isFinite(q));
309  t += dt;
310  }
311 }
312 
313 BOOST_AUTO_TEST_CASE(test_task_joint_posVelAcc_bounds) {
314  cout << "\n\n*********** TEST TASK JOINT POS VEL ACC BOUNDS ***********\n";
315  vector<string> package_dirs;
316  package_dirs.push_back(romeo_model_path);
317  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
318  RobotWrapper robot(urdfFileName, package_dirs,
319  pinocchio::JointModelFreeFlyer(), false);
320  const unsigned int na = robot.nv() - 6;
321  const double dt = 0.001;
322 
323  cout << "Gonna create task\n";
324  TaskJointPosVelAccBounds task("task-joint-posVelAcc-bounds", robot, dt);
325 
326  cout << "Gonna set limits\n" << na << endl;
327  VectorXd dq_max = VectorXd::Ones(na);
328  VectorXd dq_min = -dq_max;
329 
330  task.setPositionBounds(dq_min, dq_max);
331  task.setVelocityBounds(dq_max);
332  task.setAccelerationBounds(dq_max);
333 
334  BOOST_CHECK(task.getPositionLowerBounds().isApprox(dq_min));
335  BOOST_CHECK(task.getPositionUpperBounds().isApprox(dq_max));
336  BOOST_CHECK(task.getVelocityBounds().isApprox(dq_max));
337  BOOST_CHECK(task.getAccelerationBounds().isApprox(dq_max));
338 
339  cout << "Gonna set up for simulation\n";
340  double t = 0.0;
341 
342  VectorXd q = neutral(robot.model());
343  VectorXd v = VectorXd::Zero(robot.nv());
344  pinocchio::Data data(robot.model());
345  for (int i = 0; i < max_it; i++) {
346  robot.computeAllTerms(data, q, v);
347  const ConstraintBase &constraint = task.compute(t, q, v, data);
348  BOOST_CHECK(constraint.rows() == (Eigen::Index)robot.na());
349  BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols()) ==
350  static_cast<tsid::math::Index>(robot.nv()));
351  BOOST_REQUIRE(isFinite(constraint.lowerBound()));
352  BOOST_REQUIRE(isFinite(constraint.upperBound()));
353 
354  BOOST_REQUIRE(isFinite(v));
355  BOOST_REQUIRE(isFinite(q));
356  t += dt;
357  }
358 }
359 
360 BOOST_AUTO_TEST_SUITE_END()
pinocchio::srdf::loadReferenceConfigurations
void loadReferenceConfigurations(ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const bool verbose=false)
demo_quadruped.dt
float dt
Definition: demo_quadruped.py:40
tsid::tasks::TrajectorySample
trajectories::TrajectorySample TrajectorySample
Definition: task-motion.cpp:24
test_Tasks.error_past
int error_past
Definition: test_Tasks.py:50
i
int i
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_task_se3_equality)
Definition: tasks.cpp:60
test_Contact.Kp
Kp
Definition: test_Contact.py:44
demo_quadruped.dv
dv
Definition: demo_quadruped.py:174
SE3Tpl< double, 0 >::Random
static SE3Tpl Random()
tsid::robots::RobotWrapper::model
const Model & model() const
Accessor to model.
Definition: src/robots/robot-wrapper.cpp:102
test_Tasks.M_ref
M_ref
Definition: test_Tasks.py:157
demo_quadruped.com_ref
com_ref
Definition: demo_quadruped.py:117
setup.data
data
Definition: setup.in.py:48
pinocchio::integrate
void integrate(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< ConfigIn_t > &q, const Eigen::MatrixBase< Tangent_t > &v, const Eigen::MatrixBase< ConfigOut_t > &qout)
max_it
const int max_it
Definition: tasks.cpp:53
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
tsid::math::Vector3
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: math/fwd.hpp:40
tsid::math::isFinite
bool isFinite(const Eigen::MatrixBase< Derived > &x)
Definition: math/utils.hpp:138
test_Tasks.sample
sample
Definition: test_Tasks.py:44
test_Tasks.error
error
Definition: test_Tasks.py:67
test_Tasks.traj
traj
Definition: test_Tasks.py:43
demo_quadruped.q
q
Definition: demo_quadruped.py:73
SE3Tpl< double, 0 >::Vector3
traits< SE3Tpl >::Vector3 Vector3
test_Tasks.Jpinv
Jpinv
Definition: test_Tasks.py:49
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robots/robot-wrapper.hpp:37
demo_quadruped.robot
robot
Definition: demo_quadruped.py:50
neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
package_dirs
package_dirs
romeo_model_path
const string romeo_model_path
Definition: tasks.cpp:50
t
Transform3f t
Index
std::size_t Index
test_Contact.Kd
int Kd
Definition: test_Contact.py:45
pinocchio::centerOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & centerOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
tsid::robots::RobotWrapper::computeAllTerms
void computeAllTerms(Data &data, const Vector &q, const Vector &v) const
Definition: src/robots/robot-wrapper.cpp:105
tsid::math::pseudoInverse
void pseudoInverse(ConstRefMatrix A, RefMatrix Apinv, double tolerance, unsigned int computationOptions=Eigen::ComputeThinU|Eigen::ComputeThinV)
Definition: src/math/utils.cpp:82
demo_quadruped.q_ref
q_ref
Definition: demo_quadruped.py:121
VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
test_Tasks.na
int na
Definition: test_Tasks.py:92
REQUIRE_FINITE
#define REQUIRE_FINITE(A)
Definition: tasks.cpp:48
demo_quadruped.v
v
Definition: demo_quadruped.py:79

tsid
Author(s): Andrea Del Prete, Justin Carpentier
autogenerated on Sun Jul 2 2023 02:21:51