GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/tsid/tasks/task-joint-posVelAcc-bounds.hpp Lines: 1 2 50.0 %
Date: 2023-11-10 01:09:11 Branches: 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
#ifndef __invdyn_task_joint_posVelAcc_bounds_hpp__
19
#define __invdyn_task_joint_posVelAcc_bounds_hpp__
20
21
#include <tsid/tasks/task-motion.hpp>
22
#include <tsid/math/constraint-bound.hpp>
23
#include <tsid/math/constraint-inequality.hpp>
24
#include <tsid/deprecated.hh>
25
26
/** This class has been implemented following :
27
 * Andrea del Prete. Joint Position and Velocity Bounds in Discrete-Time
28
 * Acceleration/Torque Control of Robot Manipulators. IEEE Robotics and
29
 * Automation Letters, IEEE 2018, 3 (1),
30
 * pp.281-288.￿10.1109/LRA.2017.2738321￿. hal-01356989v3 And
31
 * https://github.com/andreadelprete/pinocchio_inv_dyn/blob/master/python/pinocchio_inv_dyn/acc_bounds_util.py
32
 */
33
namespace tsid {
34
namespace tasks {
35
36
class TaskJointPosVelAccBounds : public TaskMotion {
37
 public:
38
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39
40
  typedef math::Vector Vector;
41
  typedef math::ConstraintBound ConstraintBound;
42
  typedef math::ConstraintInequality ConstraintInequality;
43
  typedef math::VectorXi VectorXi;
44
  typedef pinocchio::Data Data;
45
46
  TaskJointPosVelAccBounds(const std::string& name, RobotWrapper& robot,
47
                           double dt, bool verbose = true);
48
49
2
  virtual ~TaskJointPosVelAccBounds() {}
50
51
  int dim() const;
52
53
  const ConstraintBase& compute(const double t, ConstRefVector q,
54
                                ConstRefVector v, Data& data);
55
56
  const ConstraintBase& getConstraint() const;
57
58
  void setTimeStep(double dt);
59
  void setPositionBounds(ConstRefVector lower, ConstRefVector upper);
60
  void setVelocityBounds(ConstRefVector upper);
61
  void setAccelerationBounds(ConstRefVector upper);
62
  const Vector& getAccelerationBounds() const;
63
  const Vector& getVelocityBounds() const;
64
  const Vector& getPositionLowerBounds() const;
65
  const Vector& getPositionUpperBounds() const;
66
67
  void setVerbose(bool verbose);
68
69
  void setImposeBounds(bool impose_position_bounds, bool impose_velocity_bounds,
70
                       bool impose_viability_bounds,
71
                       bool impose_acceleration_bounds);
72
73
  /** Check if the state is viable, otherwise it returns a measure of the
74
   * violation of the violated inequality. Fills in m_viabViol , if the
75
   * state of joint i is viable m_viabViol[i] = 0
76
   */
77
  void isStateViable(ConstRefVector q, ConstRefVector dq, bool verbose = true);
78
79
  /** Compute acceleration limits imposed by position bounds.
80
   * Fills in  m_ddqLBPos and m_ddqUBPos
81
   */
82
  void computeAccLimitsFromPosLimits(ConstRefVector q, ConstRefVector dq,
83
                                     bool verbose = true);
84
85
  /** Compute acceleration limits imposed by viability.
86
   * ddqMax is the maximum acceleration that will be necessary to stop the
87
   * joint before hitting the position limits.
88
   *
89
   * -sqrt( 2*ddqMax*(q-qMin) ) < dq[t+1] < sqrt( 2*ddqMax*(qMax-q) )
90
   * ddqMin[2] = (-sqrt(max(0.0, 2*MAX_ACC*(q[i]+DT*dq[i]-qMin))) - dq[i])/DT;
91
   * ddqMax[2] = (sqrt(max(0.0, 2*MAX_ACC*(qMax-q[i]-DT*dq[i]))) - dq[i])/DT;
92
   *
93
   * Fills in  m_ddqLBVia and m_ddqUBVia
94
   */
95
  void computeAccLimitsFromViability(ConstRefVector q, ConstRefVector dq,
96
                                     bool verbose = true);
97
98
  /** Given the current position and velocity, the bounds of position,
99
   * velocity and acceleration and the control time step, compute the
100
   * bounds of the acceleration such that all the bounds are respected
101
   * at the next time step and can be respected in the future.
102
   * ddqMax is the absolute maximum acceleration.
103
   */
104
  void computeAccLimits(ConstRefVector q, ConstRefVector dq,
105
                        bool verbose = true);
106
107
  TSID_DEPRECATED const Vector& mask() const;     // deprecated
108
  TSID_DEPRECATED void mask(const Vector& mask);  // deprecated
109
  virtual void setMask(math::ConstRefVector mask);
110
111
 protected:
112
  ConstraintInequality m_constraint;
113
  double m_dt;
114
  bool m_verbose;
115
  int m_nv, m_na;
116
117
  Vector m_mask;
118
  VectorXi m_activeAxes;
119
120
  Vector m_qa;   // actuated part of q
121
  Vector m_dqa;  // actuated part of dq
122
123
  double m_eps;  // tolerance used to check violations
124
125
  Vector m_qMin;    // joints position limits
126
  Vector m_qMax;    // joints position limits
127
  Vector m_dqMax;   // joints max velocity limits
128
  Vector m_ddqMax;  // joints max acceleration limits
129
130
  Vector m_dqMinViab;  // velocity lower limits from viability
131
  Vector m_dqMaxViab;  // velocity upper limits from viability
132
133
  Vector m_ddqLBPos;  // acceleration lower bound from position bounds
134
  Vector m_ddqUBPos;  // acceleration upper bound from position bounds
135
  Vector m_ddqLBVia;  // acceleration lower bound from viability bounds
136
  Vector m_ddqUBVia;  // acceleration upper bound from viability bounds
137
  Vector m_ddqLBVel;  // acceleration lower bound from velocity bounds
138
  Vector m_ddqUBVel;  // acceleration upper bound from velocity bounds
139
  Vector m_ddqLBAcc;  // acceleration lower bound from acceleration bounds
140
  Vector m_ddqUBAcc;  // acceleration upper bound from acceleration bounds
141
142
  Vector m_ddqLB;  // final acceleration bounds
143
  Vector m_ddqUB;  // final acceleration bounds
144
145
  bool m_impose_position_bounds;
146
  bool m_impose_velocity_bounds;
147
  bool m_impose_viability_bounds;
148
  bool m_impose_acceleration_bounds;
149
150
  Vector m_viabViol;  // 0 if the state is viable, error otherwise
151
152
  // Used in computeAccLimitsFromPosLimits
153
  double m_two_dt_sq;
154
  Vector m_ddqMax_q3;
155
  Vector m_ddqMin_q3;
156
  Vector m_ddqMax_q2;
157
  Vector m_ddqMin_q2;
158
  Vector m_minus_dq_over_dt;
159
160
  // Used in computeAccLimitsFromViability
161
  double m_dt_square;
162
  Vector m_dt_dq;
163
  Vector m_dt_two_dq;
164
  Vector m_two_ddqMax;
165
  Vector m_dt_ddqMax_dt;
166
  Vector m_dq_square;
167
  Vector m_q_plus_dt_dq;
168
  double m_two_a;
169
  Vector m_b_1;
170
  Vector m_b_2;
171
  Vector m_ddq_1;
172
  Vector m_ddq_2;
173
  Vector m_c_1;
174
  Vector m_delta_1;
175
  Vector m_c_2;
176
  Vector m_delta_2;
177
178
  // Used in computeAccLimits
179
  Vector m_ub;
180
  Vector m_lb;
181
};
182
183
}  // namespace tasks
184
}  // namespace tsid
185
186
#endif  // ifndef __invdyn_task_joint_bounds_hpp__