File content as of revision 1:59243225dcd5:

#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;*/
    float del_t[]= {_params->peak_time,_params->time_steps,2*_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=(_params->stance_end-_params->stance_start)/3; //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*_params->stance_start - 3*phi[1][1] - 6*phi[0][1] + (del_t[1]*del_t[1]*phi22)/(2*del_t[2]*del_t[2]);//assuming velocities 0
    _phi[3][1]= -(4*_phi[2][1])/3 - _phi[1][1] - (del_t[1]*del_t[1]*phi22)/(3*del_t[2]*del_t[2]);//asuming velocities=0
    _phi[4][1]= (del_t[1]*del_t[1]*phi22)/(12*del_t[2]*del_t[2]) - _phi[3][1]/2 - _phi[2][1]/6;

    _phi[0][0]=_params->max_angle;
    _phi[1][0]=0; //assume 0 velocity at peak hip; ensures maximum
    _phi[2][0]= 6*_params->stance_start - 3*phi[1][0] - 6*phi[0][0] + (del_t[0]*del_t[0]*phi22)/(2*del_t[1]*del_t[1]);//assuming velocities 0
    _phi[3][0]= -(4*_phi[2][0])/3 - _phi[1][0] - (del_t[0]*del_t[0]*_phi[2][1])/(3*del_t[1]*del_t[1]);//asuming velocities=0
    _phi[4][0]= (del_t[0]*del_t[0]*_phi[2][1])/(12*del_t[1]*del_t[1]) - _phi[3][0]/2 - _phi[2][0]/6;
};

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;

    float del_t[]= {_params->peak_time,_params->time_steps,2*_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=(_params->stance_end-_params->stance_start)/3; //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]= -(3*_phi[1][1])/4 - (del_t[1]*del_t[1]*phi22)/(4*del_t[2]*del_t[2]);//assuming velocities 0
    _phi[3][1]= -(4*_phi[2][1])/3 - _phi[1][1] - (del_t[1]*del_t[1]*phi22)/(3*del_t[2]*del_t[2]);//asuming velocities=0
    _phi[4][1]= (del_t[1]*del_t[1]*phi22)/(12*del_t[2]*del_t[2]) - _phi[3][1]/2 - _phi[2][1]/6;

    _phi[0][0]=_params->max_angle;
    _phi[1][0]=0; //assume 0 velocity at peak hip; ensures maximum
    _phi[2][0]= -(3*_phi[1][0])/4 - (del_t[0]*del_t[0]*_phi[2][1])/(4*del_t[1]*del_t[1]);//assuming velocities 0
    _phi[3][0]= -(4*_phi[2][0])/3 - _phi[1][0] - (del_t[0]*del_t[0]*_phi[2][1])/(3*del_t[1]*del_t[1]);//asuming velocities=0
    _phi[4][0]= (del_t[0]*del_t[0]*_phi[2][1])/(12*del_t[1]*del_t[1]) - _phi[3][0]/2 - _phi[2][0]/6;
};

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;*/
    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]=(_params->stance_end-_params->stance_start)/3; //phi22 assuming all node velocities are 0
    _phi[3][0]= -2/3*_phi[2][0];//asuming velocities=0
    _phi[4][0]= 0;
};

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;

    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]=(_params->stance_end-_params->stance_start)/3; //phi22 assuming all node velocities are 0
    _phi[3][0]= -2/3*_phi[2][0];//asuming velocities=0
    _phi[4][0]= 0;
};

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();
};