Robot Geometry

In order to control a robot it’s necessary to know its position and orientation — where it is at any particular time. Its position in 3-D space is defined using the set of coordinates X, Y, and Z. This coordinate system is called the Cartesian coordinate system. When the robot moves from one position to another it’s referred to as a translation. When it rotates about its center point then it has changed orientation.

For the mobile robots that we use for competition we are mostly concerned 2D space so most of this section will focus on that. There will however be times when we would need to know our pose on 3D space so that will be covered later.

Position & Orientation

Moreover, the position needs to be specifed relative to a suitable reference frame. When we are navigating in a car using a GPS system our reference frame is the Earth. In the case of FRC, a suitable reference frame is the competition field.

2D Geometry

2D Pose

Let's start by examining the orientation or rotation of the robot. This is commonly done by using Euler Angles.

Rotation Matrix

In First Robotics we don't use flying robots so we usually don't need to keep track of objects in 3D space. The WPI Library therefore has a set of data structures that keeps track of things in 2D space. Since we're looking at Euler Angles let's first examine the Rotation2d class that keeps track of rotations in 2D space. When creating an object from this class you can pass it a starting angle that describes a rotation offset from the (𝑥, 𝑦) orientation of the game field. This offset represents the orientation at which you placed the robot down on the field. If no starting angle is passed then it's assumed that you aligned the robot with the fields' (𝑥, 𝑦) orientation.

Rotation2d Constructor

Once you have a Rotation2d object you can continuously track the robots' orientation by adding or subtracting changes that are sensed from the gyro. This is done using the rotation matrixes shown in a previous diagram.

Rotation2d Plus & Minus

Translation - Change in Position

Translation is the change in position of an object. It's also referred to as displacment - how far has the object moved from the last position relative to a fixed frame of reference. In our case, the fixed reference frame would be the competition field. In order to represent how a robot changes its position we use the Translation2d data structure. When creating an object from this class you can pass in a starting position for the robot, which represents its (𝑥, 𝑦) displacement from the origin. If no starting position is defined then it's assumed that the robot starts at the origin (0, 0). Note that the origin of the reference frame can be different in various cases. For the FRC field it's placed at the top left corner as you look down on the field.

Translation2d Constructor

When the robot moves from one position to another you can update the Translation2d data structure by sending it the change in position since the last update. The change is added to the current position to represent the robots' new location.

Translation2d Translate

Robot Pose

The common way to keep track of the position and orientation of a robot is to create a class called a Pose. This class comprises the two subclasses described previously, namely Translation2d and Rotation2d. The translation class keeps track of the robot’s position whereas the rotation keeps track of its orientation. Why use translation and rotation instead of position and orientation? We want to know where the robot is as it travels from one position to another. We also want to know the robot’s heading as is rotates from one orientation to another.

Pose2d

3D Geometry

Describe... The orientation of a robot is described in terms of Roll, Pitch, Yaw.

Euler Angles and Rotation

The Euler angles are three angles introduced by Leonhard Euler to describe the orientation of an object with respect to a fixed coordinate system. The order in which you specify a change in orientation is Yaw, Pitch, Roll. One way to remember this is to think of plane about to take off from an airport. While it’s on the ground it can only change its heading, which is done by a Yaw rotation. At take-off the pilot will pull up the nose, changing its Pitch. Finally, once the plane is in the air it’s free to execute a Roll. Moving from one orientation to another is called a Rotation.

A robot that can move in all three dimensions of space and can orient itself about all three axis would have 6-degrees-of-freedom (6-DOF). An object in space can have at most only 6-degrees-of-freedom. An example of such a robot would be a drone or aircraft.

Yaw Pitch Roll

Each Yaw, Pitch, Roll maneuver can be represented by a rotation matrix that uses sines and cosines to describe the offset from the previous orientation.

References