This is the quartic polynomial gait.
Brad_poly_gait.cpp
- Committer:
- perr1940
- Date:
- 2015-06-24
- Revision:
- 0:a5986ef182dc
- Child:
- 1:59243225dcd5
File content as of revision 0:a5986ef182dc:
#include "Brad_poly_gait.h" #include "math.h" //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() { //_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][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; }; bool BradPolySwing::calculate(int time, float &value) { if (time<=_blend_steps) { _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; } if(time>_params.time_steps) { return 1; } else { return 0; } }; void BradPolySwing::set(Brad_poly_gait_t& p) { this->_params=p; }; void BradPolySwing::init(float start, float end, int time_steps) { _blend.init(start,end,time_steps); _blend_steps=time_steps; _blend.restart(); }; float BradPolySwing::convert_to_tau(int time) { if (time>_times[_current_poly+1]) { _current_poly++; } return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]); } void BradPolySwing::restart() { TrajectoryGenerator::restart(); _current_poly=0; }; /** Stance trajectory generation */ BradPolyStance::BradPolyStance(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator() { _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; }; BradPolyStance::BradPolyStance():_blend(), _blend_steps(0), _current_poly(0), _tau(0), TrajectoryGenerator() { _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; }; bool BradPolyStance::calculate(int time, float &value) { if (time<=_blend_steps) { _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; } if(time>_params.time_steps) { return 1; } else { return 0; } }; void BradPolyStance::set(Brad_poly_gait_t& p) { this->_params=p; }; void BradPolyStance::init(float start, float end, int time_steps) { _blend.init(start,end,time_steps); _blend_steps=time_steps; _blend.restart(); }; float BradPolyStance::convert_to_tau(int time) { if (time>_times[_current_poly+1]) { _current_poly++; } return (float)(time-_times[_current_poly])/(_times[_current_poly+1]-_times[_current_poly]); } void BradPolyStance::restart() { TrajectoryGenerator::restart(); _current_poly=0; }; /** FS Swing trajectory generation */ BradPolyFSSwing::BradPolyFSSwing(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), TrajectoryGenerator() {}; BradPolyFSSwing::BradPolyFSSwing():_blend(), _blend_steps(0), TrajectoryGenerator() {}; bool BradPolyFSSwing::calculate(int time, float &value) { if (time<=_blend_steps) { _blend.increment(value); } else { //This equation is specific to the trajectory value = sin(PI/_params.time_steps*time)*_params.max_fs_angle; } if(time>_params.time_steps) { return 1; } else { return 0; } }; void BradPolyFSSwing::set(Brad_poly_gait_t& p) { this->_params=p; }; void BradPolyFSSwing::init(float start, float end, int time_steps) { _blend.init(start,end,time_steps); _blend_steps=time_steps; _blend.restart(); }; /** FS Stance trajectory generation */ BradPolyFSStance::BradPolyFSStance(Brad_poly_gait_t p):_params(p),_blend(), _blend_steps(0), TrajectoryGenerator() {}; BradPolyFSStance::BradPolyFSStance():_blend(), _blend_steps(0), TrajectoryGenerator() {}; bool BradPolyFSStance::calculate(int time, float &value) { if (time<=_blend_steps) { _blend.increment(value); } else { //This equation is specific to the trajectory value = (_params.standing_angle)/_params.time_steps*time; } if(time>_params.time_steps) { return 1; } else { return 0; } }; void BradPolyFSStance::set(Brad_poly_gait_t& p) { this->_params=p; }; void BradPolyFSStance::init(float start, float end, int time_steps) { _blend.init(start,end,time_steps); _blend_steps=time_steps; _blend.restart(); };