A sparse MPC solver for walking motion generation.

Defines the parameters of the Walking Pattern Generator. More...
#include <WMG.h>
Public Member Functions  
WMG (const unsigned int N_, const unsigned int T_, const double step_height_=0.0135, double bezier_weight_1_=1.0, double bezier_weight_2_=2.0, double bezier_inclination_1_=0.01, double bezier_inclination_2_=0.008, bool use_fsr_constraints=false)  
Initializes a WMG object.  
~WMG ()  
Destructor.  
void  setFootstepParameters (const unsigned int def_periods, const unsigned int ds_periods, const unsigned int ds_number, bool use_user_constraints_=false) 
Set default parameters of footsteps, a wrapper around not so safe setFootstepParametersMS function.  
void  setFootstepParametersMS (const unsigned int def_time_ms_, const unsigned int ds_time_ms_, const unsigned int ds_number, bool use_user_constraints_=false) 
Set default parameters of footsteps.  
void  addFootstep (const double, const double, const double, fs_type type=FS_TYPE_AUTO) 
Adds a footstep to FS.  
WMGret  formPreviewWindow (smpc_parameters &) 
Forms a preview window.  
void  getFeetPositions (const unsigned int shift_from_current_ms, double *left_foot_pos, double *right_foot_pos) 
Determine position and orientation of feet.  
bool  isSupportSwitchNeeded () 
Checks if the support foot switch is needed.  
void  changeNextSSPosition (const double *posture, const bool zero_z_coordinate) 
Changes position of the next SS.  
void  repositionFootsteps (const double diff_x, const double diff_y) 
Reposition all subsequent footsteps that are not fixed at the current moment.  
void  FS2file (const std::string filename, const bool plot_ds=true) 
Outputs the footsteps in FS to a Matlab/Octave script for plotting.  
void  getFootsteps (std::vector< double > &x_coord, std::vector< double > &y_coord, std::vector< double > &angle_rot) 
Return coordinates of footstep reference points and rotation angles of footsteps (only for SS).  
Public Attributes  
std::vector< footstep >  FS 
A vector of footsteps.  
unsigned int  N 
Number of iterations in a preview window.  
unsigned int *  T_ms 
unsigned int  sampling_period 
int  current_step_number 
This is the step in FS that is at the start of the current preview window.  
int  first_preview_step 
The first step in the current preview window.  
double  step_height 
The maximum height, that can be reached by a swing foot.  
defConstraints  def_constraints 
Default SS and DS constraints.  
double  user_constraints [4] 
Constraints given by the user, initialized to the default values on initialization.  
double  user_constraints_auto_ds [4] 
Constraints given by the user, initialized to the default values on initialization.  
bool  use_user_constraints 
Constraints given by the user, initialized to the default values on initialization.  
double  bezier_weight_1 
double  bezier_weight_2 
double  bezier_inclination_1 
double  bezier_inclination_2 
Private Member Functions  
void  getDSFeetPositions (const int, double *, double *) 
Determine position and orientation of feet in DS.  
void  getSSFeetPositions (const int, const double, double *, double *) 
Determine position and orientation of feet (parabolic trajectory)  
void  getSSFeetPositionsBezier (const int, const double, double *, double *) 
Determine position and orientation of feet (using cubic Bezier curves)  
int  getNextSS (const int, const fs_type type=FS_TYPE_AUTO) 
Returns index of the next SS.  
int  getPrevSS (const int, const fs_type type=FS_TYPE_AUTO) 
Returns index of the previous SS.  
Private Attributes  
unsigned int  def_time_ms 
unsigned int  ds_time_ms 
unsigned int  ds_num 
unsigned int  last_time_decrement 
WMG::WMG  (  const unsigned int  N_, 
const unsigned int  T_,  
const double  step_height_ = 0.0135 , 

double  bezier_weight_1_ = 1.0 , 

double  bezier_weight_2_ = 2.0 , 

double  bezier_inclination_1_ = 0.01 , 

double  bezier_inclination_2_ = 0.008 , 

bool  use_fsr_constraints = false 

) 
Initializes a WMG object.
[in]  N_  Number of sampling times in a preview window 
[in]  T_  Sampling time [ms.] 
[in]  step_height_  step height (for interpolation of feet movements) [meter] 
[in]  bezier_weight_1_  see bezier_weight_1 
[in]  bezier_weight_2_  see bezier_weight_2 
[in]  bezier_inclination_1_  see bezier_inclination_1 
[in]  bezier_inclination_2_  see bezier_inclination_2 
[in]  use_fsr_constraints  Use constraints determined by positions of FSR sensors. 
void WMG::addFootstep  (  const double  x_relative, 
const double  y_relative,  
const double  angle_relative,  
fs_type  type = FS_TYPE_AUTO 

) 
Adds a footstep to FS.
[in]  x_relative  x_relative X position [meter] relative to the previous footstep. 
[in]  y_relative  y_relative Y position [meter] relative to the previous footstep. 
[in]  angle_relative  angle_relative Angle [rad.] relative to the previous footstep. 
[in]  type  (optional) type of the footstep. 
void WMG::changeNextSSPosition  (  const double *  posture, 
const bool  zero_z_coordinate  
) 
WMGret WMG::formPreviewWindow  (  smpc_parameters &  par  ) 
void WMG::FS2file  (  const std::string  filename, 
const bool  plot_ds = true 

) 
void WMG::getDSFeetPositions  (  const int  support_number, 
double *  left_foot_pos,  
double *  right_foot_pos  
)  [private] 
Determine position and orientation of feet in DS.
[in]  support_number  number of the support 
[out]  left_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
[out]  right_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
Definition at line 90 of file WMG_private.cpp.
void WMG::getFeetPositions  (  const unsigned int  shift_from_current_ms, 
double *  left_foot_pos,  
double *  right_foot_pos  
) 
Determine position and orientation of feet.
[in]  shift_from_current_ms  a positive shift in time (ms.) from the current time (allows to get positions for the future supports) 
[out]  left_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
[out]  right_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
void WMG::getFootsteps  (  std::vector< double > &  x_coord, 
std::vector< double > &  y_coord,  
std::vector< double > &  angle_rot  
) 
int WMG::getNextSS  (  const int  start_ind, 
const fs_type  type = FS_TYPE_AUTO 

)  [private] 
Returns index of the next SS.
[in]  start_ind  start search from this index. 
[in]  type  search for a footstep of certain type, by default (FS_TYPE_AUTO) both left and right are searched. 
Definition at line 23 of file WMG_private.cpp.
int WMG::getPrevSS  (  const int  start_ind, 
const fs_type  type = FS_TYPE_AUTO 

)  [private] 
Returns index of the previous SS.
[in]  start_ind  start search from this index. 
[in]  type  search for a footstep of certain type, by default (FS_TYPE_AUTO) both left and right are searched. 
Definition at line 58 of file WMG_private.cpp.
void WMG::getSSFeetPositions  (  const int  support_number, 
const double  theta,  
double *  left_foot_pos,  
double *  right_foot_pos  
)  [private] 
Determine position and orientation of feet (parabolic trajectory)
[in]  support_number  number of the support 
[in]  theta  a number between 0 and 1, a fraction of support time that have passed 
[out]  left_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
[out]  right_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
Definition at line 122 of file WMG_private.cpp.
void WMG::getSSFeetPositionsBezier  (  const int  support_number, 
const double  theta,  
double *  left_foot_pos,  
double *  right_foot_pos  
)  [private] 
Determine position and orientation of feet (using cubic Bezier curves)
[in]  support_number  number of the support 
[in]  theta  a number between 0 and 1, a fraction of support time that have passed 
[out]  left_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
[out]  right_foot_pos  4x4 homogeneous matrix, which represents position and orientation 
Definition at line 184 of file WMG_private.cpp.
bool WMG::isSupportSwitchNeeded  (  ) 
void WMG::repositionFootsteps  (  const double  diff_x, 
const double  diff_y  
) 
void WMG::setFootstepParameters  (  const unsigned int  def_periods, 
const unsigned int  ds_periods,  
const unsigned int  ds_number,  
bool  use_user_constraints_ = false 

) 
Set default parameters of footsteps, a wrapper around not so safe setFootstepParametersMS function.
[in]  def_periods  default number of sampling periods in a support (SS or DS depending on the type of added footstep) 
[in]  ds_periods  default number of sampling periods in an automatically generated DS. 
[in]  ds_number  number of DS to be generated automatically. 
[in]  use_user_constraints_  use user_constraints and user_constraints_auto_ds instead of the default constraints. 
void WMG::setFootstepParametersMS  (  const unsigned int  def_time_ms_, 
const unsigned int  ds_time_ms_,  
const unsigned int  ds_number,  
bool  use_user_constraints_ = false 

) 
Set default parameters of footsteps.
[in]  def_time_ms_  default time spent in a support (SS or DS depending on the type of added footstep) 
[in]  ds_time_ms_  default time spent in an automatically generated DS. 
[in]  ds_number  number of DS to be generated automatically. 
[in]  use_user_constraints_  use user_constraints and user_constraints_auto_ds instead of the default constraints. 
double WMG::bezier_inclination_1 
The foot trajectories, that are build using Bezier curve have four control points (0,1,3,4). The first and the last points are defined by positions of adjacent steps of a foot on the floor. The 1 and 2 points are computed depending on the given parameters.
The step height is fixed in the middle (theta = 0.5) of the step, note, that this is not the geometrical middle.
The control points have weights, the weights of the first and the last points are always equal to 1.0. The weights of other two points can be given by the user.
The 1 and 2 points also have inclination, i.e. the shift along y axis (in the frame fixed in the step reference point). The values of inclinations are given in meters.
double WMG::bezier_inclination_2 
The foot trajectories, that are build using Bezier curve have four control points (0,1,3,4). The first and the last points are defined by positions of adjacent steps of a foot on the floor. The 1 and 2 points are computed depending on the given parameters.
The step height is fixed in the middle (theta = 0.5) of the step, note, that this is not the geometrical middle.
The control points have weights, the weights of the first and the last points are always equal to 1.0. The weights of other two points can be given by the user.
The 1 and 2 points also have inclination, i.e. the shift along y axis (in the frame fixed in the step reference point). The values of inclinations are given in meters.
double WMG::bezier_weight_1 
The foot trajectories, that are build using Bezier curve have four control points (0,1,3,4). The first and the last points are defined by positions of adjacent steps of a foot on the floor. The 1 and 2 points are computed depending on the given parameters.
The step height is fixed in the middle (theta = 0.5) of the step, note, that this is not the geometrical middle.
The control points have weights, the weights of the first and the last points are always equal to 1.0. The weights of other two points can be given by the user.
The 1 and 2 points also have inclination, i.e. the shift along y axis (in the frame fixed in the step reference point). The values of inclinations are given in meters.
double WMG::bezier_weight_2 
The foot trajectories, that are build using Bezier curve have four control points (0,1,3,4). The first and the last points are defined by positions of adjacent steps of a foot on the floor. The 1 and 2 points are computed depending on the given parameters.
The step height is fixed in the middle (theta = 0.5) of the step, note, that this is not the geometrical middle.
The control points have weights, the weights of the first and the last points are always equal to 1.0. The weights of other two points can be given by the user.
The 1 and 2 points also have inclination, i.e. the shift along y axis (in the frame fixed in the step reference point). The values of inclinations are given in meters.
unsigned int WMG::def_time_ms [private] 
unsigned int WMG::ds_num [private] 
unsigned int WMG::ds_time_ms [private] 
unsigned int WMG::last_time_decrement [private] 
unsigned int WMG::sampling_period 
double WMG::step_height 
double WMG::user_constraints[4] 
double WMG::user_constraints_auto_ds[4] 