A sparse MPC solver for walking motion generation.
Public Member Functions | Public Attributes | Private Member Functions | Private Attributes

Defines the parameters of the Walking Pattern Generator. More...

#include <WMG.h>

List of all members.

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< footstepFS
 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

Detailed Description

Defines the parameters of the Walking Pattern Generator.

Definition at line 240 of file WMG.h.


Constructor & Destructor Documentation

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.

Parameters:
[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_constraintsUse constraints determined by positions of FSR sensors.
Note:
The 'CenterOfPressure' value of the NAO robot can be computed correctly only when CoP lies within the polygon determined by force sensitive resistors (FSR) located on each foot. From the documentation: "If the real center of pressure go outside the 4 FSR parallelogram, the value of weight and centre of pressure could become bad, due to internal mechanical constraint". The rectanle that fits within the FSR polygon is smaller than our default constraint rectangle, which was selected based on the measurements of the feet. See also defConstraints.

Definition at line 15 of file WMG.cpp.

WMG::~WMG ( )

Destructor.

Definition at line 65 of file WMG.cpp.


Member Function Documentation

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.

Parameters:
[in]x_relativex_relative X position [meter] relative to the previous footstep.
[in]y_relativey_relative Y position [meter] relative to the previous footstep.
[in]angle_relativeangle_relative Angle [rad.] relative to the previous footstep.
[in]type(optional) type of the footstep.
Note:
Coordinates and angle are treated as absolute for the first step in the preview window.

Definition at line 102 of file WMG.cpp.

void WMG::changeNextSSPosition ( const double *  posture,
const bool  zero_z_coordinate 
)

Changes position of the next SS.

Parameters:
[in]posturea 4x4 homogeneous matrix representing new position and orientation
[in]zero_z_coordinateset z coordinate to 0.0
Todo:
DS must be adjusted as well.

Definition at line 300 of file WMG.cpp.

Forms a preview window.

Returns:
WMG_OK or WMG_HALT (simulation must be stopped)

Definition at line 331 of file WMG.cpp.

void WMG::FS2file ( const std::string  filename,
const bool  plot_ds = true 
)

Outputs the footsteps in FS to a Matlab/Octave script for plotting.

Parameters:
[in]filenameoutput file name.
[in]plot_dsenable/disable plotting of double supports

Definition at line 423 of file WMG.cpp.

void WMG::getDSFeetPositions ( const int  support_number,
double *  left_foot_pos,
double *  right_foot_pos 
) [private]

Determine position and orientation of feet in DS.

Parameters:
[in]support_numbernumber of the support
[out]left_foot_pos4x4 homogeneous matrix, which represents position and orientation
[out]right_foot_pos4x4 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.

Parameters:
[in]shift_from_current_msa positive shift in time (ms.) from the current time (allows to get positions for the future supports)
[out]left_foot_pos4x4 homogeneous matrix, which represents position and orientation
[out]right_foot_pos4x4 homogeneous matrix, which represents position and orientation
Attention:
This function requires the walking pattern to be started and finished by single support.
Cannot be called on the first or last SS => must be called after FormPreviewWindow().

Definition at line 230 of file WMG.cpp.

void WMG::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).

Parameters:
[out]x_coordx coordinates
[out]y_coordy coordinates
[out]angle_rotangles

Definition at line 488 of file WMG.cpp.

int WMG::getNextSS ( const int  start_ind,
const fs_type  type = FS_TYPE_AUTO 
) [private]

Returns index of the next SS.

Parameters:
[in]start_indstart search from this index.
[in]typesearch for a footstep of certain type, by default (FS_TYPE_AUTO) both left and right are searched.
Returns:
index of the next SS.

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.

Parameters:
[in]start_indstart search from this index.
[in]typesearch for a footstep of certain type, by default (FS_TYPE_AUTO) both left and right are searched.
Returns:
index of the previous SS.

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)

Parameters:
[in]support_numbernumber of the support
[in]thetaa number between 0 and 1, a fraction of support time that have passed
[out]left_foot_pos4x4 homogeneous matrix, which represents position and orientation
[out]right_foot_pos4x4 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)

Parameters:
[in]support_numbernumber of the support
[in]thetaa number between 0 and 1, a fraction of support time that have passed
[out]left_foot_pos4x4 homogeneous matrix, which represents position and orientation
[out]right_foot_pos4x4 homogeneous matrix, which represents position and orientation

Definition at line 184 of file WMG_private.cpp.

Checks if the support foot switch is needed.

Returns:
true if the support foot must be switched.

Definition at line 273 of file WMG.cpp.

void WMG::repositionFootsteps ( const double  diff_x,
const double  diff_y 
)

Reposition all subsequent footsteps that are not fixed at the current moment.

Parameters:
[in]diff_xchange in position along x axis
[in]diff_ychange in position along y axis

Definition at line 306 of file WMG.cpp.

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.

Parameters:
[in]def_periodsdefault number of sampling periods in a support (SS or DS depending on the type of added footstep)
[in]ds_periodsdefault number of sampling periods in an automatically generated DS.
[in]ds_numbernumber of DS to be generated automatically.
[in]use_user_constraints_use user_constraints and user_constraints_auto_ds instead of the default constraints.

Definition at line 75 of file WMG.cpp.

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.

Parameters:
[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_numbernumber of DS to be generated automatically.
[in]use_user_constraints_use user_constraints and user_constraints_auto_ds instead of the default constraints.

Definition at line 89 of file WMG.cpp.


Member Data Documentation

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.

Definition at line 472 of file WMG.h.

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.

Definition at line 473 of file WMG.h.

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.

Definition at line 470 of file WMG.h.

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.

Definition at line 471 of file WMG.h.

This is the step in FS that is at the start of the current preview window.

Definition at line 431 of file WMG.h.

Default SS and DS constraints.

Definition at line 441 of file WMG.h.

unsigned int WMG::def_time_ms [private]

Definition at line 483 of file WMG.h.

unsigned int WMG::ds_num [private]

Definition at line 485 of file WMG.h.

unsigned int WMG::ds_time_ms [private]

Definition at line 484 of file WMG.h.

The first step in the current preview window.

Definition at line 434 of file WMG.h.

std::vector<footstep> WMG::FS

A vector of footsteps.

Definition at line 420 of file WMG.h.

unsigned int WMG::last_time_decrement [private]

Definition at line 487 of file WMG.h.

unsigned int WMG::N

Number of iterations in a preview window.

Definition at line 424 of file WMG.h.

unsigned int WMG::sampling_period

Definition at line 427 of file WMG.h.

The maximum height, that can be reached by a swing foot.

Definition at line 437 of file WMG.h.

unsigned int* WMG::T_ms

Definition at line 426 of file WMG.h.

Constraints given by the user, initialized to the default values on initialization.

Definition at line 447 of file WMG.h.

Constraints given by the user, initialized to the default values on initialization.

Definition at line 445 of file WMG.h.

Constraints given by the user, initialized to the default values on initialization.

Definition at line 446 of file WMG.h.


The documentation for this class was generated from the following files: