tasks.cpp 8.57 KB
Newer Older
1
2
3
//
// Copyright (c) 2017 CNRS
//
jcarpent's avatar
jcarpent committed
4
5
// This file is part of tsid
// tsid is free software: you can redistribute it
6
7
8
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
jcarpent's avatar
jcarpent committed
9
// tsid is distributed in the hope that it will be
10
11
12
13
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
jcarpent's avatar
jcarpent committed
14
// tsid If not, see
15
16
17
18
19
20
21
22
// <http://www.gnu.org/licenses/>.
//

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

jcarpent's avatar
jcarpent committed
23
24
#include <tsid/math/utils.hpp>
#include <tsid/robots/robot-wrapper.hpp>
25

jcarpent's avatar
jcarpent committed
26
27
28
#include <tsid/tasks/task-se3-equality.hpp>
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
29

jcarpent's avatar
jcarpent committed
30
31
#include <tsid/trajectories/trajectory-se3.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>
32

33
#include <pinocchio/parsers/srdf.hpp>
34
#include <pinocchio/algorithm/joint-configuration.hpp>
35
#include <pinocchio/algorithm/center-of-mass.hpp>
36
37
#include <Eigen/SVD>

jcarpent's avatar
jcarpent committed
38
using namespace tsid;
39
40
41
using namespace trajectories;
using namespace math;
using namespace tasks;
42
43
using namespace std;
using namespace Eigen;
jcarpent's avatar
jcarpent committed
44
using namespace tsid::robots;
45

jcarpent's avatar
jcarpent committed
46
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)
andreadelprete's avatar
andreadelprete committed
47

jcarpent's avatar
jcarpent committed
48
const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo";
49

50
51
52
#ifndef NDEBUG
const int max_it = 100;
#else
53
const int max_it = 10000;
54
55
56
57
#endif

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

58
59
BOOST_AUTO_TEST_CASE ( test_task_se3_equality )
{
60
  cout<<"\n\n*********** TEST TASK SE3 EQUALITY ***********\n";
61
  vector<string> package_dirs;
62
63
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
64
65
66
67
68
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     se3::JointModelFreeFlyer(),
                     false);

69
  TaskSE3Equality task("task-se3", robot, "RWristPitch");
70

71
72
  VectorXd Kp = VectorXd::Ones(6);
  VectorXd Kd = 2*VectorXd::Ones(6);
73
74
75
76
77
  task.Kp(Kp);
  task.Kd(Kd);
  BOOST_CHECK(task.Kp().isApprox(Kp));
  BOOST_CHECK(task.Kd().isApprox(Kd));

78
79
80
81
82
83
84
85
86
87
  se3::SE3 M_ref = se3::SE3::Random();
  TrajectoryBase *traj = new TrajectorySE3Constant("traj_SE3", M_ref);
  TrajectorySample sample;

  double t = 0.0;
  const double dt = 0.001;
  MatrixXd Jpinv(robot.nv(), 6);
  double error, error_past=1e100;
  VectorXd q = robot.model().neutralConfiguration;
  VectorXd v = VectorXd::Zero(robot.nv());
88
  se3::Data data(robot.model());
89
  for(int i=0; i<max_it; i++)
90
91
92
93
94
95
96
  {
    robot.computeAllTerms(data, q, v);
    sample = traj->computeNext();
    task.setReference(sample);
    const ConstraintBase & constraint = task.compute(t, q, v, data);
    BOOST_CHECK(constraint.rows()==6);
    BOOST_CHECK(constraint.cols()==robot.nv());
andreadelprete's avatar
andreadelprete committed
97
    REQUIRE_FINITE(constraint.matrix());
jcarpent's avatar
jcarpent committed
98
    BOOST_REQUIRE(isFinite(constraint.vector()));
99

andreadelprete's avatar
andreadelprete committed
100
    pseudoInverse(constraint.matrix(), Jpinv, 1e-4);
101
    Vector dv = Jpinv * constraint.vector();
jcarpent's avatar
jcarpent committed
102
    BOOST_REQUIRE(isFinite(Jpinv));
103
    BOOST_CHECK(MatrixXd::Identity(6,6).isApprox(constraint.matrix()*Jpinv));
jcarpent's avatar
jcarpent committed
104
    if(!isFinite(dv))
andreadelprete's avatar
andreadelprete committed
105
106
107
108
109
    {
      cout<< "Jpinv" << Jpinv.transpose() <<endl;
      cout<< "b" << constraint.vector().transpose() <<endl;
    }
    REQUIRE_FINITE(dv.transpose());
110
111
112

    v += dt*dv;
    q = se3::integrate(robot.model(), q, dt*v);
jcarpent's avatar
jcarpent committed
113
114
    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
115
116
    t += dt;

117
    error = task.position_error().norm();
jcarpent's avatar
jcarpent committed
118
    BOOST_REQUIRE(isFinite(task.position_error()));
119
120
121
122
123
    BOOST_CHECK(error <= error_past);
    error_past = error;

    if(i%100==0)
      cout << "Time "<<t<<"\t Pos error "<<error<<
124
              "\t Vel error "<<task.velocity_error().norm()<<endl;
125
126
  }
}
127

128
129
BOOST_AUTO_TEST_CASE ( test_task_com_equality )
{
130
  cout<<"\n\n*********** TEST TASK COM EQUALITY ***********\n";
131
  vector<string> package_dirs;
132
133
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
134
135
136
137
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     se3::JointModelFreeFlyer(),
                     false);
138
139
140
141
142
143
144
145
146
147
148
  
  se3::Data data(robot.model());
  const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf";
  se3::srdf::getNeutralConfigurationFromSrdf(robot.model(),srdfFileName);
  
  const unsigned int nv = robot.nv();
  VectorXd q = robot.model().neutralConfiguration;
  std::cout << "q: " << q.transpose() << std::endl;
  q(2) += 0.84;
  
  se3::centerOfMass(robot.model(),data,q);
149
150
151
152
153
154
155
156
157
158

  TaskComEquality task("task-com", robot);

  VectorXd Kp = VectorXd::Ones(3);
  VectorXd Kd = 2.0*VectorXd::Ones(3);
  task.Kp(Kp);
  task.Kd(Kd);
  BOOST_CHECK(task.Kp().isApprox(Kp));
  BOOST_CHECK(task.Kd().isApprox(Kd));

159
  Vector3 com_ref = data.com[0] + se3::SE3::Vector3(0.02,0.02,0.02);
160
  TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_com", com_ref);
161
162
163
  TrajectorySample sample;

  double t = 0.0;
164
165
166
167
  const double dt = 0.001;
  MatrixXd Jpinv(robot.nv(), 3);
  double error, error_past=1e100;
  VectorXd v = VectorXd::Zero(robot.nv());
168
  for(int i=0; i<max_it; i++)
169
  {
170
171
172
    robot.computeAllTerms(data, q, v);
    sample = traj->computeNext();
    task.setReference(sample);
173
    const ConstraintBase & constraint = task.compute(t, q, v, data);
174
175
    BOOST_CHECK(constraint.rows()==3);
    BOOST_CHECK(constraint.cols()==robot.nv());
jcarpent's avatar
jcarpent committed
176
177
    BOOST_REQUIRE(isFinite(constraint.matrix()));
    BOOST_REQUIRE(isFinite(constraint.vector()));
178
179
180

    pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
    Vector dv = Jpinv * constraint.vector();
jcarpent's avatar
jcarpent committed
181
    BOOST_REQUIRE(isFinite(Jpinv));
182
    BOOST_CHECK(MatrixXd::Identity(constraint.rows(),constraint.rows()).isApprox(constraint.matrix()*Jpinv));
jcarpent's avatar
jcarpent committed
183
    BOOST_REQUIRE(isFinite(dv));
184
185
186

    v += dt*dv;
    q = se3::integrate(robot.model(), q, dt*v);
jcarpent's avatar
jcarpent committed
187
188
    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
189
190
191
    t += dt;

    error = task.position_error().norm();
jcarpent's avatar
jcarpent committed
192
    BOOST_REQUIRE(isFinite(task.position_error()));
193
    BOOST_CHECK((error - error_past) <= 1e-4);
194
    error_past = error;
195
196
    
    if(error < 1e-8) break;
197

198
    if(i%100==0)
199
200
      cout << "Time "<<t<<"\t CoM pos error "<<error<<
              "\t CoM vel error "<<task.velocity_error().norm()<<endl;
201
202
203
  }
}

204
205
206
207
BOOST_AUTO_TEST_CASE ( test_task_joint_posture )
{
  cout<<"\n\n*********** TEST TASK JOINT POSTURE ***********\n";
  vector<string> package_dirs;
208
209
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     se3::JointModelFreeFlyer(),
                     false);
  const unsigned int na = robot.nv()-6;

  cout<<"Gonna create task\n";
  TaskJointPosture task("task-posture", robot);

  cout<<"Gonna set gains\n"<<na<<endl;
  VectorXd Kp = VectorXd::Ones(na);
  VectorXd Kd = 2.0*Kp;
  task.Kp(Kp);
  task.Kd(Kd);
  BOOST_CHECK(task.Kp().isApprox(Kp));
  BOOST_CHECK(task.Kd().isApprox(Kd));

  cout<<"Gonna create reference trajectory\n";
  Vector q_ref = Vector::Random(na);
  TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_joint", q_ref);
  TrajectorySample sample;

  cout<<"Gonna set up for simulation\n";
  double t = 0.0;
  const double dt = 0.001;
  MatrixXd Jpinv(robot.nv(), na);
  double error, error_past=1e100;
  VectorXd q = robot.model().neutralConfiguration;
  VectorXd v = VectorXd::Zero(robot.nv());
  se3::Data data(robot.model());
240
  for(int i=0; i<max_it; i++)
241
242
243
244
245
246
247
  {
    robot.computeAllTerms(data, q, v);
    sample = traj->computeNext();
    task.setReference(sample);
    const ConstraintBase & constraint = task.compute(t, q, v, data);
    BOOST_CHECK(constraint.rows()==na);
    BOOST_CHECK(constraint.cols()==robot.nv());
jcarpent's avatar
jcarpent committed
248
249
    BOOST_REQUIRE(isFinite(constraint.matrix()));
    BOOST_REQUIRE(isFinite(constraint.vector()));
250
251
252

    pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
    Vector dv = Jpinv * constraint.vector();
jcarpent's avatar
jcarpent committed
253
    BOOST_REQUIRE(isFinite(Jpinv));
254
    BOOST_CHECK(MatrixXd::Identity(na,na).isApprox(constraint.matrix()*Jpinv));
jcarpent's avatar
jcarpent committed
255
    BOOST_REQUIRE(isFinite(dv));
256
257
258

    v += dt*dv;
    q = se3::integrate(robot.model(), q, dt*v);
jcarpent's avatar
jcarpent committed
259
260
    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
261
262
263
    t += dt;

    error = task.position_error().norm();
jcarpent's avatar
jcarpent committed
264
    BOOST_REQUIRE(isFinite(task.position_error()));
265
266
267
    BOOST_CHECK(error <= error_past);
    error_past = error;

268
    if(i%100==0)
269
270
271
272
273
      cout << "Time "<<t<<"\t pos error "<<error<<
              "\t vel error "<<task.velocity_error().norm()<<endl;
  }
}

274
BOOST_AUTO_TEST_SUITE_END ()