GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
} |
Generated by: GCOVR (Version 4.2) |