Bradley Perry
This is the quartic polynomial gait.
Diff: Brad_poly_gait.cpp
- Revision:
- 3:ed0f6f302a0a
- Parent:
- 2:40f8f3a8e878
- Child:
- 4:f96bcc712a13
--- a/Brad_poly_gait.cpp Mon Jun 29 00:34:24 2015 +0000
+++ b/Brad_poly_gait.cpp Mon Jun 29 17:22:57 2015 +0000
@@ -1,36 +1,17 @@
#include "Brad_poly_gait.h"
#include "math.h"
-//TODO: Add a calculation of max flexion angle for FS _params.max_fs_angle
+//TODO: Add a calculation of max flexion angle for FS _params->max_fs_angle
const float PI =3.141592653589793;
/** Swing trajectory generation
*/
-BradPolySwing::BradPolySwing(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
+BradPolySwing::BradPolySwing(Brad_poly_gait_t& p):_params(&p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), pc(USBTX,USBRX), TrajectoryGenerator()
{
//_phi{{0, 1, 2, 3, 4}, {0, 1, 2, 3, 4}},
- _phi[0][0]=-22;
- _phi[0][1]=0;
- _phi[0][2]=232.1405;
- _phi[0][3]=-256.2810;
- _phi[0][4]=76.1405;
- _phi[1][0]=30.0000;
- _phi[1][1]=0;
- _phi[1][2]=-152.2944;
- _phi[1][3]=224.5888;
- _phi[1][4]=-92.2944;
- //_times{0, 1, 2},
- _times[0]=0;
- _times[1]=378;
- _times[2]=900;
-};
-
-BradPolySwing::BradPolySwing():_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
-{
- //_phi{{0, 1, 2, 3, 4}, {0, 1, 2, 3, 4}},
- _phi[0][0]=-22;
+ /*_phi[0][0]=-22;
_phi[0][1]=0;
_phi[0][2]=232.1405;
_phi[0][3]=-256.2810;
@@ -43,7 +24,29 @@
//_times{0, 1, 2},
_times[0]=0;
_times[1]=378;
+ _times[2]=900;*/
+ pc.baud(921600);
+ calculate_phi();
+};
+
+BradPolySwing::BradPolySwing():_blend(), _blend_steps(0), _current_poly(0), _tau(0), pc(USBTX,USBRX), TrajectoryGenerator()
+{
+ //_phi{{0, 1, 2, 3, 4}, {0, 1, 2, 3, 4}},
+ _phi[0][0]=-22;
+ _phi[1][0]=0;
+ _phi[2][0]=232.1405;
+ _phi[3][0]=-256.2810;
+ _phi[4][0]=76.1405;
+ _phi[0][1]=30.0000;
+ _phi[1][1]=0;
+ _phi[2][1]=-152.2944;
+ _phi[3][1]=224.5888;
+ _phi[4][1]=-92.2944;
+ //_times{0, 1, 2},
+ _times[0]=0;
+ _times[1]=378;
_times[2]=900;
+ pc.baud(921600);
};
bool BradPolySwing::calculate(int time, float &value)
@@ -52,10 +55,14 @@
_blend.increment(value);
} else {
float tau=convert_to_tau(time);
- 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;
+ /*pc.printf("%f, ", tau);
+ for(int ii=0; ii<5; ii++) {
+ pc.printf("%f, ", _phi[ii][_current_poly]);
+ }*/
+ 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;
}
- if(time>_params.time_steps) {
+ if(time>_params->time_steps) {
return 1;
} else {
return 0;
@@ -64,7 +71,9 @@
void BradPolySwing::set(Brad_poly_gait_t& p)
{
- this->_params=p;
+ this->_params=&p;
+ calculate_phi();
+
};
void BradPolySwing::init(float start, float end, int time_steps)
@@ -72,6 +81,7 @@
_blend.init(start,end,time_steps);
_blend_steps=time_steps;
_blend.restart();
+ calculate_phi();
};
float BradPolySwing::convert_to_tau(int time)
@@ -82,6 +92,29 @@
return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]);
}
+void BradPolySwing::calculate_phi()
+{
+ float del_t[]= {_params->peak_time,_params->time_steps-_params->peak_time,_params->time_steps};
+ _times[0]=0;
+ _times[1]=_params->peak_time;
+ _times[2]=_params->time_steps;
+
+ //calculate the phi's based on the parameters
+ float phi22=3.0*(_params->stance_end-_params->stance_start); //phi22 assuming all node velocities are 0
+ _phi[0][1]=_params->max_angle;
+ _phi[1][1]=0; //assume 0 velocity at peak hip; ensures maximum
+ _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
+ _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
+ _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;
+
+ _phi[0][0]=_params->stance_end;
+ _phi[1][0]=0; //assume 0 velocity at peak hip; ensures maximum
+ _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
+ _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
+ _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;
+
+}
+
void BradPolySwing::restart()
{
TrajectoryGenerator::restart();
@@ -90,26 +123,29 @@
/** Stance trajectory generation
*/
-BradPolyStance::BradPolyStance(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
+BradPolyStance::BradPolyStance(Brad_poly_gait_t& p):_params(&p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), pc(USBTX,USBRX), TrajectoryGenerator()
{
- _phi[0][0]=10;
+ /*_phi[0][0]=10;
_phi[0][1]=0;
_phi[0][2]=-96;
_phi[0][3]=64;
_phi[0][4]=0;
_times[0]=0;
- _times[1]=900;
+ _times[1]=900;*/
+ pc.baud(921600);
+ calculate_phi();
};
-BradPolyStance::BradPolyStance():_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator()
+BradPolyStance::BradPolyStance():_blend(), _blend_steps(0), _current_poly(0), _tau(0), pc(USBTX,USBRX), TrajectoryGenerator()
{
_phi[0][0]=10;
- _phi[0][1]=0;
- _phi[0][2]=-96;
- _phi[0][3]=64;
- _phi[0][4]=0;
+ _phi[1][0]=0;
+ _phi[2][0]=-96;
+ _phi[3][0]=64;
+ _phi[4][0]=0;
_times[0]=0;
_times[1]=900;
+ pc.baud(921600);
};
bool BradPolyStance::calculate(int time, float &value)
@@ -118,9 +154,13 @@
_blend.increment(value);
} else {
float tau=convert_to_tau(time);
- 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;
+ /*pc.printf("%f, ", tau);
+ for(int ii=0; ii<5; ii++) {
+ pc.printf("%f, ", _phi[ii][_current_poly]);
+ }*/
+ 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;
}
- if(time>_params.time_steps) {
+ if(time>_params->time_steps) {
return 1;
} else {
return 0;
@@ -130,7 +170,9 @@
void BradPolyStance::set(Brad_poly_gait_t& p)
{
- this->_params=p;
+ this->_params=&p;
+ calculate_phi();
+
};
void BradPolyStance::init(float start, float end, int time_steps)
@@ -138,6 +180,7 @@
_blend.init(start,end,time_steps);
_blend_steps=time_steps;
_blend.restart();
+ calculate_phi();
};
float BradPolyStance::convert_to_tau(int time)
@@ -148,6 +191,20 @@
return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]);
}
+void BradPolyStance::calculate_phi()
+{
+ float del_t[]= {_params->time_steps};
+ _times[0]=0;
+ _times[1]=_params->time_steps;
+
+ //calculate the phi's based on the parameters
+ _phi[0][0]=_params->stance_start;
+ _phi[1][0]=0; //assume 0 velocity at peak hip; ensures maximum
+ _phi[2][0]=3.0*(_params->stance_end-_params->stance_start); //phi22 assuming all node velocities are 0
+ _phi[3][0]= -2.0/3.0*_phi[2][0];//asuming velocities=0
+ _phi[4][0]= 0;
+}
+
void BradPolyStance::restart()
{
TrajectoryGenerator::restart();
@@ -156,7 +213,7 @@
/** FS Swing trajectory generation
*/
-BradPolyFSSwing::BradPolyFSSwing(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
+BradPolyFSSwing::BradPolyFSSwing(Brad_poly_gait_t& p):_params(&p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
BradPolyFSSwing::BradPolyFSSwing():_blend(), _blend_steps(0), TrajectoryGenerator() {};
@@ -166,9 +223,9 @@
_blend.increment(value);
} else {
//This equation is specific to the trajectory
- value = sin(PI/_params.time_steps*time)*_params.max_fs_angle;
+ value = sin(PI/_params->time_steps*time)*_params->max_fs_angle;
}
- if(time>_params.time_steps) {
+ if(time>_params->time_steps) {
return 1;
} else {
return 0;
@@ -178,7 +235,7 @@
void BradPolyFSSwing::set(Brad_poly_gait_t& p)
{
- this->_params=p;
+ this->_params=&p;
};
void BradPolyFSSwing::init(float start, float end, int time_steps)
@@ -191,7 +248,7 @@
/** FS Stance trajectory generation
*/
-BradPolyFSStance::BradPolyFSStance(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
+BradPolyFSStance::BradPolyFSStance(Brad_poly_gait_t& p):_params(&p),_blend(), _blend_steps(0), TrajectoryGenerator() {};
BradPolyFSStance::BradPolyFSStance():_blend(), _blend_steps(0), TrajectoryGenerator() {};
@@ -201,9 +258,9 @@
_blend.increment(value);
} else {
//This equation is specific to the trajectory
- value = (_params.standing_angle)/_params.time_steps*time;
+ value = (_params->standing_angle)/_params->time_steps*time;
}
- if(time>_params.time_steps) {
+ if(time>_params->time_steps) {
return 1;
} else {
return 0;
@@ -213,7 +270,7 @@
void BradPolyFSStance::set(Brad_poly_gait_t& p)
{
- this->_params=p;
+ this->_params=&p;
};
void BradPolyFSStance::init(float start, float end, int time_steps)