1 |
|
|
/* |
2 |
|
|
* Copyright 2015, Andrea Del Prete, LAAS-CNRS |
3 |
|
|
* |
4 |
|
|
*/ |
5 |
|
|
|
6 |
|
|
#include <dynamic-graph/factory.h> |
7 |
|
|
|
8 |
|
|
#include <sot/core/debug.hh> |
9 |
|
|
#include <sot/torque_control/admittance-controller.hh> |
10 |
|
|
#include <sot/torque_control/commands-helper.hh> |
11 |
|
|
#include <tsid/utils/stop-watch.hpp> |
12 |
|
|
|
13 |
|
|
namespace dynamicgraph { |
14 |
|
|
namespace sot { |
15 |
|
|
namespace torque_control { |
16 |
|
|
namespace dg = ::dynamicgraph; |
17 |
|
|
using namespace dg; |
18 |
|
|
using namespace dg::command; |
19 |
|
|
using namespace std; |
20 |
|
|
using namespace Eigen; |
21 |
|
|
using namespace tsid; |
22 |
|
|
using namespace tsid::math; |
23 |
|
|
using namespace tsid::tasks; |
24 |
|
|
using namespace dg::sot; |
25 |
|
|
|
26 |
|
|
#define PROFILE_DQ_DES_COMPUTATION "Admittance control computation" |
27 |
|
|
|
28 |
|
|
#define REF_FORCE_SIGNALS m_fRightFootRefSIN << m_fLeftFootRefSIN |
29 |
|
|
// m_fRightHandRefSIN << m_fLeftHandRefSIN |
30 |
|
|
#define FORCE_SIGNALS \ |
31 |
|
|
m_fRightFootSIN << m_fLeftFootSIN << m_fRightFootFilteredSIN \ |
32 |
|
|
<< m_fLeftFootFilteredSIN |
33 |
|
|
// m_fRightHandSIN << m_fLeftHandSIN |
34 |
|
|
#define GAIN_SIGNALS \ |
35 |
|
|
m_kp_forceSIN << m_ki_forceSIN << m_kp_velSIN << m_ki_velSIN \ |
36 |
|
|
<< m_force_integral_saturationSIN \ |
37 |
|
|
<< m_force_integral_deadzoneSIN |
38 |
|
|
#define STATE_SIGNALS m_encodersSIN << m_jointsVelocitiesSIN |
39 |
|
|
|
40 |
|
|
#define INPUT_SIGNALS \ |
41 |
|
|
STATE_SIGNALS << REF_FORCE_SIGNALS << FORCE_SIGNALS << GAIN_SIGNALS \ |
42 |
|
|
<< m_controlledJointsSIN << m_dampingSIN |
43 |
|
|
|
44 |
|
|
#define DES_VEL_SIGNALS \ |
45 |
|
|
m_vDesRightFootSOUT << m_vDesLeftFootSOUT //<< m_fRightHandErrorSOUT << |
46 |
|
|
// m_fLeftHandErrorSOUT |
47 |
|
|
#define OUTPUT_SIGNALS m_uSOUT << m_dqDesSOUT << DES_VEL_SIGNALS |
48 |
|
|
|
49 |
|
|
/// Define EntityClassName here rather than in the header file |
50 |
|
|
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION. |
51 |
|
|
typedef AdmittanceController EntityClassName; |
52 |
|
|
|
53 |
|
|
typedef Eigen::Matrix<double, 3, 1> Vector3; |
54 |
|
|
typedef Eigen::Matrix<double, 6, 1> Vector6; |
55 |
|
|
/* --- DG FACTORY ---------------------------------------------------- */ |
56 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, |
57 |
|
|
"AdmittanceController"); |
58 |
|
|
|
59 |
|
|
/* ------------------------------------------------------------------- */ |
60 |
|
|
/* --- CONSTRUCTION -------------------------------------------------- */ |
61 |
|
|
/* ------------------------------------------------------------------- */ |
62 |
|
|
AdmittanceController::AdmittanceController(const std::string& name) |
63 |
|
|
: Entity(name), |
64 |
|
|
CONSTRUCT_SIGNAL_IN(encoders, dynamicgraph::Vector), |
65 |
|
|
CONSTRUCT_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector), |
66 |
|
|
CONSTRUCT_SIGNAL_IN(kp_force, dynamicgraph::Vector), |
67 |
|
|
CONSTRUCT_SIGNAL_IN(ki_force, dynamicgraph::Vector), |
68 |
|
|
CONSTRUCT_SIGNAL_IN(kp_vel, dynamicgraph::Vector), |
69 |
|
|
CONSTRUCT_SIGNAL_IN(ki_vel, dynamicgraph::Vector), |
70 |
|
|
CONSTRUCT_SIGNAL_IN(force_integral_saturation, dynamicgraph::Vector), |
71 |
|
|
CONSTRUCT_SIGNAL_IN(force_integral_deadzone, dynamicgraph::Vector), |
72 |
|
|
CONSTRUCT_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector), |
73 |
|
|
CONSTRUCT_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector), |
74 |
|
|
CONSTRUCT_SIGNAL_IN(fRightFoot, dynamicgraph::Vector), |
75 |
|
|
CONSTRUCT_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector), |
76 |
|
|
CONSTRUCT_SIGNAL_IN(fRightFootFiltered, dynamicgraph::Vector), |
77 |
|
|
CONSTRUCT_SIGNAL_IN(fLeftFootFiltered, dynamicgraph::Vector), |
78 |
|
|
CONSTRUCT_SIGNAL_IN(controlledJoints, dynamicgraph::Vector), |
79 |
|
|
CONSTRUCT_SIGNAL_IN(damping, dynamicgraph::Vector) |
80 |
|
|
// ,CONSTRUCT_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector) |
81 |
|
|
// ,CONSTRUCT_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector) |
82 |
|
|
// ,CONSTRUCT_SIGNAL_IN(fRightHand, dynamicgraph::Vector) |
83 |
|
|
// ,CONSTRUCT_SIGNAL_IN(fLeftHand, dynamicgraph::Vector) |
84 |
|
|
, |
85 |
|
|
CONSTRUCT_SIGNAL_OUT(u, dynamicgraph::Vector, |
86 |
|
|
STATE_SIGNALS << m_controlledJointsSIN), |
87 |
|
|
CONSTRUCT_SIGNAL_OUT(dqDes, dynamicgraph::Vector, |
88 |
|
|
STATE_SIGNALS << DES_VEL_SIGNALS << m_dampingSIN), |
89 |
|
|
CONSTRUCT_SIGNAL_OUT(vDesRightFoot, dynamicgraph::Vector, |
90 |
|
|
m_fRightFootSIN << m_fRightFootFilteredSIN |
91 |
|
|
<< m_fRightFootRefSIN |
92 |
|
|
<< GAIN_SIGNALS), |
93 |
|
|
CONSTRUCT_SIGNAL_OUT(vDesLeftFoot, dynamicgraph::Vector, |
94 |
|
|
m_fLeftFootSIN << m_fLeftFootFilteredSIN |
95 |
|
|
<< m_fLeftFootRefSIN << GAIN_SIGNALS) |
96 |
|
|
// ,CONSTRUCT_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector, |
97 |
|
|
// m_fRightHandSIN << |
98 |
|
|
// m_fRightHandRefSIN) |
99 |
|
|
// ,CONSTRUCT_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector, |
100 |
|
|
// m_fLeftHandSIN << |
101 |
|
|
// m_fLeftHandRefSIN) |
102 |
|
|
, |
103 |
|
|
m_firstIter(true), |
104 |
|
|
m_initSucceeded(false), |
105 |
|
|
m_useJacobianTranspose(true) { |
106 |
|
|
Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS); |
107 |
|
|
|
108 |
|
|
m_v_RF_int.setZero(); |
109 |
|
|
m_v_LF_int.setZero(); |
110 |
|
|
|
111 |
|
|
/* Commands. */ |
112 |
|
|
addCommand( |
113 |
|
|
"getUseJacobianTranspose", |
114 |
|
|
makeDirectGetter(*this, &m_useJacobianTranspose, |
115 |
|
|
docDirectGetter("If true it uses the Jacobian " |
116 |
|
|
"transpose, otherwise the pseudoinverse", |
117 |
|
|
"bool"))); |
118 |
|
|
addCommand( |
119 |
|
|
"setUseJacobianTranspose", |
120 |
|
|
makeDirectSetter(*this, &m_useJacobianTranspose, |
121 |
|
|
docDirectSetter("If true it uses the Jacobian " |
122 |
|
|
"transpose, otherwise the pseudoinverse", |
123 |
|
|
"bool"))); |
124 |
|
|
addCommand("init", |
125 |
|
|
makeCommandVoid2(*this, &AdmittanceController::init, |
126 |
|
|
docCommandVoid2("Initialize the entity.", |
127 |
|
|
"Time period in seconds (double)", |
128 |
|
|
"Robot name (string)"))); |
129 |
|
|
} |
130 |
|
|
|
131 |
|
|
void AdmittanceController::init(const double& dt, const std::string& robotRef) { |
132 |
|
|
if (dt <= 0.0) return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR); |
133 |
|
|
if (!m_encodersSIN.isPlugged()) |
134 |
|
|
return SEND_MSG("Init failed: signal encoders is not plugged", |
135 |
|
|
MSG_TYPE_ERROR); |
136 |
|
|
if (!m_jointsVelocitiesSIN.isPlugged()) |
137 |
|
|
return SEND_MSG("Init failed: signal jointsVelocities is not plugged", |
138 |
|
|
MSG_TYPE_ERROR); |
139 |
|
|
if (!m_controlledJointsSIN.isPlugged()) |
140 |
|
|
return SEND_MSG("Init failed: signal controlledJoints is not plugged", |
141 |
|
|
MSG_TYPE_ERROR); |
142 |
|
|
|
143 |
|
|
m_dt = dt; |
144 |
|
|
m_initSucceeded = true; |
145 |
|
|
|
146 |
|
|
/* Retrieve m_robot_util informations */ |
147 |
|
|
std::string localName(robotRef); |
148 |
|
|
if (isNameInRobotUtil(localName)) |
149 |
|
|
m_robot_util = getRobotUtil(localName); |
150 |
|
|
else |
151 |
|
|
return SEND_MSG( |
152 |
|
|
"You should have an entity controller manager initialized before", |
153 |
|
|
MSG_TYPE_ERROR); |
154 |
|
|
|
155 |
|
|
try { |
156 |
|
|
vector<string> package_dirs; |
157 |
|
|
m_robot = |
158 |
|
|
new robots::RobotWrapper(m_robot_util->m_urdf_filename, package_dirs, |
159 |
|
|
pinocchio::JointModelFreeFlyer()); |
160 |
|
|
m_data = new pinocchio::Data(m_robot->model()); |
161 |
|
|
|
162 |
|
|
assert(m_robot->nv() >= 6); |
163 |
|
|
m_robot_util->m_nbJoints = m_robot->nv() - 6; |
164 |
|
|
m_nj = m_robot->nv() - 6; |
165 |
|
|
m_u.setZero(m_nj); |
166 |
|
|
m_dq_des_urdf.setZero(m_nj); |
167 |
|
|
m_dqErrIntegral.setZero(m_nj); |
168 |
|
|
// m_dqDesIntegral.setZero(m_nj); |
169 |
|
|
|
170 |
|
|
m_q_urdf.setZero(m_robot->nq()); |
171 |
|
|
m_q_urdf(6) = 1.0; |
172 |
|
|
m_v_urdf.setZero(m_robot->nv()); |
173 |
|
|
m_J_RF.setZero(6, m_robot->nv()); |
174 |
|
|
m_J_LF.setZero(6, m_robot->nv()); |
175 |
|
|
|
176 |
|
|
m_frame_id_rf = (int)m_robot->model().getFrameId( |
177 |
|
|
m_robot_util->m_foot_util.m_Right_Foot_Frame_Name); |
178 |
|
|
m_frame_id_lf = (int)m_robot->model().getFrameId( |
179 |
|
|
m_robot_util->m_foot_util.m_Left_Foot_Frame_Name); |
180 |
|
|
|
181 |
|
|
} catch (const std::exception& e) { |
182 |
|
|
std::cout << e.what(); |
183 |
|
|
return SEND_MSG( |
184 |
|
|
"Init failed: Could load URDF :" + m_robot_util->m_urdf_filename, |
185 |
|
|
MSG_TYPE_ERROR); |
186 |
|
|
} |
187 |
|
|
} |
188 |
|
|
|
189 |
|
|
/* ------------------------------------------------------------------- */ |
190 |
|
|
/* --- SIGNALS ------------------------------------------------------- */ |
191 |
|
|
/* ------------------------------------------------------------------- */ |
192 |
|
|
|
193 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector) { |
194 |
|
|
if (!m_initSucceeded) { |
195 |
|
|
SEND_WARNING_STREAM_MSG("Cannot compute signal u before initialization!"); |
196 |
|
|
return s; |
197 |
|
|
} |
198 |
|
|
|
199 |
|
|
const Vector& dqDes = m_dqDesSOUT(iter); // n |
200 |
|
|
const Vector& q = m_encodersSIN(iter); // n |
201 |
|
|
const Vector& dq = m_jointsVelocitiesSIN(iter); // n |
202 |
|
|
const Vector& kp = m_kp_velSIN(iter); |
203 |
|
|
const Vector& ki = m_ki_velSIN(iter); |
204 |
|
|
|
205 |
|
|
if (m_firstIter) { |
206 |
|
|
m_qPrev = q; |
207 |
|
|
m_firstIter = false; |
208 |
|
|
} |
209 |
|
|
|
210 |
|
|
// estimate joint velocities using finite differences |
211 |
|
|
m_dq_fd = (q - m_qPrev) / m_dt; |
212 |
|
|
m_qPrev = q; |
213 |
|
|
|
214 |
|
|
m_dqErrIntegral += m_dt * ki.cwiseProduct(dqDes - m_dq_fd); |
215 |
|
|
s = kp.cwiseProduct(dqDes - dq) + m_dqErrIntegral; |
216 |
|
|
|
217 |
|
|
return s; |
218 |
|
|
} |
219 |
|
|
|
220 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(dqDes, dynamicgraph::Vector) { |
221 |
|
|
if (!m_initSucceeded) { |
222 |
|
|
SEND_WARNING_STREAM_MSG( |
223 |
|
|
"Cannot compute signal dqDes before initialization!"); |
224 |
|
|
return s; |
225 |
|
|
} |
226 |
|
|
|
227 |
|
|
getProfiler().start(PROFILE_DQ_DES_COMPUTATION); |
228 |
|
|
{ |
229 |
|
|
const dg::sot::Vector6d v_des_LF = m_vDesLeftFootSOUT(iter); |
230 |
|
|
const dg::sot::Vector6d v_des_RF = m_vDesRightFootSOUT(iter); |
231 |
|
|
const Vector& q_sot = m_encodersSIN(iter); // n |
232 |
|
|
// const Vector& dq_sot = m_jointsVelocitiesSIN(iter); // |
233 |
|
|
// n |
234 |
|
|
// const Vector& qMask = m_controlledJointsSIN(iter); // n |
235 |
|
|
// const Eigen::Vector4d& damping = m_dampingSIN(iter); // 4 |
236 |
|
|
|
237 |
|
|
assert(q_sot.size() == m_nj && "Unexpected size of signal encoder"); |
238 |
|
|
|
239 |
|
|
/// *** Compute all Jacobians *** |
240 |
|
|
m_robot_util->joints_sot_to_urdf(q_sot, m_q_urdf.tail(m_nj)); |
241 |
|
|
m_robot->computeAllTerms(*m_data, m_q_urdf, m_v_urdf); |
242 |
|
|
m_robot->frameJacobianLocal(*m_data, m_frame_id_rf, m_J_RF); |
243 |
|
|
m_robot->frameJacobianLocal(*m_data, m_frame_id_lf, m_J_LF); |
244 |
|
|
|
245 |
|
|
// SEND_INFO_STREAM_MSG("RFoot Jacobian :" + |
246 |
|
|
// toString(m_J_RF.rightCols(m_nj))); set to zero columns of Jacobian |
247 |
|
|
// corresponding to unactive joints |
248 |
|
|
// for(int i=0; i<m_nj; i++) |
249 |
|
|
// if(qMask(i)==0) |
250 |
|
|
// m_J_all.col(6+i).setZero(); |
251 |
|
|
|
252 |
|
|
/// Compute admittance control law |
253 |
|
|
if (m_useJacobianTranspose) { |
254 |
|
|
m_dq_des_urdf = m_J_RF.rightCols(m_nj).transpose() * v_des_RF; |
255 |
|
|
m_dq_des_urdf += m_J_LF.rightCols(m_nj).transpose() * v_des_LF; |
256 |
|
|
} else { |
257 |
|
|
m_J_RF_QR.compute(m_J_RF.rightCols(m_nj)); |
258 |
|
|
m_J_LF_QR.compute(m_J_LF.rightCols(m_nj)); |
259 |
|
|
|
260 |
|
|
m_dq_des_urdf = m_J_RF_QR.solve(v_des_RF); |
261 |
|
|
m_dq_des_urdf += m_J_LF_QR.solve(v_des_LF); |
262 |
|
|
} |
263 |
|
|
|
264 |
|
|
if (s.size() != m_nj) s.resize(m_nj); |
265 |
|
|
|
266 |
|
|
m_robot_util->joints_urdf_to_sot(m_dq_des_urdf, s); |
267 |
|
|
} |
268 |
|
|
getProfiler().stop(PROFILE_DQ_DES_COMPUTATION); |
269 |
|
|
|
270 |
|
|
return s; |
271 |
|
|
} |
272 |
|
|
|
273 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(vDesRightFoot, dynamicgraph::Vector) { |
274 |
|
|
if (!m_initSucceeded) { |
275 |
|
|
SEND_MSG("Cannot compute signal vDesRightFoot before initialization!", |
276 |
|
|
MSG_TYPE_WARNING_STREAM); |
277 |
|
|
return s; |
278 |
|
|
} |
279 |
|
|
const Vector6& f = m_fRightFootSIN(iter); |
280 |
|
|
const Vector6& f_filt = m_fRightFootFilteredSIN(iter); |
281 |
|
|
const Vector6& fRef = m_fRightFootRefSIN(iter); |
282 |
|
|
const Vector6d& kp = m_kp_forceSIN(iter); |
283 |
|
|
const Vector6d& ki = m_ki_forceSIN(iter); |
284 |
|
|
const Vector6d& f_sat = m_force_integral_saturationSIN(iter); |
285 |
|
|
const Vector6d& dz = m_force_integral_deadzoneSIN(iter); |
286 |
|
|
|
287 |
|
|
dg::sot::Vector6d err = fRef - f; |
288 |
|
|
dg::sot::Vector6d err_filt = fRef - f_filt; |
289 |
|
|
dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt); |
290 |
|
|
|
291 |
|
|
for (int i = 0; i < 6; i++) { |
292 |
|
|
if (err(i) > dz(i)) |
293 |
|
|
m_v_RF_int(i) -= m_dt * ki(i) * (err(i) - dz(i)); |
294 |
|
|
else if (err(i) < -dz(i)) |
295 |
|
|
m_v_RF_int(i) -= m_dt * ki(i) * (err(i) + dz(i)); |
296 |
|
|
} |
297 |
|
|
|
298 |
|
|
// saturate |
299 |
|
|
bool saturating = false; |
300 |
|
|
for (int i = 0; i < 6; i++) { |
301 |
|
|
if (m_v_RF_int(i) > f_sat(i)) { |
302 |
|
|
saturating = true; |
303 |
|
|
m_v_RF_int(i) = f_sat(i); |
304 |
|
|
} else if (m_v_RF_int(i) < -f_sat(i)) { |
305 |
|
|
saturating = true; |
306 |
|
|
m_v_RF_int(i) = -f_sat(i); |
307 |
|
|
} |
308 |
|
|
} |
309 |
|
|
if (saturating) |
310 |
|
|
SEND_INFO_STREAM_MSG("Saturate m_v_RF_int integral: " + |
311 |
|
|
toString(m_v_RF_int.transpose())); |
312 |
|
|
s = v_des + m_v_RF_int; |
313 |
|
|
return s; |
314 |
|
|
} |
315 |
|
|
|
316 |
|
|
DEFINE_SIGNAL_OUT_FUNCTION(vDesLeftFoot, dynamicgraph::Vector) { |
317 |
|
|
if (!m_initSucceeded) { |
318 |
|
|
SEND_MSG("Cannot compute signal vDesLeftFoot before initialization!", |
319 |
|
|
MSG_TYPE_WARNING_STREAM); |
320 |
|
|
return s; |
321 |
|
|
} |
322 |
|
|
const Vector6& f = m_fLeftFootSIN(iter); |
323 |
|
|
const Vector6& f_filt = m_fLeftFootFilteredSIN(iter); |
324 |
|
|
const Vector6& fRef = m_fLeftFootRefSIN(iter); |
325 |
|
|
const Vector6d& kp = m_kp_forceSIN(iter); |
326 |
|
|
const Vector6d& ki = m_ki_forceSIN(iter); |
327 |
|
|
const Vector6d& f_sat = m_force_integral_saturationSIN(iter); |
328 |
|
|
const Vector6d& dz = m_force_integral_deadzoneSIN(iter); |
329 |
|
|
|
330 |
|
|
dg::sot::Vector6d err = fRef - f; |
331 |
|
|
dg::sot::Vector6d err_filt = fRef - f_filt; |
332 |
|
|
dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt); |
333 |
|
|
|
334 |
|
|
for (int i = 0; i < 6; i++) { |
335 |
|
|
if (err(i) > dz(i)) |
336 |
|
|
m_v_LF_int(i) -= m_dt * ki(i) * (err(i) - dz(i)); |
337 |
|
|
else if (err(i) < -dz(i)) |
338 |
|
|
m_v_LF_int(i) -= m_dt * ki(i) * (err(i) + dz(i)); |
339 |
|
|
} |
340 |
|
|
|
341 |
|
|
// saturate |
342 |
|
|
bool saturating = false; |
343 |
|
|
for (int i = 0; i < 6; i++) { |
344 |
|
|
if (m_v_LF_int(i) > f_sat(i)) { |
345 |
|
|
saturating = true; |
346 |
|
|
m_v_LF_int(i) = f_sat(i); |
347 |
|
|
} else if (m_v_LF_int(i) < -f_sat(i)) { |
348 |
|
|
saturating = true; |
349 |
|
|
m_v_LF_int(i) = -f_sat(i); |
350 |
|
|
} |
351 |
|
|
} |
352 |
|
|
if (saturating) |
353 |
|
|
SEND_INFO_STREAM_MSG("Saturate m_v_LF_int integral: " + |
354 |
|
|
toString(m_v_LF_int.transpose())); |
355 |
|
|
s = v_des + m_v_LF_int; |
356 |
|
|
return s; |
357 |
|
|
} |
358 |
|
|
|
359 |
|
|
// DEFINE_SIGNAL_OUT_FUNCTION(fRightHandError,dynamicgraph::Vector) |
360 |
|
|
// { |
361 |
|
|
// if(!m_initSucceeded) |
362 |
|
|
// { |
363 |
|
|
// SEND_MSG("Cannot compute signal fRightHandError before |
364 |
|
|
// initialization!", MSG_TYPE_WARNING_STREAM); return s; |
365 |
|
|
// } |
366 |
|
|
|
367 |
|
|
// const Eigen::Matrix<double,24,1>& Kf = m_KfSIN(iter); // 6*4 |
368 |
|
|
// const Vector6& f = m_fRightHandSIN(iter); // 6 |
369 |
|
|
// const Vector6& fRef = m_fRightHandRefSIN(iter); // 6 |
370 |
|
|
// assert(f.size()==6 && "Unexpected size of signal fRightHand"); |
371 |
|
|
// assert(fRef.size()==6 && "Unexpected size of signal fRightHandRef"); |
372 |
|
|
|
373 |
|
|
// if(s.size()!=6) |
374 |
|
|
// s.resize(6); |
375 |
|
|
// s.tail<3>() = Kf.segment<3>(12).cwiseProduct(fRef.head<3>() - |
376 |
|
|
// f.head<3>() ); s.head<3>() = |
377 |
|
|
// Kf.segment<3>(15).cwiseProduct(fRef.tail<3>() - f.tail<3>()); return |
378 |
|
|
// s; |
379 |
|
|
// } |
380 |
|
|
|
381 |
|
|
// DEFINE_SIGNAL_OUT_FUNCTION(fLeftHandError,dynamicgraph::Vector) |
382 |
|
|
// { |
383 |
|
|
// if(!m_initSucceeded) |
384 |
|
|
// { |
385 |
|
|
// SEND_MSG("Cannot compute signal fLeftHandError before |
386 |
|
|
// initialization!", MSG_TYPE_WARNING_STREAM); return s; |
387 |
|
|
// } |
388 |
|
|
|
389 |
|
|
// const Eigen::Matrix<double,24,1>& Kf = m_KfSIN(iter); // 6*4 |
390 |
|
|
// const Vector6& f = m_fLeftHandSIN(iter); // 6 |
391 |
|
|
// const Vector6& fRef = m_fLeftHandRefSIN(iter); // 6 |
392 |
|
|
// assert(f.size()==6 && "Unexpected size of signal fLeftHand"); |
393 |
|
|
// assert(fRef.size()==6 && "Unexpected size of signal fLeftHandRef"); |
394 |
|
|
|
395 |
|
|
// if(s.size()!=6) |
396 |
|
|
// s.resize(6); |
397 |
|
|
// s.tail<3>() = Kf.segment<3>(18).cwiseProduct(fRef.head<3>() - |
398 |
|
|
// f.head<3>() ); s.head<3>() = |
399 |
|
|
// Kf.segment<3>(21).cwiseProduct(fRef.tail<3>() - f.tail<3>()); |
400 |
|
|
|
401 |
|
|
// return s; |
402 |
|
|
// } |
403 |
|
|
|
404 |
|
|
/* --- COMMANDS ---------------------------------------------------------- */ |
405 |
|
|
|
406 |
|
|
/* ------------------------------------------------------------------- */ |
407 |
|
|
/* --- ENTITY -------------------------------------------------------- */ |
408 |
|
|
/* ------------------------------------------------------------------- */ |
409 |
|
|
|
410 |
|
|
void AdmittanceController::display(std::ostream& os) const { |
411 |
|
|
os << "AdmittanceController " << getName(); |
412 |
|
|
try { |
413 |
|
|
getProfiler().report_all(3, os); |
414 |
|
|
} catch (ExceptionSignal e) { |
415 |
|
|
} |
416 |
|
|
} |
417 |
|
|
|
418 |
|
|
//************************************************************************************************** |
419 |
|
|
VectorXd svdSolveWithDamping(const JacobiSVD<MatrixXd>& A, const VectorXd& b, |
420 |
|
|
double damping) { |
421 |
|
|
eigen_assert(A.computeU() && A.computeV() && |
422 |
|
|
"svdSolveWithDamping() requires both unitaries U and V to be " |
423 |
|
|
"computed (thin unitaries suffice)."); |
424 |
|
|
assert(A.rows() == b.size()); |
425 |
|
|
|
426 |
|
|
// cout<<"b = "<<toString(b,1)<<endl; |
427 |
|
|
VectorXd tmp(A.cols()); |
428 |
|
|
int nzsv = static_cast<int>(A.nonzeroSingularValues()); |
429 |
|
|
tmp.noalias() = A.matrixU().leftCols(nzsv).adjoint() * b; |
430 |
|
|
// cout<<"U^T*b = "<<toString(tmp,1)<<endl; |
431 |
|
|
double sv, d2 = damping * damping; |
432 |
|
|
for (int i = 0; i < nzsv; i++) { |
433 |
|
|
sv = A.singularValues()(i); |
434 |
|
|
tmp(i) *= sv / (sv * sv + d2); |
435 |
|
|
} |
436 |
|
|
// cout<<"S^+ U^T b = "<<toString(tmp,1)<<endl; |
437 |
|
|
VectorXd res = A.matrixV().leftCols(nzsv) * tmp; |
438 |
|
|
|
439 |
|
|
// getLogger().sendMsg("sing val = "+toString(A.singularValues(),3), |
440 |
|
|
// MSG_STREAM_INFO); getLogger().sendMsg("solution with damp |
441 |
|
|
// "+toString(damping)+" = "+toString(res.norm()), MSG_STREAM_INFO); |
442 |
|
|
// getLogger().sendMsg("solution without damping |
443 |
|
|
// ="+toString(A.solve(b).norm()), MSG_STREAM_INFO); |
444 |
|
|
|
445 |
|
|
return res; |
446 |
|
|
} |
447 |
|
|
|
448 |
|
|
} // namespace torque_control |
449 |
|
|
} // namespace sot |
450 |
|
|
} // namespace dynamicgraph |