GCC Code Coverage Report

Directory: ./
File: src/control/control-pd.cpp
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 51 57 89.5%
Branches: 68 144 47.2%
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 1 TimeStep(0),
36
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 KpSIN(NULL, "ControlPD(" + name + ")::input(vector)::Kp"),
37
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 KdSIN(NULL, "ControlPD(" + name + ")::input(vector)::Kd"),
38
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 positionSIN(NULL, "ControlPD(" + name + ")::input(vector)::position"),
39
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 desiredpositionSIN(
40
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 NULL, "ControlPD(" + name + ")::input(vector)::desired_position"),
41
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 velocitySIN(NULL, "ControlPD(" + name + ")::input(vector)::velocity"),
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 desiredvelocitySIN(
43
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 NULL, "ControlPD(" + name + ")::input(vector)::desired_velocity"),
44
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 controlSOUT(boost::bind(&ControlPD::computeControl, this, _1, _2),
45
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 KpSIN << KdSIN << positionSIN << desiredpositionSIN
46
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 << velocitySIN << desiredvelocitySIN,
47
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 "ControlPD(" + name + ")::output(vector)::control"),
48
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 positionErrorSOUT(
49 boost::bind(&ControlPD::getPositionError, this, _1, _2), controlSOUT,
50
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 "ControlPD(" + name + ")::output(vector)::position_error"),
51
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 velocityErrorSOUT(
52 boost::bind(&ControlPD::getVelocityError, this, _1, _2), controlSOUT,
53
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 3 "ControlPD(" + name + ")::output(vector)::velocity_error") {
54
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 init(TimeStep);
55
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 Entity::signalRegistration(KpSIN << KdSIN << positionSIN << desiredpositionSIN
56
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 << velocitySIN << desiredvelocitySIN
57
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 << controlSOUT << positionErrorSOUT
58
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 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
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 position_error_.array() = desired_position.array() - position.array();
100
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 velocity_error_.array() = desired_velocity.array() - velocity.array();
101
102
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 tau.array() = position_error_.array() * Kp.array() +
103
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 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 }
123