GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/tasks/task-joint-bounds.cpp Lines: 0 49 0.0 %
Date: 2024-02-02 08:47:34 Branches: 0 112 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-bounds.hpp>
19
#include "tsid/robots/robot-wrapper.hpp"
20
21
namespace tsid {
22
namespace tasks {
23
using namespace math;
24
using namespace trajectories;
25
using namespace pinocchio;
26
27
TaskJointBounds::TaskJointBounds(const std::string& name, RobotWrapper& robot,
28
                                 double dt)
29
    : TaskMotion(name, robot),
30
      m_constraint(name, robot.nv()),
31
      m_dt(dt),
32
      m_nv(robot.nv()),
33
      m_na(robot.na()) {
34
  PINOCCHIO_CHECK_INPUT_ARGUMENT(dt > 0.0, "dt needs to be positive");
35
  m_v_lb = -1e10 * Vector::Ones(m_na);
36
  m_v_ub = +1e10 * Vector::Ones(m_na);
37
  m_a_lb = -1e10 * Vector::Ones(m_na);
38
  m_a_ub = +1e10 * Vector::Ones(m_na);
39
  m_ddq_max_due_to_vel.setZero(m_na);
40
  m_ddq_max_due_to_vel.setZero(m_na);
41
42
  int offset = m_nv - m_na;
43
  for (int i = 0; i < offset; i++) {
44
    m_constraint.upperBound()(i) = 1e10;
45
    m_constraint.lowerBound()(i) = -1e10;
46
  }
47
}
48
49
int TaskJointBounds::dim() const { return m_nv; }
50
51
const Vector& TaskJointBounds::getAccelerationLowerBounds() const {
52
  return m_a_lb;
53
}
54
55
const Vector& TaskJointBounds::getAccelerationUpperBounds() const {
56
  return m_a_ub;
57
}
58
59
const Vector& TaskJointBounds::getVelocityLowerBounds() const { return m_v_lb; }
60
61
const Vector& TaskJointBounds::getVelocityUpperBounds() const { return m_v_ub; }
62
63
void TaskJointBounds::setTimeStep(double dt) {
64
  PINOCCHIO_CHECK_INPUT_ARGUMENT(dt > 0.0, "dt needs to be positive");
65
  m_dt = dt;
66
}
67
68
void TaskJointBounds::setVelocityBounds(ConstRefVector lower,
69
                                        ConstRefVector upper) {
70
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
71
      lower.size() == m_na,
72
      "The size of the lower velocity bounds vector needs to equal " +
73
          std::to_string(m_na));
74
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
75
      upper.size() == m_na,
76
      "The size of the upper velocity bounds vector needs to equal " +
77
          std::to_string(m_na));
78
  m_v_lb = lower;
79
  m_v_ub = upper;
80
}
81
82
void TaskJointBounds::setAccelerationBounds(ConstRefVector lower,
83
                                            ConstRefVector upper) {
84
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
85
      lower.size() == m_na,
86
      "The size of the lower acceleration bounds vector needs to equal " +
87
          std::to_string(m_na));
88
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
89
      upper.size() == m_na,
90
      "The size of the upper acceleration bounds vector needs to equal " +
91
          std::to_string(m_na));
92
  m_a_lb = lower;
93
  m_a_ub = upper;
94
}
95
96
const ConstraintBase& TaskJointBounds::getConstraint() const {
97
  return m_constraint;
98
}
99
100
void TaskJointBounds::setMask(ConstRefVector mask) { m_mask = mask; }
101
102
const ConstraintBase& TaskJointBounds::compute(const double, ConstRefVector,
103
                                               ConstRefVector v, Data&) {
104
  // compute min/max joint acc imposed by velocity limits
105
  m_ddq_max_due_to_vel = (m_v_ub - v.tail(m_na)) / m_dt;
106
  m_ddq_min_due_to_vel = (m_v_lb - v.tail(m_na)) / m_dt;
107
108
  // take most conservative limit between vel and acc
109
  int offset = m_nv - m_na;
110
  for (int i = 0; i < m_na; i++) {
111
    // TODO: use mask here
112
    m_constraint.upperBound()(offset + i) =
113
        std::min(m_ddq_max_due_to_vel(i), m_a_ub(i));
114
    m_constraint.lowerBound()(offset + i) =
115
        std::max(m_ddq_min_due_to_vel(i), m_a_lb(i));
116
  }
117
  return m_constraint;
118
}
119
120
}  // namespace tasks
121
}  // namespace tsid