Directory: | ./ |
---|---|
File: | src/control/admittance-control-op-point.cpp |
Date: | 2024-08-13 12:13:25 |
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 |