task-capture-point-inequality.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 CNRS, NYU, MPI Tübingen, PAL Robotics
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_capture_point_inequality_hpp__
19 #define __invdyn_task_capture_point_inequality_hpp__
20 
24 #include <vector>
25 #include <pinocchio/multibody/model.hpp>
26 #include <pinocchio/multibody/data.hpp>
27 
28 namespace tsid
29 {
30  namespace tasks
31  {
32 
34  {
35  public:
36  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 
38  typedef math::Index Index;
45 
46  TaskCapturePointInequality(const std::string & name,
47  RobotWrapper & robot,
48  const double timeStep);
49 
50  int dim() const;
51 
52  const ConstraintBase & compute(const double t,
55  Data & data);
56 
57  const ConstraintBase & getConstraint() const;
58 
59  Vector getAcceleration(ConstRefVector dv) const;
60 
61  const Vector & position() const;
62 
63  void setSupportLimitsXAxis(const double x_min, const double x_max);
64 
65  void setSupportLimitsYAxis(const double y_min, const double y_max);
66 
67  void setSafetyMargin(const double x_margin, const double y_margin);
68 
69  protected:
70 
71 
72  Vector m_drift_vec;
73  Vector3 m_drift;
74  Vector m_p_com, m_v_com;
75  Vector m_rp_min;
76  Vector m_rp_max;
77 
78  ConstraintInequality m_constraint;
79 
83 
85  double m_delta_t;
86  double m_g;
87  double m_w;
88  double m_ka;
89  int m_dim;
90 
91  Vector b_lower;
92  Vector b_upper;
93  };
94 
95  }
96 }
97 
98 #endif //ifndef __invdyn_task_capture_point_inequality_hpp__
std::size_t Index
Definition: fwd.hpp:55
ConstraintInequality m_constraint
Definition: task-capture-point-inequality.hpp:78
TaskCapturePointInequality(const std::string &name, RobotWrapper &robot, const double timeStep)
Definition: task-capture-point-inequality.cpp:34
Vector b_upper
Definition: task-capture-point-inequality.hpp:92
Vector b_lower
Definition: task-capture-point-inequality.hpp:91
math::ConstraintInequality ConstraintInequality
Definition: task-capture-point-inequality.hpp:42
Vector m_rp_min
Definition: task-capture-point-inequality.hpp:75
const std::string & name() const
Definition: task-base.cpp:30
void setSupportLimitsXAxis(const double x_min, const double x_max)
Definition: task-capture-point-inequality.cpp:83
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
Eigen::Index m_nv
Definition: task-capture-point-inequality.hpp:84
math::Vector3 Vector3
Definition: task-capture-point-inequality.hpp:41
Vector m_support_limits_x
Definition: task-capture-point-inequality.hpp:81
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Index Index
Definition: task-capture-point-inequality.hpp:38
Vector m_v_com
Definition: task-capture-point-inequality.hpp:74
Vector m_rp_max
Definition: task-capture-point-inequality.hpp:76
double m_ka
Definition: task-capture-point-inequality.hpp:88
Vector m_safety_margin
Definition: task-capture-point-inequality.hpp:80
const ConstraintBase & getConstraint() const
Definition: task-capture-point-inequality.cpp:78
Definition: task-motion.hpp:28
void setSafetyMargin(const double x_margin, const double y_margin)
Definition: task-capture-point-inequality.cpp:97
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
double m_delta_t
Definition: task-capture-point-inequality.hpp:85
math::ConstRefVector ConstRefVector
Definition: task-base.hpp:42
Vector m_support_limits_y
Definition: task-capture-point-inequality.hpp:82
pinocchio::Data Data
Definition: task-capture-point-inequality.hpp:43
Vector3 m_drift
Definition: task-capture-point-inequality.hpp:73
pinocchio::SE3 SE3
Definition: task-capture-point-inequality.hpp:44
void setSupportLimitsYAxis(const double y_min, const double y_max)
Definition: task-capture-point-inequality.cpp:90
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
math::Vector Vector
Definition: task-capture-point-inequality.hpp:39
Vector getAcceleration(ConstRefVector dv) const
Definition: task-capture-point-inequality.cpp:69
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
Definition: constraint-inequality.hpp:28
Definition: task-capture-point-inequality.hpp:33
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: fwd.hpp:38
int dim() const
Return the dimension of the task. should be overloaded in the child class.
Definition: task-capture-point-inequality.cpp:64
double m_g
Definition: task-capture-point-inequality.hpp:86
Definition: constraint-bound.hpp:26
math::Matrix Matrix
Definition: task-capture-point-inequality.hpp:40
const Vector & position() const
Definition: task-capture-point-inequality.cpp:74
Vector m_p_com
Definition: task-capture-point-inequality.hpp:74
double m_w
Definition: task-capture-point-inequality.hpp:87
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:36
Vector m_drift_vec
Definition: task-capture-point-inequality.hpp:72
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
int m_dim
Definition: task-capture-point-inequality.hpp:89
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-capture-point-inequality.cpp:103