Loading...
Searching...
No Matches
task-joint-posVelAcc-bounds.hpp
Go to the documentation of this file.
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
24#include <tsid/deprecated.hh>
25
33namespace tsid
34{
35 namespace tasks
36 {
37
39 {
40 public:
41 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42
47 typedef pinocchio::Data Data;
48
49 TaskJointPosVelAccBounds(const std::string & name,
50 RobotWrapper & robot,
51 double dt,
52 bool verbose=true);
53
54 int dim() const;
55
56 const ConstraintBase & compute(const double t,
59 Data & data);
60
61 const ConstraintBase & getConstraint() const;
62
63 void setTimeStep(double dt);
67 const Vector & getAccelerationBounds() const;
68 const Vector & getVelocityBounds() const;
69 const Vector & getPositionLowerBounds() const;
70 const Vector & getPositionUpperBounds() const;
71
72 void setVerbose(bool verbose);
73
74 void setImposeBounds(bool impose_position_bounds,
75 bool impose_velocity_bounds,
76 bool impose_viability_bounds,
77 bool impose_acceleration_bounds);
78
83 void isStateViable(ConstRefVector q,ConstRefVector dq ,bool verbose=true);
84
89
100 void computeAccLimitsFromViability(ConstRefVector q,ConstRefVector dq, bool verbose=true);
101
108 void computeAccLimits(ConstRefVector q,ConstRefVector dq,bool verbose=true);
109
110 TSID_DEPRECATED const Vector & mask() const; // deprecated
111 TSID_DEPRECATED void mask(const Vector & mask); // deprecated
112 virtual void setMask(math::ConstRefVector mask);
113
114
115 protected:
117 double m_dt;
119 int m_nv, m_na;
120
123
124 Vector m_qa; //actuated part of q
125 Vector m_dqa; //actuated part of dq
126
127 double m_eps; // tolerance used to check violations
128
129 Vector m_qMin;//joints position limits
130 Vector m_qMax;//joints position limits
131 Vector m_dqMax;//joints max velocity limits
132 Vector m_ddqMax;//joints max acceleration limits
133
134 Vector m_dqMinViab;//velocity lower limits from viability
135 Vector m_dqMaxViab;//velocity upper limits from viability
136
137 Vector m_ddqLBPos;//acceleration lower bound from position bounds
138 Vector m_ddqUBPos;//acceleration upper bound from position bounds
139 Vector m_ddqLBVia;//acceleration lower bound from viability bounds
140 Vector m_ddqUBVia;//acceleration upper bound from viability bounds
141 Vector m_ddqLBVel;//acceleration lower bound from velocity bounds
142 Vector m_ddqUBVel;//acceleration upper bound from velocity bounds
143 Vector m_ddqLBAcc;//acceleration lower bound from acceleration bounds
144 Vector m_ddqUBAcc;//acceleration upper bound from acceleration bounds
145
146 Vector m_ddqLB;//final acceleration bounds
147 Vector m_ddqUB;//final acceleration bounds
148
153
154 Vector m_viabViol;// 0 if the state is viable, error otherwise
155
156 //Used in computeAccLimitsFromPosLimits
163
164 //Used in computeAccLimitsFromViability
172 double m_two_a;
181
182 //Used in computeAccLimits
185 };
186
187 }
188}
189
190#endif // ifndef __invdyn_task_joint_bounds_hpp__
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:37
Definition: constraint-bound.hpp:29
Definition: constraint-inequality.hpp:29
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:41
const std::string & name() const
Definition: task-base.cpp:30
Definition: task-joint-posVelAcc-bounds.hpp:39
Vector m_dqMinViab
Definition: task-joint-posVelAcc-bounds.hpp:134
Vector m_ddqLBVel
Definition: task-joint-posVelAcc-bounds.hpp:141
void setVerbose(bool verbose)
Definition: task-joint-posVelAcc-bounds.cpp:167
Vector m_q_plus_dt_dq
Definition: task-joint-posVelAcc-bounds.hpp:171
Vector m_two_ddqMax
Definition: task-joint-posVelAcc-bounds.hpp:168
Vector m_lb
Definition: task-joint-posVelAcc-bounds.hpp:184
Vector m_ub
Definition: task-joint-posVelAcc-bounds.hpp:183
TSID_DEPRECATED const Vector & mask() const
Definition: task-joint-posVelAcc-bounds.cpp:115
bool m_impose_position_bounds
Definition: task-joint-posVelAcc-bounds.hpp:149
Vector m_ddqLB
Definition: task-joint-posVelAcc-bounds.hpp:146
Vector m_dqMax
Definition: task-joint-posVelAcc-bounds.hpp:131
Vector m_dqa
Definition: task-joint-posVelAcc-bounds.hpp:125
void computeAccLimits(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:399
void setPositionBounds(ConstRefVector lower, ConstRefVector upper)
Definition: task-joint-posVelAcc-bounds.cpp:171
bool m_impose_acceleration_bounds
Definition: task-joint-posVelAcc-bounds.hpp:152
double m_two_a
Definition: task-joint-posVelAcc-bounds.hpp:172
Vector m_ddqMax_q2
Definition: task-joint-posVelAcc-bounds.hpp:160
Vector m_c_2
Definition: task-joint-posVelAcc-bounds.hpp:179
Vector m_minus_dq_over_dt
Definition: task-joint-posVelAcc-bounds.hpp:162
double m_eps
Definition: task-joint-posVelAcc-bounds.hpp:127
int m_nv
Definition: task-joint-posVelAcc-bounds.hpp:119
void setTimeStep(double dt)
Definition: task-joint-posVelAcc-bounds.cpp:161
Vector m_ddqLBPos
Definition: task-joint-posVelAcc-bounds.hpp:137
bool m_verbose
Definition: task-joint-posVelAcc-bounds.hpp:118
Vector m_delta_1
Definition: task-joint-posVelAcc-bounds.hpp:178
math::ConstraintInequality ConstraintInequality
Definition: task-joint-posVelAcc-bounds.hpp:45
Vector m_ddqMax
Definition: task-joint-posVelAcc-bounds.hpp:132
int m_na
Definition: task-joint-posVelAcc-bounds.hpp:119
const ConstraintBase & getConstraint() const
Definition: task-joint-posVelAcc-bounds.cpp:195
Vector m_ddqLBVia
Definition: task-joint-posVelAcc-bounds.hpp:139
pinocchio::Data Data
Definition: task-joint-posVelAcc-bounds.hpp:47
double m_dt
Definition: task-joint-posVelAcc-bounds.hpp:117
math::ConstraintBound ConstraintBound
Definition: task-joint-posVelAcc-bounds.hpp:44
int dim() const
Return the dimension of the task. \info should be overloaded in the child class.
Definition: task-joint-posVelAcc-bounds.cpp:146
VectorXi m_activeAxes
Definition: task-joint-posVelAcc-bounds.hpp:122
Vector m_dqMaxViab
Definition: task-joint-posVelAcc-bounds.hpp:135
Vector m_ddqMax_q3
Definition: task-joint-posVelAcc-bounds.hpp:158
Vector m_ddqMin_q2
Definition: task-joint-posVelAcc-bounds.hpp:161
Vector m_ddqLBAcc
Definition: task-joint-posVelAcc-bounds.hpp:143
Vector m_b_2
Definition: task-joint-posVelAcc-bounds.hpp:174
Vector m_dt_ddqMax_dt
Definition: task-joint-posVelAcc-bounds.hpp:169
Vector m_ddqUB
Definition: task-joint-posVelAcc-bounds.hpp:147
Vector m_ddqUBPos
Definition: task-joint-posVelAcc-bounds.hpp:138
void setVelocityBounds(ConstRefVector upper)
Definition: task-joint-posVelAcc-bounds.cpp:181
bool m_impose_viability_bounds
Definition: task-joint-posVelAcc-bounds.hpp:151
Vector m_dq_square
Definition: task-joint-posVelAcc-bounds.hpp:170
void isStateViable(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:228
virtual void setMask(math::ConstRefVector mask)
Definition: task-joint-posVelAcc-bounds.cpp:126
Vector m_delta_2
Definition: task-joint-posVelAcc-bounds.hpp:180
void setAccelerationBounds(ConstRefVector upper)
Definition: task-joint-posVelAcc-bounds.cpp:188
Vector m_qMax
Definition: task-joint-posVelAcc-bounds.hpp:130
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Vector Vector
Definition: task-joint-posVelAcc-bounds.hpp:43
void computeAccLimitsFromPosLimits(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:287
Vector m_ddq_2
Definition: task-joint-posVelAcc-bounds.hpp:176
double m_two_dt_sq
Definition: task-joint-posVelAcc-bounds.hpp:157
Vector m_ddqUBAcc
Definition: task-joint-posVelAcc-bounds.hpp:144
Vector m_qa
Definition: task-joint-posVelAcc-bounds.hpp:124
Vector m_ddqUBVia
Definition: task-joint-posVelAcc-bounds.hpp:140
double m_dt_square
Definition: task-joint-posVelAcc-bounds.hpp:165
Vector m_dt_dq
Definition: task-joint-posVelAcc-bounds.hpp:166
void computeAccLimitsFromViability(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:346
const Vector & getVelocityBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:152
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-joint-posVelAcc-bounds.cpp:200
bool m_impose_velocity_bounds
Definition: task-joint-posVelAcc-bounds.hpp:150
Vector m_mask
Definition: task-joint-posVelAcc-bounds.hpp:121
Vector m_viabViol
Definition: task-joint-posVelAcc-bounds.hpp:154
math::VectorXi VectorXi
Definition: task-joint-posVelAcc-bounds.hpp:46
ConstraintInequality m_constraint
Definition: task-joint-posVelAcc-bounds.hpp:116
Vector m_ddqMin_q3
Definition: task-joint-posVelAcc-bounds.hpp:159
Vector m_dt_two_dq
Definition: task-joint-posVelAcc-bounds.hpp:167
Vector m_b_1
Definition: task-joint-posVelAcc-bounds.hpp:173
void setImposeBounds(bool impose_position_bounds, bool impose_velocity_bounds, bool impose_viability_bounds, bool impose_acceleration_bounds)
Definition: task-joint-posVelAcc-bounds.cpp:216
Vector m_ddqUBVel
Definition: task-joint-posVelAcc-bounds.hpp:142
const Vector & getPositionUpperBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:158
const Vector & getAccelerationBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:149
Vector m_c_1
Definition: task-joint-posVelAcc-bounds.hpp:177
const Vector & getPositionLowerBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:155
Vector m_qMin
Definition: task-joint-posVelAcc-bounds.hpp:129
Vector m_ddq_1
Definition: task-joint-posVelAcc-bounds.hpp:175
Definition: task-motion.hpp:29
#define TSID_DEPRECATED
Definition: deprecated.hh:37
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
Eigen::VectorXi VectorXi
Definition: fwd.hpp:39
Definition: constraint-bound.hpp:27