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 
33 namespace tsid
34 {
35  namespace tasks
36  {
37 
39  {
40  public:
41  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
42 
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 
88  void computeAccLimitsFromPosLimits(ConstRefVector q,ConstRefVector dq, bool verbose=true);
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;
118  bool m_verbose;
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
157  double m_two_dt_sq;
163 
164  //Used in computeAccLimitsFromViability
165  double m_dt_square;
172  double m_two_a;
181 
182  //Used in computeAccLimits
185  };
186 
187  }
188 }
189 
190 #endif // ifndef __invdyn_task_joint_bounds_hpp__
tsid::tasks::TaskJointPosVelAccBounds::compute
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-joint-posVelAcc-bounds.cpp:200
tsid::tasks::TaskJointPosVelAccBounds::m_ddqUB
Vector m_ddqUB
Definition: task-joint-posVelAcc-bounds.hpp:147
tsid::tasks::TaskJointPosVelAccBounds::setTimeStep
void setTimeStep(double dt)
Definition: task-joint-posVelAcc-bounds.cpp:161
tsid::tasks::TaskJointPosVelAccBounds::m_ddqUBAcc
Vector m_ddqUBAcc
Definition: task-joint-posVelAcc-bounds.hpp:144
tsid::tasks::TaskJointPosVelAccBounds::m_ddqMax_q2
Vector m_ddqMax_q2
Definition: task-joint-posVelAcc-bounds.hpp:160
tsid::tasks::TaskJointPosVelAccBounds::m_dqMinViab
Vector m_dqMinViab
Definition: task-joint-posVelAcc-bounds.hpp:134
tsid::tasks::TaskJointPosVelAccBounds::computeAccLimitsFromPosLimits
void computeAccLimitsFromPosLimits(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:287
tsid::tasks::TaskJointPosVelAccBounds::m_impose_velocity_bounds
bool m_impose_velocity_bounds
Definition: task-joint-posVelAcc-bounds.hpp:150
Data
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
task-motion.hpp
tsid::tasks::TaskJointPosVelAccBounds::m_mask
Vector m_mask
Definition: task-joint-posVelAcc-bounds.hpp:121
tsid::tasks::TaskJointPosVelAccBounds::m_ddqLB
Vector m_ddqLB
Definition: task-joint-posVelAcc-bounds.hpp:146
tsid::math::ConstraintBase
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:36
tsid::tasks::TaskJointPosVelAccBounds::m_ddq_2
Vector m_ddq_2
Definition: task-joint-posVelAcc-bounds.hpp:176
tsid::tasks::TaskJointPosVelAccBounds::TaskJointPosVelAccBounds
TaskJointPosVelAccBounds(const std::string &name, RobotWrapper &robot, double dt, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:37
tsid::tasks::TaskJointPosVelAccBounds::m_ddqMax
Vector m_ddqMax
Definition: task-joint-posVelAcc-bounds.hpp:132
tsid::tasks::TaskJointPosVelAccBounds::m_ddqLBPos
Vector m_ddqLBPos
Definition: task-joint-posVelAcc-bounds.hpp:137
tsid::math::Vector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
tsid::math::ConstraintBound
Definition: constraint-bound.hpp:28
tsid::tasks::TaskJointPosVelAccBounds::m_b_1
Vector m_b_1
Definition: task-joint-posVelAcc-bounds.hpp:173
tsid::tasks::TaskJointPosVelAccBounds::getConstraint
const ConstraintBase & getConstraint() const
Definition: task-joint-posVelAcc-bounds.cpp:195
tsid::tasks::TaskJointPosVelAccBounds::m_dt_two_dq
Vector m_dt_two_dq
Definition: task-joint-posVelAcc-bounds.hpp:167
tsid::tasks::TaskJointPosVelAccBounds::m_ub
Vector m_ub
Definition: task-joint-posVelAcc-bounds.hpp:183
tsid::tasks::TaskJointPosVelAccBounds::m_two_dt_sq
double m_two_dt_sq
Definition: task-joint-posVelAcc-bounds.hpp:157
tsid::tasks::TaskJointPosVelAccBounds::m_qMax
Vector m_qMax
Definition: task-joint-posVelAcc-bounds.hpp:130
tsid::tasks::TaskJointPosVelAccBounds::mask
const TSID_DEPRECATED Vector & mask() const
Definition: task-joint-posVelAcc-bounds.cpp:115
tsid::tasks::TaskJointPosVelAccBounds::m_impose_viability_bounds
bool m_impose_viability_bounds
Definition: task-joint-posVelAcc-bounds.hpp:151
tsid::tasks::TaskJointPosVelAccBounds::m_minus_dq_over_dt
Vector m_minus_dq_over_dt
Definition: task-joint-posVelAcc-bounds.hpp:162
tsid::tasks::TaskJointPosVelAccBounds::m_viabViol
Vector m_viabViol
Definition: task-joint-posVelAcc-bounds.hpp:154
tsid::tasks::TaskJointPosVelAccBounds::setPositionBounds
void setPositionBounds(ConstRefVector lower, ConstRefVector upper)
Definition: task-joint-posVelAcc-bounds.cpp:171
tsid::tasks::TaskJointPosVelAccBounds::m_dqMaxViab
Vector m_dqMaxViab
Definition: task-joint-posVelAcc-bounds.hpp:135
tsid::tasks::TaskJointPosVelAccBounds::m_dt_ddqMax_dt
Vector m_dt_ddqMax_dt
Definition: task-joint-posVelAcc-bounds.hpp:169
tsid::tasks::TaskJointPosVelAccBounds::m_activeAxes
VectorXi m_activeAxes
Definition: task-joint-posVelAcc-bounds.hpp:122
tsid::tasks::TaskJointPosVelAccBounds::setMask
virtual void setMask(math::ConstRefVector mask)
Definition: task-joint-posVelAcc-bounds.cpp:126
tsid::tasks::TaskJointPosVelAccBounds::setVerbose
void setVerbose(bool verbose)
Definition: task-joint-posVelAcc-bounds.cpp:167
tsid::tasks::TaskJointPosVelAccBounds::m_ddqLBAcc
Vector m_ddqLBAcc
Definition: task-joint-posVelAcc-bounds.hpp:143
tsid::tasks::TaskJointPosVelAccBounds::m_ddqLBVel
Vector m_ddqLBVel
Definition: task-joint-posVelAcc-bounds.hpp:141
tsid::tasks::TaskJointPosVelAccBounds::setAccelerationBounds
void setAccelerationBounds(ConstRefVector upper)
Definition: task-joint-posVelAcc-bounds.cpp:188
tsid::tasks::TaskJointPosVelAccBounds::m_c_2
Vector m_c_2
Definition: task-joint-posVelAcc-bounds.hpp:179
tsid::tasks::TaskJointPosVelAccBounds::m_ddqMin_q2
Vector m_ddqMin_q2
Definition: task-joint-posVelAcc-bounds.hpp:161
tsid::tasks::TaskJointPosVelAccBounds::getAccelerationBounds
const Vector & getAccelerationBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:149
tsid::tasks::TaskJointPosVelAccBounds::m_dt
double m_dt
Definition: task-joint-posVelAcc-bounds.hpp:117
tsid::tasks::TaskJointPosVelAccBounds::m_two_a
double m_two_a
Definition: task-joint-posVelAcc-bounds.hpp:172
tsid::tasks::TaskJointPosVelAccBounds::computeAccLimitsFromViability
void computeAccLimitsFromViability(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:346
tsid::tasks::TaskJointPosVelAccBounds::VectorXi
math::VectorXi VectorXi
Definition: task-joint-posVelAcc-bounds.hpp:46
tsid::tasks::TaskJointPosVelAccBounds::m_qa
Vector m_qa
Definition: task-joint-posVelAcc-bounds.hpp:124
tsid::tasks::TaskJointPosVelAccBounds::m_dt_dq
Vector m_dt_dq
Definition: task-joint-posVelAcc-bounds.hpp:166
tsid::math::VectorXi
Eigen::VectorXi VectorXi
Definition: fwd.hpp:39
tsid::tasks::TaskJointPosVelAccBounds::ConstraintBound
math::ConstraintBound ConstraintBound
Definition: task-joint-posVelAcc-bounds.hpp:44
tsid::tasks::TaskJointPosVelAccBounds::m_impose_acceleration_bounds
bool m_impose_acceleration_bounds
Definition: task-joint-posVelAcc-bounds.hpp:152
tsid::tasks::TaskJointPosVelAccBounds::m_ddqLBVia
Vector m_ddqLBVia
Definition: task-joint-posVelAcc-bounds.hpp:139
tsid::tasks::TaskJointPosVelAccBounds::m_ddqUBPos
Vector m_ddqUBPos
Definition: task-joint-posVelAcc-bounds.hpp:138
tsid::tasks::TaskJointPosVelAccBounds::m_verbose
bool m_verbose
Definition: task-joint-posVelAcc-bounds.hpp:118
tsid::tasks::TaskJointPosVelAccBounds::m_two_ddqMax
Vector m_two_ddqMax
Definition: task-joint-posVelAcc-bounds.hpp:168
tsid::tasks::TaskJointPosVelAccBounds::m_eps
double m_eps
Definition: task-joint-posVelAcc-bounds.hpp:127
tsid::tasks::TaskJointPosVelAccBounds::m_c_1
Vector m_c_1
Definition: task-joint-posVelAcc-bounds.hpp:177
tsid::tasks::TaskJointPosVelAccBounds::m_impose_position_bounds
bool m_impose_position_bounds
Definition: task-joint-posVelAcc-bounds.hpp:149
tsid::tasks::TaskJointPosVelAccBounds::m_dqMax
Vector m_dqMax
Definition: task-joint-posVelAcc-bounds.hpp:131
tsid::math::ConstraintInequality
Definition: constraint-inequality.hpp:28
tsid::tasks::TaskJointPosVelAccBounds::setImposeBounds
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
tsid::tasks::TaskJointPosVelAccBounds::m_na
int m_na
Definition: task-joint-posVelAcc-bounds.hpp:119
tsid::tasks::TaskJointPosVelAccBounds::m_ddqMax_q3
Vector m_ddqMax_q3
Definition: task-joint-posVelAcc-bounds.hpp:158
tsid::tasks::TaskJointPosVelAccBounds
Definition: task-joint-posVelAcc-bounds.hpp:38
tsid::tasks::TaskMotion
Definition: task-motion.hpp:28
tsid::math::ConstRefVector
const typedef Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
tsid::tasks::TaskJointPosVelAccBounds::m_delta_2
Vector m_delta_2
Definition: task-joint-posVelAcc-bounds.hpp:180
tsid::tasks::TaskBase::Data
pinocchio::Data Data
Definition: task-base.hpp:43
constraint-inequality.hpp
tsid::tasks::TaskJointPosVelAccBounds::m_ddqUBVia
Vector m_ddqUBVia
Definition: task-joint-posVelAcc-bounds.hpp:140
tsid::tasks::TaskJointPosVelAccBounds::m_ddqUBVel
Vector m_ddqUBVel
Definition: task-joint-posVelAcc-bounds.hpp:142
tsid::tasks::TaskJointPosVelAccBounds::m_q_plus_dt_dq
Vector m_q_plus_dt_dq
Definition: task-joint-posVelAcc-bounds.hpp:171
tsid::tasks::TaskJointPosVelAccBounds::m_nv
int m_nv
Definition: task-joint-posVelAcc-bounds.hpp:119
tsid
Definition: constraint-bound.hpp:26
tsid::tasks::TaskJointPosVelAccBounds::m_delta_1
Vector m_delta_1
Definition: task-joint-posVelAcc-bounds.hpp:178
tsid::tasks::TaskJointPosVelAccBounds::m_dqa
Vector m_dqa
Definition: task-joint-posVelAcc-bounds.hpp:125
tsid::tasks::TaskJointPosVelAccBounds::dim
int dim() const
Return the dimension of the task. \info should be overloaded in the child class.
Definition: task-joint-posVelAcc-bounds.cpp:146
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::tasks::TaskJointPosVelAccBounds::Data
pinocchio::Data Data
Definition: task-joint-posVelAcc-bounds.hpp:47
constraint-bound.hpp
tsid::tasks::TaskJointPosVelAccBounds::getPositionUpperBounds
const Vector & getPositionUpperBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:158
tsid::tasks::TaskBase::name
const std::string & name() const
Definition: task-base.cpp:30
tsid::tasks::TaskJointPosVelAccBounds::computeAccLimits
void computeAccLimits(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:399
tsid::tasks::TaskJointPosVelAccBounds::setVelocityBounds
void setVelocityBounds(ConstRefVector upper)
Definition: task-joint-posVelAcc-bounds.cpp:181
tsid::tasks::TaskJointPosVelAccBounds::m_qMin
Vector m_qMin
Definition: task-joint-posVelAcc-bounds.hpp:129
tsid::tasks::TaskJointPosVelAccBounds::m_lb
Vector m_lb
Definition: task-joint-posVelAcc-bounds.hpp:184
tsid::tasks::TaskJointPosVelAccBounds::isStateViable
void isStateViable(ConstRefVector q, ConstRefVector dq, bool verbose=true)
Definition: task-joint-posVelAcc-bounds.cpp:228
tsid::tasks::TaskJointPosVelAccBounds::m_dq_square
Vector m_dq_square
Definition: task-joint-posVelAcc-bounds.hpp:170
tsid::tasks::TaskJointPosVelAccBounds::getPositionLowerBounds
const Vector & getPositionLowerBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:155
tsid::tasks::TaskJointPosVelAccBounds::m_b_2
Vector m_b_2
Definition: task-joint-posVelAcc-bounds.hpp:174
tsid::tasks::TaskJointPosVelAccBounds::Vector
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Vector Vector
Definition: task-joint-posVelAcc-bounds.hpp:43
tsid::tasks::TaskJointPosVelAccBounds::m_constraint
ConstraintInequality m_constraint
Definition: task-joint-posVelAcc-bounds.hpp:116
tsid::tasks::TaskJointPosVelAccBounds::m_ddqMin_q3
Vector m_ddqMin_q3
Definition: task-joint-posVelAcc-bounds.hpp:159
tsid::tasks::TaskJointPosVelAccBounds::ConstraintInequality
math::ConstraintInequality ConstraintInequality
Definition: task-joint-posVelAcc-bounds.hpp:45
tsid::tasks::TaskJointPosVelAccBounds::m_dt_square
double m_dt_square
Definition: task-joint-posVelAcc-bounds.hpp:165
tsid::tasks::TaskBase::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: task-base.hpp:42
tsid::tasks::TaskJointPosVelAccBounds::m_ddq_1
Vector m_ddq_1
Definition: task-joint-posVelAcc-bounds.hpp:175
tsid::tasks::TaskJointPosVelAccBounds::getVelocityBounds
const Vector & getVelocityBounds() const
Definition: task-joint-posVelAcc-bounds.cpp:152