GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/control/control-pd.cpp Lines: 45 51 88.2 %
Date: 2023-03-13 12:09:37 Branches: 68 142 47.9 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * François Bleibel,
4
 * Olivier Stasse,
5
 *
6
 * CNRS/AIST
7
 *
8
 */
9
10
/* SOT */
11
#include <sot/core/control-pd.hh>
12
13
/* --------------------------------------------------------------------- */
14
/* --------------------------------------------------------------------- */
15
/* --------------------------------------------------------------------- */
16
#include <dynamic-graph/factory.h>
17
18
#include <sot/core/debug.hh>
19
20
using namespace dynamicgraph::sot;
21
using namespace dynamicgraph;
22
23
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ControlPD, "ControlPD");
24
25
const double ControlPD::TIME_STEP_DEFAULT = .001;
26
27
/* --------------------------------------------------------------------- */
28
/* --------------------------------------------------------------------- */
29
/* --------------------------------------------------------------------- */
30
31
#define __SOT_ControlPD_INIT
32
33
1
ControlPD::ControlPD(const std::string &name)
34
    : Entity(name),
35
      TimeStep(0),
36
2
      KpSIN(NULL, "ControlPD(" + name + ")::input(vector)::Kp"),
37
2
      KdSIN(NULL, "ControlPD(" + name + ")::input(vector)::Kd"),
38
2
      positionSIN(NULL, "ControlPD(" + name + ")::input(vector)::position"),
39
      desiredpositionSIN(
40
2
          NULL, "ControlPD(" + name + ")::input(vector)::desired_position"),
41
2
      velocitySIN(NULL, "ControlPD(" + name + ")::input(vector)::velocity"),
42
      desiredvelocitySIN(
43
2
          NULL, "ControlPD(" + name + ")::input(vector)::desired_velocity"),
44
      controlSOUT(boost::bind(&ControlPD::computeControl, this, _1, _2),
45

1
                  KpSIN << KdSIN << positionSIN << desiredpositionSIN
46

1
                        << velocitySIN << desiredvelocitySIN,
47
2
                  "ControlPD(" + name + ")::output(vector)::control"),
48
      positionErrorSOUT(
49
          boost::bind(&ControlPD::getPositionError, this, _1, _2), controlSOUT,
50
2
          "ControlPD(" + name + ")::output(vector)::position_error"),
51
      velocityErrorSOUT(
52
          boost::bind(&ControlPD::getVelocityError, this, _1, _2), controlSOUT,
53














17
          "ControlPD(" + name + ")::output(vector)::velocity_error") {
54
1
  init(TimeStep);
55

2
  Entity::signalRegistration(KpSIN << KdSIN << positionSIN << desiredpositionSIN
56

1
                                   << velocitySIN << desiredvelocitySIN
57

1
                                   << controlSOUT << positionErrorSOUT
58

1
                                   << velocityErrorSOUT);
59
1
}
60
61
2
void ControlPD::init(const double &Stept) {
62
2
  TimeStep = Stept;
63
64
2
  return;
65
}
66
67
/* --------------------------------------------------------------------- */
68
/* --------------------------------------------------------------------- */
69
/* --------------------------------------------------------------------- */
70
71
void ControlPD::display(std::ostream &os) const {
72
  os << "ControlPD " << getName();
73
  try {
74
    os << "control = " << controlSOUT;
75
  } catch (ExceptionSignal e) {
76
  }
77
  os << " (" << TimeStep << ") ";
78
}
79
80
/* --------------------------------------------------------------------- */
81
/* --------------------------------------------------------------------- */
82
/* --------------------------------------------------------------------- */
83
84
1
dynamicgraph::Vector &ControlPD::computeControl(dynamicgraph::Vector &tau,
85
                                                int t) {
86
  sotDEBUGIN(15);
87
1
  const dynamicgraph::Vector &Kp = KpSIN(t);
88
1
  const dynamicgraph::Vector &Kd = KdSIN(t);
89
1
  const dynamicgraph::Vector &position = positionSIN(t);
90
1
  const dynamicgraph::Vector &desired_position = desiredpositionSIN(t);
91
1
  const dynamicgraph::Vector &velocity = velocitySIN(t);
92
1
  const dynamicgraph::Vector &desired_velocity = desiredvelocitySIN(t);
93
94
1
  dynamicgraph::Vector::Index size = Kp.size();
95
1
  tau.resize(size);
96
1
  position_error_.resize(size);
97
1
  velocity_error_.resize(size);
98
99


1
  position_error_.array() = desired_position.array() - position.array();
100


1
  velocity_error_.array() = desired_velocity.array() - velocity.array();
101
102


1
  tau.array() = position_error_.array() * Kp.array() +
103

2
                velocity_error_.array() * Kd.array();
104
105
  sotDEBUGOUT(15);
106
1
  return tau;
107
}
108
109
1
dynamicgraph::Vector &ControlPD::getPositionError(
110
    dynamicgraph::Vector &position_error, int t) {
111
  // sotDEBUGOUT(15) ??
112
1
  controlSOUT(t);
113
1
  position_error = position_error_;
114
1
  return position_error;
115
}
116
117
1
dynamicgraph::Vector &ControlPD::getVelocityError(
118
    dynamicgraph::Vector &velocity_error, int t) {
119
1
  controlSOUT(t);
120
1
  velocity_error = velocity_error_;
121
1
  return velocity_error;
122
}