task-contact-force-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_contact_force_equality_hpp__
19 #define __invdyn_task_contact_force_equality_hpp__
20 
21 #include "tsid/math/fwd.hpp"
26 
27 namespace tsid
28 {
29  namespace tasks
30  {
31 
33  {
34  public:
35  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 
37  typedef math::Index Index;
44 
45  TaskContactForceEquality(const std::string & name,
46  RobotWrapper & robot,
47  const double dt,
48  contacts::ContactBase & contact);
49 
50  int dim() const;
51 
52  virtual const std::string& getAssociatedContactName();
55 
56  // Task expressed as a PID between the reference force and the external one
57  const ConstraintBase & compute(const double t,
60  Data & data);
61 
62  const ConstraintBase & compute(const double t,
65  Data & data,
66  const std::vector<std::shared_ptr<ContactLevel> > *contacts);
67 
68  const ConstraintBase & getConstraint() const;
69 
70  void setReference(TrajectorySample & ref);
71  const TrajectorySample & getReference() const;
72 
73  void setExternalForce(TrajectorySample & f_ext);
74  const TrajectorySample & getExternalForce() const;
75 
76  const Vector & Kp() const;
77  const Vector & Kd() const;
78  const Vector & Ki() const;
79  const double & getLeakRate() const;
80  void Kp(ConstRefVector Kp);
81  void Kd(ConstRefVector Kp);
82  void Ki(ConstRefVector Ki);
83  void setLeakRate(double leak);
84 
85  protected:
86  // contact associated to the force task
88  std::string m_contact_name; // the associated contact name or an empty string
90  TrajectorySample m_ref; // reference Force 6D to follow
91  TrajectorySample m_fext; // external Force 6D in the same frame than the ref
92  Vector m_forceIntegralError; // Integral error of the PID
96  double m_dt;
97  double m_leak_rate;
98  };
99 
100  }
101 }
102 
103 #endif // ifndef __invdyn_task_contact_force_equality_hpp__
tsid::tasks::TaskContactForceEquality::Vector
math::Vector Vector
Definition: task-contact-force-equality.hpp:39
tsid::tasks::TaskContactForceEquality::Kd
const Vector & Kd() const
Definition: task-contact-force-equality.cpp:46
tsid::contacts::ContactBase
Base template of a Contact.
Definition: contact-base.hpp:34
tsid::tasks::TaskContactForceEquality::m_dt
double m_dt
Definition: task-contact-force-equality.hpp:96
tsid::trajectories::TrajectorySample
Definition: trajectory-base.hpp:35
tsid::tasks::TaskContactForceEquality::Vector6
math::Vector6 Vector6
Definition: task-contact-force-equality.hpp:40
tsid::math::ConstraintEquality
Definition: constraint-equality.hpp:28
tsid::tasks::TaskContactForceEquality::m_Kd
Vector m_Kd
Definition: task-contact-force-equality.hpp:94
tsid::tasks::TaskContactForceEquality::TrajectorySample
trajectories::TrajectorySample TrajectorySample
Definition: task-contact-force-equality.hpp:38
tsid::tasks::TaskContactForceEquality::setExternalForce
void setExternalForce(TrajectorySample &f_ext)
Definition: task-contact-force-equality.cpp:94
tsid::tasks::TaskContactForceEquality::getLeakRate
const double & getLeakRate() const
Definition: task-contact-force-equality.cpp:48
tsid::tasks::TaskContactForceEquality::setLeakRate
void setLeakRate(double leak)
Definition: task-contact-force-equality.cpp:68
tsid::math::ConstraintBase
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:36
tsid::tasks::TaskContactForceEquality::getConstraint
const ConstraintBase & getConstraint() const
Definition: task-contact-force-equality.cpp:146
tsid::tasks::TaskContactForceEquality::m_Kp
Vector m_Kp
Definition: task-contact-force-equality.hpp:93
tsid::math::Vector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
tsid::tasks::TaskContactForce
Definition: task-contact-force.hpp:30
tsid::math::Vector6
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: fwd.hpp:43
tsid::tasks::TaskContactForceEquality::Index
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Index Index
Definition: task-contact-force-equality.hpp:37
constraint-equality.hpp
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::tasks::TaskContactForceEquality::getExternalForce
const TrajectorySample & getExternalForce() const
Definition: task-contact-force-equality.cpp:98
tsid::tasks::TaskContactForceEquality::m_leak_rate
double m_leak_rate
Definition: task-contact-force-equality.hpp:97
trajectory-base.hpp
tsid::tasks::TaskContactForceEquality::m_fext
TrajectorySample m_fext
Definition: task-contact-force-equality.hpp:91
tsid::tasks::TaskContactForceEquality::setAssociatedContact
void setAssociatedContact(contacts::ContactBase &contact)
Definition: task-contact-force-equality.cpp:81
tsid::tasks::TaskContactForceEquality::m_contact_name
std::string m_contact_name
Definition: task-contact-force-equality.hpp:88
tsid::math::Index
std::size_t Index
Definition: fwd.hpp:55
tsid::tasks::TaskContactForceEquality::setReference
void setReference(TrajectorySample &ref)
Definition: task-contact-force-equality.cpp:86
tsid::tasks::TaskContactForceEquality
Definition: task-contact-force-equality.hpp:32
tsid::tasks::TaskContactForceEquality::getReference
const TrajectorySample & getReference() const
Definition: task-contact-force-equality.cpp:90
fwd.hpp
tsid::tasks::TaskContactForceEquality::getAssociatedContactName
virtual const std::string & getAssociatedContactName()
Definition: task-contact-force-equality.cpp:73
tsid::tasks::TaskContactForceEquality::dim
int dim() const
Return the dimension of the task. \info should be overloaded in the child class.
Definition: task-contact-force-equality.cpp:41
tsid::tasks::TaskContactForceEquality::Kp
const Vector & Kp() const
Definition: task-contact-force-equality.cpp:45
tsid::tasks::TaskContactForceEquality::getAssociatedContact
virtual const contacts::ContactBase & getAssociatedContact()
Definition: task-contact-force-equality.cpp:77
tsid::tasks::TaskContactForceEquality::m_ref
TrajectorySample m_ref
Definition: task-contact-force-equality.hpp:90
tsid::tasks::TaskBase::Data
pinocchio::Data Data
Definition: task-base.hpp:43
tsid::tasks::TaskContactForceEquality::compute
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-contact-force-equality.cpp:128
tsid
Definition: constraint-bound.hpp:26
task-contact-force.hpp
tsid::tasks::TaskContactForceEquality::TaskContactForceEquality
TaskContactForceEquality(const std::string &name, RobotWrapper &robot, const double dt, contacts::ContactBase &contact)
Definition: task-contact-force-equality.cpp:28
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::tasks::TaskContactForceEquality::ConstraintEquality
math::ConstraintEquality ConstraintEquality
Definition: task-contact-force-equality.hpp:42
tsid::tasks::TaskBase::name
const std::string & name() const
Definition: task-base.cpp:30
tsid::tasks::TaskContactForceEquality::m_forceIntegralError
Vector m_forceIntegralError
Definition: task-contact-force-equality.hpp:92
tsid::tasks::TaskContactForceEquality::Ki
const Vector & Ki() const
Definition: task-contact-force-equality.cpp:47
tsid::tasks::TaskContactForceEquality::m_constraint
ConstraintEquality m_constraint
Definition: task-contact-force-equality.hpp:89
tsid::tasks::TaskContactForceEquality::Vector3
math::Vector3 Vector3
Definition: task-contact-force-equality.hpp:41
tsid::tasks::TaskContactForceEquality::m_Ki
Vector m_Ki
Definition: task-contact-force-equality.hpp:95
tsid::tasks::TaskContactForceEquality::SE3
pinocchio::SE3 SE3
Definition: task-contact-force-equality.hpp:43
tsid::math::Vector3
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
contact-base.hpp
tsid::tasks::TaskBase::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: task-base.hpp:42
tsid::tasks::TaskContactForceEquality::m_contact
contacts::ContactBase * m_contact
Definition: task-contact-force-equality.hpp:87