Bradley Perry / Brad_poly_gait

This is the quartic polynomial gait.

Dependents:   Motion-Control

Brad_poly_gait.h@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 #ifndef BRADGAIT_H
perr1940 0:a5986ef182dc 2 #define BRADGAIT_H
perr1940 0:a5986ef182dc 3 #include "gait_generator.h"
perr1940 0:a5986ef182dc 4 #include "blend_generator.h"
perr1940 0:a5986ef182dc 5
perr1940 0:a5986ef182dc 6 //TODO: Add time_steps calculation in another file (App input should be a time in seconds)
perr1940 0:a5986ef182dc 7 //TODO: Add peak_time calculation in another file (App input should be a fraction)
perr1940 0:a5986ef182dc 8 //TODO: Add Feet Together Trajectories
perr1940 0:a5986ef182dc 9
perr1940 0:a5986ef182dc 10 /**
perr1940 0:a5986ef182dc 11 * Copyright (c) 2015
perr1940 0:a5986ef182dc 12 * All rights reserved.
perr1940 0:a5986ef182dc 13 * Bradley Perry quartic spline gait functions.
perr1940 0:a5986ef182dc 14 *
perr1940 0:a5986ef182dc 15 * @file Brad_poly_gait.h
perr1940 0:a5986ef182dc 16 * @author Bradley Perry
perr1940 0:a5986ef182dc 17 *
perr1940 0:a5986ef182dc 18 * @brief This contains the trajectories and gaits that define the Bradley Perry quartic spline gait. This is dependent on blend_generator.h and gait_generator.h
perr1940 0:a5986ef182dc 19 */
perr1940 0:a5986ef182dc 20
perr1940 0:a5986ef182dc 21 /** A struct that holds all the variables necessary to develop gaits.
perr1940 0:a5986ef182dc 22 */
perr1940 0:a5986ef182dc 23 struct Brad_poly_gait_t {
perr1940 0:a5986ef182dc 24 /** number of time steps per step
perr1940 0:a5986ef182dc 25 */
perr1940 0:a5986ef182dc 26 int time_steps;
perr1940 0:a5986ef182dc 27 /** time of peak hip angle
perr1940 0:a5986ef182dc 28 */
perr1940 0:a5986ef182dc 29 float peak_time;
perr1940 0:a5986ef182dc 30 /** stance start hip angle
perr1940 0:a5986ef182dc 31 */
perr1940 0:a5986ef182dc 32 float stance_start;
perr1940 0:a5986ef182dc 33 /** stance end hip angle
perr1940 0:a5986ef182dc 34 */
perr1940 0:a5986ef182dc 35 float stance_end;
perr1940 0:a5986ef182dc 36 /** max hip flexion angle
perr1940 0:a5986ef182dc 37 */
perr1940 0:a5986ef182dc 38 float max_angle;
perr1940 0:a5986ef182dc 39 /** offset to hip angles during double stance for comfort
perr1940 0:a5986ef182dc 40 */
perr1940 0:a5986ef182dc 41 float doublestance_offset;
perr1940 0:a5986ef182dc 42 /** maximum angle achieved during the first step
perr1940 0:a5986ef182dc 43 */
perr1940 0:a5986ef182dc 44 float max_fs_angle;
perr1940 0:a5986ef182dc 45 /** the angle of both hips during standing
perr1940 0:a5986ef182dc 46 */
perr1940 0:a5986ef182dc 47 float standing_angle;
perr1940 0:a5986ef182dc 48 };
perr1940 0:a5986ef182dc 49
perr1940 0:a5986ef182dc 50 /** A class that defines the swing trajectory per Michael McKinley's design.
perr1940 0:a5986ef182dc 51 */
perr1940 0:a5986ef182dc 52 class BradPolySwing: public TrajectoryGenerator
perr1940 0:a5986ef182dc 53 {
perr1940 0:a5986ef182dc 54 public:
perr1940 0:a5986ef182dc 55 /** Construct the object by passing in a set of parameters.
perr1940 0:a5986ef182dc 56 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 57 */
perr1940 3:ed0f6f302a0a 58 BradPolySwing (Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 59 /** Basic constructor.
perr1940 0:a5986ef182dc 60 */
perr1940 0:a5986ef182dc 61 BradPolySwing();
perr1940 0:a5986ef182dc 62 using TrajectoryGenerator::set;
perr1940 0:a5986ef182dc 63 /** Pass in a set of gait parameters.
perr1940 0:a5986ef182dc 64 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 65 */
perr1940 0:a5986ef182dc 66 void set(Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 67 using TrajectoryGenerator::init;
perr1940 0:a5986ef182dc 68 /** Initialize the linear blend by passing in a couple arguments.
perr1940 0:a5986ef182dc 69 * @param start The position you want to start the linear blend from.
perr1940 0:a5986ef182dc 70 * @param end The position you want to end the linear blend at.
perr1940 0:a5986ef182dc 71 * @param time_steps The number of time steps needed for the linear blend.
perr1940 0:a5986ef182dc 72 */
perr1940 0:a5986ef182dc 73 virtual void init(float start, float end, int time_steps);
perr1940 0:a5986ef182dc 74 using TrajectoryGenerator::calculate;
perr1940 0:a5986ef182dc 75 /** Calculates the swing reference based on the current time.
perr1940 0:a5986ef182dc 76 * @param time The time to calculate the trajectory on.
perr1940 0:a5986ef182dc 77 * @param value Pointer to the variable that you're saving the trajectory at the specified time to.
perr1940 0:a5986ef182dc 78 * @returns A flag that signals if there was an error.
perr1940 0:a5986ef182dc 79 */
perr1940 0:a5986ef182dc 80 virtual bool calculate(int time, float& value);
perr1940 0:a5986ef182dc 81 /** Restart from the start of the trajectory.
perr1940 0:a5986ef182dc 82 */
perr1940 0:a5986ef182dc 83 virtual void restart();
perr1940 0:a5986ef182dc 84 private:
perr1940 0:a5986ef182dc 85 /** The private struct to store all the parameters in.
perr1940 0:a5986ef182dc 86 */
perr1940 3:ed0f6f302a0a 87 Brad_poly_gait_t* _params;
perr1940 0:a5986ef182dc 88 /** The linear blend object for blending.
perr1940 0:a5986ef182dc 89 */
perr1940 0:a5986ef182dc 90 LinearBlend _blend;
perr1940 0:a5986ef182dc 91
perr1940 0:a5986ef182dc 92 /**Converts to normalized time*/
perr1940 0:a5986ef182dc 93 float convert_to_tau(int time);
perr1940 0:a5986ef182dc 94
perr1940 3:ed0f6f302a0a 95 /**Converts from the paramters to the matrix of coefficients*/
perr1940 3:ed0f6f302a0a 96 void calculate_phi();
perr1940 3:ed0f6f302a0a 97
perr1940 0:a5986ef182dc 98 /** The number of time steps spent blending.
perr1940 0:a5986ef182dc 99 */
perr1940 0:a5986ef182dc 100 int _blend_steps;
perr1940 0:a5986ef182dc 101
perr1940 0:a5986ef182dc 102 /** A temporary variable that contains the polynomial coefficients */
perr1940 3:ed0f6f302a0a 103 float _phi[5][2];
perr1940 0:a5986ef182dc 104
perr1940 0:a5986ef182dc 105 /**A temporary variable that contains the change times */
perr1940 0:a5986ef182dc 106 int _times[3];
perr1940 0:a5986ef182dc 107
perr1940 0:a5986ef182dc 108 /** Which polynomial we have selected*/
perr1940 0:a5986ef182dc 109 int _current_poly;
perr1940 0:a5986ef182dc 110
perr1940 0:a5986ef182dc 111 /**Normalized time */
perr1940 0:a5986ef182dc 112 float _tau;
perr1940 0:a5986ef182dc 113
perr1940 3:ed0f6f302a0a 114
perr1940 0:a5986ef182dc 115 };
perr1940 0:a5986ef182dc 116
perr1940 0:a5986ef182dc 117 /** A class that defines the stance trajectory per Michael McKinley's design.
perr1940 0:a5986ef182dc 118 */
perr1940 0:a5986ef182dc 119
perr1940 0:a5986ef182dc 120 class BradPolyStance: public TrajectoryGenerator
perr1940 0:a5986ef182dc 121 {
perr1940 0:a5986ef182dc 122 public:
perr1940 0:a5986ef182dc 123 /** Construct the object by passing in a set of parameters.
perr1940 0:a5986ef182dc 124 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 125 */
perr1940 3:ed0f6f302a0a 126 BradPolyStance(Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 127 /** Basic constructor.
perr1940 0:a5986ef182dc 128 */
perr1940 0:a5986ef182dc 129 BradPolyStance();
perr1940 0:a5986ef182dc 130 using TrajectoryGenerator::set;
perr1940 0:a5986ef182dc 131 /** Pass in a set of gait parameters.
perr1940 0:a5986ef182dc 132 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 133 */
perr1940 0:a5986ef182dc 134 void set(Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 135 using TrajectoryGenerator::init;
perr1940 0:a5986ef182dc 136 /** Initialize the linear blend by passing in a couple arguments.
perr1940 0:a5986ef182dc 137 * @param start The position you want to start the linear blend from.
perr1940 0:a5986ef182dc 138 * @param end The position you want to end the linear blend at.
perr1940 0:a5986ef182dc 139 * @param time_steps The number of time steps needed for the linear blend.
perr1940 0:a5986ef182dc 140 */
perr1940 0:a5986ef182dc 141 virtual void init(float start, float end, int time_steps);
perr1940 0:a5986ef182dc 142 /** Calculates the stance reference based on the current time.
perr1940 0:a5986ef182dc 143 * @param time The time to calculate the trajectory on.
perr1940 0:a5986ef182dc 144 * @param value Pointer to the variable that you're saving the trajectory at the specified time to.
perr1940 0:a5986ef182dc 145 * @returns A flag that signals if there was an error.
perr1940 0:a5986ef182dc 146 */
perr1940 0:a5986ef182dc 147 virtual bool calculate(int time, float& value);
perr1940 0:a5986ef182dc 148 /** Restart from the start of the trajectory.
perr1940 0:a5986ef182dc 149 */
perr1940 0:a5986ef182dc 150 virtual void restart();
perr1940 0:a5986ef182dc 151 private:
perr1940 0:a5986ef182dc 152 /** The private struct to store all the parameters in.
perr1940 0:a5986ef182dc 153 */
perr1940 3:ed0f6f302a0a 154 Brad_poly_gait_t* _params;
perr1940 0:a5986ef182dc 155 /** The linear blend object for blending.
perr1940 0:a5986ef182dc 156 */
perr1940 0:a5986ef182dc 157 LinearBlend _blend;
perr1940 0:a5986ef182dc 158 /**Converts to normalized time*/
perr1940 0:a5986ef182dc 159 float convert_to_tau(int time);
perr1940 0:a5986ef182dc 160
perr1940 3:ed0f6f302a0a 161 /**Converts from the paramters to the matrix of coefficients*/
perr1940 3:ed0f6f302a0a 162 void calculate_phi();
perr1940 3:ed0f6f302a0a 163
perr1940 0:a5986ef182dc 164 /** The number of time steps spent blending.
perr1940 0:a5986ef182dc 165 */
perr1940 0:a5986ef182dc 166 int _blend_steps;
perr1940 0:a5986ef182dc 167
perr1940 0:a5986ef182dc 168 /** A temporary variable that contains the polynomial coefficients */
perr1940 3:ed0f6f302a0a 169 float _phi[5][1];
perr1940 0:a5986ef182dc 170
perr1940 0:a5986ef182dc 171 /**A temporary variable that contains the change times */
perr1940 0:a5986ef182dc 172 int _times[2];
perr1940 0:a5986ef182dc 173
perr1940 0:a5986ef182dc 174 /** Which polynomial we have selected*/
perr1940 0:a5986ef182dc 175 int _current_poly;
perr1940 0:a5986ef182dc 176
perr1940 0:a5986ef182dc 177 /**Normalized time */
perr1940 0:a5986ef182dc 178 float _tau;
perr1940 3:ed0f6f302a0a 179
perr1940 0:a5986ef182dc 180 };
perr1940 0:a5986ef182dc 181
perr1940 0:a5986ef182dc 182 /** A class that defines the first step swing trajectory per Michael McKinley's design.
perr1940 0:a5986ef182dc 183 */
perr1940 0:a5986ef182dc 184
perr1940 0:a5986ef182dc 185 class BradPolyFSSwing: public TrajectoryGenerator
perr1940 0:a5986ef182dc 186 {
perr1940 0:a5986ef182dc 187 public:
perr1940 0:a5986ef182dc 188 /** Construct the object by passing in a set of parameters.
perr1940 0:a5986ef182dc 189 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 190 */
perr1940 3:ed0f6f302a0a 191 BradPolyFSSwing(Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 192 /** Basic constructor.
perr1940 0:a5986ef182dc 193 */
perr1940 0:a5986ef182dc 194 BradPolyFSSwing();
perr1940 0:a5986ef182dc 195 using TrajectoryGenerator::set;
perr1940 0:a5986ef182dc 196 /** Pass in a set of gait parameters.
perr1940 0:a5986ef182dc 197 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 198 */
perr1940 0:a5986ef182dc 199 void set(Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 200 using TrajectoryGenerator::init;
perr1940 0:a5986ef182dc 201 /** Initialize the linear blend by passing in a couple arguments.
perr1940 0:a5986ef182dc 202 * @param start The position you want to start the linear blend from.
perr1940 0:a5986ef182dc 203 * @param end The position you want to end the linear blend at.
perr1940 0:a5986ef182dc 204 * @param time_steps The number of time steps needed for the linear blend.
perr1940 0:a5986ef182dc 205 */
perr1940 0:a5986ef182dc 206 virtual void init(float start, float end, int time_steps);
perr1940 0:a5986ef182dc 207 /** Calculates the first step swing reference based on the current time.
perr1940 0:a5986ef182dc 208 * @param time The time to calculate the trajectory on.
perr1940 0:a5986ef182dc 209 * @param value Pointer to the variable that you're saving the trajectory at the specified time to.
perr1940 0:a5986ef182dc 210 * @returns A flag that signals if there was an error.
perr1940 0:a5986ef182dc 211 */
perr1940 0:a5986ef182dc 212 virtual bool calculate(int time, float& value);
perr1940 0:a5986ef182dc 213 private:
perr1940 0:a5986ef182dc 214 /** The private struct to store all the parameters in.
perr1940 0:a5986ef182dc 215 */
perr1940 3:ed0f6f302a0a 216 Brad_poly_gait_t* _params;
perr1940 0:a5986ef182dc 217 /** The linear blend object for blending.
perr1940 0:a5986ef182dc 218 */
perr1940 0:a5986ef182dc 219 LinearBlend _blend;
perr1940 0:a5986ef182dc 220 /** The number of time steps spent blending.
perr1940 0:a5986ef182dc 221 */
perr1940 0:a5986ef182dc 222 int _blend_steps;
perr1940 0:a5986ef182dc 223 };
perr1940 0:a5986ef182dc 224
perr1940 0:a5986ef182dc 225 /** A class that defines the first step stance trajectory per Michael McKinley's design.
perr1940 0:a5986ef182dc 226 */
perr1940 0:a5986ef182dc 227
perr1940 0:a5986ef182dc 228 class BradPolyFSStance: public TrajectoryGenerator
perr1940 0:a5986ef182dc 229 {
perr1940 0:a5986ef182dc 230 public:
perr1940 0:a5986ef182dc 231 /** Construct the object by passing in a set of parameters.
perr1940 0:a5986ef182dc 232 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 233 */
perr1940 3:ed0f6f302a0a 234 BradPolyFSStance(Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 235 /** Basic constructor.
perr1940 0:a5986ef182dc 236 */
perr1940 0:a5986ef182dc 237 BradPolyFSStance();
perr1940 0:a5986ef182dc 238 using TrajectoryGenerator::set;
perr1940 0:a5986ef182dc 239 /** Pass in a set of gait parameters.
perr1940 0:a5986ef182dc 240 * @param p A set of parameters that defines the gait.
perr1940 0:a5986ef182dc 241 */
perr1940 0:a5986ef182dc 242 void set(Brad_poly_gait_t& p);
perr1940 0:a5986ef182dc 243 using TrajectoryGenerator::init;
perr1940 0:a5986ef182dc 244 /** Initialize the linear blend by passing in a couple arguments.
perr1940 0:a5986ef182dc 245 * @param start The position you want to start the linear blend from.
perr1940 0:a5986ef182dc 246 * @param end The position you want to end the linear blend at.
perr1940 0:a5986ef182dc 247 * @param time_steps The number of time steps needed for the linear blend.
perr1940 0:a5986ef182dc 248 */
perr1940 0:a5986ef182dc 249 virtual void init(float start, float end, int time_steps);
perr1940 0:a5986ef182dc 250 /** Calculates the first step stance reference based on the current time.
perr1940 0:a5986ef182dc 251 * @param time The time to calculate the trajectory on.
perr1940 0:a5986ef182dc 252 * @param value Pointer to the variable that you're saving the trajectory at the specified time to.
perr1940 0:a5986ef182dc 253 * @returns A flag that signals if there was an error.
perr1940 0:a5986ef182dc 254 */
perr1940 0:a5986ef182dc 255 virtual bool calculate(int time, float& value);
perr1940 0:a5986ef182dc 256 private:
perr1940 0:a5986ef182dc 257 /** The private struct to store all the parameters in.
perr1940 0:a5986ef182dc 258 */
perr1940 3:ed0f6f302a0a 259 Brad_poly_gait_t* _params;
perr1940 0:a5986ef182dc 260 /** The linear blend object for blending.
perr1940 0:a5986ef182dc 261 */
perr1940 0:a5986ef182dc 262 LinearBlend _blend;
perr1940 0:a5986ef182dc 263 /** The number of time steps spent blending.
perr1940 0:a5986ef182dc 264 */
perr1940 0:a5986ef182dc 265 int _blend_steps;
perr1940 0:a5986ef182dc 266 };
perr1940 0:a5986ef182dc 267
perr1940 0:a5986ef182dc 268 #endif