Skip to main content

Tracks & Waypoints

Understanding Transforms & Poses

Before we can send our robot out to drive a track (or path), we need to understand where our robot is and where we want to send it. For that purposes, please refer to Concepts - Transforms & Poses before continuing on with this overview.

Creating and Propagating Poses for the Track Follower

When you want the Amiga to perform a specific movement, you need to provide it with a series of poses that describe that movement. We call these poses waypoints and the series of them a track.

For a better understanding of the Pose structure, please refer to our Pose proto overview.

Example

There are several ways of creating poses and commanding your track_follower to follow them. Let's check how to use the concepts learned here to create poses that represent a pi turn (also know as a U turn) using the pose structure:

NOTE: Please note that in this example we propagate the proposes from the robot frame.

def create_pi_turn_segment(
previous_pose: Pose3F64, next_frame_b: str, radius: float, spacing: float = 0.1
) -> list[Pose3F64]:
"""Compute a pi turn segment.

Args:
previous_pose (Pose3F64): The previous pose.
next_frame_b (str): The name of the child frame of the next pose.
radius (float): The radius of the pi turn.
spacing (float): The spacing between waypoints, in meters.

Returns:
list[Pose3F64]: The poses of the pi turn segment.
"""
# Calculate the total arc length of the half-circle
arc_length = pi * radius

# Determine the number of segments, ensuring at least one segment
num_segments = max(int(arc_length / spacing), 1)

# Angle increment per segment
delta_angle = pi / num_segments

# Distance increment per segment
delta_distance = arc_length / num_segments

# Create a container to store the track segment waypoints
segment_poses: list[Pose3F64] = [previous_pose]

for i in range(1, num_segments + 1):

# Calculate the pose for the current segment
turn_segment: Pose3F64 = Pose3F64(
a_from_b=Isometry3F64([delta_distance, 0, 0], Rotation3F64.Rz(delta_angle)),
frame_a=segment_poses[-1].frame_b,
frame_b=f"{next_frame_b}_{i-1}",
)
segment_poses.append(segment_poses[-1] * turn_segment)

# Rename the last pose to the desired name
segment_poses[-1].frame_b = next_frame_b
return segment_poses

Code Breakdown

Since we're in the robot frame, we always command it to drive forward, for this reason the y and z components of our Isometry3F64 are always zero.

  • The robot moves forward by delta_distance (linear movement along the x-axis).
  • It rotates by delta_angle about the z-axis.
  • This transformation is then used to calculate the new pose of the robot for that segment.

In essence, by using Isometry3F64, you're able to succinctly describe both the linear and angular movements of the robot for each segment of its pi turn.

Next Steps

Make sure to check the Concepts page to know more about all the services available in your Amiga and how they interact with each other.

Make sure to also check our Track Follower Examples to test the track_follower on your Amiga.