GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/core/control-pd.hh Lines: 0 1 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 0 - %

Line Branch Exec Source
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
 public: /* --- CONSTRUCTOR ---- */
48
  ControlPD(const std::string &name);
49
50
 public: /* --- INIT --- */
51
  void init(const double &step);
52
53
 public: /* --- CONSTANTS --- */
54
  /* Default values. */
55
  static const double TIME_STEP_DEFAULT;  // = 0.001
56
57
 public: /* --- ENTITY INHERITANCE --- */
58
  static const std::string CLASS_NAME;
59
  virtual void display(std::ostream &os) const;
60
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
61
62
 protected:
63
  /* Parameters of the torque-control function:
64
   * tau = kp * (qd-q) + kd* (dqd-dq) */
65
  double TimeStep;
66
67
 public: /* --- SIGNALS --- */
68
  SignalPtr<dynamicgraph::Vector, int> KpSIN;
69
  SignalPtr<dynamicgraph::Vector, int> KdSIN;
70
  SignalPtr<dynamicgraph::Vector, int> positionSIN;
71
  SignalPtr<dynamicgraph::Vector, int> desiredpositionSIN;
72
  SignalPtr<dynamicgraph::Vector, int> velocitySIN;
73
  SignalPtr<dynamicgraph::Vector, int> desiredvelocitySIN;
74
  SignalTimeDependent<dynamicgraph::Vector, int> controlSOUT;
75
  SignalTimeDependent<dynamicgraph::Vector, int> positionErrorSOUT;
76
  SignalTimeDependent<dynamicgraph::Vector, int> velocityErrorSOUT;
77
78
 protected:
79
  dynamicgraph::Vector &computeControl(dynamicgraph::Vector &tau, int t);
80
  dynamicgraph::Vector position_error_;
81
  dynamicgraph::Vector velocity_error_;
82
  dynamicgraph::Vector &getPositionError(dynamicgraph::Vector &position_error,
83
                                         int t);
84
  dynamicgraph::Vector &getVelocityError(dynamicgraph::Vector &velocity_error,
85
                                         int t);
86
};
87
88
}  // namespace sot
89
}  // namespace dynamicgraph
90
91
#endif  // #ifndef __SOT_Control_PD_HH__