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
/**
 * @file ParallelDance.h
 *
 * @author <a href="mailto:mellmann@informatik.hu-berlin.de">Xu, Yuan</a>
 */

#include "ParallelDance.h"

using namespace naoth;

ParallelDance::ParallelDance() 
  :
  AbstractMotion(motion::parallel_dance, getMotionLock()),
  radius(0),
  speed(0),
  t(0)
{
  
}

void ParallelDance::execute()
{
  if(getMotionRequest().id != getId())
  {
    setCurrentState(motion::stopped);
    return;
  }

  // some parameters
  double max_dance_radius = 0.25; // maximal radius of the motion in mm
  MODIFY("DanceMotion:max_dance_radius", max_dance_radius);
  double max_dance_speed = 100; // maximal speed of the motion in degrees / second
  MODIFY("DanceMotion:max_dance_speed", max_dance_speed);

  // increase or decrease the current radius slowly
  double radius_step = 0.001;
  if (radius < max_dance_radius - radius_step) radius += radius_step;
  if (radius > max_dance_radius + radius_step) radius -= radius_step;

  // increase or decrease the current speed slowly
  double speed_step = 1;
  if (speed < max_dance_speed - speed_step) speed += speed_step;
  if (speed > max_dance_speed + speed_step) speed -= speed_step;

  // increase the current time
  double s = speed*getRobotInfo().getBasicTimeStepInSecond();
  t += Math::fromDegrees(s);

  double alpha_z = 0.8;
  MODIFY("ParallelDance:alpha_z", alpha_z);

  double alpha_x = 0.0;<--- Variable 'alpha_x' is reassigned a value before the old one has been used.
  alpha_x = cos(t) * radius;
  MODIFY("ParallelDance:alpha_x", alpha_x);

  double alpha_y = 0.0;<--- Variable 'alpha_y' is reassigned a value before the old one has been used.
  alpha_y = sin(t) * radius;
  MODIFY("ParallelDance:alpha_y", alpha_y);

  double bs = Math::fromDegrees(s);
  PLOT("a:bs",bs);
  PLOT("a:alpha_y",alpha_y);

  Kinematics::ParallelKinematic::Pose pose;
  pose.alpha.x = alpha_x;
  pose.alpha.y = alpha_y;
  pose.alpha.z = alpha_z;

  // apply parallel kinematics to calculate the leg joints
  theParallelKinematic.calculateLegs(pose, getMotorJointData());

  for(int i = JointData::RHipPitch; i < JointData::numOfJoint; i++)
  {
    PLOT(JointData::getJointName((JointData::JointID)i), (getMotorJointData().position[i]));
  }

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

double ParallelDance::factorial(int num){
        double fact = 1;
        for(int i = 1; i <= num; i++){
                fact *= i;
        }
        return fact;
}
 
//Taylor series sin(x) = (-1)^n*x^(2n+1)/(2n+1)!
//any precision less then 19 and your values will start to skew
double ParallelDance::taylorSine(double num, double precision){
        double value = 0;
        for(int n = 0; n < precision; n++){
                value += pow(-1.0, n) * pow(num, 2*n+1) / factorial(2*n + 1);
        }
        return value;
}