task-com-equality.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_com_equality_hpp__
19 #define __invdyn_task_com_equality_hpp__
20 
21 #include "tsid/math/fwd.hpp"
25 
26 namespace tsid
27 {
28  namespace tasks
29  {
30 
31  class TaskComEquality : public TaskMotion
32  {
33  public:
34  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 
36  typedef math::Index Index;
41 
42  TaskComEquality(const std::string & name,
43  RobotWrapper & robot);
44 
45  int dim() const;
46 
47  const ConstraintBase & compute(const double t,
50  Data & data);
51 
52  const ConstraintBase & getConstraint() const;
53 
54  void setReference(const TrajectorySample & ref);
55  const TrajectorySample & getReference() const;
56 
57  const Vector & getDesiredAcceleration() const;
58  Vector getAcceleration(ConstRefVector dv) const;
59  virtual void setMask(math::ConstRefVector mask);
60 
61  const Vector & position_error() const;
62  const Vector & velocity_error() const;
63  const Vector & position() const;
64  const Vector & velocity() const;
65  const Vector & position_ref() const;
66  const Vector & velocity_ref() const;
67 
68  const Vector3 & Kp();
69  const Vector3 & Kd();
70  void Kp(ConstRefVector Kp);
71  void Kd(ConstRefVector Kp);
72 
73  protected:
74  Vector3 m_Kp;
75  Vector3 m_Kd;
76  Vector3 m_p_error, m_v_error;
78  Vector3 m_a_des;
80  Vector3 m_drift;
82  Vector m_p_com, m_v_com;
84  TrajectorySample m_ref;
85  ConstraintEquality m_constraint;
86  };
87 
88  }
89 }
90 
91 #endif // ifndef __invdyn_task_com_equality_hpp__
std::size_t Index
Definition: fwd.hpp:55
Definition: constraint-equality.hpp:28
Vector3 m_Kp
Definition: task-com-equality.hpp:74
pinocchio::Data Data
Definition: task-base.hpp:43
const Vector & position_error() const
Definition: task-com-equality.cpp:101
ConstraintEquality m_constraint
Definition: task-com-equality.hpp:85
int dim() const
Return the dimension of the task. should be overloaded in the child class.
Definition: task-com-equality.cpp:60
Vector m_a_des_vec
Definition: task-com-equality.hpp:79
const std::string & name() const
Definition: task-base.cpp:30
math::ConstraintEquality ConstraintEquality
Definition: task-com-equality.hpp:40
Vector m_drift_masked
Definition: task-com-equality.hpp:81
Vector m_p_error_vec
Definition: task-com-equality.hpp:83
Vector3 m_v_error
Definition: task-com-equality.hpp:76
Vector m_v_error_vec
Definition: task-com-equality.hpp:83
const Vector3 & Kd()
Definition: task-com-equality.cpp:67
const Vector & velocity() const
Definition: task-com-equality.cpp:116
Vector m_v_com
Definition: task-com-equality.hpp:82
trajectories::TrajectorySample TrajectorySample
Definition: task-com-equality.hpp:37
TrajectorySample m_ref
Definition: task-com-equality.hpp:84
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-com-equality.cpp:136
const Vector & getDesiredAcceleration() const
Definition: task-com-equality.cpp:91
Vector3 m_Kd
Definition: task-com-equality.hpp:75
Vector getAcceleration(ConstRefVector dv) const
Definition: task-com-equality.cpp:96
const ConstraintBase & getConstraint() const
Definition: task-com-equality.cpp:131
Definition: task-motion.hpp:28
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
Definition: trajectory-base.hpp:35
Vector3 m_drift
Definition: task-com-equality.hpp:80
Vector m_a_des_masked
Definition: task-com-equality.hpp:79
math::ConstRefVector ConstRefVector
Definition: task-base.hpp:42
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
TaskComEquality(const std::string &name, RobotWrapper &robot)
Definition: task-com-equality.cpp:29
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
Vector m_v_error_masked_vec
Definition: task-com-equality.hpp:77
const Vector & velocity_error() const
Definition: task-com-equality.cpp:106
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
math::Vector3 Vector3
Definition: task-com-equality.hpp:39
void setReference(const TrajectorySample &ref)
Definition: task-com-equality.cpp:81
math::Vector Vector
Definition: task-com-equality.hpp:38
const Vector & position_ref() const
Definition: task-com-equality.cpp:121
Vector m_p_com
Definition: task-com-equality.hpp:82
Vector m_p_error_masked_vec
Definition: task-com-equality.hpp:77
const Vector3 & Kp()
Definition: task-com-equality.cpp:65
Definition: constraint-bound.hpp:26
virtual void setMask(math::ConstRefVector mask)
Definition: task-com-equality.cpp:48
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Index Index
Definition: task-com-equality.hpp:36
Vector3 m_p_error
Definition: task-com-equality.hpp:76
const Vector & position() const
Definition: task-com-equality.cpp:111
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:36
const Vector & velocity_ref() const
Definition: task-com-equality.cpp:126
const TrajectorySample & getReference() const
Definition: task-com-equality.cpp:86
Definition: task-com-equality.hpp:31
Vector3 m_a_des
Definition: task-com-equality.hpp:78