├── README.md
├── bipeds
└── bipeds_gait_generation.m
├── images
├── 2D_footsteps.png
├── 3D.PNG
├── 3D_2.gif
├── 3D_2.png
├── 3D_3.PNG
├── animation.gif
├── com_dyn.png
├── com_sine.png
├── global_traj.png
├── lip.jpg
├── lip2.png
├── output.gif
├── trot_gait.png
└── zmp.png
└── quadrupeds
└── quadrupeds_gait_generation.m
/README.md:
--------------------------------------------------------------------------------
1 | # Footsteps Planning: Step-by-Step Guide
2 |
3 |
4 |
5 |
6 |
7 | Footstep planning is a fundamental component of locomotion for legged robots, allowing them to move efficiently and adaptively in complex environments. Whether it’s a biped navigating a cluttered household or a quadruped traversing uneven terrain, this process involves generating precise foot trajectories while considering the robot's dynamics and stability constraints.
8 |
9 | Typically, the planning process consists of two stages:
10 |
11 | - **Global Path Planning**: This module computes a high-level path for the robot to follow, usually avoiding obstacles and determining a viable route.
12 | - **Footstep Planning**: Based on the global path, this step computes the exact placement of each foot to ensure the robot follows the intended trajectory.
13 |
14 | ## Bipeds
15 |
16 | Footstep planning for bipeds typically involves generating foot placements in both 2D and 3D spaces. In addition to foot placements, the trajectories of the robot’s Center of Mass (CoM) and the Zero Moment Point (ZMP) play a vital role in maintaining balance. The CoM trajectory ensures the robot's weight remains balanced, while the ZMP trajectory prevents tipping by maintaining stability during walking.
17 |
18 | **Note**: All these reference trajectories (footsteps, CoM) will be then followed by a high-level tracking controller. However, the design and implementation of such a controller is beyond the scope of this article.
19 |
20 | ### 1) Global Trajectory
21 |
22 |
23 |
24 |
25 |
26 | The global trajectory represents the motion of a robot from a starting location to a goal location, typically computed by a high-level planner. This planner generates an obstacle-free sequence of positions for the robot to follow.
27 |
28 | However, in this example, we generate a simplified trajectory using just two primary inputs:
29 |
30 | - **Linear velocity**: The forward speed $v$ of the robot.
31 | - **Angular velocity**: The rate of rotation $w$ of the robot around its vertical axis.
32 |
33 | Given these inputs, the robot’s trajectory can be computed over a time interval $T$ using small time steps of duration $\Delta T$. At each step, the position ($x, y$) and orientation $\theta$ are updated iteratively based on the following equations of motion:
34 |
35 | - $x_{i+1} = x_i + v \cos(\theta_i) \Delta T$
36 | - $y_{i+1} = y_i + v \sin(\theta_i) \Delta T$
37 | - $\theta_{i+1} = \theta_i + w \Delta T$
38 |
39 | This method provides a simple way to simulate a global trajectory for navigation tasks.
40 |
41 | ### 2) 2D Footsteps Planning
42 |
43 |
44 |
45 |
46 |
47 | 2D Footstep Planning involves generating precise foot placements around the global trajectory, alternating between the left and right foot. Key parameters include:
48 |
49 | - **Step Length L**: The forward distance between consecutive foot placements.
50 | - **Step Width W**: The lateral distance between the left and right feet.
51 |
52 | #### Steps of the Algorithm:
53 |
54 | 1. **Tangent and Normal Calculation**: At each point along the trajectory, a tangent vector $\mathbf{t} = [\cos(\theta), \sin(\theta)]$ is computed to determine the robot’s direction of motion. The normal vector $\mathbf{n}$ is derived from the tangent to define lateral offsets for foot placement:
55 |
56 | $$
57 | \mathbf{n} = [-\sin(\theta), \cos(\theta)]
58 | $$
59 |
60 | 2. **Foot Placement**:
61 | A new foot placement is added whenever the robot travels a distance equal to the step length $L$. The position of the foot is calculated as:
62 |
63 | ```math
64 | x_{\text{foot}} = x_{\text{trajectory}} + \frac{W}{2} \cdot n_x \cdot (2 \cdot \text{is\_right\_foot} - 1)
65 | ```
66 |
67 | ```math
68 | y_{\text{foot}} = y_{\text{trajectory}} + \frac{W}{2} \cdot n_y \cdot (2 \cdot \text{is\_right\_foot} - 1)
69 | ```
70 |
71 | The orientation of the foot is set to align with the tangent:
72 |
73 | ```math
74 | \theta_{\text{foot}} = \arctan2(t_y, t_x)
75 | ```
76 |
77 | 4. **Alternating Steps**:
78 | The algorithm alternates between placing left and right foot positions, ensuring symmetric gait patterns.
79 |
80 | By following this approach, the robot can accurately generate a sequence of 2D foot placements that follow the global trajectory while adhering to step length and width constraints.
81 |
82 |
83 | ### 3) 3D Foot Trajectory Planning
84 |
85 |
86 |
87 |
88 |
89 | 3D Foot Trajectory Planning extends the 2D footstep positions into a fully defined 3D trajectory, including height variations for each step.
90 |
91 | Each step's trajectory is defined as a sequence of homogeneous transformations in 3D space, generated from the following elements:
92 |
93 | - **2D footsteps**: The 2D feet positions generated from the previous step.
94 | - **Step Height**: The vertical lift of the foot, ensuring clearance over obstacles.
95 | - **Step Time**: The duration of each step.
96 |
97 | #### Algorithm Steps:
98 |
99 | 1. **Time Normalization**:
100 | Each single step trajectory is generated over a normalized time interval $[0, 1]$, split into $n$ discrete steps:
101 |
102 | $$
103 | t = \text{linspace}(0, 1, n)
104 | $$
105 |
106 | 2. **Interpolation for x, y Trajectory**:
107 | The positions in the $x-y$ plane are interpolated between the start and end positions of the step using cubic splines:
108 |
109 | $$
110 | x_{\text{traj}}(t) = \text{interp}(x_{\text{start}}, x_{\text{end}}, t)
111 | $$
112 |
113 | $$
114 | y_{\text{traj}}(t) = \text{interp}(y_{\text{start}}, y_{\text{end}}, t)
115 | $$
116 |
117 | 3. **Parabolic z Trajectory**:
118 | To ensure smooth lifting and landing, the $z$-trajectory follows a parabolic profile:
119 |
120 | $$
121 | z_{\text{traj}}(t) = 4 \cdot H \cdot t \cdot (1 - t)
122 | $$
123 |
124 | 4. **Homogeneous Transformation Matrices**:
125 | At each time step, the 3D foot position is represented as a homogeneous transformation matrix:
126 |
127 | $$
128 | T(t) =
129 | \begin{bmatrix}
130 | R_z(\theta) & \mathbf{p}(t) \\
131 | 0 & 1
132 | \end{bmatrix}
133 | $$
134 |
135 | where:
136 | - $R_z(\theta)$ is the rotation matrix for the foot orientation around $z$-axis:
137 |
138 | $$
139 | R_z(\theta) =
140 | \begin{bmatrix}
141 | \cos(\theta) & -\sin(\theta) & 0 \\
142 | \sin(\theta) & \cos(\theta) & 0 \\
143 | 0 & 0 & 1
144 | \end{bmatrix}
145 | $$
146 |
147 | - $\theta$ is the foot orientation.
148 | - $\mathbf{p}(t) = [x_{\text{traj}}(t), y_{\text{traj}}(t), z_{\text{traj}}(t)]^\top$ is the 3D position of the foot.
149 |
150 | This process generates a sequence of 3D foot placements that follow the global trajectory, by computing poth 3D positions and 3D orientations for each foot.
151 |
152 | ### 4) ZMP Trajectory Generation
153 |
154 |
155 |
156 |
157 |
158 | The **Zero Moment Point (ZMP)** is a critical concept in legged locomotion, serving as a stability indicator for robots. It represents the point on the ground where the resultant moment of forces, generated by gravity and inertia, equals zero. In simpler terms, the ZMP helps determine if the robot’s motion is dynamically stable. If the ZMP lies within the support polygon, the robot is stable and unlikely to tip over.
159 |
160 | #### What is the Support Polygon?
161 |
162 | The **Support Polygon** is the area enclosed by the contact points of the robot's feet with the ground. For example:
163 | - For a biped robot in a single support phase, the support polygon is the area under the single foot in contact.
164 | - For a biped in a double support phase, the support polygon is the area formed between both feet.
165 |
166 | The ZMP must remain inside the support polygon to maintain stability. If it moves outside, the robot becomes unstable and risks falling.
167 |
168 | #### ZMP Trajectory Generation
169 |
170 | To generate a smooth and stable ZMP trajectory, the planned 2D footstep positions serve as the foundation. Using cubic spline interpolation, a continuous and smooth path is calculated between consecutive footstep positions. This interpolation method guarantees that the ZMP trajectory transitions naturally between steps without abrupt changes, maintaining stability. The resulting trajectory consists of a series of smooth points $(x_{\text{ZMP}}, y_{\text{ZMP}})$ that lie within the support polygon, ensuring dynamic balance throughout the robot’s motion.
171 |
172 | ### 5) CoM Pelvis Trajectory Generation
173 |
174 | The Center of Mass (CoM) trajectory plays a critical role in ensuring the robot remains dynamically stable during motion. We can procede in couple of ways to compute this reference.
175 |
176 | #### 5.1) Dynamics Relations with ZMP
177 |
178 |
179 |
180 |
181 |
182 | The CoM and the ZMP relationship can be expressed through the Linear Inverted Pendulum (LIP) model, which is a simplified dynamic representation of a robot's motion.
183 |
184 | #### **Linear Inverted Pendulum (LIP) Model**
185 |
186 |
187 |
188 |
189 |
190 | The LIP model assumes the following:
191 | - The robot's mass is concentrated at its CoM.
192 | - The CoM moves at a constant height $h$ above the ground.
193 | - The contact forces act at the ZMP, which lies within the support polygon.
194 |
195 |
196 |
197 |
198 |
199 | Using these assumptions, the horizontal CoM accelerations are directly related to the ZMP position. Indeed, recalling the ZMP definition (*it represents the point on the ground where the resultant moment of forces equals zero*), the following dynamic equations hold:
200 |
201 | ```math
202 | M_{y_{{zmp}}} = m \cdot \ddot{x}_{\text{CoM}} \cdot {z}_{\text{CoM}} - mg \cdot (\dot{x}_{\text{CoM}} - x_{\text{ZMP}}) = 0
203 | ```
204 |
205 | ```math
206 | M_{x_{{zmp}}} = m \cdot \ddot{y}_{\text{CoM}} \cdot {z}_{\text{CoM}} - mg \cdot (\dot{y}_{\text{CoM}} - y_{\text{ZMP}}) = 0
207 | ```
208 |
209 | Rearranging the equations, we obtain:
210 |
211 | ```math
212 | \ddot{x}_{\text{CoM}} = \frac{g}{h} (x_{\text{CoM}} - x_{\text{ZMP}})
213 | ```
214 |
215 | ```math
216 | \ddot{y}_{\text{CoM}} = \frac{g}{h} (y_{\text{CoM}} - y_{\text{ZMP}})
217 | ```
218 |
219 | where:
220 |
221 | - $\ddot{x}_{\text{CoM}}$ is the acceleration of the CoM in the $x$-direction.
222 | - $\ddot{y}_{\text{CoM}}$ is the acceleration of the CoM in the $y$-direction.
223 | - $x_{\text{ZMP}}$ and $y_{\text{ZMP}}$ are the ZMP coordinates in the $x$- and $y$-directions.
224 | - $g$ is the gravitational constant.
225 | - $h$ is the constant height of the CoM above the ground.
226 |
227 | #### **Computation of CoM Trajectory**
228 |
229 | The CoM trajectory can be calculated iteratively using the ZMP trajectory as input. At each time step, the following steps are performed:
230 |
231 | 1. Compute the CoM acceleration:
232 |
233 | ```math
234 | \ddot{x}_{\text{CoM}} = \frac{g}{h} (x_{\text{CoM}} - x_{\text{ZMP}})
235 | ```
236 |
237 | ```math
238 | \ddot{y}_{\text{CoM}} = \frac{g}{h} (y_{\text{CoM}} - y_{\text{ZMP}})
239 | ```
240 |
241 | 3. Integrate the acceleration to update the CoM velocity (keep in mind it is an intertial acceleration):
242 |
243 | ```math
244 | \dot{x}_{\text{CoM}} = \dot{x}_{\text{CoM}} - \ddot{x}_{\text{CoM}} \cdot \Delta t
245 | ```
246 |
247 | ```math
248 | \dot{y}_{\text{CoM}} = \dot{y}_{\text{CoM}} - \ddot{y}_{\text{CoM}} \cdot \Delta t
249 | ```
250 |
251 | 5. Integrate the velocity to update the CoM position:
252 |
253 | ```math
254 | x_{\text{CoM}} = x_{\text{CoM}} + \dot{x}_{\text{CoM}} \cdot \Delta t
255 | ```
256 |
257 | ```math
258 | y_{\text{CoM}} = y_{\text{CoM}} + \dot{y}_{\text{CoM}} \cdot \Delta t
259 | ```
260 |
261 | The resulting trajectory $(x_{\text{CoM}}, y_{\text{CoM}})$ provides a smooth and stable path for the robot's Center of Mass, ensuring dynamic balance throughout the motion.
262 |
263 |
264 | #### 5.2) Open-Loop Sine Wave
265 |
266 |
267 |
268 |
269 |
270 | Another alternative to generate the CoM trajectory consists in introducing a sinusoidal perturbation around the global trajectory. This perturbation, added along the normal direction at each point, mimics the lateral oscillations observed in biological locomotion. While this approach doesn't derive directly from dynamic equations, it is inspired by the natural, rhythmic oscillations seen in biological systems.
271 |
272 | #### Sinusoidal Perturbation Model
273 |
274 | The sinusoidal perturbation is defined as:
275 |
276 | $$
277 | s(t) = A \cdot \sin\left(\frac{2 \pi s_{\text{arc}}}{\lambda}\right)
278 | $$
279 |
280 | where:
281 | - $A$: Amplitude of the sine wave, controlling the lateral deviation of the CoM.
282 | - $\lambda$: Wavelength of the sine wave, determining the frequency of oscillation.
283 | - $s_{\text{arc}}$: The arc length along the robot's trajectory.
284 |
285 | #### Generating the Perturbed CoM Trajectory
286 |
287 | 1. **Compute Arc Length**:
288 | The arc length is computed incrementally along the trajectory as:
289 |
290 | ```math
291 | s(i) = s(i-1) + \sqrt{\left(x(i) - x(i-1)\right)^2 + \left(y(i) - y(i-1)\right)^2}
292 | ```
293 |
294 | 3. **Tangent and Normal Vectors**:
295 | At each point, the tangent vector $\vec{t}$ is calculated as:
296 |
297 | ```math
298 | \vec{t} = \frac{\partial \vec{p}}{\partial s} = \left( \frac{\partial x}{\partial s}, \frac{\partial y}{\partial s} \right)
299 | ```
300 |
301 | The normal vector $\vec{n}$ is derived by rotating the tangent vector 90°:
302 |
303 | ```math
304 | \vec{n} = (-t_y, t_x)
305 | ```
306 |
307 | 5. **Apply Perturbation**:
308 | The sine wave perturbation is applied along the normal vector:
309 |
310 | ```math
311 | \vec{p}_{\text{perturbed}} = \vec{p}_{\text{global\_trajectory}} + s(t) \cdot \vec{n}
312 | ```
313 |
314 | This simpler approach generates an open-loop CoM trajectory, simulating oscillating gait patterns often observed in biological locomotion.
315 |
316 | ## Quadrupeds
317 |
318 | Quadrupeds, with their four-legged structure, offer inherent stability but require precise coordination between legs for smooth and efficient movement. Locomotion in quadrupeds is characterized by various gait patterns, each suited for specific speeds or terrains. Effective footstep planning for quadrupeds involves managing the "phase" of each leg’s movement, which refers to whether a leg is in the *stance phase* (in contact with the ground) or the *swing phase* (moving forward through the air).
319 |
320 | Common Gait Patterns:
321 |
322 | - **Walk**: A slow, stable gait where legs move one at a time, ensuring at least three points of contact with the ground at all times.
323 | - **Trot**: A faster gait where diagonally opposite legs move together in pairs, offering a balance of speed and stability.
324 | - **Gallop**: A dynamic gait used for higher speeds, featuring a suspended phase where all legs are momentarily off the ground.
325 |
326 | ### 3D Foot Trajectory for Quadrupeds
327 |
328 |
329 |
330 |
331 |
332 | The 3D foot trajectory describes the path of each foot in three-dimensional space (x, y, z) throughout the gait cycle. For quadrupeds, we use an elliptical trajectory to represent the foot motion in the xz-plane:
333 |
334 | $$
335 | \text{foot}_x(t) = A_x \cos(\phi(t))
336 | $$
337 | $$
338 | \text{foot}_z(t) = -A_z \sin(\phi(t))
339 | $$
340 |
341 | Where:
342 | - $A_x$ is the amplitude of the foot movement in the forward (x) direction.
343 | - $A_z$ is the amplitude of the vertical (z) direction.
344 |
345 | While $\phi(t)$ is the phase at time $t$, and is defined as:
346 |
347 | $$
348 | \phi(t) = \phi_0 + 2 \pi f_0 t
349 | $$
350 |
351 | Where:
352 | - $\phi_0$ is the initial phase of the leg.
353 | - $f_0$ is the frequency of the gait.
354 | - $t$ is the time at which the foot trajectory is computed.
355 |
356 | In our example, we choose to perform a Trot Gait. Thus, the legs move in a diagonal gait pattern, where the front left leg and the rear right leg move together, and the front right leg and rear left leg move together in the opposite phase. The phase shifts $\phi_0$ between these legs are defined as:
357 |
358 | - Front Left Leg: $\phi_{\text{FL}} = 0$
359 | - Rear Right Leg: $\phi_{\text{RR}} = 0$
360 | - Front Right Leg: $\phi_{\text{FR}} = \pi$ (opposite phase to FL and RR)
361 | - Rear Left Leg: $\phi_{\text{RL}} = \pi$ (opposite phase to FL and RR)
362 |
363 | This approach allows for precise control over the motion of each foot, enabling smooth and stable locomotion.
364 |
365 | ## Key Works and Citations
366 |
367 | - **Clement Gaspard**: [*FootstepNet: an Efficient Actor-Critic Method for Fast On-line Bipedal Footstep Planning*](https://arxiv.org/pdf/2403.12589v2)
368 | - **Shuuji Kajita**: [*The 3D Linear Inverted Pendulum Mode: a simple modeling for a biped walking pattern generation*](https://www.cs.cmu.edu/~hgeyer/Teaching/R16-899B/Papers/KajiitaEA01IEEE_ICIRS.pdf)
369 | - **Shuuji Kajita**: [*Biped Walking Pattern Generation by using Zero-Moment Point*](https://mzucker.github.io/swarthmore/e91_s2013/readings/kajita2003preview.pdf)
370 | - **Maurice Rahme**: [*Spot Mini Mini Open Source*](https://moribots.github.io/project/spot-mini-mini)
371 |
372 |
373 |
374 |
--------------------------------------------------------------------------------
/bipeds/bipeds_gait_generation.m:
--------------------------------------------------------------------------------
1 | %% Generate footsteps, CoM and ZMP trajectories for a biped given a global path.
2 |
3 | clear all
4 | close all
5 |
6 |
7 | function [trajectory] = generate_global_trajectory(v,w,T, delta_T)
8 |
9 | % Generate a global trajectory given v,w as input.
10 |
11 | % Number of points
12 | num_points = T/delta_T;
13 |
14 | % Init starting position
15 | pos_x = 0.0;
16 | pos_y = 0.0;
17 | pos_z = 1.0;
18 | theta = 0.0;
19 |
20 | % Store trajectory values
21 | trajectory = zeros(num_points, 4);
22 |
23 | % Compute trajectory
24 | for i = 1:num_points
25 | pos_x = pos_x + v*cos(theta)*delta_T;
26 | pos_y = pos_y + v*sin(theta)*delta_T;
27 | theta = theta + w*delta_T;
28 |
29 | trajectory(i,:) = [pos_x, pos_y, pos_z, theta];
30 | end
31 |
32 | % Visualization
33 | figure;
34 | hold on;
35 | plot(trajectory(:, 1), trajectory(:, 2), 'r-', 'LineWidth', 2); % CoM
36 | title('Global trajectory', 'FontSize', 15 );
37 | xlabel('X Position (m)');
38 | ylabel('Y Position (m)');
39 | grid on;
40 | axis equal;
41 | end
42 |
43 |
44 | function [CoM_trajectory] = generate_pelvis_trajectory(trajectory, T, delta_T)
45 |
46 | % Generate a CoM trajectory for pelvis, by applying a sine wave perturbation
47 | % around the original global trajectory.
48 |
49 | % Parameters
50 | A = 0.2; % Amplitude of sine wave
51 | lambda = 1.0; % Wavelength of sine wave
52 |
53 | % Number of points
54 | num_points = T/delta_T;
55 |
56 | x_base = trajectory(:,1); % x(t)
57 | y_base = trajectory(:,2); % y(t)
58 |
59 | % Calculate arc length
60 | s = zeros(1, num_points);
61 |
62 | % Get arc length values
63 | for i = 2:num_points
64 | s(i) = s(i-1) + sqrt((x_base(i) - x_base(i-1))^2 + (y_base(i) - y_base(i-1))^2);
65 | end
66 |
67 | % Uniformly sample arc length
68 | s_uniform = linspace(s(1), s(end), num_points);
69 | x_uniform = interp1(s, x_base, s_uniform);
70 | y_uniform = interp1(s, y_base, s_uniform);
71 |
72 | % Compute tangent and normal vectors
73 | dx_ds = gradient(x_uniform, s_uniform);
74 | dy_ds = gradient(y_uniform, s_uniform);
75 | tangent_magnitude = sqrt(dx_ds.^2 + dy_ds.^2);
76 | t_x = dx_ds ./ tangent_magnitude;
77 | t_y = dy_ds ./ tangent_magnitude;
78 | n_x = -t_y;
79 | n_y = t_x;
80 |
81 | % Apply sine wave perturbation along normal
82 | sine_wave = A * sin(2 * pi * s_uniform / lambda);
83 | x_perturbed = x_uniform + sine_wave .* n_x;
84 | y_perturbed = y_uniform + sine_wave .* n_y;
85 | z_perturbed = trajectory(:,3);
86 |
87 | % Generate homogeneous transformation matrices
88 | CoM_trajectory = cell(num_points, 1);
89 |
90 | for i = 1:num_points
91 | CoM_trajectory{i} = [x_perturbed(i), y_perturbed(i), z_perturbed(i)];
92 | end
93 |
94 | % Plot results
95 | figure;
96 | hold on;
97 | plot(trajectory(:, 1), trajectory(:, 2), 'r-', 'LineWidth', 2); % CoM
98 | plot(x_perturbed, y_perturbed, 'b-', 'LineWidth', 2); % Perturbed trajectory
99 | title('CoM Trajectory', 'FontSize', 15 );
100 | xlabel('x');
101 | ylabel('y');
102 | legend('Original Trajectory', 'Perturbed Trajectory');
103 | grid on;
104 | axis equal;
105 | end
106 |
107 |
108 | function foot_positions = generate_foot_trajectory_2d(trajectory, step_length, step_width, foot_size)
109 |
110 | % Generate 2D foot positions around the global trajectory.
111 |
112 | % Variables
113 | num_points = size(trajectory, 1);
114 | foot_positions = []; % [x, y, theta, is_right]
115 | dist_accumulated = 0; % Store accumulated distance
116 | is_right_foot = true; % Alternate between left and right foot
117 |
118 | for i = 2:num_points-1
119 | % Compute distance between consecutive points
120 | dx = trajectory(i+1, 1) - trajectory(i, 1);
121 | dy = trajectory(i+1, 2) - trajectory(i, 2);
122 | dist_step = sqrt(dx^2 + dy^2);
123 |
124 | % Update accumulated distance
125 | dist_accumulated = dist_accumulated + dist_step;
126 |
127 | % Check if it is time to place a new step
128 | if dist_accumulated >= step_length
129 |
130 | % Reset variable
131 | dist_accumulated = 0;
132 |
133 | % Compute vector tangent to trajectory
134 | tangent = [dx, dy];
135 | tangent = tangent / norm(tangent); % Normalizza
136 |
137 | % Compute vector normal to trajectory
138 | normal = [-tangent(2), tangent(1)];
139 |
140 | % Compute foot position
141 | x_foot = trajectory(i, 1) + (step_width / 2) * normal(1) * (2 * is_right_foot - 1);
142 | y_foot = trajectory(i, 2) + (step_width / 2) * normal(2) * (2 * is_right_foot - 1);
143 |
144 | % Compute orientation of the foot
145 | theta_foot = atan2(tangent(2), tangent(1));
146 |
147 | % Save data
148 | foot_positions = [foot_positions; x_foot, y_foot, theta_foot, is_right_foot];
149 |
150 | % Alternate steps
151 | is_right_foot = ~is_right_foot;
152 | end
153 | end
154 |
155 | % Visualization
156 | figure;
157 | hold on;
158 |
159 | for i = 1:size(foot_positions, 1)
160 |
161 | % Plot rectangles of the feet
162 | x_foot = foot_positions(i, 1);
163 | y_foot = foot_positions(i, 2);
164 | theta_foot = foot_positions(i, 3);
165 |
166 | % Compute rectangle vertices
167 | half_length = foot_size(2) / 2;
168 | half_width = foot_size(1) / 2;
169 | corners = [
170 | -half_length, -half_width;
171 | half_length, -half_width;
172 | half_length, half_width;
173 | -half_length, half_width
174 | ];
175 |
176 | % Do rotation and translation
177 | rotation_matrix = [cos(theta_foot), -sin(theta_foot);
178 | sin(theta_foot), cos(theta_foot)];
179 | rotated_corners = (rotation_matrix * corners')';
180 | translated_corners = rotated_corners + [x_foot, y_foot];
181 |
182 | % Draw rectangle
183 | fill(translated_corners(:, 1), translated_corners(:, 2), 'b', 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'LineWidth', 1.5);
184 | end
185 |
186 | % Plot results
187 | plot(trajectory(:, 1), trajectory(:, 2), 'r--', 'LineWidth', 1.5);
188 | scatter(foot_positions(:, 1), foot_positions(:, 2), 5, 'filled', 'MarkerEdgeColor', 'k');
189 | axis equal;
190 | title('Foot Placement 2D', 'FontSize', 15 );
191 | xlabel('x-axis');
192 | ylabel('y-axis');
193 | grid on;
194 | end
195 |
196 |
197 | function foot_trajectory = generate_foot_trajectory_3d(foot_positions, step_height, step_length, step_time, delta_T)
198 |
199 | % Generate 3D foot positions starting from 2D positions.
200 |
201 | % Number of points
202 | num_points = step_time/delta_T;
203 |
204 | t = linspace(0, 1, num_points); % Normalized time
205 | foot_trajectory = cell(size(foot_positions, 1) - 2, 1); % Trajectory for each step
206 |
207 | for i = 1:size(foot_positions, 1) - 2
208 | % Initial and final step positions
209 | p_start = foot_positions(i, 1:2);
210 | p_end = foot_positions(i + 2, 1:2);
211 |
212 | % Interpolate with cubix spline for xy direction
213 | x_traj = interp1([0, 1], [p_start(1), p_end(1)], t, 'pchip');
214 | y_traj = interp1([0, 1], [p_start(2), p_end(2)], t, 'pchip');
215 |
216 | % Parabolic trajectory for z direction
217 | z_traj = 4 * step_height * t .* (1 - t); % Profilo parabolico
218 |
219 | % Compute transformation matrix
220 | HTM_trajectory = cell(num_points, 1);
221 | yaw_angle = foot_positions(i + 1);
222 |
223 | for j = 1:num_points
224 | % Rotation matrix around z
225 | R_z = [cos(yaw_angle), -sin(yaw_angle), 0;
226 | sin(yaw_angle), cos(yaw_angle), 0;
227 | 0, 0, 1];
228 |
229 | % Transformation matrix
230 | T = eye(4);
231 | T(1:3, 1:3) = R_z;
232 | T(1:3, 4) = [x_traj(j); y_traj(j); z_traj(j)];
233 | HTM_trajectory{j} = T;
234 | end
235 |
236 | % Save trajectory
237 | foot_trajectory{i} = HTM_trajectory;
238 | end
239 |
240 | % Visualization
241 | figure;
242 | hold on;
243 | for i = 1:length(foot_trajectory)
244 | traj = foot_trajectory{i};
245 | x = arrayfun(@(j) traj{j}(1, 4), 1:num_points);
246 | y = arrayfun(@(j) traj{j}(2, 4), 1:num_points);
247 | z = arrayfun(@(j) traj{j}(3, 4), 1:num_points);
248 | plot3(x,y,z, 'LineWidth', 1.5);
249 | end
250 | axis equal;
251 | title('Foot Trajectory 3D');
252 | xlabel('x-axis');
253 | ylabel('y-axis');
254 | zlabel('z-axis');
255 | grid on;
256 | end
257 |
258 |
259 | function zmp_trajectory = generate_zmp_trajectory_spline(foot_positions, delta_size_spline, step_length, step_width, foot_size)
260 |
261 | % Generate ZMP trajectory given the 2D footsteps.
262 |
263 | % Get feet positions.
264 | x_foot = foot_positions(:, 1);
265 | y_foot = foot_positions(:, 2);
266 |
267 | % Add origin (0, 0) as first point in the trajectory.
268 | x_foot = [0; x_foot];
269 | y_foot = [0; y_foot];
270 |
271 | % Temporal parameter
272 | t = 1:length(x_foot);
273 | t_fine = 1:delta_size_spline:length(x_foot); % Greater granularity.
274 |
275 | % Cubic interpolation between feet positions.
276 | x_spline = interp1(t, x_foot, t_fine);
277 | y_spline = interp1(t, y_foot, t_fine);
278 |
279 | % Store results in matrix
280 | zmp_trajectory = [x_spline', y_spline'];
281 |
282 | % Visualize ZMP trajectory
283 | figure;
284 | hold on;
285 | plot(zmp_trajectory(:, 1), zmp_trajectory(:, 2), 'k-', 'LineWidth', 1.5); % ZMP spline
286 |
287 | for i = 1:size(foot_positions, 1)
288 | % Plot rectangles of the feet
289 | x_foot = foot_positions(i, 1);
290 | y_foot = foot_positions(i, 2);
291 | theta_foot = foot_positions(i, 3);
292 |
293 | % Compute rectangle vertices
294 | half_length = foot_size(2) / 2;
295 | half_width = foot_size(1) / 2;
296 | corners = [
297 | -half_length, -half_width;
298 | half_length, -half_width;
299 | half_length, half_width;
300 | -half_length, half_width
301 | ];
302 |
303 | % Do rotation and translation
304 | rotation_matrix = [cos(theta_foot), -sin(theta_foot);
305 | sin(theta_foot), cos(theta_foot)];
306 | rotated_corners = (rotation_matrix * corners')';
307 | translated_corners = rotated_corners + [x_foot, y_foot];
308 |
309 | % Draw rectangle
310 | fill(translated_corners(:, 1), translated_corners(:, 2), 'b', 'FaceAlpha', 0.3, 'EdgeColor', 'k', 'LineWidth', 1.5);
311 | end
312 |
313 | axis equal;
314 | title('ZMP trajectory', 'FontSize', 15 );
315 | xlabel('x-axis');
316 | ylabel('y-axis');
317 | grid on;
318 | end
319 |
320 |
321 |
322 | function CoM_trajectory = generate_CoM_trajectory_from_ZMP(zmp_trajectory, h, delta_t)
323 |
324 | # Alternative approach: generate CoM trajectory for pelvis by exploiting dynamics
325 | % equations related to ZMP.
326 |
327 | % Gravity constant
328 | g = 9.81;
329 |
330 | % Number of points
331 | num_points = size(zmp_trajectory, 1);
332 |
333 | % Init CoM position and velocity
334 | vel_x = 0;
335 | vel_y = 0;
336 | pos_x = zmp_trajectory(1, 1); % Initial CoM position is the initial ZMP position
337 | pos_y = zmp_trajectory(1, 2); % Same for Y
338 |
339 | % Store values
340 | CoM_trajectory = zeros(num_points, 2);
341 |
342 | % Initial CoM position is the initial ZMP position
343 | CoM_trajectory(1, :) = [pos_x, pos_y];
344 |
345 | % Compute CoM trajectory
346 | for i = 2:num_points
347 |
348 | % CoM acceleration
349 | acc_x = (g/h)*(pos_x - zmp_trajectory(i, 1));
350 | acc_y = (g/h)*(pos_y - zmp_trajectory(i, 2));
351 |
352 | % Compute CoM velocity by integrating CoM acceleration
353 | vel_x = vel_x - acc_x * delta_t;
354 | vel_y = vel_y - acc_y * delta_t;
355 |
356 | % Compute CoM position by integrating CoM velocity
357 | pos_x = pos_x + vel_x * delta_t;
358 | pos_y = pos_y + vel_y * delta_t;
359 |
360 | % Update CoM trajectory
361 | CoM_trajectory(i, :) = [pos_x, pos_y];
362 | end
363 |
364 | % Visualization of CoM and ZMP
365 | figure;
366 | plot(zmp_trajectory(:, 1), zmp_trajectory(:, 2), 'r-', 'LineWidth', 2); % ZMP
367 | hold on;
368 | plot(CoM_trajectory(:, 1), CoM_trajectory(:, 2), 'b-', 'LineWidth', 2); % CoM
369 | title('ZMP and CoM Trajectory', 'FontSize', 15 );
370 | xlabel('X Position (m)');
371 | ylabel('Y Position (m)');
372 | legend('ZMP Trajectory', 'CoM Trajectory');
373 | grid on;
374 | axis equal;
375 | end
376 |
377 |
378 |
379 |
380 | %% MAIN
381 |
382 | % trajectory variables
383 | v = 1.5; % [m/s]
384 | w = 0.7; % [rad/s]
385 | T = 5.0; % [s]
386 | delta_T = 0.01; % [s]
387 |
388 | % gait variables
389 | step_length = 0.4; % [m]
390 | step_width = 0.2; % [m]
391 | foot_size = [0.1, 0.2]; % width x length
392 |
393 | % generate global trajectory based on v,w
394 | trajectory = generate_global_trajectory(v, w, T, delta_T);
395 |
396 | % generate CoM in world frame
397 | CoM_trajectory = generate_pelvis_trajectory(trajectory, T, delta_T);
398 |
399 | % generate 2D foot positions in world frame
400 | foot_positions = generate_foot_trajectory_2d(trajectory, step_length, step_width, foot_size);
401 |
402 | % generate 3D foot trajectories in world frame
403 | step_height = 0.1; % [m]
404 | step_time = 1.0; % [s]
405 | foot_trajectory = generate_foot_trajectory_3d(foot_positions, step_height, step_length, step_time, delta_T);
406 |
407 | % generate ZMP trajectory
408 | delta_size_spline = 0.001;
409 | zmp_trajectory = generate_zmp_trajectory_spline(foot_positions, delta_size_spline, step_length, step_width, foot_size);
410 |
411 | % alternative: generate CoM trajectory by using dynamics relations with ZMP
412 | h_com = 1.5; % [m]
413 | CoM_trajectory_dynamics = generate_CoM_trajectory_from_ZMP(zmp_trajectory, h_com, delta_T);
414 |
415 |
416 |
417 |
418 |
419 |
420 |
421 |
422 |
--------------------------------------------------------------------------------
/images/2D_footsteps.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/2D_footsteps.png
--------------------------------------------------------------------------------
/images/3D.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/3D.PNG
--------------------------------------------------------------------------------
/images/3D_2.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/3D_2.gif
--------------------------------------------------------------------------------
/images/3D_2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/3D_2.png
--------------------------------------------------------------------------------
/images/3D_3.PNG:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/3D_3.PNG
--------------------------------------------------------------------------------
/images/animation.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/animation.gif
--------------------------------------------------------------------------------
/images/com_dyn.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/com_dyn.png
--------------------------------------------------------------------------------
/images/com_sine.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/com_sine.png
--------------------------------------------------------------------------------
/images/global_traj.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/global_traj.png
--------------------------------------------------------------------------------
/images/lip.jpg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/lip.jpg
--------------------------------------------------------------------------------
/images/lip2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/lip2.png
--------------------------------------------------------------------------------
/images/output.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/output.gif
--------------------------------------------------------------------------------
/images/trot_gait.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/trot_gait.png
--------------------------------------------------------------------------------
/images/zmp.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Argo-Robot/footsteps_planning/ae37220c2e1f313a84bcd66a6da82e191b936826/images/zmp.png
--------------------------------------------------------------------------------
/quadrupeds/quadrupeds_gait_generation.m:
--------------------------------------------------------------------------------
1 | %% Generate an elliptical foot trajectory in 2D (xz plane) for a quadruped doing Trot Gait.
2 |
3 | %% The front left leg and the rear right leg move together.
4 | %% The front right leg and the rear left leg move together.
5 |
6 | % Parameters for the elliptical foot trajectory
7 | A_x = 0.1; % Amplitude in the forward (x) direction [meters]
8 | A_z = 0.05; % Amplitude in the vertical (z) direction [meters]
9 |
10 | % Define phase offsets for trot gait
11 | phi_rear_rigth_0 = 0; % Rear right leg initial phase
12 | phi_front_right_0 = pi; % Front right leg initial phase (opposite phase)
13 |
14 | % Define stepping frequency
15 | T0 = 3.0; # Gait cycle [s]
16 | f0 = 1/T0; # Frequency [Hz]
17 |
18 | % Distance between diagonal legs in the x plane
19 | offset_x = 0.2; % [meters]
20 | offset_y = 0.0; % [meters]
21 |
22 | num_steps = 100; % Number of steps in the trajectory
23 | time = linspace(0, T0, num_steps);
24 |
25 | % Function to calculate phase at time t
26 | phase = @(phi_0, t) mod(phi_0 + 2 * pi * f0 * t, 2 * pi);
27 |
28 | % Function to generate elliptical trajectory for a given phase
29 | foot_trajectory = @(phi) [A_x * cos(phi); -A_z * sin(phi)];
30 |
31 | % Initialize storage for trajectories
32 | rear_rigth_traj = zeros(2, num_steps);
33 | front_right_traj = zeros(2, num_steps);
34 |
35 | % Compute trajectories for each leg
36 | for i = 1:num_steps
37 | % Compute phase for each leg
38 | phi_rr = phase(phi_rear_rigth_0, time(i));
39 | phi_fr = phase(phi_front_right_0, time(i));
40 |
41 | % Compute foot trajectories
42 | rear_rigth_traj(:, i) = foot_trajectory(phi_rr) + [-offset_x; offset_y];
43 | front_right_traj(:, i) = foot_trajectory(phi_fr) + [offset_x; offset_y];
44 | end
45 |
46 | % Animation setup
47 | figure;
48 | hold on;
49 | grid on;
50 | axis equal;
51 | xlim([-0.4, 0.4]);
52 | ylim([-0.3, 0.3]);
53 | xlabel('Forward X-direction [m]');
54 | ylabel('Vertical Z-direction [m]');
55 | title('Foot Trajectory Animation (Trot Gait)', 'FontSize', 15 );
56 |
57 | % Define plots for the legs
58 | rear_right_plot = plot(0, 0, 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g');
59 | front_right_plot = plot(0, 0, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
60 | trajectory_line_rl = plot(rear_rigth_traj(1, :), rear_rigth_traj(2, :), 'g--');
61 | trajectory_line_fr = plot(front_right_traj(1, :), front_right_traj(2, :), 'r--');
62 |
63 | % Add legend
64 | legend([rear_right_plot, front_right_plot], 'Rear Right Leg', 'Front Right Leg', 'Location', 'best');
65 |
66 | % Animate foot trajectories
67 | for i = 1:num_steps
68 | % Update positions
69 | set(rear_right_plot, 'XData', rear_rigth_traj(1, i), 'YData', rear_rigth_traj(2, i));
70 | set(front_right_plot, 'XData', front_right_traj(1, i), 'YData', front_right_traj(2, i));
71 |
72 | % Pause to simulate real-time motion
73 | pause(T0 / num_steps);
74 | end
75 |
76 | hold off;
77 |
78 |
--------------------------------------------------------------------------------