GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/tasks/task-joint-posVelAcc-bounds.cpp Lines: 0 272 0.0 %
Date: 2024-02-02 08:47:34 Branches: 0 576 0.0 %

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 <tsid/tasks/task-joint-posVelAcc-bounds.hpp>
19
#include "tsid/robots/robot-wrapper.hpp"
20
// #include <tsid/utils/stop-watch.hpp>
21
22
/** This class has been implemented following :
23
 * Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
24
 * Acceleration/Torque Control of Robot Manipulators. IEEE Robotics and
25
 * Automation Letters, IEEE 2018, 3 (1),
26
 * pp.281-288.￿10.1109/LRA.2017.2738321￿. hal-01356989v3 And
27
 * https://github.com/andreadelprete/pinocchio_inv_dyn/blob/master/python/pinocchio_inv_dyn/acc_bounds_util.py
28
 */
29
namespace tsid {
30
namespace tasks {
31
using namespace math;
32
using namespace trajectories;
33
using namespace pinocchio;
34
35
TaskJointPosVelAccBounds::TaskJointPosVelAccBounds(const std::string& name,
36
                                                   RobotWrapper& robot,
37
                                                   double dt, bool verbose)
38
    : TaskMotion(name, robot),
39
      m_constraint(name, robot.na(), robot.nv()),
40
      m_dt(2 * dt),
41
      m_verbose(verbose),
42
      m_nv(robot.nv()),
43
      m_na(robot.na()) {
44
  PINOCCHIO_CHECK_INPUT_ARGUMENT(dt > 0.0, "dt needs to be positive");
45
  m_eps = 1e-10;
46
  m_qMin = Vector::Constant(m_na, 1, -1e10);
47
  m_qMax = Vector::Constant(m_na, 1, 1e10);
48
  m_dqMax = Vector::Constant(m_na, 1, 1e10);
49
  m_ddqMax = Vector::Constant(m_na, 1, 1e10);
50
  m_impose_position_bounds = false;
51
  m_impose_velocity_bounds = false;
52
  m_impose_viability_bounds = false;
53
  m_impose_acceleration_bounds = false;
54
55
  // Used in computeAccLimitsFromPosLimits
56
  m_two_dt_sq = 2.0 / (m_dt * m_dt);
57
  m_ddqMax_q3 = Vector::Zero(m_na);
58
  m_ddqMin_q3 = Vector::Zero(m_na);
59
  m_ddqMax_q2 = Vector::Zero(m_na);
60
  m_ddqMin_q2 = Vector::Zero(m_na);
61
  m_minus_dq_over_dt = Vector::Zero(m_na);
62
63
  // Used in computeAccLimitsFromViability
64
  m_dt_square = m_dt * m_dt;
65
  m_two_a = 2 * m_dt_square;
66
  m_dt_dq = Vector::Zero(m_na);
67
  m_dt_two_dq = Vector::Zero(m_na);
68
  m_two_ddqMax = Vector::Zero(m_na);
69
  m_dt_ddqMax_dt = Vector::Zero(m_na);
70
  m_dq_square = Vector::Zero(m_na);
71
  m_q_plus_dt_dq = Vector::Zero(m_na);
72
  m_b_1 = Vector::Zero(m_na);
73
  m_b_2 = Vector::Zero(m_na);
74
  m_ddq_1 = Vector::Zero(m_na);
75
  m_ddq_2 = Vector::Zero(m_na);
76
  m_c_1 = Vector::Zero(m_na);
77
  m_delta_1 = Vector::Zero(m_na);
78
  m_c_2 = Vector::Zero(m_na);
79
  m_delta_2 = Vector::Zero(m_na);
80
81
  // Used in computeAccLimits
82
  m_ub = Vector::Constant(4, 1, 1e10);
83
  m_lb = Vector::Constant(4, 1, -1e10);
84
85
  m_ddqLBPos = Vector::Constant(m_na, 1, -1e10);
86
  m_ddqUBPos = Vector::Constant(m_na, 1, 1e10);
87
  m_ddqLBVia = Vector::Constant(m_na, 1, -1e10);
88
  m_ddqUBVia = Vector::Constant(m_na, 1, 1e10);
89
  m_ddqLBVel = Vector::Constant(m_na, 1, -1e10);
90
  m_ddqUBVel = Vector::Constant(m_na, 1, 1e10);
91
  m_ddqLBAcc = Vector::Constant(m_na, 1, -1e10);
92
  m_ddqUBAcc = Vector::Constant(m_na, 1, 1e10);
93
  m_ddqLB = Vector::Constant(m_na, 1, -1e10);
94
  m_ddqUB = Vector::Constant(m_na, 1, 1e10);
95
  m_viabViol = Vector::Zero(m_na);
96
97
  m_qa = Vector::Zero(m_na);
98
  m_dqa = Vector::Zero(m_na);
99
100
  Vector m = Vector::Ones(robot.na());
101
  setMask(m);
102
103
  for (int i = 0; i < m_na; i++) {
104
    m_constraint.upperBound()(i) = 1e10;
105
    m_constraint.lowerBound()(i) = -1e10;
106
  }
107
}
108
109
const Vector& TaskJointPosVelAccBounds::mask() const { return m_mask; }
110
111
void TaskJointPosVelAccBounds::mask(const Vector& m) {
112
  // std::cerr<<"The method TaskJointPosVelAccBounds::mask is deprecated. Use
113
  // TaskJointPosVelAccBounds::setMask instead.\n";
114
  return setMask(m);
115
}
116
117
void TaskJointPosVelAccBounds::setMask(ConstRefVector m) {
118
  PINOCCHIO_CHECK_INPUT_ARGUMENT(m.size() == m_robot.na(),
119
                                 "The size of the mask vector needs to equal " +
120
                                     std::to_string(m_robot.na()));
121
  m_mask = m;
122
  const Vector::Index dim = static_cast<Vector::Index>(m.sum());
123
  Matrix S = Matrix::Zero(dim, m_robot.nv());
124
  m_activeAxes.resize(dim);
125
  unsigned int j = 0;
126
  for (unsigned int i = 0; i < m.size(); i++)
127
    if (m(i) != 0.0) {
128
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
129
          m(i) == 1.0, "Mask entries need to be either 0.0 or 1.0");
130
      S(j, m_robot.nv() - m_robot.na() + i) = 1.0;
131
      m_activeAxes(j) = i;
132
      j++;
133
    }
134
  m_constraint.resize((unsigned int)dim, m_robot.nv());
135
  m_constraint.setMatrix(S);
136
}
137
138
int TaskJointPosVelAccBounds::dim() const { return m_na; }
139
140
const Vector& TaskJointPosVelAccBounds::getAccelerationBounds() const {
141
  return m_ddqMax;
142
}
143
144
const Vector& TaskJointPosVelAccBounds::getVelocityBounds() const {
145
  return m_dqMax;
146
}
147
148
const Vector& TaskJointPosVelAccBounds::getPositionLowerBounds() const {
149
  return m_qMin;
150
}
151
152
const Vector& TaskJointPosVelAccBounds::getPositionUpperBounds() const {
153
  return m_qMax;
154
}
155
156
void TaskJointPosVelAccBounds::setTimeStep(double dt) {
157
  PINOCCHIO_CHECK_INPUT_ARGUMENT(dt > 0.0, "dt needs to be positive");
158
  m_dt = dt;
159
}
160
161
void TaskJointPosVelAccBounds::setVerbose(bool verbose) { m_verbose = verbose; }
162
163
void TaskJointPosVelAccBounds::setPositionBounds(ConstRefVector lower,
164
                                                 ConstRefVector upper) {
165
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
166
      lower.size() == m_na,
167
      "The size of the lower position bounds vector needs to equal " +
168
          std::to_string(m_na));
169
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
170
      upper.size() == m_na,
171
      "The size of the upper position bounds vector needs to equal " +
172
          std::to_string(m_na));
173
  m_qMin = lower;
174
  m_qMax = upper;
175
  m_impose_position_bounds = true;
176
  m_impose_viability_bounds = true;
177
}
178
179
void TaskJointPosVelAccBounds::setVelocityBounds(ConstRefVector upper) {
180
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
181
      upper.size() == m_na,
182
      "The size of the (absolute) velocity bounds vector needs to equal " +
183
          std::to_string(m_na));
184
  m_dqMax = upper;
185
  m_impose_velocity_bounds = true;
186
}
187
188
void TaskJointPosVelAccBounds::setAccelerationBounds(ConstRefVector upper) {
189
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
190
      upper.size() == m_na,
191
      "The size of the (absolute) acceleration bounds vector needs to equal " +
192
          std::to_string(m_na));
193
  m_ddqMax = upper;
194
  m_impose_acceleration_bounds = true;
195
}
196
197
const ConstraintBase& TaskJointPosVelAccBounds::getConstraint() const {
198
  return m_constraint;
199
}
200
201
const ConstraintBase& TaskJointPosVelAccBounds::compute(const double,
202
                                                        ConstRefVector q,
203
                                                        ConstRefVector v,
204
                                                        Data&) {
205
  // getProfiler().start("TaskJointPosVelAccBounds");
206
  // Eigen::internal::set_is_malloc_allowed(false);
207
  computeAccLimits(q, v, m_verbose);
208
  m_constraint.upperBound() = m_ddqUB;
209
  m_constraint.lowerBound() = m_ddqLB;
210
  // Eigen::internal::set_is_malloc_allowed(true);
211
  // getProfiler().stop("TaskJointPosVelAccBounds");
212
  // getProfiler().report_all(9, std::cout);
213
  return m_constraint;
214
}
215
216
void TaskJointPosVelAccBounds::setImposeBounds(
217
    bool impose_position_bounds, bool impose_velocity_bounds,
218
    bool impose_viability_bounds, bool impose_acceleration_bounds) {
219
  m_impose_position_bounds = impose_position_bounds;
220
  m_impose_velocity_bounds = impose_velocity_bounds;
221
  m_impose_viability_bounds = impose_viability_bounds;
222
  m_impose_acceleration_bounds = impose_acceleration_bounds;
223
}
224
225
void TaskJointPosVelAccBounds::isStateViable(ConstRefVector qa,
226
                                             ConstRefVector dqa, bool verbose) {
227
  m_viabViol.setZero(m_na);
228
  for (int i = 0; i < m_na; i++) {
229
    if (qa[i] < (m_qMin[i] - m_eps)) {
230
      if (verbose) {
231
        std::cout << "State of joint " << i
232
                  << " is not viable because q[i]< qMin[i] : " << qa[i] << "<"
233
                  << m_qMin[i] << std::endl;
234
      }
235
      m_viabViol[i] = m_qMin[i] - qa[i];
236
    }
237
    if (qa[i] > (m_qMax[i] + m_eps)) {
238
      if (verbose) {
239
        std::cout << "State of joint " << i
240
                  << " is not viable because qa[i]>m_qMax[i] : " << qa[i] << ">"
241
                  << m_qMax[i] << std::endl;
242
      }
243
      m_viabViol[i] = qa[i] - m_qMax[i];
244
    }
245
    if (std::abs(dqa[i]) > (m_dqMax[i] + m_eps)) {
246
      if (verbose) {
247
        std::cout << "State (q,dq) :(" << qa[i] << "," << dqa[i]
248
                  << ") of joint " << i
249
                  << " is not viable because |dq|>dqMax : " << std::abs(dqa[i])
250
                  << ">" << m_dqMax[i] << std::endl;
251
      }
252
      m_viabViol[i] = std::abs(dqa[i]) - m_dqMax[i];
253
    }
254
    double dqMaxViab =
255
        std::sqrt(std::max(0.0, 2 * m_ddqMax[i] * (m_qMax[i] - qa[i])));
256
    if (dqa[i] > (dqMaxViab + m_eps)) {
257
      if (verbose) {
258
        std::cout << "State (q,dq,dqMaxViab) :(" << qa[i] << "," << dqa[i]
259
                  << "," << dqMaxViab << ") of joint " << i
260
                  << " is not viable because dq>dqMaxViab : " << dqa[i] << ">"
261
                  << dqMaxViab << std::endl;
262
      }
263
      m_viabViol[i] = dqa[i] - dqMaxViab;
264
    }
265
    double dqMinViab =
266
        -std::sqrt(std::max(0.0, 2 * m_ddqMax[i] * (qa[i] - m_qMin[i])));
267
    if (dqa[i] < (dqMinViab + m_eps)) {
268
      if (verbose) {
269
        std::cout << "State (q,dq,dqMinViab) :(" << qa[i] << "," << dqa[i]
270
                  << "," << dqMinViab << ") of joint " << i
271
                  << " is not viable because dq<dqMinViab : " << dqa[i] << "<"
272
                  << dqMinViab << std::endl;
273
      }
274
      m_viabViol[i] = dqMinViab - dqa[i];
275
    }
276
  }
277
}
278
279
void TaskJointPosVelAccBounds::computeAccLimitsFromPosLimits(ConstRefVector qa,
280
                                                             ConstRefVector dqa,
281
                                                             bool verbose) {
282
  m_ddqMax_q3 = m_two_dt_sq * (m_qMax - qa - m_dt * dqa);
283
  m_ddqMin_q3 = m_two_dt_sq * (m_qMin - qa - m_dt * dqa);
284
  m_ddqMax_q2.setZero(m_na);
285
  m_ddqMin_q2.setZero(m_na);
286
  m_ddqLBPos.setConstant(m_na, 1, -1e10);
287
  m_ddqUBPos.setConstant(m_na, 1, 1e10);
288
  m_minus_dq_over_dt = -dqa / m_dt;
289
  for (int i = 0; i < m_na; i++) {
290
    if (dqa[i] <= 0.0) {
291
      m_ddqUBPos[i] = m_ddqMax_q3[i];
292
      if (m_ddqMin_q3[i] < m_minus_dq_over_dt[i]) {
293
        m_ddqLBPos[i] = m_ddqMin_q3[i];
294
      } else if (qa[i] != m_qMin[i]) {
295
        m_ddqMin_q2[i] = (dqa[i] * dqa[i]) / (2.0 * (qa[i] - m_qMin[i]));
296
        m_ddqLBPos[i] = std::max(m_ddqMin_q2[i], m_minus_dq_over_dt[i]);
297
      } else {
298
        if (verbose == true) {
299
          std::cout << "WARNING  qa[i]==m_qMin[i] for joint" << i << std::endl;
300
          std::cout << "You are going to violate the position bound " << i
301
                    << std::endl;
302
        }
303
        m_ddqLBPos[i] = 0.0;
304
      }
305
    } else {
306
      m_ddqLBPos[i] = m_ddqMin_q3[i];
307
      if (m_ddqMax_q3[i] > m_minus_dq_over_dt[i]) {
308
        m_ddqUBPos[i] = m_ddqMax_q3[i];
309
      } else if (qa[i] != m_qMax[i]) {
310
        m_ddqMax_q2[i] = -(dqa[i] * dqa[i]) / (2 * (m_qMax[i] - qa[i]));
311
        m_ddqUBPos[i] = std::min(m_ddqMax_q2[i], m_minus_dq_over_dt[i]);
312
      } else {
313
        if (verbose == true) {
314
          std::cout << "WARNING  qa[i]==m_qMax[i] for joint" << i << std::endl;
315
          std::cout << "You are going to violate the position bound " << i
316
                    << std::endl;
317
        }
318
        m_ddqUBPos[i] = 0.0;
319
      }
320
    }
321
  }
322
}
323
void TaskJointPosVelAccBounds::computeAccLimitsFromViability(ConstRefVector qa,
324
                                                             ConstRefVector dqa,
325
                                                             bool verbose) {
326
  m_ddqLBVia.setConstant(m_na, 1, -1e10);
327
  m_ddqUBVia.setConstant(m_na, 1, 1e10);
328
  m_dt_dq = m_dt * dqa;
329
  m_minus_dq_over_dt = -dqa / m_dt;
330
  m_dt_two_dq = 2 * m_dt_dq;
331
  m_two_ddqMax = 2 * m_ddqMax;
332
  m_dt_ddqMax_dt = m_ddqMax * m_dt_square;
333
  m_dq_square = dqa.cwiseProduct(dqa);
334
  m_q_plus_dt_dq = qa + m_dt_dq;
335
  m_b_1 = m_dt_two_dq + m_dt_ddqMax_dt;
336
  m_b_2 = m_dt_two_dq - m_dt_ddqMax_dt;
337
  m_ddq_1.setZero(m_na);
338
  m_ddq_2.setZero(m_na);
339
  m_c_1 = m_dq_square - m_two_ddqMax.cwiseProduct(m_qMax - m_q_plus_dt_dq);
340
  m_delta_1 = m_b_1.cwiseProduct(m_b_1) - 2 * m_two_a * m_c_1;
341
  m_c_2 = m_dq_square - m_two_ddqMax.cwiseProduct(m_q_plus_dt_dq - m_qMin);
342
  m_delta_2 = m_b_2.cwiseProduct(m_b_2) - 2 * m_two_a * m_c_2;
343
  for (int i = 0; i < m_na; i++) {
344
    if (m_delta_1[i] >= 0.0) {
345
      m_ddq_1[i] = (-m_b_1[i] + std::sqrt(m_delta_1[i])) / (m_two_a);
346
    } else {
347
      m_ddq_1[i] = m_minus_dq_over_dt[i];
348
      if (verbose == true) {
349
        std::cout << "Error: state (" << qa[i] << "," << dqa[i] << ") of joint "
350
                  << i << "not viable because delta is negative: " << m_delta_1
351
                  << std::endl;
352
      }
353
    }
354
    if (m_delta_2[i] >= 0.0) {
355
      m_ddq_2[i] = (-m_b_2[i] - std::sqrt(m_delta_2[i])) / (m_two_a);
356
    } else {
357
      m_ddq_2[i] = m_minus_dq_over_dt[i];
358
      if (verbose == true) {
359
        std::cout << "Error: state (" << qa[i] << "," << dqa[i] << ") of joint "
360
                  << i << "not viable because delta is negative: " << m_delta_2
361
                  << std::endl;
362
      }
363
    }
364
  }
365
  m_ddqUBVia = m_ddq_1.cwiseMax(m_minus_dq_over_dt);
366
  m_ddqLBVia = m_ddq_2.cwiseMin(m_minus_dq_over_dt);
367
}
368
369
void TaskJointPosVelAccBounds::computeAccLimits(ConstRefVector q,
370
                                                ConstRefVector dq,
371
                                                bool verbose) {
372
  m_qa = q.tail(m_na);
373
  m_dqa = dq.tail(m_na);
374
  isStateViable(m_qa, m_dqa, m_verbose);
375
  if (verbose == true) {
376
    for (int i = 0; i < m_na; i++) {
377
      if (m_viabViol[i] > m_eps) {
378
        std::cout << "WARNING: specified state ( < " << m_qa[i] << " , "
379
                  << m_dqa[i] << ") is not viable violation : " << m_viabViol[i]
380
                  << std::endl;
381
      }
382
    }
383
  }
384
385
  // Acceleration limits imposed by position bounds
386
  if (m_impose_position_bounds == true) {
387
    computeAccLimitsFromPosLimits(m_qa, m_dqa, verbose);
388
  }
389
  // Acceleration limits imposed by velocity bounds
390
  // dq[t+1] = dq + dt*ddq < dqMax
391
  // ddqMax = (dqMax-dq)/dt
392
  // ddqMin = (dqMin-dq)/dt = (-dqMax-dq)/dt
393
  m_ddqLBVel.setConstant(m_na, 1, -1e10);
394
  m_ddqUBVel.setConstant(m_na, 1, 1e10);
395
  if (m_impose_velocity_bounds == true) {
396
    m_ddqLBVel = (-m_dqMax - m_dqa) / m_dt;
397
    m_ddqUBVel = (m_dqMax - m_dqa) / m_dt;
398
  }
399
  // Acceleration limits imposed by viability
400
  if (m_impose_viability_bounds == true) {
401
    computeAccLimitsFromViability(m_qa, m_dqa, verbose);
402
  }
403
  // Acceleration limits
404
  m_ddqLBAcc.setConstant(m_na, 1, -1e10);
405
  m_ddqUBAcc.setConstant(m_na, 1, 1e10);
406
  if (m_impose_acceleration_bounds == true) {
407
    m_ddqLBAcc = -m_ddqMax;
408
    m_ddqUBAcc = m_ddqMax;
409
  }
410
  // Take the most conservative limit for each joint
411
  m_ub.setConstant(4, 1, 1e10);
412
  m_lb.setConstant(4, 1, -1e10);
413
  m_ddqLB.setConstant(m_na, 1, -1e10);
414
  m_ddqUB.setConstant(m_na, 1, 1e10);
415
  for (int i = 0; i < m_na; i++) {
416
    m_ub[0] = m_ddqUBPos[i];
417
    m_ub[1] = m_ddqUBVia[i];
418
    m_ub[2] = m_ddqUBVel[i];
419
    m_ub[3] = m_ddqUBAcc[i];
420
421
    m_lb[0] = m_ddqLBPos[i];
422
    m_lb[1] = m_ddqLBVia[i];
423
    m_lb[2] = m_ddqLBVel[i];
424
    m_lb[3] = m_ddqLBAcc[i];
425
426
    m_ddqLB[i] = m_lb.maxCoeff();
427
    m_ddqUB[i] = m_ub.minCoeff();
428
429
    if (m_ddqUB[i] < m_ddqLB[i]) {
430
      if (verbose == true) {
431
        std::cout << "Conflict between pos/vel/acc bound ddqMin " << m_ddqLB[i]
432
                  << " ddqMax " << m_ddqUB[i] << std::endl;
433
        std::cout << "ub " << m_ub.transpose() << std::endl;
434
        std::cout << "lb " << m_lb.transpose() << std::endl;
435
      }
436
      if (m_ddqUB[i] == m_ub[0]) {
437
        m_ddqLB[i] = m_ddqUB[i];
438
      } else {
439
        m_ddqUB[i] = m_ddqLB[i];
440
      }
441
      if (verbose == true) {
442
        std::cout << "New bounds are  ddqMin " << m_ddqLB[i] << " ddqMax "
443
                  << m_ddqUB[i] << std::endl;
444
      }
445
    }
446
  }
447
}
448
}  // namespace tasks
449
}  // namespace tsid