Bradley Perry / Brad_poly_gait

This is the quartic polynomial gait.

Dependents:   Motion-Control

Brad_poly_gait.cpp@4:f96bcc712a13, 2015-06-29 (annotated)

Committer:
perr1940
Date:
Mon Jun 29 17:25:21 2015 +0000
Revision:
4:f96bcc712a13
Parent:
3:ed0f6f302a0a 
Removed pc.  Moving on to test rig testing.

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 3:ed0f6f302a0a 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 4:f96bcc712a13 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 3:ed0f6f302a0a 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 3:ed0f6f302a0a 27 _times[2]=900;*/
perr1940 3:ed0f6f302a0a 28 calculate_phi();
perr1940 3:ed0f6f302a0a 29 };
perr1940 3:ed0f6f302a0a 30
perr1940 4:f96bcc712a13 31 BradPolySwing::BradPolySwing():_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
perr1940 3:ed0f6f302a0a 32 {
perr1940 3:ed0f6f302a0a 33 //_phi{{0, 1, 2, 3, 4}, {0, 1, 2, 3, 4}},
perr1940 3:ed0f6f302a0a 34 _phi[0][0]=-22;
perr1940 3:ed0f6f302a0a 35 _phi[1][0]=0;
perr1940 3:ed0f6f302a0a 36 _phi[2][0]=232.1405;
perr1940 3:ed0f6f302a0a 37 _phi[3][0]=-256.2810;
perr1940 3:ed0f6f302a0a 38 _phi[4][0]=76.1405;
perr1940 3:ed0f6f302a0a 39 _phi[0][1]=30.0000;
perr1940 3:ed0f6f302a0a 40 _phi[1][1]=0;
perr1940 3:ed0f6f302a0a 41 _phi[2][1]=-152.2944;
perr1940 3:ed0f6f302a0a 42 _phi[3][1]=224.5888;
perr1940 3:ed0f6f302a0a 43 _phi[4][1]=-92.2944;
perr1940 3:ed0f6f302a0a 44 //_times{0, 1, 2},
perr1940 3:ed0f6f302a0a 45 _times[0]=0;
perr1940 3:ed0f6f302a0a 46 _times[1]=378;
perr1940 0:a5986ef182dc 47 _times[2]=900;
perr1940 0:a5986ef182dc 48 };
perr1940 0:a5986ef182dc 49
perr1940 0:a5986ef182dc 50 bool BradPolySwing::calculate(int time, float &value)
perr1940 0:a5986ef182dc 51 {
perr1940 0:a5986ef182dc 52 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 53 _blend.increment(value);
perr1940 0:a5986ef182dc 54 } else {
perr1940 0:a5986ef182dc 55 float tau=convert_to_tau(time);
perr1940 3:ed0f6f302a0a 56 value=_phi[0][_current_poly]+_phi[1][_current_poly]*tau+_phi[2][_current_poly]*tau*tau+_phi[3][_current_poly]*tau*tau*tau+_phi[4][_current_poly]*tau*tau*tau*tau;
perr1940 0:a5986ef182dc 57 }
perr1940 0:a5986ef182dc 58
perr1940 3:ed0f6f302a0a 59 if(time>_params->time_steps) {
perr1940 0:a5986ef182dc 60 return 1;
perr1940 0:a5986ef182dc 61 } else {
perr1940 0:a5986ef182dc 62 return 0;
perr1940 0:a5986ef182dc 63 }
perr1940 0:a5986ef182dc 64 };
perr1940 0:a5986ef182dc 65
perr1940 0:a5986ef182dc 66 void BradPolySwing::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 67 {
perr1940 3:ed0f6f302a0a 68 this->_params=&p;
perr1940 3:ed0f6f302a0a 69 calculate_phi();
perr1940 3:ed0f6f302a0a 70
perr1940 0:a5986ef182dc 71 };
perr1940 0:a5986ef182dc 72
perr1940 0:a5986ef182dc 73 void BradPolySwing::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 74 {
perr1940 0:a5986ef182dc 75 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 76 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 77 _blend.restart();
perr1940 3:ed0f6f302a0a 78 calculate_phi();
perr1940 0:a5986ef182dc 79 };
perr1940 0:a5986ef182dc 80
perr1940 0:a5986ef182dc 81 float BradPolySwing::convert_to_tau(int time)
perr1940 0:a5986ef182dc 82 {
perr1940 0:a5986ef182dc 83 if (time>_times[_current_poly+1]) {
perr1940 0:a5986ef182dc 84 _current_poly++;
perr1940 0:a5986ef182dc 85 }
perr1940 0:a5986ef182dc 86 return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]);
perr1940 0:a5986ef182dc 87 }
perr1940 0:a5986ef182dc 88
perr1940 3:ed0f6f302a0a 89 void BradPolySwing::calculate_phi()
perr1940 3:ed0f6f302a0a 90 {
perr1940 3:ed0f6f302a0a 91 float del_t[]= {_params->peak_time,_params->time_steps-_params->peak_time,_params->time_steps};
perr1940 3:ed0f6f302a0a 92 _times[0]=0;
perr1940 3:ed0f6f302a0a 93 _times[1]=_params->peak_time;
perr1940 3:ed0f6f302a0a 94 _times[2]=_params->time_steps;
perr1940 3:ed0f6f302a0a 95
perr1940 3:ed0f6f302a0a 96 //calculate the phi's based on the parameters
perr1940 3:ed0f6f302a0a 97 float phi22=3.0*(_params->stance_end-_params->stance_start); //phi22 assuming all node velocities are 0
perr1940 3:ed0f6f302a0a 98 _phi[0][1]=_params->max_angle;
perr1940 3:ed0f6f302a0a 99 _phi[1][1]=0; //assume 0 velocity at peak hip; ensures maximum
perr1940 3:ed0f6f302a0a 100 _phi[2][1]= 6.0*_params->stance_start - 6.0*_params->max_angle + (del_t[1]*del_t[1]*phi22)/(del_t[2]*del_t[2]);//assuming velocities 0
perr1940 3:ed0f6f302a0a 101 _phi[3][1]= -(4.0*_phi[2][1])/3.0 - _phi[1][1] - (2.0*del_t[1]*del_t[1]*phi22)/(3.0*del_t[2]*del_t[2]);//asuming velocities=0
perr1940 3:ed0f6f302a0a 102 _phi[4][1]= (del_t[1]*del_t[1]*phi22)/(6.0*del_t[2]*del_t[2]) - _phi[3][1]/2 - _phi[2][1]/6.0;
perr1940 3:ed0f6f302a0a 103
perr1940 3:ed0f6f302a0a 104 _phi[0][0]=_params->stance_end;
perr1940 3:ed0f6f302a0a 105 _phi[1][0]=0; //assume 0 velocity at peak hip; ensures maximum
perr1940 3:ed0f6f302a0a 106 _phi[2][0]= 6.0*_params->max_angle - 6.0*_params->stance_end + (del_t[0]*del_t[0]*_phi[2][1])/(del_t[1]*del_t[1]);//assuming velocities 0
perr1940 3:ed0f6f302a0a 107 _phi[3][0]= -(4.0*_phi[2][0])/3.0 - _phi[1][0] - (2.0*del_t[0]*del_t[0]*_phi[2][1])/(3.0*del_t[1]*del_t[1]);//asuming velocities=0
perr1940 3:ed0f6f302a0a 108 _phi[4][0]= (del_t[0]*del_t[0]*_phi[2][1])/(6.0*del_t[1]*del_t[1]) - _phi[3][0]/2.0 - _phi[2][0]/6.0;
perr1940 3:ed0f6f302a0a 109
perr1940 3:ed0f6f302a0a 110 }
perr1940 3:ed0f6f302a0a 111
perr1940 0:a5986ef182dc 112 void BradPolySwing::restart()
perr1940 0:a5986ef182dc 113 {
perr1940 0:a5986ef182dc 114 TrajectoryGenerator::restart();
perr1940 0:a5986ef182dc 115 _current_poly=0;
perr1940 0:a5986ef182dc 116 };
perr1940 0:a5986ef182dc 117
perr1940 0:a5986ef182dc 118 /** Stance trajectory generation
perr1940 0:a5986ef182dc 119 */
perr1940 4:f96bcc712a13 120 BradPolyStance::BradPolyStance(Brad_poly_gait_t& p):_params(&p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
perr1940 0:a5986ef182dc 121 {
perr1940 3:ed0f6f302a0a 122 /*_phi[0][0]=10;
perr1940 0:a5986ef182dc 123 _phi[0][1]=0;
perr1940 0:a5986ef182dc 124 _phi[0][2]=-96;
perr1940 0:a5986ef182dc 125 _phi[0][3]=64;
perr1940 0:a5986ef182dc 126 _phi[0][4]=0;
perr1940 0:a5986ef182dc 127 _times[0]=0;
perr1940 3:ed0f6f302a0a 128 _times[1]=900;*/
perr1940 3:ed0f6f302a0a 129 calculate_phi();
perr1940 0:a5986ef182dc 130 };
perr1940 0:a5986ef182dc 131
perr1940 4:f96bcc712a13 132 BradPolyStance::BradPolyStance():_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
perr1940 0:a5986ef182dc 133 {
perr1940 0:a5986ef182dc 134 _phi[0][0]=10;
perr1940 3:ed0f6f302a0a 135 _phi[1][0]=0;
perr1940 3:ed0f6f302a0a 136 _phi[2][0]=-96;
perr1940 3:ed0f6f302a0a 137 _phi[3][0]=64;
perr1940 3:ed0f6f302a0a 138 _phi[4][0]=0;
perr1940 0:a5986ef182dc 139 _times[0]=0;
perr1940 0:a5986ef182dc 140 _times[1]=900;
perr1940 0:a5986ef182dc 141 };
perr1940 0:a5986ef182dc 142
perr1940 0:a5986ef182dc 143 bool BradPolyStance::calculate(int time, float &value)
perr1940 0:a5986ef182dc 144 {
perr1940 0:a5986ef182dc 145 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 146 _blend.increment(value);
perr1940 0:a5986ef182dc 147 } else {
perr1940 0:a5986ef182dc 148 float tau=convert_to_tau(time);
perr1940 3:ed0f6f302a0a 149 value=_phi[0][_current_poly]+_phi[1][_current_poly]*tau+_phi[2][_current_poly]*tau*tau+_phi[3][_current_poly]*tau*tau*tau+_phi[4][_current_poly]*tau*tau*tau*tau;
perr1940 0:a5986ef182dc 150 }
perr1940 3:ed0f6f302a0a 151 if(time>_params->time_steps) {
perr1940 0:a5986ef182dc 152 return 1;
perr1940 0:a5986ef182dc 153 } else {
perr1940 0:a5986ef182dc 154 return 0;
perr1940 0:a5986ef182dc 155 }
perr1940 0:a5986ef182dc 156
perr1940 0:a5986ef182dc 157 };
perr1940 0:a5986ef182dc 158
perr1940 0:a5986ef182dc 159 void BradPolyStance::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 160 {
perr1940 3:ed0f6f302a0a 161 this->_params=&p;
perr1940 3:ed0f6f302a0a 162 calculate_phi();
perr1940 3:ed0f6f302a0a 163
perr1940 0:a5986ef182dc 164 };
perr1940 0:a5986ef182dc 165
perr1940 0:a5986ef182dc 166 void BradPolyStance::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 167 {
perr1940 0:a5986ef182dc 168 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 169 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 170 _blend.restart();
perr1940 3:ed0f6f302a0a 171 calculate_phi();
perr1940 0:a5986ef182dc 172 };
perr1940 0:a5986ef182dc 173
perr1940 0:a5986ef182dc 174 float BradPolyStance::convert_to_tau(int time)
perr1940 0:a5986ef182dc 175 {
perr1940 0:a5986ef182dc 176 if (time>_times[_current_poly+1]) {
perr1940 0:a5986ef182dc 177 _current_poly++;
perr1940 0:a5986ef182dc 178 }
perr1940 0:a5986ef182dc 179 return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]);
perr1940 0:a5986ef182dc 180 }
perr1940 0:a5986ef182dc 181
perr1940 3:ed0f6f302a0a 182 void BradPolyStance::calculate_phi()
perr1940 3:ed0f6f302a0a 183 {
perr1940 3:ed0f6f302a0a 184 float del_t[]= {_params->time_steps};
perr1940 3:ed0f6f302a0a 185 _times[0]=0;
perr1940 3:ed0f6f302a0a 186 _times[1]=_params->time_steps;
perr1940 3:ed0f6f302a0a 187
perr1940 3:ed0f6f302a0a 188 //calculate the phi's based on the parameters
perr1940 3:ed0f6f302a0a 189 _phi[0][0]=_params->stance_start;
perr1940 3:ed0f6f302a0a 190 _phi[1][0]=0; //assume 0 velocity at peak hip; ensures maximum
perr1940 3:ed0f6f302a0a 191 _phi[2][0]=3.0*(_params->stance_end-_params->stance_start); //phi22 assuming all node velocities are 0
perr1940 3:ed0f6f302a0a 192 _phi[3][0]= -2.0/3.0*_phi[2][0];//asuming velocities=0
perr1940 3:ed0f6f302a0a 193 _phi[4][0]= 0;
perr1940 3:ed0f6f302a0a 194 }
perr1940 3:ed0f6f302a0a 195
perr1940 0:a5986ef182dc 196 void BradPolyStance::restart()
perr1940 0:a5986ef182dc 197 {
perr1940 0:a5986ef182dc 198 TrajectoryGenerator::restart();
perr1940 0:a5986ef182dc 199 _current_poly=0;
perr1940 0:a5986ef182dc 200 };
perr1940 0:a5986ef182dc 201
perr1940 0:a5986ef182dc 202 /** FS Swing trajectory generation
perr1940 0:a5986ef182dc 203 */
perr1940 3:ed0f6f302a0a 204 BradPolyFSSwing::BradPolyFSSwing(Brad_poly_gait_t& p):_params(&p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 205
perr1940 0:a5986ef182dc 206 BradPolyFSSwing::BradPolyFSSwing():_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 207
perr1940 0:a5986ef182dc 208 bool BradPolyFSSwing::calculate(int time, float &value)
perr1940 0:a5986ef182dc 209 {
perr1940 0:a5986ef182dc 210 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 211 _blend.increment(value);
perr1940 0:a5986ef182dc 212 } else {
perr1940 0:a5986ef182dc 213 //This equation is specific to the trajectory
perr1940 3:ed0f6f302a0a 214 value = sin(PI/_params->time_steps*time)*_params->max_fs_angle;
perr1940 0:a5986ef182dc 215 }
perr1940 3:ed0f6f302a0a 216 if(time>_params->time_steps) {
perr1940 0:a5986ef182dc 217 return 1;
perr1940 0:a5986ef182dc 218 } else {
perr1940 0:a5986ef182dc 219 return 0;
perr1940 0:a5986ef182dc 220 }
perr1940 0:a5986ef182dc 221
perr1940 0:a5986ef182dc 222 };
perr1940 0:a5986ef182dc 223
perr1940 0:a5986ef182dc 224 void BradPolyFSSwing::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 225 {
perr1940 3:ed0f6f302a0a 226 this->_params=&p;
perr1940 0:a5986ef182dc 227 };
perr1940 0:a5986ef182dc 228
perr1940 0:a5986ef182dc 229 void BradPolyFSSwing::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 230 {
perr1940 0:a5986ef182dc 231 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 232 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 233 _blend.restart();
perr1940 0:a5986ef182dc 234 };
perr1940 0:a5986ef182dc 235
perr1940 0:a5986ef182dc 236 /** FS Stance trajectory generation
perr1940 0:a5986ef182dc 237 */
perr1940 0:a5986ef182dc 238
perr1940 3:ed0f6f302a0a 239 BradPolyFSStance::BradPolyFSStance(Brad_poly_gait_t& p):_params(&p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 240
perr1940 0:a5986ef182dc 241 BradPolyFSStance::BradPolyFSStance():_blend(), _blend_steps(0), TrajectoryGenerator() {};
perr1940 0:a5986ef182dc 242
perr1940 0:a5986ef182dc 243 bool BradPolyFSStance::calculate(int time, float &value)
perr1940 0:a5986ef182dc 244 {
perr1940 0:a5986ef182dc 245 if (time<=_blend_steps) {
perr1940 0:a5986ef182dc 246 _blend.increment(value);
perr1940 0:a5986ef182dc 247 } else {
perr1940 0:a5986ef182dc 248 //This equation is specific to the trajectory
perr1940 3:ed0f6f302a0a 249 value = (_params->standing_angle)/_params->time_steps*time;
perr1940 0:a5986ef182dc 250 }
perr1940 3:ed0f6f302a0a 251 if(time>_params->time_steps) {
perr1940 0:a5986ef182dc 252 return 1;
perr1940 0:a5986ef182dc 253 } else {
perr1940 0:a5986ef182dc 254 return 0;
perr1940 0:a5986ef182dc 255 }
perr1940 0:a5986ef182dc 256
perr1940 0:a5986ef182dc 257 };
perr1940 0:a5986ef182dc 258
perr1940 0:a5986ef182dc 259 void BradPolyFSStance::set(Brad_poly_gait_t& p)
perr1940 0:a5986ef182dc 260 {
perr1940 3:ed0f6f302a0a 261 this->_params=&p;
perr1940 0:a5986ef182dc 262 };
perr1940 0:a5986ef182dc 263
perr1940 0:a5986ef182dc 264 void BradPolyFSStance::init(float start, float end, int time_steps)
perr1940 0:a5986ef182dc 265 {
perr1940 0:a5986ef182dc 266 _blend.init(start,end,time_steps);
perr1940 0:a5986ef182dc 267 _blend_steps=time_steps;
perr1940 0:a5986ef182dc 268 _blend.restart();
perr1940 0:a5986ef182dc 269 };