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
/**
* @author <a href="mailto:schlottb@informatik.hu-berlin.de">Schlotter, Benjamin</a>
*/
#include "FallMotion.h"

using namespace naoth;

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

  for (int i = 0; i < JointData::numOfJoint; i++)
  {
    // retain some stiffness in the head joints in order to turn the head a little during falling.
    if (JointData::getJointID(i) == JointData::HeadPitch || JointData::getJointID(i) == JointData::HeadYaw){
      freeStiffness[i] = 0.3;
      getMotorJointData().position[i] = 0.0;
    }
    else{
      freeStiffness[i] = -1.0;
    }
 
  }
}

void FallMotion::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