Bradley Perry / Brad_poly_gait

This is the quartic polynomial gait.

Dependents:   Motion-Control

Brad_poly_gait.cpp@2:40f8f3a8e878, 2015-06-29 (annotated)

Committer:
perr1940
Date:
Mon Jun 29 00:34:24 2015 +0000
Revision:
2:40f8f3a8e878
Parent:
1:59243225dcd5
Child:
3:ed0f6f302a0a
there should be nothing special about this, just a reverted copy... before tunable params

Who changed what in which revision?

UserRevisionLine numberNew contents of line
perr1940 0:a5986ef182dc 1 #include "Brad_poly_gait.h"
perr1940 0:a5986ef182dc 2 #include "math.h"
perr1940 0:a5986ef182dc 3
perr1940 2:40f8f3a8e878 4 //TODO: Add a calculation of max flexion angle for FS _params.max_fs_angle
perr1940 0:a5986ef182dc 5
perr1940 0:a5986ef182dc 6
perr1940 0:a5986ef182dc 7 const float PI =3.141592653589793;
perr1940 0:a5986ef182dc 8
perr1940 0:a5986ef182dc 9 /** Swing trajectory generation
perr1940 0:a5986ef182dc 10 */
perr1940 2:40f8f3a8e878 11 BradPolySwing::BradPolySwing(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
perr1940 0:a5986ef182dc 12 {
perr1940 0:a5986ef182dc 13 //_phi{{0, 1, 2, 3, 4}, {0, 1, 2, 3, 4}},
perr1940 2:40f8f3a8e878 14 _phi[0][0]=-22;
perr1940 0:a5986ef182dc 15 _phi[0][1]=0;
perr1940 0:a5986ef182dc 16 _phi[0][2]=232.1405;
perr1940 0:a5986ef182dc 17 _phi[0][3]=-256.2810;
perr1940 0:a5986ef182dc 18 _phi[0][4]=76.1405;
perr1940 0:a5986ef182dc 19 _phi[1][0]=30.0000;
perr1940 0:a5986ef182dc 20 _phi[1][1]=0;
perr1940 0:a5986ef182dc 21 _phi[1][2]=-152.2944;
perr1940 0:a5986ef182dc 22 _phi[1][3]=224.5888;
perr1940 0:a5986ef182dc 23 _phi[1][4]=-92.2944;
perr1940 0:a5986ef182dc 24 //_times{0, 1, 2},
perr1940 0:a5986ef182dc 25 _times[0]=0;
perr1940 0:a5986ef182dc 26 _times[1]=378;
perr1940 2:40f8f3a8e878 27 _times[2]=900;
perr1940 0:a5986ef182dc 28 };
perr1940 0:a5986ef182dc 29
perr1940 0:a5986ef182dc 30 BradPolySwing::BradPolySwing():_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
perr1940 0:a5986ef182dc 31 {
perr1940 0:a5986ef182dc 32 //_phi{{0, 1, 2, 3, 4}, {0, 1, 2, 3, 4}},
perr1940 0:a5986ef182dc 33 _phi[0][0]=-22;
perr1940 0:a5986ef182dc 34 _phi[0][1]=0;
perr1940 0:a5986ef182dc 35 _phi[0][2]=232.1405;
perr1940 0:a5986ef182dc 36 _phi[0][3]=-256.2810;
perr1940 0:a5986ef182dc 37 _phi[0][4]=76.1405;
perr1940 0:a5986ef182dc 38 _phi[1][0]=30.0000;
perr1940 0:a5986ef182dc 39 _phi[1][1]=0;
perr1940 0:a5986ef182dc 40 _phi[1][2]=-152.2944;
perr1940 0:a5986ef182dc 41 _phi[1][3]=224.5888;
perr1940 0:a5986ef182dc 42 _phi[1][4]=-92.2944;
perr1940 0:a5986ef182dc 43 //_times{0, 1, 2},
perr1940 0:a5986ef182dc 44 _times[0]=0;
perr1940 0:a5986ef182dc 45 _times[1]=378;
perr1940 0:a5986ef182dc 46 _times[2]=900;
perr1940 0:a5986ef182dc 47 };
perr1940 0:a5986ef182dc 48
perr1940 0:a5986ef182dc 49 bool BradPolySwing::calculate(int time, float &value)
perr1940 0:a5986ef182dc 50 {
perr1940 0:a5986ef182dc 51 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 52 _blend.increment(value);
perr1940 0:a5986ef182dc 53 } else {
perr1940 0:a5986ef182dc 54 float tau=convert_to_tau(time);
perr1940 0:a5986ef182dc 55 value=_phi[_current_poly][0]+_phi[_current_poly][1]*tau+_phi[_current_poly][2]*tau*tau+_phi[_current_poly][3]*tau*tau*tau+_phi[_current_poly][4]*tau*tau*tau*tau;
perr1940 0:a5986ef182dc 56 }
perr1940 0:a5986ef182dc 57
perr1940 2:40f8f3a8e878 58 if(time>_params.time_steps) {
perr1940 0:a5986ef182dc 59 return 1;
perr1940 0:a5986ef182dc 60 } else {
perr1940 0:a5986ef182dc 61 return 0;
perr1940 0:a5986ef182dc 62 }
perr1940 0:a5986ef182dc 63 };
perr1940 0:a5986ef182dc 64
perr1940 0:a5986ef182dc 65 void BradPolySwing::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 66 {
perr1940 2:40f8f3a8e878 67 this->_params=p;
perr1940 0:a5986ef182dc 68 };
perr1940 0:a5986ef182dc 69
perr1940 0:a5986ef182dc 70 void BradPolySwing::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 71 {
perr1940 0:a5986ef182dc 72 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 73 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 74 _blend.restart();
perr1940 0:a5986ef182dc 75 };
perr1940 0:a5986ef182dc 76
perr1940 0:a5986ef182dc 77 float BradPolySwing::convert_to_tau(int time)
perr1940 0:a5986ef182dc 78 {
perr1940 0:a5986ef182dc 79 if (time>_times[_current_poly+1]) {
perr1940 0:a5986ef182dc 80 _current_poly++;
perr1940 0:a5986ef182dc 81 }
perr1940 0:a5986ef182dc 82 return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]);
perr1940 0:a5986ef182dc 83 }
perr1940 0:a5986ef182dc 84
perr1940 0:a5986ef182dc 85 void BradPolySwing::restart()
perr1940 0:a5986ef182dc 86 {
perr1940 0:a5986ef182dc 87 TrajectoryGenerator::restart();
perr1940 0:a5986ef182dc 88 _current_poly=0;
perr1940 0:a5986ef182dc 89 };
perr1940 0:a5986ef182dc 90
perr1940 0:a5986ef182dc 91 /** Stance trajectory generation
perr1940 0:a5986ef182dc 92 */
perr1940 2:40f8f3a8e878 93 BradPolyStance::BradPolyStance(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
perr1940 0:a5986ef182dc 94 {
perr1940 2:40f8f3a8e878 95 _phi[0][0]=10;
perr1940 0:a5986ef182dc 96 _phi[0][1]=0;
perr1940 0:a5986ef182dc 97 _phi[0][2]=-96;
perr1940 0:a5986ef182dc 98 _phi[0][3]=64;
perr1940 0:a5986ef182dc 99 _phi[0][4]=0;
perr1940 0:a5986ef182dc 100 _times[0]=0;
perr1940 2:40f8f3a8e878 101 _times[1]=900;
perr1940 0:a5986ef182dc 102 };
perr1940 0:a5986ef182dc 103
perr1940 0:a5986ef182dc 104 BradPolyStance::BradPolyStance():_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
perr1940 0:a5986ef182dc 105 {
perr1940 0:a5986ef182dc 106 _phi[0][0]=10;
perr1940 0:a5986ef182dc 107 _phi[0][1]=0;
perr1940 0:a5986ef182dc 108 _phi[0][2]=-96;
perr1940 0:a5986ef182dc 109 _phi[0][3]=64;
perr1940 0:a5986ef182dc 110 _phi[0][4]=0;
perr1940 0:a5986ef182dc 111 _times[0]=0;
perr1940 0:a5986ef182dc 112 _times[1]=900;
perr1940 0:a5986ef182dc 113 };
perr1940 0:a5986ef182dc 114
perr1940 0:a5986ef182dc 115 bool BradPolyStance::calculate(int time, float &value)
perr1940 0:a5986ef182dc 116 {
perr1940 0:a5986ef182dc 117 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 118 _blend.increment(value);
perr1940 0:a5986ef182dc 119 } else {
perr1940 0:a5986ef182dc 120 float tau=convert_to_tau(time);
perr1940 0:a5986ef182dc 121 value=_phi[_current_poly][0]+_phi[_current_poly][1]*tau+_phi[_current_poly][2]*tau*tau+_phi[_current_poly][3]*tau*tau*tau+_phi[_current_poly][4]*tau*tau*tau*tau;
perr1940 0:a5986ef182dc 122 }
perr1940 2:40f8f3a8e878 123 if(time>_params.time_steps) {
perr1940 0:a5986ef182dc 124 return 1;
perr1940 0:a5986ef182dc 125 } else {
perr1940 0:a5986ef182dc 126 return 0;
perr1940 0:a5986ef182dc 127 }
perr1940 0:a5986ef182dc 128
perr1940 0:a5986ef182dc 129 };
perr1940 0:a5986ef182dc 130
perr1940 0:a5986ef182dc 131 void BradPolyStance::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 132 {
perr1940 2:40f8f3a8e878 133 this->_params=p;
perr1940 0:a5986ef182dc 134 };
perr1940 0:a5986ef182dc 135
perr1940 0:a5986ef182dc 136 void BradPolyStance::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 137 {
perr1940 0:a5986ef182dc 138 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 139 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 140 _blend.restart();
perr1940 0:a5986ef182dc 141 };
perr1940 0:a5986ef182dc 142
perr1940 0:a5986ef182dc 143 float BradPolyStance::convert_to_tau(int time)
perr1940 0:a5986ef182dc 144 {
perr1940 0:a5986ef182dc 145 if (time>_times[_current_poly+1]) {
perr1940 0:a5986ef182dc 146 _current_poly++;
perr1940 0:a5986ef182dc 147 }
perr1940 0:a5986ef182dc 148 return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]);
perr1940 0:a5986ef182dc 149 }
perr1940 0:a5986ef182dc 150
perr1940 0:a5986ef182dc 151 void BradPolyStance::restart()
perr1940 0:a5986ef182dc 152 {
perr1940 0:a5986ef182dc 153 TrajectoryGenerator::restart();
perr1940 0:a5986ef182dc 154 _current_poly=0;
perr1940 0:a5986ef182dc 155 };
perr1940 0:a5986ef182dc 156
perr1940 0:a5986ef182dc 157 /** FS Swing trajectory generation
perr1940 0:a5986ef182dc 158 */
perr1940 2:40f8f3a8e878 159 BradPolyFSSwing::BradPolyFSSwing(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 160
perr1940 0:a5986ef182dc 161 BradPolyFSSwing::BradPolyFSSwing():_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 162
perr1940 0:a5986ef182dc 163 bool BradPolyFSSwing::calculate(int time, float &value)
perr1940 0:a5986ef182dc 164 {
perr1940 0:a5986ef182dc 165 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 166 _blend.increment(value);
perr1940 0:a5986ef182dc 167 } else {
perr1940 0:a5986ef182dc 168 //This equation is specific to the trajectory
perr1940 2:40f8f3a8e878 169 value = sin(PI/_params.time_steps*time)*_params.max_fs_angle;
perr1940 0:a5986ef182dc 170 }
perr1940 2:40f8f3a8e878 171 if(time>_params.time_steps) {
perr1940 0:a5986ef182dc 172 return 1;
perr1940 0:a5986ef182dc 173 } else {
perr1940 0:a5986ef182dc 174 return 0;
perr1940 0:a5986ef182dc 175 }
perr1940 0:a5986ef182dc 176
perr1940 0:a5986ef182dc 177 };
perr1940 0:a5986ef182dc 178
perr1940 0:a5986ef182dc 179 void BradPolyFSSwing::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 180 {
perr1940 2:40f8f3a8e878 181 this->_params=p;
perr1940 0:a5986ef182dc 182 };
perr1940 0:a5986ef182dc 183
perr1940 0:a5986ef182dc 184 void BradPolyFSSwing::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 185 {
perr1940 0:a5986ef182dc 186 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 187 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 188 _blend.restart();
perr1940 0:a5986ef182dc 189 };
perr1940 0:a5986ef182dc 190
perr1940 0:a5986ef182dc 191 /** FS Stance trajectory generation
perr1940 0:a5986ef182dc 192 */
perr1940 0:a5986ef182dc 193
perr1940 2:40f8f3a8e878 194 BradPolyFSStance::BradPolyFSStance(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 195
perr1940 0:a5986ef182dc 196 BradPolyFSStance::BradPolyFSStance():_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 197
perr1940 0:a5986ef182dc 198 bool BradPolyFSStance::calculate(int time, float &value)
perr1940 0:a5986ef182dc 199 {
perr1940 0:a5986ef182dc 200 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 201 _blend.increment(value);
perr1940 0:a5986ef182dc 202 } else {
perr1940 0:a5986ef182dc 203 //This equation is specific to the trajectory
perr1940 2:40f8f3a8e878 204 value = (_params.standing_angle)/_params.time_steps*time;
perr1940 0:a5986ef182dc 205 }
perr1940 2:40f8f3a8e878 206 if(time>_params.time_steps) {
perr1940 0:a5986ef182dc 207 return 1;
perr1940 0:a5986ef182dc 208 } else {
perr1940 0:a5986ef182dc 209 return 0;
perr1940 0:a5986ef182dc 210 }
perr1940 0:a5986ef182dc 211
perr1940 0:a5986ef182dc 212 };
perr1940 0:a5986ef182dc 213
perr1940 0:a5986ef182dc 214 void BradPolyFSStance::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 215 {
perr1940 2:40f8f3a8e878 216 this->_params=p;
perr1940 0:a5986ef182dc 217 };
perr1940 0:a5986ef182dc 218
perr1940 0:a5986ef182dc 219 void BradPolyFSStance::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 220 {
perr1940 0:a5986ef182dc 221 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 222 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 223 _blend.restart();
perr1940 0:a5986ef182dc 224 };