A sparse MPC solver for walking motion generation.
footstep.cpp
Go to the documentation of this file.
1 
9 /****************************************
10  * INCLUDES
11  ****************************************/
12 
13 #include <cmath> // sin, cos
14 
15 #include "footstep.h"
16 
17 
18 /****************************************
19  * FUNCTIONS
20  ****************************************/
21 
33  const double angle_,
34  const Transform<double, 3>& posture_,
35  const Vector3d& ZMPref_,
36  const unsigned int time_period_,
37  const fs_type type_,
38  const double *d_) :
40  ZMPref(ZMPref_)
41 {
42  posture = new Transform<double, 3>(posture_);
43  type = type_;
44  angle = angle_;
45  ca = cos(angle);
46  sa = sin(angle);
47  rotate_translate(ca, sa, x(), y());
48  time_left = time_period = time_period_;
49 }
50 
51 
57 footstep::footstep (const footstep& copy_from) :
58  RectangularConstraint_ZMP(copy_from),
59  ZMPref(copy_from.ZMPref)
60 {
61  posture = new Transform<double, 3>(*copy_from.posture);
62  type = copy_from.type;
63  angle = copy_from.angle;
64  ca = copy_from.ca;
65  sa = copy_from.sa;
66  time_left = copy_from.time_left;
67  time_period = copy_from.time_period;
68 }
69 
70 
71 
76 {
77  delete posture;
78 }
79 
80 
84 double footstep::x()
85 {
86  return (posture->translation()[0]);
87 }
88 
89 
93 double footstep::y()
94 {
95  return (posture->translation()[1]);
96 }
97 
98 
99 
106 void footstep::changePosture (const double * new_posture, const bool zero_z_coordinate)
107 {
108  posture->matrix() = Matrix4d::Map(new_posture);
109  if (zero_z_coordinate)
110  {
111  posture->translation()[2] = 0.0;
112  }
113  Matrix3d rotation = posture->matrix().corner(TopLeft,3,3);
114  angle = rotation.eulerAngles(0,1,2)[2];
115  ca = cos(angle);
116  sa = sin(angle);
117  rotate_translate(ca, sa, x(), y());
118 }
double angle
Angle (relative to the world frame) of a footstep [rad.].
Definition: footstep.h:49
void changePosture(const double *, const bool)
Correct position of the footstep.
Definition: footstep.cpp:106
fs_type type
type of the step.
Definition: footstep.h:64
Transform< double, 3 > * posture
Definition: footstep.h:70
~footstep()
Destructor.
Definition: footstep.cpp:75
unsigned int time_left
the amount of time left in this support (=time_period on initialization)
Definition: footstep.h:61
double sa
sin(angle).
Definition: footstep.h:55
Defines a footstep.
Definition: footstep.h:29
Defines rectangular constraints (of the form D*z <= d) for the ZMP.
double x()
Definition: footstep.cpp:84
double y()
Definition: footstep.cpp:93
double ca
cos(angle).
Definition: footstep.h:52
footstep(const double, const Transform< double, 3 > &, const Vector3d &, const unsigned int, const fs_type, const double *)
Defines a footstep at a given position with a given orientation.
Definition: footstep.cpp:32
void rotate_translate(const double, const double, const double, const double)
translates from [0;0] to "p" and rotates from 0 to "angle". The 0 initial angle implies that D = [eye...
fs_type
Definition: WMG.h:42
unsigned int time_period
the period of time spent in this support
Definition: footstep.h:58