GCC Code Coverage Report

Directory: ./
File: src/control/admittance-control-op-point.cpp
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 75 96 78.1%
Branches: 169 404 41.8%
Line Branch Exec Source
1 /*
2 * Copyright 2019
3 *
4 * LAAS-CNRS
5 *
6 * Noëlie Ramuzat
7 * This file is part of sot-core.
8 * See license file.
9 */
10
11 #include "sot/core/admittance-control-op-point.hh"
12
13 #include <dynamic-graph/all-commands.h>
14 #include <dynamic-graph/factory.h>
15
16 #include <sot/core/debug.hh>
17 #include <sot/core/stop-watch.hh>
18
19 namespace dynamicgraph {
20 namespace sot {
21 namespace core {
22 namespace dg = ::dynamicgraph;
23 using namespace dg;
24 using namespace pinocchio;
25 using namespace dg::command;
26
27 #define PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION \
28 "AdmittanceControlOpPoint: w_force computation "
29
30 #define PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION \
31 "AdmittanceControlOpPoint: w_dq computation "
32
33 #define PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION \
34 "AdmittanceControlOpPoint: dq computation "
35
36 #define INPUT_SIGNALS \
37 m_KpSIN << m_KdSIN << m_dqSaturationSIN << m_forceSIN << m_w_forceDesSIN \
38 << m_opPoseSIN << m_sensorPoseSIN
39
40 #define INNER_SIGNALS m_w_forceSINNER << m_w_dqSINNER
41
42 #define OUTPUT_SIGNALS m_dqSOUT
43
44 /// Define EntityClassName here rather than in the header file
45 /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
46 typedef AdmittanceControlOpPoint EntityClassName;
47
48 /* --- DG FACTORY ---------------------------------------------------- */
49 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControlOpPoint,
50 "AdmittanceControlOpPoint");
51
52 /* ------------------------------------------------------------------- */
53 /* --- CONSTRUCTION -------------------------------------------------- */
54 /* ------------------------------------------------------------------- */
55 1 AdmittanceControlOpPoint::AdmittanceControlOpPoint(const std::string &name)
56 : Entity(name),
57
7/14
✓ 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.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 1 CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
58
7/14
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector),
59
7/14
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 CONSTRUCT_SIGNAL_IN(dqSaturation, dynamicgraph::Vector),
60
7/14
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 CONSTRUCT_SIGNAL_IN(force, dynamicgraph::Vector),
61
7/14
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 CONSTRUCT_SIGNAL_IN(w_forceDes, dynamicgraph::Vector),
62
7/14
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 CONSTRUCT_SIGNAL_IN(opPose, dynamicgraph::sot::MatrixHomogeneous),
63
7/14
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 CONSTRUCT_SIGNAL_IN(sensorPose, dynamicgraph::sot::MatrixHomogeneous),
64
10/20
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
 1 CONSTRUCT_SIGNAL_INNER(w_force, dynamicgraph::Vector, m_forceSIN),
65
16/32
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 1 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 1 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
 1 CONSTRUCT_SIGNAL_INNER(w_dq, dynamicgraph::Vector,
66 INPUT_SIGNALS << m_w_forceSINNER),
67
10/20
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
 1 CONSTRUCT_SIGNAL_OUT(dq, dynamicgraph::Vector, m_w_dqSINNER),
68
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 m_initSucceeded(false) {
69
10/20
✓ 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.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
 1 Entity::signalRegistration(INPUT_SIGNALS << INNER_SIGNALS << OUTPUT_SIGNALS);
70
71 /* Commands. */
72
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 addCommand("init", makeCommandVoid1(*this, &AdmittanceControlOpPoint::init,
73
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 2 docCommandVoid1("Initialize the entity.",
74 "time step")));
75
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 addCommand("resetDq",
76
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 makeCommandVoid0(*this, &AdmittanceControlOpPoint::resetDq,
77
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 docCommandVoid0("resetDq")));
78 1 }
79
80 1 void AdmittanceControlOpPoint::init(const double &dt) {
81
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!m_dqSaturationSIN.isPlugged())
82 ✗ return SEND_MSG("Init failed: signal dqSaturation is not plugged",
83 MSG_TYPE_ERROR);
84
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!m_KpSIN.isPlugged())
85 ✗ return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
86
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!m_KdSIN.isPlugged())
87 ✗ return SEND_MSG("Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
88
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!m_forceSIN.isPlugged())
89 ✗ return SEND_MSG("Init failed: signal force is not plugged", MSG_TYPE_ERROR);
90
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!m_w_forceDesSIN.isPlugged())
91 ✗ return SEND_MSG("Init failed: signal w_forceDes is not plugged",
92 MSG_TYPE_ERROR);
93
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!m_opPoseSIN.isPlugged())
94 ✗ return SEND_MSG("Init failed: signal opPose is not plugged",
95 MSG_TYPE_ERROR);
96
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!m_sensorPoseSIN.isPlugged())
97 ✗ return SEND_MSG("Init failed: signal sensorPose is not plugged",
98 MSG_TYPE_ERROR);
99
100 1 m_n = 6;
101 1 m_dt = dt;
102 1 m_w_dq.setZero(m_n);
103 1 m_initSucceeded = true;
104 }
105
106 ✗ void AdmittanceControlOpPoint::resetDq() {
107 ✗ m_w_dq.setZero(m_n);
108 ✗ return;
109 }
110
111 /* ------------------------------------------------------------------- */
112 /* --- SIGNALS ------------------------------------------------------- */
113 /* ------------------------------------------------------------------- */
114 1 DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector) {
115
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (!m_initSucceeded) {
116 ✗ SEND_WARNING_STREAM_MSG(
117 "Cannot compute signal w_force before initialization!");
118 ✗ return s;
119 }
120
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 if (s.size() != 6) s.resize(6);
121
122
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION);
123
124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Vector &force = m_forceSIN(iter);
125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const MatrixHomogeneous &sensorPose = m_sensorPoseSIN(iter);
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(force.size() == m_n && "Unexpected size of signal force");
127 pinocchio::SE3 sensorPlacement(
128
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 sensorPose.matrix()); // homogeneous matrix to SE3
129
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 s = sensorPlacement.act(pinocchio::Force(force)).toVector();
130
131
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION);
132
133 1 return s;
134 }
135
136 1 DEFINE_SIGNAL_INNER_FUNCTION(w_dq, dynamicgraph::Vector) {
137
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (!m_initSucceeded) {
138 ✗ SEND_WARNING_STREAM_MSG(
139 "Cannot compute signal w_dq before initialization!");
140 ✗ return s;
141 }
142
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 if (s.size() != 6) s.resize(6);
143
144
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION);
145
146 1 const Vector &w_forceDes = m_w_forceDesSIN(iter);
147 1 const Vector &w_force = m_w_forceSINNER(iter);
148 1 const Vector &Kp = m_KpSIN(iter);
149 1 const Vector &Kd = m_KdSIN(iter);
150 1 const Vector &dqSaturation = m_dqSaturationSIN(iter);
151
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(w_force.size() == m_n && "Unexpected size of signal force");
152
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(w_forceDes.size() == m_n && "Unexpected size of signal w_forceDes");
153
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(Kp.size() == m_n && "Unexpected size of signal Kp");
154
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(Kd.size() == m_n && "Unexpected size of signal Kd");
155
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(dqSaturation.size() == m_n &&
156 "Unexpected size of signal dqSaturation");
157
158
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 m_w_dq = m_w_dq + m_dt * (Kp.cwiseProduct(w_forceDes - w_force)) -
159
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 Kd.cwiseProduct(m_w_dq);
160
161
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (int i = 0; i < m_n; i++) {
162
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
 6 if (m_w_dq[i] > dqSaturation[i]) m_w_dq[i] = dqSaturation[i];
163
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
 6 if (m_w_dq[i] < -dqSaturation[i]) m_w_dq[i] = -dqSaturation[i];
164 }
165
166 1 s = m_w_dq;
167
168
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION);
169
170 1 return s;
171 }
172
173 1 DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector) {
174
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (!m_initSucceeded) {
175 ✗ SEND_WARNING_STREAM_MSG("Cannot compute signal dq before initialization!");
176 ✗ return s;
177 }
178
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 if (s.size() != 6) s.resize(6);
179
180
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION);
181
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Vector &w_dq = m_w_dqSINNER(iter);
183
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const MatrixHomogeneous &opPose = m_opPoseSIN(iter);
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(w_dq.size() == m_n && "Unexpected size of signal w_dq");
185 pinocchio::SE3 opPointPlacement(
186
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 opPose.matrix()); // homogeneous matrix to SE3
187
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 s = opPointPlacement.actInv(pinocchio::Motion(w_dq)).toVector();
188
189
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION);
190
191 1 return s;
192 }
193
194 /* --- COMMANDS ---------------------------------------------------------- */
195
196 /* ------------------------------------------------------------------- */
197 /* --- ENTITY -------------------------------------------------------- */
198 /* ------------------------------------------------------------------- */
199 ✗ void AdmittanceControlOpPoint::display(std::ostream &os) const {
200 ✗ os << "AdmittanceControlOpPoint " << getName();
201 try {
202 ✗ getProfiler().report_all(3, os);
203 ✗ } catch (ExceptionSignal e) {
204 }
205 }
206 } // namespace core
207 } // namespace sot
208 } // namespace dynamicgraph
209