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
/**
* @author <a href="mailto:xu@informatik.hu-berlin.de">Xu, Yuan</a>
*/
#include "DeadMotion.h"

using namespace naoth;

DeadMotion::DeadMotion()<--- Member variable 'DeadMotion::oldStiffness' is not initialized in the constructor.
  :
  AbstractMotion(motion::dead, getMotionLock())
{
  stiffness_increase = getRobotInfo().getBasicTimeStepInSecond() * 5;

  for (size_t i = 0; i < JointData::numOfJoint; i++) {
    freeStiffness[i] = -1.0;
  }
}

void DeadMotion::execute()
{
  if(getMotionRequest().id != getId())
  {
    // gradually restore hardness
    if ( setStiffness(getMotorJointData(), getSensorJointData(), oldStiffness, stiffness_increase) ) {
      setCurrentState(motion::stopped);
    }

    // copy sensor positions
    for (size_t i = 0; i < JointData::numOfJoint; ++i) {
      getMotorJointData().position[i] = getSensorJointData().position[i];
    }

    return;
  }
  else if( isStopped() ) // executed the first time
  {
    // store hardness
    for (size_t i = 0; i < JointData::numOfJoint; ++i) {
      oldStiffness[i] = getSensorJointData().stiffness[i];
    }
  }

  // set joint free
  setStiffness(getMotorJointData(), getSensorJointData(), freeStiffness, 10);

  setCurrentState(motion::running);
}//end execute