3. Generating gaits#
The next step in developing the robot kinematics is to generate various gaits. First, lets familiarize ourselves with the concept of gaits. Gait is a coordinated sequence of movements of the robot legs and body to achieve smooth locomotion.
The gait cycle of a hexapod robot refers to the sequential movement of its six legs to achieve locomotion. It consists of two main phases for each leg:
Stance Phase – The leg is in contact with the ground, providing support and propulsion as it moves backward relative to the body.
Swing Phase – The leg lifts off the ground, moves forward, and prepares for the next stance phase.
To get an idea of the variance and complexity between gaits, Fig 1. shows four typical gaits. The six legs, right and left hind, middle and front, indicated as RH, RM, RF, LH, LM, and LF. Stance phase is shown in white, swing phase in black.
Fig 1. Four typical hexapodal gaits, depicting each of the six legs as either supporting (white) or recovering (black). Image source: (Chen et al. 2012) borrowed from hexapodrobot.weebly.com
The three most common gaits for hexapods are the Wave, Transition (Ripple) and Tripod gaits. These are the three gaits we are going to implement in this notebook. Wave gait is the simplest and most stable as robot lifts only one leg at a time, it makes it the slowest one as well. Transition (ripple) gait is an improvement over the Wave gait as it allows to lift two legs at a time, with the half-phase shift. This makes it faster, at the slight cost of stability. Finally, the Tripod gait is the fastest and least stable one as it lifts three legs at a time.
Parametric gait generator#
The easiest generic way to implement gait generation is to use a parametric function. The parametric function is defined in the leg’s local coordinate system, with the origin at the leg’s base and the X axis pointing forward. The function takes a single parameter, the phase of the gait cycle (0 to 1), and returns the leg’s offset in the local coordinate system at that phase.
The parametric function is defined as a piecewise function that describes the leg’s movement in the swing and stance phases. In the swing phase, the leg moves forward in the X direction and up in the Z direction. In the stance phase, the leg moves backward in the X direction and stays at the ground level (Z=0).
The swing and stance phases are defined by the swing_duration parameter, which is the fraction of the gait cycle that the leg is in the air. The swing phase starts at the beginning of the gait cycle and ends at swing_duration. The stance phase starts at swing_duration and ends at the end of the gait cycle.
The parametric function is defined as follows:
def get_offsets_at_phase_for_leg(self, leg: HexapodLeg, phase: float) -> Point3D:
gait = self.gaits[self.current_gait]
leg_phase = phase - gait.swing_phase_start_offsets[leg]
leg_phase %= 1.000001 # Fractional part of the phase avoids overlap of 1.0 and 0.0
label = ''
half_step = self.step_length / 2
if leg_phase < gait.swing_duration:
# Swing phase - leg in air moving forward
t = np.interp(leg_phase, [0, gait.swing_duration], [0, 1])
x_offset = np.interp(leg_phase, [0, gait.swing_duration], [-half_step, half_step])
z_offset = np.sin(t * np.pi) * self.step_height
label = 'swing'
else:
# Stance phase - leg on ground moving backward
x_offset = np.interp(leg_phase, [gait.swing_duration, 1], [half_step, -half_step])
z_offset = 0.0 # On ground
label = 'stance'
return Point3D(
[
float(x_offset),
0.0,
float(z_offset),
],
label,
)
With this function in place, we can now implement the gait generators for the three gaits. The gait generators are defined in the gaits dictionary in the ParametricGaitGenerator class. The keys of the dictionary are the gait types, and the values are the parameters of the gait. The parameters include the swing duration and the swing phase start offsets for each leg. The swing phase start offsets define the phase at which the leg starts the swing phase.
self.gaits = {
GaitType.wave: self.GaitPhaseParams(
swing_duration=1 / 6,
swing_phase_start_offsets={
HexapodLeg.right_back: 0,
HexapodLeg.right_middle: 1 / 6,
HexapodLeg.right_front: 2 / 6,
HexapodLeg.left_back: 3 / 6,
HexapodLeg.left_middle: 4 / 6,
HexapodLeg.left_front: 5 / 6,
},
),
GaitType.ripple: self.GaitPhaseParams(
swing_duration=1 / 3,
swing_phase_start_offsets={
HexapodLeg.right_back: 0,
HexapodLeg.right_middle: 2 / 6,
HexapodLeg.right_front: 4 / 6,
HexapodLeg.left_back: 3 / 6,
HexapodLeg.left_middle: 5 / 6,
HexapodLeg.left_front: 1 / 6,
},
),
GaitType.tripod: self.GaitPhaseParams(
swing_duration=1 / 2,
swing_phase_start_offsets={
HexapodLeg.right_back: 0,
HexapodLeg.right_middle: 1 / 2,
HexapodLeg.right_front: 0,
HexapodLeg.left_back: 1 / 2,
HexapodLeg.left_middle: 0,
HexapodLeg.left_front: 1 / 2,
},
),
}
Visualizing the gaits#
The GaitsVisualizer class provides a convenient way to visualize the gaits. The visualize_continuous method visualizes each axis of the gait in 2D, while the visualize_continuous_in_3d method visualizes the gait in 3D.
from drqp_brain.parametric_gait_generator import GaitType, ParametricGaitGenerator
from drqp_kinematics.models import HexapodModel
from plotting import animate_hexapod_gait, GaitsVisualizer, is_sphinx_build
hexapod = HexapodModel()
hexapod.forward_kinematics(0, -25, 110)
gait_gen = ParametricGaitGenerator(step_length=120, step_height=50)
visualizer = GaitsVisualizer()
Animating the gaits#
In order to better understand how the gait works in 3D, we can animate the hexapod moving in the gait. The animate_hexapod_gait function below does exactly that. It takes the hexapod model, the gait generator, and the number of steps to animate. It then animates the hexapod moving in the gait for the specified number of steps.
def animate_hexapod_gait(
hexapod: HexapodModel,
gaits_gen,
interactive=False,
skip=False,
total_steps=60,
interval=16,
view_elev=47.0,
view_azim=-160,
repeat=1,
feet_trails_frames=0,
):
if skip:
return
if is_sphinx_build():
repeat = 1
hexapod.forward_kinematics(0, -25, 110)
leg_centers = {leg.label: leg.tibia_end.copy() for leg in hexapod.legs}
leg_tips = [leg.tibia_end.copy() for leg in hexapod.legs]
def set_pose(step):
step = step % total_steps # handle repeats
phase = step / total_steps # interpolation phase
for leg, leg_tip in zip(hexapod.legs, leg_tips):
offsets = gaits_gen.get_offsets_at_phase_for_leg(leg.label, phase)
leg.move_to(leg_tip + offsets)
fig, ax, plot_data = plot_hexapod(hexapod, feet_trails_frames=feet_trails_frames)
ax.view_init(elev=view_elev, azim=view_azim)
visualizer = GaitsVisualizer()
visualizer.visualize_continuous_in_3d(
_gait_generator=gaits_gen,
_steps=total_steps,
_ax=ax,
_plot_lines=None,
_leg_centers=leg_centers,
)
def update(frame=0):
set_pose(frame)
update_hexapod_plot(hexapod, plot_data)
fig.canvas.draw_idle()
animate_plot(
fig,
update,
_interactive=interactive,
_frames=total_steps * repeat,
_interval=interval,
)
Wave gait#
Wave gait is the simplest gait where only one leg is in swing phase at a time. The swing phase progresses from one leg to the next in a wave-like manner.
gait_gen.current_gait = GaitType.wave
visualizer.visualize_continuous(gait_gen, _steps=100)
_ = visualizer.visualize_continuous_in_3d(gait_gen, _steps=100)
anim = animate_hexapod_gait(hexapod, gait_gen, interactive=True, skip=False)
---------------------------------------------------------------------------
RuntimeError Traceback (most recent call last)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/IPython/core/formatters.py:406, in BaseFormatter.__call__(self, obj)
404 method = get_real_method(obj, self.print_method)
405 if method is not None:
--> 406 return method()
407 return None
408 else:
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1385, in Animation._repr_html_(self)
1383 fmt = mpl.rcParams['animation.html']
1384 if fmt == 'html5':
-> 1385 return self.to_html5_video()
1386 elif fmt == 'jshtml':
1387 return self.to_jshtml()
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1302, in Animation.to_html5_video(self, embed_limit)
1299 path = Path(tmpdir, "temp.m4v")
1300 # We create a writer manually so that we can get the
1301 # appropriate size for the tag
-> 1302 Writer = writers[mpl.rcParams['animation.writer']]
1303 writer = Writer(codec='h264',
1304 bitrate=mpl.rcParams['animation.bitrate'],
1305 fps=1000. / self._interval)
1306 self.save(str(path), writer=writer)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:121, in MovieWriterRegistry.__getitem__(self, name)
119 if self.is_available(name):
120 return self._registered[name]
--> 121 raise RuntimeError(f"Requested MovieWriter ({name}) not available")
RuntimeError: Requested MovieWriter (ffmpeg) not available
<matplotlib.animation.FuncAnimation at 0x733964889640>
Ripple gait#
Ripple gait is similar to wave gait, but two legs are in swing phase at a time. The swing phase progresses from one leg to the next with a half-phase offset.
gait_gen.current_gait = GaitType.ripple
visualizer.visualize_continuous(gait_gen, _steps=100)
_ = visualizer.visualize_continuous_in_3d(gait_gen, _steps=100)
anim = animate_hexapod_gait(hexapod, gait_gen, interactive=True, skip=False)
---------------------------------------------------------------------------
RuntimeError Traceback (most recent call last)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/IPython/core/formatters.py:406, in BaseFormatter.__call__(self, obj)
404 method = get_real_method(obj, self.print_method)
405 if method is not None:
--> 406 return method()
407 return None
408 else:
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1385, in Animation._repr_html_(self)
1383 fmt = mpl.rcParams['animation.html']
1384 if fmt == 'html5':
-> 1385 return self.to_html5_video()
1386 elif fmt == 'jshtml':
1387 return self.to_jshtml()
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1302, in Animation.to_html5_video(self, embed_limit)
1299 path = Path(tmpdir, "temp.m4v")
1300 # We create a writer manually so that we can get the
1301 # appropriate size for the tag
-> 1302 Writer = writers[mpl.rcParams['animation.writer']]
1303 writer = Writer(codec='h264',
1304 bitrate=mpl.rcParams['animation.bitrate'],
1305 fps=1000. / self._interval)
1306 self.save(str(path), writer=writer)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:121, in MovieWriterRegistry.__getitem__(self, name)
119 if self.is_available(name):
120 return self._registered[name]
--> 121 raise RuntimeError(f"Requested MovieWriter ({name}) not available")
RuntimeError: Requested MovieWriter (ffmpeg) not available
<matplotlib.animation.FuncAnimation at 0x73395fb51640>
Tripod gait#
In tripod gait, the robot legs move in two groups of three:
group A: left-front, right-middle, and left-back
group B: right-front, left-middle, and right-back.
while one group is in stance phase, the other group is in swing phase and cycle repeats.
gait_gen.current_gait = GaitType.tripod
visualizer.visualize_continuous(gait_gen, _steps=100)
_ = visualizer.visualize_continuous_in_3d(gait_gen, _steps=100)
anim = animate_hexapod_gait(hexapod, gait_gen, interactive=True, skip=False)
/home/docs/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:908: UserWarning: Animation was deleted without rendering anything. This is most likely not intended. To prevent deletion, assign the Animation to a variable, e.g. `anim`, that exists until you output the Animation using `plt.show()` or `anim.save()`.
warnings.warn(
---------------------------------------------------------------------------
RuntimeError Traceback (most recent call last)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/IPython/core/formatters.py:406, in BaseFormatter.__call__(self, obj)
404 method = get_real_method(obj, self.print_method)
405 if method is not None:
--> 406 return method()
407 return None
408 else:
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1385, in Animation._repr_html_(self)
1383 fmt = mpl.rcParams['animation.html']
1384 if fmt == 'html5':
-> 1385 return self.to_html5_video()
1386 elif fmt == 'jshtml':
1387 return self.to_jshtml()
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1302, in Animation.to_html5_video(self, embed_limit)
1299 path = Path(tmpdir, "temp.m4v")
1300 # We create a writer manually so that we can get the
1301 # appropriate size for the tag
-> 1302 Writer = writers[mpl.rcParams['animation.writer']]
1303 writer = Writer(codec='h264',
1304 bitrate=mpl.rcParams['animation.bitrate'],
1305 fps=1000. / self._interval)
1306 self.save(str(path), writer=writer)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:121, in MovieWriterRegistry.__getitem__(self, name)
119 if self.is_available(name):
120 return self._registered[name]
--> 121 raise RuntimeError(f"Requested MovieWriter ({name}) not available")
RuntimeError: Requested MovieWriter (ffmpeg) not available
<matplotlib.animation.FuncAnimation at 0x73395ffdfa70>
Directional Gait Decorator#
In order to add direction to the generated gait, we can create a decorator class that will take the generated offsets and apply a rotation to them. This way we can control the direction of the movement.
TL;DR#
We need a 2D rotation matrix that aligns the offsets (originally along the X-axis) with an arbitrary direction vector \([dx, dy]\). The rotation matrix that achieves this is:
(1)#\[\begin{equation}
\begin{bmatrix}
dx & -dy\\
dy & dx
\end{bmatrix}
\end{equation}\]
Why This Works#
Original offsets are along the X-axis, meaning they can be represented as \([x, 0]\).
A standard 2D rotation matrix for an angle \(\theta\) is:
(2)#\[\begin{equation}
R=\begin{bmatrix}
\cos\theta & -\sin\theta\\
\sin\theta & \cos\theta
\end{bmatrix}
\end{equation}\]
The unit direction vector \([dx, dy]\) corresponds to the cosine and sine of some angle, where:
(3)#\[\begin{equation}
\begin{aligned}
dx = \cos\theta\\
dy = \sin\theta
\end{aligned}
\end{equation}\]
Substituting these into the rotation matrix gives us the desired transformation matrix.
(4)#\[\begin{equation}
R=\begin{bmatrix}
dx & -dy\\
dy & dx
\end{bmatrix}
\end{equation}\]
from drqp_kinematics.geometry import AffineTransform, Point3D
class DirectionalGaitGenerator:
"""Gait generator decorator to allow steering in any direction."""
def __init__(self, decorated):
super().__init__()
self.decorated = decorated
def get_offsets_at_phase_for_leg(self, leg, phase, direction=Point3D([1, 0, 0])) -> Point3D:
tf = self.__make_transform(direction)
offsets = self.decorated.get_offsets_at_phase_for_leg(leg, phase)
return tf.apply_point(offsets)
@staticmethod
def __make_transform(direction):
# Normalize direction vector
norm_direction = direction.normalized().numpy()
# Create rotation matrix to align direction with x-axis
# Ignore z-component as robot can't walk up. This also allows to generate steering in place
direction_transform = AffineTransform.from_rotmatrix(
[
[norm_direction[0], -norm_direction[1], 0],
[norm_direction[1], norm_direction[0], 0],
[0, 0, 1],
]
)
return direction_transform
# Example usage
directional_tripod_gen = DirectionalGaitGenerator(gait_gen)
visualizer = GaitsVisualizer()
visualizer.visualize_continuous_in_3d(
_gait_generator=directional_tripod_gen, direction=Point3D([1, 0, 0], 'Forward')
)
visualizer.visualize_continuous_in_3d(
_gait_generator=directional_tripod_gen, direction=Point3D([0, 1, 0], 'Left')
)
visualizer.visualize_continuous_in_3d(
_gait_generator=directional_tripod_gen, direction=Point3D([1, -1, 0], 'Forward-right')
)
# stomp in place
_ = visualizer.visualize_continuous_in_3d(
_gait_generator=directional_tripod_gen, direction=Point3D([0, 0, 1], 'UP/Stomp')
)
Adding a direction vector did the trick, at least charts look good. Let’s see it on the hexapod.
from plotting import animate_plot, is_sphinx_build, plot_hexapod, update_hexapod_plot
def animate_hexapod_gait_with_direction(
hexapod: HexapodModel,
gaits_gen,
interactive=False,
animate_trajectory=False,
skip=False,
total_steps=60,
interval=16,
view_elev=47.0,
view_azim=-160,
repeat=1,
gait_lines=None,
direction_degrees=0,
animate_direction_degrees=False,
direction_vector_length=100,
trajectory_animation_start=0,
trajectory_animation_end=1,
feet_trails_frames=0,
):
if skip:
return
if is_sphinx_build():
repeat = 4
leg_tips = [leg.tibia_end.copy() for leg in hexapod.legs]
leg_centers = {leg.label: leg.tibia_end.copy() for leg in hexapod.legs}
def set_pose(step, direction):
step = step % total_steps # handle repeats
phase = step / total_steps # interpolation phase
for leg, leg_tip in zip(hexapod.legs, leg_tips):
offsets = gaits_gen.get_offsets_at_phase_for_leg(leg.label, phase, direction=direction)
leg.move_to(leg_tip + offsets)
fig, ax, plot_data = plot_hexapod(hexapod, feet_trails_frames=feet_trails_frames)
ax.view_init(elev=view_elev, azim=view_azim)
dir_plot = ax.plot([0, direction_vector_length], [0, 0], [0, 0], 'y--')
if animate_trajectory:
trajectory_animation_end = 0
visualizer = GaitsVisualizer()
_, gait_lines = visualizer.visualize_continuous_in_3d(
_gait_generator=gaits_gen,
_steps=total_steps,
_ax=ax,
_phase_start=trajectory_animation_start,
_phase_end=trajectory_animation_end,
_plot_lines=None,
_leg_centers=leg_centers,
)
def update(frame=0, direction_degrees=direction_degrees):
if animate_direction_degrees:
direction_degrees = (frame / (total_steps * repeat)) * 360
direction = AffineTransform.from_rotvec(
[0, 0, direction_degrees], degrees=True
).apply_point(Point3D([1, 0, 0]))
set_pose(frame, direction)
update_hexapod_plot(hexapod, plot_data)
dir_line = direction * direction_vector_length
dir_plot[0].set_data_3d([0, dir_line.x], [0, dir_line.y], [0, dir_line.z])
nonlocal gait_lines
nonlocal trajectory_animation_end
if animate_trajectory:
step = frame % total_steps # handle repeats
trajectory_animation_end = step / total_steps # interpolation phase
_, gait_lines = visualizer.visualize_continuous_in_3d(
_gait_generator=gaits_gen,
_steps=total_steps,
_ax=ax,
_phase_start=trajectory_animation_start,
_phase_end=trajectory_animation_end,
_plot_lines=gait_lines,
_leg_centers=leg_centers,
direction=direction,
)
if interactive:
fig.canvas.draw_idle()
animate_plot(
fig,
update,
_interactive=interactive,
_skip=skip,
_frames=total_steps * repeat,
_interval=interval,
direction_degrees=(-180, 180, 1),
)
hexapod = HexapodModel()
hexapod.forward_kinematics(0, -25, 110)
animate_hexapod_gait_with_direction(
hexapod,
directional_tripod_gen,
animate_trajectory=True,
animate_direction_degrees=True,
)
---------------------------------------------------------------------------
RuntimeError Traceback (most recent call last)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/IPython/core/formatters.py:406, in BaseFormatter.__call__(self, obj)
404 method = get_real_method(obj, self.print_method)
405 if method is not None:
--> 406 return method()
407 return None
408 else:
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1385, in Animation._repr_html_(self)
1383 fmt = mpl.rcParams['animation.html']
1384 if fmt == 'html5':
-> 1385 return self.to_html5_video()
1386 elif fmt == 'jshtml':
1387 return self.to_jshtml()
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1302, in Animation.to_html5_video(self, embed_limit)
1299 path = Path(tmpdir, "temp.m4v")
1300 # We create a writer manually so that we can get the
1301 # appropriate size for the tag
-> 1302 Writer = writers[mpl.rcParams['animation.writer']]
1303 writer = Writer(codec='h264',
1304 bitrate=mpl.rcParams['animation.bitrate'],
1305 fps=1000. / self._interval)
1306 self.save(str(path), writer=writer)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:121, in MovieWriterRegistry.__getitem__(self, name)
119 if self.is_available(name):
120 return self._registered[name]
--> 121 raise RuntimeError(f"Requested MovieWriter ({name}) not available")
RuntimeError: Requested MovieWriter (ffmpeg) not available
<matplotlib.animation.FuncAnimation at 0x73395e511820>
Putting it all together#
Now that we have all the pieces in place, we can put them together to create a full walk controller. The controller will take care of the following:
Process input command of the walk direction and rotation
Generate a walk trajectory based on the input direction
Generate a turn trajectory based on the input rotation
Combine the two trajectories into a single walk trajectory
Apply the walk trajectory to the robot based on the current robot legs position
from drqp_brain.parametric_gait_generator import GaitType, ParametricGaitGenerator
from drqp_kinematics.geometry import AffineTransform, Point3D
import numpy as np
class WalkController:
def __init__(
self,
hexapod,
step_length=60.0,
step_height=40.0,
rotation_speed_degrees=10.0,
phase_steps_per_cycle=30.0,
gait=GaitType.wave,
):
self.hexapod = hexapod
self.leg_tips_on_ground = [(leg, leg.tibia_end.copy()) for leg in hexapod.legs]
self.step_length = step_length
self.step_height = step_height
self.rotation_speed_degrees = rotation_speed_degrees
self.gait_gen = ParametricGaitGenerator(step_length=1.0, step_height=1.0, gait=gait)
self.phase_step = 1 / phase_steps_per_cycle
self.reset()
@property
def current_gait(self):
return self.gait_gen.current_gait
@current_gait.setter
def current_gait(self, gait):
self.gait_gen.current_gait = gait
def reset(self):
self.current_direction = Point3D([0, 0, 0])
self.current_rotation_direction = 0
self.current_phase = 0.0
def next_step(
self,
stride_direction: Point3D,
rotation_direction: float,
phase_override: float | None = None,
body_direction: Point3D | None = None,
body_rotation: Point3D | None = None,
verbose: bool = False,
):
feet_targets = self.next_step_targets(
stride_direction=stride_direction,
rotation_direction=rotation_direction,
phase_override=phase_override,
body_direction=body_direction,
body_rotation=body_rotation,
verbose=verbose,
)
self.apply_feet_targets(feet_targets)
return feet_targets
def next_step_targets(
self,
stride_direction: Point3D,
rotation_direction: float,
phase_override: float | None = None,
body_direction: Point3D | None = None,
body_rotation: Point3D | None = None,
verbose: bool = False,
):
self.__update_body_transform(body_direction, body_rotation)
self.__next_phase(phase_override)
feet_targets = self.__next_feet_targets(stride_direction, rotation_direction, verbose)
return feet_targets
def apply_feet_targets(self, legs_and_targets):
self.__move_feet(legs_and_targets)
def __next_phase(self, phase_override: float | None = None):
if phase_override is not None:
self.current_phase = phase_override
else:
self.current_phase += self.phase_step
def __next_feet_targets(
self, stride_direction: Point3D, rotation_direction: float, verbose: bool
):
old_stride_ratio = (
abs(self.current_direction.x)
+ abs(self.current_direction.y)
+ abs(self.current_direction.z)
)
no_motion_eps = 0.05
had_stride = abs(old_stride_ratio) > no_motion_eps
had_rotation = abs(self.current_rotation_direction) > no_motion_eps
rotation_direction = np.clip(rotation_direction, -1, 1)
self.current_rotation_direction = np.interp(
0.3, [0, 1], [self.current_rotation_direction, rotation_direction]
)
self.current_direction = self.current_direction.interpolate(stride_direction, 0.3)
new_stride_ratio = (
abs(self.current_direction.x)
+ abs(self.current_direction.y)
+ abs(self.current_direction.z)
)
new_stride_ratio = float(np.clip(new_stride_ratio, 0, 1))
self.current_rotation_direction = np.clip(self.current_rotation_direction, -1, 1)
has_stride = abs(new_stride_ratio) > no_motion_eps
has_rotation = abs(self.current_rotation_direction) > no_motion_eps
had_motion = had_stride or had_rotation
has_motion = has_stride or has_rotation
starting = not had_motion and has_motion
stopped = not had_motion and not has_motion
if starting or stopped:
self.current_phase = 0
###############################################################
result = []
direction_transform = self.__make_direction_transform(self.current_direction)
for leg, leg_tip in self.leg_tips_on_ground:
gait_offsets = self.gait_gen.get_offsets_at_phase_for_leg(
leg.label,
self.current_phase,
)
# Apply steering
stride_offsets = Point3D([0, 0, 0])
direction_offsets = Point3D([0, 0, 0])
stride_target = leg_tip
if has_stride:
stride_offsets = gait_offsets * Point3D(
[self.step_length * new_stride_ratio, 0.0, 0.0]
)
direction_offsets = direction_transform.apply_point(stride_offsets)
stride_target = stride_target + direction_offsets
# Apply rotation
rotation_target = leg_tip
if has_rotation:
rotation_degrees = (
self.rotation_speed_degrees * self.current_rotation_direction * gait_offsets.x
)
rotation_transform = AffineTransform.from_rotvec(
[0, 0, rotation_degrees], degrees=True
)
rotation_target = rotation_transform.apply_point(rotation_target)
if has_stride or has_rotation:
mix_weights = np.array([new_stride_ratio, abs(self.current_rotation_direction)])
mix_weights /= mix_weights.sum()
stride_weight, rotation_weight = mix_weights
foot_target = stride_target * stride_weight + rotation_target * rotation_weight
foot_target.z += gait_offsets.z * self.step_height
else:
foot_target = leg_tip
if verbose:
print(f'{leg.label} {self.current_phase=}')
print(f'{leg.tibia_end=}')
print(f'{leg_tip=}')
print(f'{gait_offsets=}')
print(f'{stride_offsets=}')
print(f'{direction_offsets=}')
print(f'{foot_target=}')
print()
result.append((leg, foot_target))
return result
@staticmethod
def __make_direction_transform(direction):
# Normalize direction vector
norm_direction = direction.normalized().numpy()
# Create rotation matrix to align direction with x-axis
# Ignore z-component as robot can't walk up
direction_transform = AffineTransform.from_rotmatrix(
[
[norm_direction[0], -norm_direction[1], 0],
[norm_direction[1], norm_direction[0], 0],
[0, 0, 1],
]
)
return direction_transform
def __move_feet(self, legs_and_targets):
for leg, foot_target in legs_and_targets:
leg.move_to(foot_target)
def __update_body_transform(
self, body_direction: Point3D | None, body_rotation: Point3D | None
):
self.hexapod.body_transform = AffineTransform.identity()
if body_direction is None and body_rotation is None:
return
if body_direction is not None:
self.hexapod.body_transform = AffineTransform.from_translation(body_direction.numpy())
if body_rotation is not None:
rotation_transform = AffineTransform.from_rotvec(body_rotation.numpy())
self.hexapod.body_transform = rotation_transform @ self.hexapod.body_transform
from drqp_brain.walk_controller import WalkController
from drqp_kinematics.models import HexapodModel
import numpy as np
from plotting import animate_plot, is_sphinx_build
def animate_hexapod_walk(
walk_controller: WalkController,
interactive=False,
skip=False,
fps=30,
view_elev=47.0,
view_azim=-160,
repeat=1,
feet_trails_frames=0,
):
if skip:
return
if interactive:
repeat = 100
if is_sphinx_build():
repeat = 8
interactive = False
fig, ax, plot_data = plot_hexapod(
walk_controller.hexapod, feet_trails_frames=feet_trails_frames
)
ax.view_init(elev=view_elev, azim=view_azim)
total_frames = fps * repeat
last_frame = 0
def animate(
frame=0,
direction_degrees=0,
walk_speed=1,
rotation_direction=0,
):
nonlocal last_frame
if interactive and frame == last_frame:
# other params are changing, don't update walker
return
phase = frame % fps
phase = phase / fps
if not interactive:
walk_speed = np.interp(
frame, [0, total_frames * 0.25, total_frames * 0.75, total_frames], [0, 1, 1, 0]
)
if frame < total_frames * 0.25 or frame > total_frames * 0.60:
direction_degrees = 0
else:
direction_degrees = 30
if frame < total_frames * 0.60:
rotation_direction = 0
else:
rotation_direction = 1.0
stride_direction = Point3D([1, 0, 0])
stride_direction = AffineTransform.from_rotvec(
[0, 0, direction_degrees], degrees=True
).apply_point(stride_direction)
walk_controller.next_step(
phase_override=phase,
stride_direction=stride_direction * walk_speed,
rotation_direction=rotation_direction,
verbose=False,
)
update_hexapod_plot(hexapod, plot_data)
fig.canvas.draw_idle()
last_frame = frame
animate_plot(
fig,
animate,
_interactive=interactive,
_frames=total_frames,
_interval=1000 / fps,
walk_speed=(0, 2, 0.1),
direction_degrees=(-180, 180, 1),
rotation_direction=(-2, 2, 0.1),
)
hexapod = HexapodModel()
hexapod.forward_kinematics(0, -25, 110)
walker = WalkController(
hexapod, step_length=120, step_height=60, rotation_speed_degrees=10, gait=GaitType.ripple
)
anim = animate_hexapod_walk(
walker,
interactive=True,
skip=False,
feet_trails_frames=40,
repeat=5,
view_elev=70,
view_azim=180,
fps=30,
)
---------------------------------------------------------------------------
RuntimeError Traceback (most recent call last)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/IPython/core/formatters.py:406, in BaseFormatter.__call__(self, obj)
404 method = get_real_method(obj, self.print_method)
405 if method is not None:
--> 406 return method()
407 return None
408 else:
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1385, in Animation._repr_html_(self)
1383 fmt = mpl.rcParams['animation.html']
1384 if fmt == 'html5':
-> 1385 return self.to_html5_video()
1386 elif fmt == 'jshtml':
1387 return self.to_jshtml()
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:1302, in Animation.to_html5_video(self, embed_limit)
1299 path = Path(tmpdir, "temp.m4v")
1300 # We create a writer manually so that we can get the
1301 # appropriate size for the tag
-> 1302 Writer = writers[mpl.rcParams['animation.writer']]
1303 writer = Writer(codec='h264',
1304 bitrate=mpl.rcParams['animation.bitrate'],
1305 fps=1000. / self._interval)
1306 self.save(str(path), writer=writer)
File ~/checkouts/readthedocs.org/user_builds/drqp/checkouts/latest/.venv/lib/python3.12/site-packages/matplotlib/animation.py:121, in MovieWriterRegistry.__getitem__(self, name)
119 if self.is_available(name):
120 return self._registered[name]
--> 121 raise RuntimeError(f"Requested MovieWriter ({name}) not available")
RuntimeError: Requested MovieWriter (ffmpeg) not available
<matplotlib.animation.FuncAnimation at 0x73395e5130e0>