1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
/**
* @file StandMotion.h
*
* @author <a href="mailto:mellmann@informatik.hu-berlin.de">Heinrich Mellmann</a>
*
* the stand motion goes from current pose to the default stand pose.
* running - this motion is cyclic and there is allways running when the motion
*           request is set to stand
* stopped - the motion stopps when the motion request is not stand and the
*           default stand pose has been reached
*
*/

#include "StandMotion.h"

#include <array>

using namespace naoth;

StandMotion::StandMotion()<--- Member variable 'StandMotion::stiffness' is not initialized in the constructor.
  :
  IKMotion(getInverseKinematicsMotionEngineService(), motion::stand, getMotionLock()),

  firstRun(true),
  state(GotoStandPose),
  lastState(None),
  state_time(0),

  totalTime(0),
  time(0),
  
  height(-1000),
  standardStand(true),
  isLiftedUp(false),

  fullCorrection(true),
  stiffnessIsReady(false),
  lastFrameInfo(getFrameInfo())
{
  // create
  std::vector<double> x = {0.0, 0.5, 1.0};
  std::vector<double> y = {0.0, 0.5, 1.0};

  interpolationSpline.set_boundary(tk::spline::first_deriv,0.0, tk::spline::first_deriv,0.0, false);
  interpolationSpline.set_points(x,y);

  getMotionStatus().target_reached = false;
}


void StandMotion::execute()
{
  setCurrentState(motion::running);

  // HACK: make it parameter or so?
  const double timeBeforeRelax = static_cast<double>(getRobotInfo().basicTimeStep*100);
  const double stiffDelta = getRobotInfo().getBasicTimeStepInSecond();

  // update joint monitors
  for( int i = naoth::JointData::RShoulderRoll; i <= naoth::JointData::LAnkleRoll; i++) {
    // are the jointOffsets applied twice? Doesn't getMotorJointData contain the offsets already?
    jointMonitors[i].updateMonitor(getMotorJointData().position[i] + jointOffsets[i], getSensorJointData().position[i],getSensorJointData().electricCurrent[i]);
  }

  PLOT("StandMotion:State",(int)state);

  // a change of stand pose was induced from outside, either by lifting the robot 
  // or by changing the parameters of the stand
  if( firstRun ||
      isLiftedUp    != getBodyState().isLiftedUp ||
      height        != getMotionRequest().standHeight ||
      standardStand != getMotionRequest().standardStand) 
  {
    fullCorrection = true;
    
    // force new correction
    state = GotoStandPose;
    lastState = None;
  }

  // remember for later
  isLiftedUp = getBodyState().isLiftedUp;

  switch(state)
  {
  case GotoStandPose:
  {
    // initialize on fist execution
    if(lastState != state) 
    {
      lastState = state;

      // set target stiffness from parameters
      setStiffnessBuffer(getEngine().getParameters().stand.stiffnessGotoPose);

      calcStandPose(fullCorrection);

      stiffnessIsReady = false;
    }

    bool d = interpolateToPose();
    PLOT("StandMotion:stiffnessIsReady",stiffnessIsReady);
    PLOT("StandMotion:interpolateToPose",d);

    if(stiffnessIsReady && d) 
    {
      getMotionStatus().target_reached = true;
      if(getMotionRequest().id != getId())
      {
        // NOTE: this is the opnly place where the stand motion can exit
        setCurrentState(motion::stopped);
        getMotionStatus().target_reached = false;
      }
      // HACK: always wait some time before going to relax mode
      else if (static_cast<double>(state_time) > totalTime + timeBeforeRelax
                  && getEngine().getParameters().stand.relax.enable
                  && relaxedPoseIsStillOk() //only relax if a valid stand pose is reached
                  && !getMotionRequest().disable_relaxed_stand) // and requested
      {
        state = Relax;
      }
    }
  }
  break;

  case Relax:

    // initialize relax on fist execution
    if(lastState != state) 
    {
      lastState = state;

      // set target stiffness from parameters
      setStiffnessBuffer(getEngine().getParameters().stand.stiffnessRelax);

      // move to the pose that is measured by our sensors = relax :)
      startPose = targetPose;
      targetPose = getEngine().getCoMFeetPoseBasedOnSensor();
      time = 0;
      totalTime = 1000;

      // reset stuff for StandMotion:online_tuning
      jointOffsets.resetOffsets();
      for(int i = 0; i < naoth::JointData::numOfJoint; i++){
        jointMonitors[i].resetAll();
      }
    }

    // run adjustments after the relaxed pose had been reached
    if(interpolateToPose()) 
    {
      if(getEngine().getParameters().stand.relax.jointOffsetTuning.enable){
        tuneJointOffsets();
      }

      if(getEngine().getParameters().stand.relax.stiffnessControl.enable){
        tuneStiffness();
      }

      // correct the pose needs to be corrected (also before leaving stand)
      if ( getMotionRequest().id != getId() || !relaxedPoseIsStillOk()) {
        fullCorrection = false; // only adjust the com, not the feet
        state = GotoStandPose;
      } 
    }
    
    break;

  default:
    THROW("[StandMotion] unexpected (state, lastState) = (" << state << ", " << lastState);
  } // end switch

  // update the time of the current step
  if(lastState != state) {
    state_time = 0;
  } else {
    state_time += getRobotInfo().basicTimeStep;
  }

  // apply stiffness from buffer
  /*
  for( int i = naoth::JointData::RShoulderRoll; i < naoth::JointData::numOfJoint; i++) {
    getMotorJointData().stiffness[i] = stiffness[i];
  }*/
  stiffnessIsReady = setStiffness(getMotorJointData(), getSensorJointData(), stiffness, stiffDelta, naoth::JointData::RHipYawPitch, naoth::JointData::LWristYaw);

  //turnOffStiffnessWhenJointIsOutOfRange();

  firstRun = false;
}//end execute

void StandMotion::setStiffnessBuffer(double s)
{
  for( int i = naoth::JointData::RShoulderRoll; i<naoth::JointData::numOfJoint; i++) {
    stiffness[i] = s;
  }
  // HACK: turn off the hands
  stiffness[JointData::LHand] = -1;
  stiffness[JointData::RHand] = -1;
}


void StandMotion::calcStandPose(bool fullCorrection)
{
  // use the sensors to estimate the current pose
  startPose  = getEngine().getCurrentCoMFeetPose();//getEngine().getCoMFeetPoseBasedOnModel();
  
  // HACK: don't do anything if after walk
  //       copy the pose only the first time
  if(firstRun && getMotionStatus().lastMotion == motion::walk) 
  {
    targetPose = startPose;
  }
  else // calculate full correction pose
  {
    // adjust the positions of the feet only if a full correction of the standing pose is requested
    standardStand = fullCorrection && getMotionRequest().standardStand;
    
    // init pose
    height = getMotionRequest().standHeight;
    double comHeight = (height < 0.0) ? getWalk2018Parameters().zmpPlanner2018Params.comHeight : getMotionRequest().standHeight;
    comHeight = Math::clamp(comHeight, 160.0, 270.0); // valid range

    targetPose = getStandPose(comHeight,
                            getEngine().getParameters().stand.hipOffsetX,
                            getEngine().getParameters().stand.bodyPitchOffset,
                            standardStand);
  }

  // TODO: there mus be a better way to do it
  // estimate the maximal error between the targetPose and startPose in order to estimate the time
  // which is necessary to move from one to another. In order to take the rotation into account we 
  // calculate the maximal error from the perspective of each of the poses: left/right foot and com.
  
  // error from the perspective of the left foot
  targetPose.localInLeftFoot();
  startPose.localInLeftFoot();
  const double distLeft = std::max(
    (targetPose.com.translation        - startPose.com.translation).abs(), 
    (targetPose.feet.right.translation - startPose.feet.right.translation).abs()
  );

  // error from the perspective of the right foot
  targetPose.localInRightFoot();
  startPose.localInRightFoot();
  const double distRight = std::max(
    (targetPose.com.translation        - startPose.com.translation).abs(), 
    (targetPose.feet.left.translation  - startPose.feet.left.translation).abs()
  );

  // error from the perspective of the com
  targetPose.localInCoM();
  startPose.localInCoM();
  const double distCom = std::max(
    (targetPose.feet.left.translation  - startPose.feet.left.translation).abs(), 
    (targetPose.feet.right.translation - startPose.feet.right.translation).abs()
  );
  
  double distMax = std::max(std::max(distLeft,distRight),distCom);

  // initialize the time
  double speed = getEngine().getParameters().stand.speed;
  totalTime = distMax / speed;
  time = 0;

} // end calcStandPose


bool StandMotion::interpolateToPose()
{
  PLOT("StandMotion:totalTime", totalTime);
  PLOT("StandMotion:time", time);
  // execute the stand motion
  if(totalTime >= 0 && time <= totalTime)
  {
      InverseKinematic::CoMFeetPose p;

      if(totalTime > 0) {
          double t = time/totalTime;
          //double k = Math::clamp(0.5*(1.0-cos(t*Math::pi)), 0.0, 1.0);
          double k = interpolationSpline(t);
          p = getEngine().interpolate(startPose, targetPose, k);
      } else {
          p = targetPose;
      }

      time += getRobotInfo().basicTimeStep;

      applyPose(p);
      return false;
  }

  // done
  applyPose(targetPose);
  return true;
}//end interpolateToPose


void StandMotion::applyPose(const InverseKinematic::CoMFeetPose& p)
{
  bool solved = false;
  InverseKinematic::HipFeetPose c = getEngine().controlCenterOfMass(getMotorJointData(), p, solved, false);

  //InverseKinematic::HipFeetPose c(tc);
  if(getCalibrationData().calibrated)
  {
    c.localInLeftFoot();

    if(getEngine().getParameters().stand.enableStabilization) {
      getEngine().rotationStabilize(
        getInertialModel(),
        getGyrometerData(),
        getRobotInfo().getBasicTimeStepInSecond(),
        getEngine().getParameters().stand.stabilization.rotation.P,
        getEngine().getParameters().stand.stabilization.rotation.VelocityP,
        getEngine().getParameters().stand.stabilization.rotation.D,
        c);
    } else if(getEngine().getParameters().stand.enableStabilizationRC16) {
      getEngine().rotationStabilizeRC16(
        getInertialSensorData().data,
        getGyrometerData(),
        getRobotInfo().getBasicTimeStepInSecond(),
        getEngine().getParameters().stand.stabilization.rotationRC16.P,
        getEngine().getParameters().stand.stabilization.rotationRC16.VelocityP,
        getEngine().getParameters().stand.stabilization.rotationRC16.D,
        c);
    }
  }

  getEngine().solveHipFeetIK(c);
  getEngine().copyLegJoints(getMotorJointData().position);
}//end applyPose


void StandMotion::tuneJointOffsets()
{
  jointOffsets.setMinimalStep(getEngine().getParameters().stand.relax.jointOffsetTuning.minimalJointStep);

  if(getFrameInfo().getTime() > lastFrameInfo.getTime() + getEngine().getParameters().stand.relax.jointOffsetTuning.deadTime)
  {
    const std::array<JointData::JointID, 4> jointToMonitor = {
      JointData::LKneePitch, JointData::RKneePitch, JointData::LAnklePitch, JointData::RAnklePitch
    };

    PLOT("StandMotion:filteredCurrent:LKneePitch", jointMonitors[JointData::LKneePitch].filteredCurrent());
    PLOT("StandMotion:filteredCurrent:RKneePitch", jointMonitors[JointData::RKneePitch].filteredCurrent());
    PLOT("StandMotion:filteredCurrent:LAnklePitch",jointMonitors[JointData::LAnklePitch].filteredCurrent());
    PLOT("StandMotion:filteredCurrent:RAnklePitch",jointMonitors[JointData::RAnklePitch].filteredCurrent());

    // find the joint with the max current
    JointData::JointID maxId = jointToMonitor.front();
    for(JointData::JointID id: jointToMonitor) {
      if(jointMonitors[id].filteredCurrent() > jointMonitors[maxId].filteredCurrent()) {
        maxId = id;
      }
    }

    // DEBUG: print max joint
    //std::cout << "max: " << maxId << " : " << jointMonitors[maxId].filteredCurrent() << std::endl; 

    // if greater than Threshold then try to tune the offsets
    if(jointMonitors[maxId].filteredCurrent() > getEngine().getParameters().stand.relax.jointOffsetTuning.currentThreshold) {
      // HACK: make ot more general (maybe adjust in the direction of the error)
      if(maxId == JointData::LKneePitch || maxId == JointData::RKneePitch) {
        jointOffsets.increaseOffset(maxId);
      } else {
        jointOffsets.decreaseOffset(maxId);
      }
    }

    lastFrameInfo = getFrameInfo();
  }

  for( int i = naoth::JointData::RHipYawPitch; i <= naoth::JointData::LAnkleRoll; i++) {
    getMotorJointData().position[i] += jointOffsets[i];
  }
}//end tuneJointOffsets

void StandMotion::tuneStiffness()
{
  double minAngle = getEngine().getParameters().stand.relax.stiffnessControl.minAngle;
  double maxAngle = getEngine().getParameters().stand.relax.stiffnessControl.maxAngle;
  double minStiff = getEngine().getParameters().stand.relax.stiffnessControl.minStiffness;
  double maxStiff = getEngine().getParameters().stand.relax.stiffnessControl.maxStiffness;

  for( int i = naoth::JointData::RShoulderRoll; i <= naoth::JointData::LAnkleRoll; i++) {
    stiffnessController[i].setMinMaxValues(minAngle,maxAngle,minStiff,maxStiff);
    stiffness[i] = stiffnessController[i].control(jointMonitors[i].getError());
  }

  lastStiffnessUpdate = getFrameInfo();

  PLOT("StandMotion:AverageError:LKneePitch",jointMonitors[naoth::JointData::LKneePitch].getError());
  PLOT("StandMotion:AverageError:RKneePitch",jointMonitors[naoth::JointData::RKneePitch].getError());
  PLOT("StandMotion:Control:LKneePitch",stiffnessController[naoth::JointData::LKneePitch].control(jointMonitors[naoth::JointData::LKneePitch].getError()));
  PLOT("StandMotion:Control:RKneePitch",stiffnessController[naoth::JointData::RKneePitch].control(jointMonitors[naoth::JointData::RKneePitch].getError()));
}// end tuneStiffness


bool StandMotion::relaxedPoseIsStillOk()
{
  InverseKinematic::CoMFeetPose currentPose = getEngine().getCurrentCoMFeetPose();
  currentPose.localInLeftFoot();

  // NOTE: calculated while going to the stand pose
  InverseKinematic::CoMFeetPose target(targetPose);
  target.localInLeftFoot();

  PLOT("StandMotion:Deviations:Distance", (currentPose.com.translation - target.com.translation).abs());
  PLOT("StandMotion:Deviations:RotationX", Math::toDegrees(fabs(currentPose.com.rotation.getXAngle() - target.com.rotation.getXAngle())));
  PLOT("StandMotion:Deviations:RotationY", Math::toDegrees(fabs(currentPose.com.rotation.getYAngle() - target.com.rotation.getYAngle())));

  double max_rotation_error = getEngine().getParameters().stand.relax.allowedRotationDeviation;
  double max_translation_error = getEngine().getParameters().stand.relax.allowedDeviation;

  return   (currentPose.com.translation - target.com.translation).abs() < max_translation_error
    && fabs(currentPose.com.rotation.getXAngle() - target.com.rotation.getXAngle()) < max_rotation_error
    && fabs(currentPose.com.rotation.getYAngle() - target.com.rotation.getYAngle()) < max_rotation_error;
}// end relaxedPoseIsStillOk

/*
void StandMotion::turnOffStiffnessWhenJointIsOutOfRange()
{
  const double* pos = getMotorJointData().position;
  double* stiff = getMotorJointData().stiffness;
  // Knee pitch
  if ( pos[naoth::JointData::LKneePitch] > getMotorJointData().max[naoth::JointData::LKneePitch] ) {
    stiff[naoth::JointData::LKneePitch] = -1;
  } else {
    stiff[naoth::JointData::LKneePitch] = getEngine().getParameters().stand.stiffness;
  }
  if ( pos[naoth::JointData::RKneePitch] > getMotorJointData().max[naoth::JointData::RKneePitch] ) {
    stiff[naoth::JointData::RKneePitch] = -1;
  } else {
    stiff[naoth::JointData::RKneePitch] = getEngine().getParameters().stand.stiffness;
  }

  // Ankle pitch
  if ( pos[naoth::JointData::LAnklePitch] < getMotorJointData().min[naoth::JointData::LAnklePitch] ) {
    stiff[naoth::JointData::LAnklePitch] = -1;
  } else {
    stiff[naoth::JointData::LAnklePitch] = getEngine().getParameters().stand.stiffness;
  }
  if ( pos[naoth::JointData::RAnklePitch] < getMotorJointData().min[naoth::JointData::RAnklePitch] ) {
    stiff[naoth::JointData::RAnklePitch] = -1;
  } else {
    stiff[naoth::JointData::RAnklePitch] = getEngine().getParameters().stand.stiffness;
  }
}//end turnOffStiffnessWhenJointIsOutOfRange
*/