| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector), |
58 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector), |
59 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(dqSaturation, dynamicgraph::Vector), |
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(force, dynamicgraph::Vector), |
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(w_forceDes, dynamicgraph::Vector), |
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(opPose, dynamicgraph::sot::MatrixHomogeneous), |
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_IN(sensorPose, dynamicgraph::sot::MatrixHomogeneous), |
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_INNER(w_force, dynamicgraph::Vector, m_forceSIN), |
65 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
CONSTRUCT_SIGNAL_INNER(w_dq, dynamicgraph::Vector, |
66 |
INPUT_SIGNALS << m_w_forceSINNER), |
||
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
CONSTRUCT_SIGNAL_OUT(dq, dynamicgraph::Vector, m_w_dqSINNER), |
68 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
21 |
m_initSucceeded(false) { |
69 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1 |
Entity::signalRegistration(INPUT_SIGNALS << INNER_SIGNALS << OUTPUT_SIGNALS); |
70 |
|||
71 |
/* Commands. */ |
||
72 |
✓✗✓✗ ✓✗ |
1 |
addCommand("init", makeCommandVoid1(*this, &AdmittanceControlOpPoint::init, |
73 |
✓✗✓✗ ✓✗ |
2 |
docCommandVoid1("Initialize the entity.", |
74 |
"time step"))); |
||
75 |
✓✗✓✗ |
1 |
addCommand("resetDq", |
76 |
✓✗ | 1 |
makeCommandVoid0(*this, &AdmittanceControlOpPoint::resetDq, |
77 |
✓✗✓✗ |
2 |
docCommandVoid0("resetDq"))); |
78 |
1 |
} |
|
79 |
|||
80 |
1 |
void AdmittanceControlOpPoint::init(const double &dt) { |
|
81 |
✗✓ | 1 |
if (!m_dqSaturationSIN.isPlugged()) |
82 |
return SEND_MSG("Init failed: signal dqSaturation is not plugged", |
||
83 |
MSG_TYPE_ERROR); |
||
84 |
✗✓ | 1 |
if (!m_KpSIN.isPlugged()) |
85 |
return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR); |
||
86 |
✗✓ | 1 |
if (!m_KdSIN.isPlugged()) |
87 |
return SEND_MSG("Init failed: signal Kd is not plugged", MSG_TYPE_ERROR); |
||
88 |
✗✓ | 1 |
if (!m_forceSIN.isPlugged()) |
89 |
return SEND_MSG("Init failed: signal force is not plugged", MSG_TYPE_ERROR); |
||
90 |
✗✓ | 1 |
if (!m_w_forceDesSIN.isPlugged()) |
91 |
return SEND_MSG("Init failed: signal w_forceDes is not plugged", |
||
92 |
MSG_TYPE_ERROR); |
||
93 |
✗✓ | 1 |
if (!m_opPoseSIN.isPlugged()) |
94 |
return SEND_MSG("Init failed: signal opPose is not plugged", |
||
95 |
MSG_TYPE_ERROR); |
||
96 |
✗✓ | 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 |
if (!m_initSucceeded) { |
116 |
SEND_WARNING_STREAM_MSG( |
||
117 |
"Cannot compute signal w_force before initialization!"); |
||
118 |
return s; |
||
119 |
} |
||
120 |
✓✗✓✗ ✓✗ |
1 |
if (s.size() != 6) s.resize(6); |
121 |
|||
122 |
✓✗✓✗ ✓✗ |
1 |
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION); |
123 |
|||
124 |
✓✗ | 1 |
const Vector &force = m_forceSIN(iter); |
125 |
✓✗ | 1 |
const MatrixHomogeneous &sensorPose = m_sensorPoseSIN(iter); |
126 |
✓✗✓✗ |
1 |
assert(force.size() == m_n && "Unexpected size of signal force"); |
127 |
pinocchio::SE3 sensorPlacement( |
||
128 |
✓✗ | 1 |
sensorPose.matrix()); // homogeneous matrix to SE3 |
129 |
✓✗✓✗ ✓✗✓✗ |
1 |
s = sensorPlacement.act(pinocchio::Force(force)).toVector(); |
130 |
|||
131 |
✓✗✓✗ ✓✗ |
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 |
if (!m_initSucceeded) { |
138 |
SEND_WARNING_STREAM_MSG( |
||
139 |
"Cannot compute signal w_dq before initialization!"); |
||
140 |
return s; |
||
141 |
} |
||
142 |
✓✗ | 1 |
if (s.size() != 6) s.resize(6); |
143 |
|||
144 |
✓✗✓✗ |
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 |
assert(w_force.size() == m_n && "Unexpected size of signal force"); |
152 |
✓✗ | 1 |
assert(w_forceDes.size() == m_n && "Unexpected size of signal w_forceDes"); |
153 |
✓✗ | 1 |
assert(Kp.size() == m_n && "Unexpected size of signal Kp"); |
154 |
✓✗ | 1 |
assert(Kd.size() == m_n && "Unexpected size of signal Kd"); |
155 |
✓✗ | 1 |
assert(dqSaturation.size() == m_n && |
156 |
"Unexpected size of signal dqSaturation"); |
||
157 |
|||
158 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
m_w_dq = m_w_dq + m_dt * (Kp.cwiseProduct(w_forceDes - w_force)) - |
159 |
✓✗ | 2 |
Kd.cwiseProduct(m_w_dq); |
160 |
|||
161 |
✓✓ | 7 |
for (int i = 0; i < m_n; i++) { |
162 |
✗✓ | 6 |
if (m_w_dq[i] > dqSaturation[i]) m_w_dq[i] = dqSaturation[i]; |
163 |
✗✓ | 6 |
if (m_w_dq[i] < -dqSaturation[i]) m_w_dq[i] = -dqSaturation[i]; |
164 |
} |
||
165 |
|||
166 |
1 |
s = m_w_dq; |
|
167 |
|||
168 |
✓✗✓✗ |
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 |
if (!m_initSucceeded) { |
175 |
SEND_WARNING_STREAM_MSG("Cannot compute signal dq before initialization!"); |
||
176 |
return s; |
||
177 |
} |
||
178 |
✓✗✓✗ ✓✗ |
1 |
if (s.size() != 6) s.resize(6); |
179 |
|||
180 |
✓✗✓✗ ✓✗ |
1 |
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION); |
181 |
|||
182 |
✓✗ | 1 |
const Vector &w_dq = m_w_dqSINNER(iter); |
183 |
✓✗ | 1 |
const MatrixHomogeneous &opPose = m_opPoseSIN(iter); |
184 |
✓✗✓✗ |
1 |
assert(w_dq.size() == m_n && "Unexpected size of signal w_dq"); |
185 |
pinocchio::SE3 opPointPlacement( |
||
186 |
✓✗ | 1 |
opPose.matrix()); // homogeneous matrix to SE3 |
187 |
✓✗✓✗ ✓✗✓✗ |
1 |
s = opPointPlacement.actInv(pinocchio::Motion(w_dq)).toVector(); |
188 |
|||
189 |
✓✗✓✗ ✓✗ |
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 |
| Generated by: GCOVR (Version 4.2) |