GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/force-compensation.cpp Lines: 0 154 0.0 %
Date: 2023-03-28 11:05:13 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
}