Loading...
Searching...
No Matches
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
27namespace tsid
28{
29 namespace tasks
30 {
31
33 {
34 public:
35 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36
43 typedef pinocchio::SE3 SE3;
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
71 const TrajectorySample & getReference() const;
72
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;
98 };
99
100 }
101}
102
103#endif // ifndef __invdyn_task_contact_force_equality_hpp__
Base template of a Contact.
Definition: contact-base.hpp:35
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:37
Definition: constraint-equality.hpp:29
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:41
const std::string & name() const
Definition: task-base.cpp:30
pinocchio::Data Data
Definition: task-base.hpp:43
Definition: task-contact-force-equality.hpp:33
void setReference(TrajectorySample &ref)
Definition: task-contact-force-equality.cpp:86
const TrajectorySample & getReference() const
Definition: task-contact-force-equality.cpp:90
math::Vector Vector
Definition: task-contact-force-equality.hpp:39
pinocchio::SE3 SE3
Definition: task-contact-force-equality.hpp:43
void setExternalForce(TrajectorySample &f_ext)
Definition: task-contact-force-equality.cpp:94
TrajectorySample m_ref
Definition: task-contact-force-equality.hpp:90
TrajectorySample m_fext
Definition: task-contact-force-equality.hpp:91
const Vector & Kp() const
Definition: task-contact-force-equality.cpp:45
const TrajectorySample & getExternalForce() const
Definition: task-contact-force-equality.cpp:98
Vector m_Kd
Definition: task-contact-force-equality.hpp:94
Vector m_forceIntegralError
Definition: task-contact-force-equality.hpp:92
const ConstraintBase & getConstraint() const
Definition: task-contact-force-equality.cpp:146
void setAssociatedContact(contacts::ContactBase &contact)
Definition: task-contact-force-equality.cpp:81
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-contact-force-equality.cpp:128
ConstraintEquality m_constraint
Definition: task-contact-force-equality.hpp:89
trajectories::TrajectorySample TrajectorySample
Definition: task-contact-force-equality.hpp:38
const Vector & Kd() const
Definition: task-contact-force-equality.cpp:46
double m_dt
Definition: task-contact-force-equality.hpp:96
const double & getLeakRate() const
Definition: task-contact-force-equality.cpp:48
contacts::ContactBase * m_contact
Definition: task-contact-force-equality.hpp:87
std::string m_contact_name
Definition: task-contact-force-equality.hpp:88
const Vector & Ki() const
Definition: task-contact-force-equality.cpp:47
virtual const std::string & getAssociatedContactName()
Definition: task-contact-force-equality.cpp:73
math::ConstraintEquality ConstraintEquality
Definition: task-contact-force-equality.hpp:42
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Index Index
Definition: task-contact-force-equality.hpp:37
Vector m_Kp
Definition: task-contact-force-equality.hpp:93
void setLeakRate(double leak)
Definition: task-contact-force-equality.cpp:68
virtual const contacts::ContactBase & getAssociatedContact()
Definition: task-contact-force-equality.cpp:77
int dim() const
Return the dimension of the task. \info should be overloaded in the child class.
Definition: task-contact-force-equality.cpp:41
math::Vector3 Vector3
Definition: task-contact-force-equality.hpp:41
double m_leak_rate
Definition: task-contact-force-equality.hpp:97
math::Vector6 Vector6
Definition: task-contact-force-equality.hpp:40
Vector m_Ki
Definition: task-contact-force-equality.hpp:95
Definition: task-contact-force.hpp:31
Definition: trajectory-base.hpp:36
std::size_t Index
Definition: fwd.hpp:55
Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition: fwd.hpp:42
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
Eigen::Matrix< Scalar, 6, 1 > Vector6
Definition: fwd.hpp:43
Definition: constraint-bound.hpp:27