sot-core  4.11.2
Hierarchical task solver plug-in for dynamic-graph.
control-pd.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #ifndef __SOT_Control_PD_HH__
11 #define __SOT_Control_PD_HH__
12 
13 /* --------------------------------------------------------------------- */
14 /* --- INCLUDE --------------------------------------------------------- */
15 /* --------------------------------------------------------------------- */
16 
17 /* Matrix */
18 #include <dynamic-graph/linear-algebra.h>
19 
20 /* SOT */
21 #include <dynamic-graph/entity.h>
22 #include <dynamic-graph/signal-ptr.h>
23 #include <dynamic-graph/signal-time-dependent.h>
24 
25 /* --------------------------------------------------------------------- */
26 /* --- API ------------------------------------------------------------- */
27 /* --------------------------------------------------------------------- */
28 
29 #if defined(WIN32)
30 #if defined(control_pd_EXPORTS)
31 #define ControlPD_EXPORT __declspec(dllexport)
32 #else
33 #define ControlPD_EXPORT __declspec(dllimport)
34 #endif
35 #else
36 #define ControlPD_EXPORT
37 #endif
38 
39 namespace dynamicgraph {
40 namespace sot {
41 
42 /* --------------------------------------------------------------------- */
43 /* --- CLASS ----------------------------------------------------------- */
44 /* --------------------------------------------------------------------- */
45 
46 class ControlPD_EXPORT ControlPD : public Entity {
47 
48 public: /* --- CONSTRUCTOR ---- */
49  ControlPD(const std::string &name);
50 
51 public: /* --- INIT --- */
52  void init(const double &step);
53 
54 public: /* --- CONSTANTS --- */
55  /* Default values. */
56  static const double TIME_STEP_DEFAULT; // = 0.001
57 
58 public: /* --- ENTITY INHERITANCE --- */
59  static const std::string CLASS_NAME;
60  virtual void display(std::ostream &os) const;
61  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
62 
63 protected:
64  /* Parameters of the torque-control function:
65  * tau = kp * (qd-q) + kd* (dqd-dq) */
66  double TimeStep;
67 
68 public: /* --- SIGNALS --- */
69  SignalPtr<dynamicgraph::Vector, int> KpSIN;
70  SignalPtr<dynamicgraph::Vector, int> KdSIN;
71  SignalPtr<dynamicgraph::Vector, int> positionSIN;
72  SignalPtr<dynamicgraph::Vector, int> desiredpositionSIN;
73  SignalPtr<dynamicgraph::Vector, int> velocitySIN;
74  SignalPtr<dynamicgraph::Vector, int> desiredvelocitySIN;
75  SignalTimeDependent<dynamicgraph::Vector, int> controlSOUT;
76  SignalTimeDependent<dynamicgraph::Vector, int> positionErrorSOUT;
77  SignalTimeDependent<dynamicgraph::Vector, int> velocityErrorSOUT;
78 
79 protected:
80  dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t);
81  dynamicgraph::Vector position_error_;
82  dynamicgraph::Vector velocity_error_;
83  dynamicgraph::Vector &getPositionError(dynamicgraph::Vector &position_error,
84  int t);
85  dynamicgraph::Vector &getVelocityError(dynamicgraph::Vector &velocity_error,
86  int t);
87 };
88 
89 } // namespace sot
90 } // namespace dynamicgraph
91 
92 #endif // #ifndef __SOT_Control_PD_HH__
double TimeStep
Definition: control-pd.hh:66
dynamicgraph::Vector velocity_error_
Definition: control-pd.hh:82
static const double TIME_STEP_DEFAULT
Definition: control-pd.hh:56
SignalTimeDependent< dynamicgraph::Vector, int > velocityErrorSOUT
Definition: control-pd.hh:77
SignalPtr< dynamicgraph::Vector, int > KpSIN
Definition: control-pd.hh:69
SignalPtr< dynamicgraph::Vector, int > desiredvelocitySIN
Definition: control-pd.hh:74
SignalPtr< dynamicgraph::Vector, int > desiredpositionSIN
Definition: control-pd.hh:72
Definition: control-pd.hh:46
#define ControlPD_EXPORT
Definition: control-pd.hh:36
SignalPtr< dynamicgraph::Vector, int > velocitySIN
Definition: control-pd.hh:73
SignalPtr< dynamicgraph::Vector, int > KdSIN
Definition: control-pd.hh:70
SignalTimeDependent< dynamicgraph::Vector, int > controlSOUT
Definition: control-pd.hh:75
SignalTimeDependent< dynamicgraph::Vector, int > positionErrorSOUT
Definition: control-pd.hh:76
static const std::string CLASS_NAME
Definition: control-pd.hh:59
dynamicgraph::Vector position_error_
Definition: control-pd.hh:81
virtual const std::string & getClassName(void) const
Definition: control-pd.hh:61
Definition: abstract-sot-external-interface.hh:17
SignalPtr< dynamicgraph::Vector, int > positionSIN
Definition: control-pd.hh:71