GCC Code Coverage Report

Directory: ./
File: src/force-compensation.cpp
Date: 2024-09-28 11:08:19
Exec Total Coverage
Lines: 0 166 0.0%
Branches: 0 568 0.0%
Line Branch Exec Source
1 /*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10 #include <dynamic-graph/factory.h>
11 #include <sot/dynamic-pinocchio/force-compensation.h>
12
13 #include <sot/core/debug.hh>
14 #include <sot/core/macros-signal.hh>
15
16 using namespace dynamicgraph::sot;
17 using namespace dynamicgraph;
18 ✗ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ForceCompensationPlugin,
19 "ForceCompensation");
20
21 /* --- PLUGIN --------------------------------------------------------------- */
22 /* --- PLUGIN --------------------------------------------------------------- */
23 /* --- PLUGIN --------------------------------------------------------------- */
24 ✗ ForceCompensation::ForceCompensation(void) : usingPrecompensation(false) {}
25
26 ✗ ForceCompensationPlugin::ForceCompensationPlugin(const std::string& name)
27 : Entity(name),
28 ✗ calibrationStarted(false)
29
30 ,
31 ✗ torsorSIN(NULL,
32 ✗ "sotForceCompensation(" + name + ")::input(vector6)::torsorIN"),
33 ✗ worldRhandSIN(NULL, "sotForceCompensation(" + name +
34 ")::input(MatrixRotation)::worldRhand")
35
36 ,
37 ✗ handRsensorSIN(NULL, "sotForceCompensation(" + name +
38 ")::input(MatrixRotation)::handRsensor"),
39 ✗ translationSensorComSIN(NULL, "sotForceCompensation(" + name +
40 ")::input(vector3)::sensorCom"),
41 ✗ gravitySIN(NULL,
42 ✗ "sotForceCompensation(" + name + ")::input(vector6)::gravity"),
43 ✗ precompensationSIN(NULL, "sotForceCompensation(" + name +
44 ")::input(vector6)::precompensation"),
45 ✗ gainSensorSIN(NULL,
46 ✗ "sotForceCompensation(" + name + ")::input(matrix6)::gain"),
47 ✗ deadZoneLimitSIN(NULL, "sotForceCompensation(" + name +
48 ")::input(vector6)::deadZoneLimit"),
49 ✗ transSensorJointSIN(NULL, "sotForceCompensation(" + name +
50 ")::input(vector6)::sensorJoint"),
51 ✗ inertiaJointSIN(NULL, "sotForceCompensation(" + name +
52 ")::input(matrix6)::inertiaJoint"),
53 ✗ velocitySIN(
54 ✗ NULL, "sotForceCompensation(" + name + ")::input(vector6)::velocity"),
55 ✗ accelerationSIN(NULL, "sotForceCompensation(" + name +
56 ")::input(vector6)::acceleration")
57
58 ,
59 ✗ handXworldSOUT(
60 ✗ SOT_INIT_SIGNAL_2(ForceCompensation::computeHandXworld, worldRhandSIN,
61 MatrixRotation, translationSensorComSIN,
62 dynamicgraph::Vector),
63 ✗ "sotForceCompensation(" + name +
64 ")::output(MatrixForce)::handXworld"),
65 ✗ handVsensorSOUT(SOT_INIT_SIGNAL_1(ForceCompensation::computeHandVsensor,
66 handRsensorSIN, MatrixRotation),
67 ✗ "sotForceCompensation(" + name +
68 ")::output(MatrixForce)::handVsensor"),
69 ✗ torsorDeadZoneSIN(NULL, "sotForceCompensation(" + name +
70 ")::input(vector6)::torsorNullifiedIN")
71
72 ,
73 ✗ sensorXhandSOUT(
74 ✗ SOT_INIT_SIGNAL_2(ForceCompensation::computeSensorXhand,
75 handRsensorSIN, MatrixRotation, transSensorJointSIN,
76 dynamicgraph::Vector),
77 ✗ "sotForceCompensation(" + name +
78 ")::output(MatrixForce)::sensorXhand")
79 // ,inertiaSensorSOUT( SOT_INIT_SIGNAL_2(
80 // ForceCompensation::computeInertiaSensor,
81 // inertiaJointSIN,dynamicgraph::Matrix,
82 // sensorXhandSOUT,MatrixForce ),
83 // "ForceCompensation("+name+")::output(MatrixForce)::inertiaSensor"
84 // )
85 ,
86 ✗ momentumSOUT(
87 ✗ SOT_INIT_SIGNAL_4(ForceCompensation::computeMomentum, velocitySIN,
88 dynamicgraph::Vector, accelerationSIN,
89 dynamicgraph::Vector, sensorXhandSOUT, MatrixForce,
90 inertiaJointSIN, dynamicgraph::Matrix),
91 ✗ "sotForceCompensation(" + name + ")::output(Vector6)::momentum"),
92 ✗ momentumSIN(NULL, "sotForceCompensation(" + name +
93 ")::input(vector6)::momentumIN")
94
95 ,
96 ✗ torsorCompensatedSOUT(
97 ✗ SOT_INIT_SIGNAL_7(
98 ForceCompensation::computeTorsorCompensated, torsorSIN,
99 dynamicgraph::Vector, precompensationSIN, dynamicgraph::Vector,
100 gravitySIN, dynamicgraph::Vector, handXworldSOUT, MatrixForce,
101 handVsensorSOUT, MatrixForce, gainSensorSIN, dynamicgraph::Matrix,
102 momentumSIN, dynamicgraph::Vector),
103 ✗ "sotForceCompensation(" + name + ")::output(Vector6)::torsor"),
104 ✗ torsorDeadZoneSOUT(
105 ✗ SOT_INIT_SIGNAL_2(ForceCompensation::computeDeadZone,
106 torsorDeadZoneSIN, dynamicgraph::Vector,
107 deadZoneLimitSIN, dynamicgraph::Vector),
108 ✗ "sotForceCompensation(" + name +
109 ")::output(Vector6)::torsorNullified"),
110 ✗ calibrationTrigerSOUT(
111 boost::bind(&ForceCompensationPlugin::calibrationTriger, this, _1,
112 _2),
113 ✗ torsorSIN << worldRhandSIN,
114 ✗ "sotForceCompensation(" + name +
115 ✗ ")::output(Dummy)::calibrationTriger") {
116 sotDEBUGIN(5);
117
118 ✗ signalRegistration(torsorSIN);
119 ✗ signalRegistration(worldRhandSIN);
120 ✗ signalRegistration(handRsensorSIN);
121 ✗ signalRegistration(translationSensorComSIN);
122 ✗ signalRegistration(gravitySIN);
123 ✗ signalRegistration(precompensationSIN);
124 ✗ signalRegistration(gainSensorSIN);
125 ✗ signalRegistration(deadZoneLimitSIN);
126 ✗ signalRegistration(transSensorJointSIN);
127 ✗ signalRegistration(inertiaJointSIN);
128 ✗ signalRegistration(velocitySIN);
129 ✗ signalRegistration(accelerationSIN);
130 ✗ signalRegistration(handXworldSOUT);
131 ✗ signalRegistration(handVsensorSOUT);
132 ✗ signalRegistration(torsorDeadZoneSIN);
133 ✗ signalRegistration(sensorXhandSOUT);
134 ✗ signalRegistration(momentumSOUT);
135 ✗ signalRegistration(momentumSIN);
136 ✗ signalRegistration(torsorCompensatedSOUT);
137 ✗ signalRegistration(torsorDeadZoneSOUT);
138 ✗ signalRegistration(calibrationTrigerSOUT);
139 ✗ torsorDeadZoneSIN.plug(&torsorCompensatedSOUT);
140
141 // By default, I choose: momentum is not compensated.
142 // momentumSIN.plug( &momentumSOUT );
143 ✗ dynamicgraph::Vector v(6);
144 ✗ v.fill(0);
145 ✗ momentumSIN = v;
146
147 sotDEBUGOUT(5);
148 }
149
150 ✗ ForceCompensationPlugin::~ForceCompensationPlugin(void) { return; }
151
152 /* --- CALIB --------------------------------------------------------------- */
153 /* --- CALIB --------------------------------------------------------------- */
154 /* --- CALIB --------------------------------------------------------------- */
155
156 MatrixRotation ForceCompensation::I3;
157
158 ✗ void ForceCompensation::clearCalibration(void) {
159 ✗ torsorList.clear();
160 ✗ rotationList.clear();
161 }
162
163 ✗ void ForceCompensation::addCalibrationValue(
164 const dynamicgraph::Vector& /*torsor*/,
165 const MatrixRotation& /*worldRhand*/) {
166 sotDEBUGIN(45);
167
168 // sotDEBUG(25) << "Add torsor: t"<<torsorList.size() <<" = " << torsor;
169 // sotDEBUG(25) << "Add Rotation: wRh"<<torsorList.size() <<" = " <<
170 // worldRhand;
171
172 // torsorList.push_back(torsor);
173 // rotationList.push_back(worldRhand);
174
175 sotDEBUGOUT(45);
176 }
177
178 ✗ dynamicgraph::Vector ForceCompensation::calibrateTransSensorCom(
179 const dynamicgraph::Vector& gravity,
180 const MatrixRotation& /*handRsensor*/) {
181 // sotDEBUGIN(25);
182
183 // dynamicgraph::Vector grav3(3);
184 // dynamicgraph::Vector Rgrav3(3),tau(3),Rtau(3);
185 // for( unsigned int j=0;j<3;++j ) { grav3(j)=gravity(j); }
186
187 // std::list< dynamicgraph::Vector >::iterator iterTorsor =
188 // torsorList.begin(); std::list< MatrixRotation >::const_iterator
189 // iterRotation
190 // = rotationList.begin();
191
192 // const unsigned int NVAL = torsorList.size();
193 // if( 0==NVAL )
194 // {
195 // dynamicgraph::Vector zero(3); zero.fill(0);
196 // return zero;
197 // }
198
199 // if(NVAL!=rotationList.size() )
200 // {
201 // // TODO: ERROR THROW
202 // }
203 // dynamicgraph::Matrix torsors( 3,NVAL );
204 // dynamicgraph::Matrix gravitys( 3,NVAL );
205
206 // for( unsigned int i=0;i<NVAL;++i )
207 // {
208 // if(
209 // (torsorList.end()==iterTorsor)||(rotationList.end()==iterRotation) )
210 // {
211 // // TODO: ERROR THROW
212 // break;
213 // }
214 // const dynamicgraph::Vector & torsor = *iterTorsor;
215 // const MatrixRotation & worldRhand = *iterRotation;
216
217 // for( unsigned int j=0;j<3;++j ) { tau(j)=torsor(j+3); }
218 // handRsensor.multiply(tau,Rtau);
219 // worldRhand.transpose().multiply( grav3,Rgrav3 );
220 // for( unsigned int j=0;j<3;++j )
221 // {
222 // torsors( j,i ) = -Rtau(j);
223 // gravitys( j,i ) = Rgrav3(j);
224 // }
225 // sotDEBUG(35) << "R" << i << " = " << worldRhand;
226 // sotDEBUG(35) << "Rtau" << i << " = " << Rtau;
227 // sotDEBUG(35) << "Rg" << i << " = " << Rgrav3;
228
229 // iterTorsor++; iterRotation++;
230 // }
231
232 // sotDEBUG(35) << "Rgs = " << gravitys;
233 // sotDEBUG(35) << "Rtaus = " << torsors;
234
235 // dynamicgraph::Matrix gravsInv( gravitys.nbCols(),gravitys.nbRows() );
236 // sotDEBUG(25) << "Compute the pinv..." << std::endl;
237 // gravitys.pseudoInverse(gravsInv);
238 // sotDEBUG(25) << "Compute the pinv... [DONE]" << std::endl;
239 // sotDEBUG(25) << "gravsInv = " << gravsInv << std::endl;
240
241 // dynamicgraph::Matrix Skew(3,3);
242 // torsors.multiply( gravsInv,Skew );
243 // sotDEBUG(25) << "Skew = " << Skew << std::endl;
244
245 // dynamicgraph::Vector sc(3);
246 // sc(0)=(Skew(2,1)-Skew(1,2))*.5 ;
247 // sc(1)=(Skew(0,2)-Skew(2,0))*.5 ;
248 // sc(2)=(Skew(1,0)-Skew(0,1))*.5 ;
249 // sotDEBUG(15) << "SC = " << sc << std::endl;
250 // /* TAKE the antisym constraint into account inside the minimization pbm.
251 // */
252
253 // sotDEBUGOUT(25);
254 // return sc;
255 ✗ return gravity;
256 }
257
258 ✗ dynamicgraph::Vector ForceCompensation::calibrateGravity(
259 const MatrixRotation& /*handRsensor*/, bool /*precompensationCalibration*/,
260 const MatrixRotation& /*hand0RsensorArg*/) {
261 sotDEBUGIN(25);
262
263 // MatrixRotation hand0Rsensor;
264 // if( &hand0Rsensor==&I3 ) hand0Rsensor.setIdentity();
265 // else hand0Rsensor=hand0RsensorArg;
266
267 // std::list< dynamicgraph::Vector >::const_iterator iterTorsor =
268 // torsorList.begin(); std::list< MatrixRotation >::const_iterator
269 // iterRotation
270 // = rotationList.begin();
271
272 // const unsigned int NVAL = torsorList.size();
273 // if(NVAL!=rotationList.size() )
274 // {
275 // // TODO: ERROR THROW
276 // }
277 // if( 0==NVAL )
278 // {
279 // dynamicgraph::Vector zero(6); zero.fill(0);
280 // return zero;
281 // }
282
283 // dynamicgraph::Vector force(3),forceHand(3),forceWorld(3);
284 // dynamicgraph::Vector sumForce(3); sumForce.fill(0);
285
286 // for( unsigned int i=0;i<NVAL;++i )
287 // {
288 // if(
289 // (torsorList.end()==iterTorsor)||(rotationList.end()==iterRotation) )
290 // {
291 // // TODO: ERROR THROW
292 // break;
293 // }
294 // const dynamicgraph::Vector & torsor = *iterTorsor;
295 // const MatrixRotation & R = *iterRotation;
296
297 // /* The sensor read [-] the value, and the grav is [-] the sensor
298 // force.
299 // * [-]*[-] = [+] -> force = + torsor(1:3). */
300 // for( unsigned int j=0;j<3;++j ) { force(j)=-torsor(j); }
301 // handRsensor.multiply(force,forceHand);
302 // if( usingPrecompensation )
303 // {
304 // dynamicgraph::Matrix R_I(3,3); R_I = R.transpose();
305 // R_I -= hand0Rsensor;
306 // R_I.pseudoInverse(.01).multiply( forceHand,forceWorld );
307 // }
308 // else
309 // { R.multiply( forceHand,forceWorld ); }
310
311 // sotDEBUG(35) << "R(" << i << "*3+1:" << i << "*3+3,:) = " << R <<
312 // "';"; sotDEBUG(35) << "rhFs(" << i << "*3+1:" << i << "*3+3) = " <<
313 // forceHand; sotDEBUG(45) << "fworld(" << i << "*3+1:" << i << "*3+3) =
314 // " << forceWorld;
315
316 // sumForce+= forceWorld;
317
318 // iterTorsor++; iterRotation++;
319 // }
320
321 // sumForce*= (1./NVAL);
322 // sotDEBUG(35) << "Fmean = " << sumForce;
323 // sumForce.resize(6,false);
324 // sumForce(3)=sumForce(4)=sumForce(5)=0.;
325
326 // sotDEBUG(25)<<"mg = " << sumForce<<std::endl;
327
328 sotDEBUGOUT(25);
329 ✗ dynamicgraph::Vector sumForce(3);
330 ✗ sumForce.fill(0);
331 ✗ return sumForce;
332 }
333
334 /* --- SIGNALS -------------------------------------------------------------- */
335 /* --- SIGNALS -------------------------------------------------------------- */
336 /* --- SIGNALS -------------------------------------------------------------- */
337 ✗ MatrixForce& ForceCompensation::computeHandXworld(
338 const MatrixRotation& worldRhand,
339 const dynamicgraph::Vector& transSensorCom, MatrixForce& res) {
340 sotDEBUGIN(35);
341
342 sotDEBUG(25) << "wRrh = " << worldRhand << std::endl;
343 sotDEBUG(25) << "SC = " << transSensorCom << std::endl;
344
345 ✗ MatrixHomogeneous scRw;
346 ✗ scRw.linear() = worldRhand.transpose();
347 ✗ scRw.translation() = transSensorCom;
348 sotDEBUG(25) << "scMw = " << scRw << std::endl;
349
350 /////////////////////////////
351 // res.buildFrom( scRw );
352 ✗ Eigen::Vector3d _t = scRw.translation();
353 ✗ MatrixRotation R(scRw.linear());
354 ✗ Eigen::Matrix3d Tx;
355 ✗ Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
356 ✗ Eigen::Matrix3d sk;
357 ✗ sk = Tx * R;
358
359 ✗ res.block<3, 3>(0, 0) = R;
360 ✗ res.block<3, 3>(0, 3).setZero();
361 ✗ res.block<3, 3>(3, 0) = sk;
362 ✗ res.block<3, 3>(3, 3) = R;
363 ////////////////////////////
364
365 sotDEBUG(15) << "scXw = " << res << std::endl;
366
367 sotDEBUGOUT(35);
368 ✗ return res;
369 }
370
371 ✗ MatrixForce& ForceCompensation::computeHandVsensor(
372 const MatrixRotation& handRsensor, MatrixForce& res) {
373 sotDEBUGIN(35);
374
375 ✗ for (unsigned int i = 0; i < 3; ++i)
376 ✗ for (unsigned int j = 0; j < 3; ++j) {
377 ✗ res(i, j) = handRsensor(i, j);
378 ✗ res(i + 3, j + 3) = handRsensor(i, j);
379 ✗ res(i + 3, j) = 0.;
380 ✗ res(i, j + 3) = 0.;
381 }
382
383 sotDEBUG(25) << "hVs" << res << std::endl;
384
385 sotDEBUGOUT(35);
386 ✗ return res;
387 }
388
389 ✗ MatrixForce& ForceCompensation::computeSensorXhand(
390 const MatrixRotation& /*handRsensor*/,
391 const dynamicgraph::Vector& transJointSensor, MatrixForce& res) {
392 sotDEBUGIN(35);
393
394 /* Force Matrix to pass from the joint frame (ie frame located
395 * at the position of the motor, in which the acc is computed by Spong)
396 * to the frame SensorHand where all the forces are expressed (ie
397 * frame located at the sensor position bu oriented like the hand). */
398
399 ✗ Eigen::Matrix3d Tx;
400 ✗ Tx << 0, -transJointSensor(2), transJointSensor(1), transJointSensor(2), 0,
401 ✗ -transJointSensor(0), -transJointSensor(1), transJointSensor(0), 0;
402
403 ✗ res.block<3, 3>(0, 0).setIdentity();
404 ✗ res.block<3, 3>(0, 3).setZero();
405 ✗ res.block<3, 3>(3, 0) = Tx;
406 ✗ res.block<3, 3>(3, 3).setIdentity();
407
408 sotDEBUG(25) << "shXJ" << res << std::endl;
409
410 sotDEBUGOUT(35);
411 ✗ return res;
412 }
413
414 // dynamicgraph::Matrix& ForceCompensation::
415 // computeInertiaSensor( const dynamicgraph::Matrix& inertiaJoint,
416 // const MatrixForce& sensorXhand,
417 // dynamicgraph::Matrix& res )
418 // {
419 // sotDEBUGIN(35);
420
421 // /* Inertia felt at the sensor position, expressed in the orientation
422 // * of the hand. */
423
424 // res.resize(6,6);
425 // sensorXhand.multiply( inertiaJoint,res );
426
427 // sotDEBUGOUT(35);
428 // return res;
429 // }
430
431 ✗ dynamicgraph::Vector& ForceCompensation::computeTorsorCompensated(
432 const dynamicgraph::Vector& torqueInput,
433 const dynamicgraph::Vector& torquePrecompensation,
434 const dynamicgraph::Vector& gravity, const MatrixForce& handXworld,
435 const MatrixForce& handVsensor, const dynamicgraph::Matrix& gainSensor,
436 const dynamicgraph::Vector& momentum, dynamicgraph::Vector& res)
437
438 {
439 sotDEBUGIN(35);
440 /* Torsor in the sensor frame: K*(-torsred-gamma)+sVh*hXw*mg */
441 /* Torsor in the hand frame: hVs*K*(-torsred-gamma)+hXw*mg */
442 /* With gamma expressed in the sensor frame (gamma_s = sVh*gamma_h) */
443
444 sotDEBUG(25) << "t_nc = " << torqueInput;
445 ✗ dynamicgraph::Vector torquePrecompensated(6);
446 // if( usingPrecompensation )
447 ✗ torquePrecompensated = torqueInput + torquePrecompensation;
448 // else { torquePrecompensated = torqueInput; }
449 sotDEBUG(25) << "t_pre = " << torquePrecompensated;
450
451 ✗ dynamicgraph::Vector torqueS(6), torqueRH(6);
452 ✗ torqueS = gainSensor * torquePrecompensated;
453 ✗ res = handVsensor * torqueS;
454 sotDEBUG(25) << "t_rh = " << res;
455
456 ✗ dynamicgraph::Vector grh(6);
457 ✗ grh = handXworld * gravity;
458 ✗ grh *= -1;
459 sotDEBUG(25) << "g_rh = " << grh;
460
461 ✗ res += grh;
462 sotDEBUG(25) << "fcomp = " << res;
463
464 ✗ res += momentum;
465 sotDEBUG(25) << "facc = " << res;
466
467 /* TODO res += m xddot */
468
469 sotDEBUGOUT(35);
470 ✗ return res;
471 }
472
473 ✗ dynamicgraph::Vector& ForceCompensation::crossProduct_V_F(
474 const dynamicgraph::Vector& velocity, const dynamicgraph::Vector& force,
475 dynamicgraph::Vector& res) {
476 /* [ v;w] x [ f;tau ] = [ w x f; v x f + w x tau ] */
477 ✗ Eigen::Vector3d v, w, f, tau;
478 ✗ for (unsigned int i = 0; i < 3; ++i) {
479 ✗ v(i) = velocity(i);
480 ✗ w(i) = velocity(i + 3);
481 ✗ f(i) = force(i);
482 ✗ tau(i) = force(i + 3);
483 }
484 ✗ Eigen::Vector3d res1(3), res2a(3), res2b(3);
485 ✗ res1 = w.cross(f);
486 ✗ res2a = v.cross(f);
487 ✗ res2b = w.cross(tau);
488 ✗ res2a += res2b;
489
490 ✗ res.resize(6);
491 ✗ for (unsigned int i = 0; i < 3; ++i) {
492 ✗ res(i) = res1(i);
493 ✗ res(i + 3) = res2a(i);
494 }
495 ✗ return res;
496 }
497
498 ✗ dynamicgraph::Vector& ForceCompensation::computeMomentum(
499 const dynamicgraph::Vector& velocity,
500 const dynamicgraph::Vector& acceleration, const MatrixForce& sensorXhand,
501 const dynamicgraph::Matrix& inertiaJoint, dynamicgraph::Vector& res) {
502 sotDEBUGIN(35);
503
504 /* Fs + Fext = I acc + V x Iv */
505 ✗ dynamicgraph::Vector Iacc(6);
506 ✗ Iacc = inertiaJoint * acceleration;
507 ✗ res.resize(6);
508 ✗ res = sensorXhand * Iacc;
509
510 ✗ dynamicgraph::Vector Iv(6);
511 ✗ Iv = inertiaJoint * velocity;
512 ✗ dynamicgraph::Vector vIv(6);
513 ✗ crossProduct_V_F(velocity, Iv, vIv);
514 ✗ dynamicgraph::Vector XvIv(6);
515 ✗ XvIv = sensorXhand * vIv;
516 ✗ res += XvIv;
517
518 sotDEBUGOUT(35);
519 ✗ return res;
520 }
521
522 ✗ dynamicgraph::Vector& ForceCompensation::computeDeadZone(
523 const dynamicgraph::Vector& torqueInput,
524 const dynamicgraph::Vector& deadZone, dynamicgraph::Vector& res) {
525 sotDEBUGIN(35);
526
527 ✗ if (torqueInput.size() > deadZone.size()) return res;
528 ✗ res.resize(torqueInput.size());
529 ✗ for (int i = 0; i < torqueInput.size(); ++i) {
530 ✗ const double th = fabs(deadZone(i));
531 ✗ if ((torqueInput(i) < th) && (torqueInput(i) > -th)) {
532 ✗ res(i) = 0;
533 ✗ } else if (torqueInput(i) < 0)
534 ✗ res(i) = torqueInput(i) + th;
535 else
536 ✗ res(i) = torqueInput(i) - th;
537 }
538
539 sotDEBUGOUT(35);
540 ✗ return res;
541 }
542
543 ForceCompensationPlugin::sotDummyType&
544 ✗ ForceCompensationPlugin::calibrationTriger(
545 ForceCompensationPlugin::sotDummyType& dummy, int /*time*/) {
546 // sotDEBUGIN(45);
547 // if(! calibrationStarted ) { sotDEBUGOUT(45); return dummy=0; }
548
549 // addCalibrationValue( torsorSIN(time),worldRhandSIN(time) );
550 // sotDEBUGOUT(45);
551 ✗ return dummy = 1;
552 }
553