GCC Code Coverage Report

Directory: ./
File: include/tsid/tasks/task-joint-posVelAcc-bounds.hpp
Date: 2025-03-10 01:09:54
Exec Total Coverage
Lines: 1 1 100.0%
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__
187