World Model

Poses

The state of every object in the world is represented by its 2D pose:

pose:
  x: x-axis coordinate [m]
  y: y-axis coordinate [m]
  a: orientation angle [rad]

vector form: {x, y, a}

NOTE: All calculations are done using the right hand rule.

Frames

There are two important frames in the system, the world frame and robot frame.

The world frame is the global frame and is fixed to the center of the field. The positive x-axis points towards the yellow goal as shown below.

There is also a robot frame attached to each robot. These frames are defined such that the origin is at the center of the robot with the x-axis forward and the y-axis to the left.

Transforming Data

There are two functions in util.lua that can be used to transform poses between frames.

util.pose_global(pRelative, pose) - transform the pose pose in the pRelative frame to the global frame

util.pose_relative(pGlobal, pose) - transform the pose pGlobal into the frame defined by pose

both of these functions use the vector form of a pose {x, y, a}

Example Usage

The following are some example cases of how to transform data from one frame to another. All the values in the example are taken at the same time as the figure of the field above.

Ball

Each robot calculates the position of the ball in its local robot frame. The current ball location is stored in shared memory. In this example we will get the position of the ball in the world frame.

First, get the ball location:

> ball = wcm.get_ball();
> util.ptable(ball);
y     -0.025673485446293
x     0.91375672453896
t     29.48 
vx    0
vy    0

and the pose of the robot:

> robotPose = wcm.get_pose()
> util.ptable(robotPose);
y    -0.37558883234059
x    -1.5945999399009
a    -2.6230558191345

The robot pose is relative to the world frame and the ball position is relative to the robot frame, so we can get the position of the ball in the world frame as follows:

> ballGlobal = util.pose_global({ball.x, ball.y, 0}, {robotPose.x, robotPose.y, robotPose.a});
> print(ballGlobal)
{-2.40096, -0.806157, -2.62306}

NOTE: The ball is symmetric so its orientation is not defined. Here we are just using an angle of 0 for the orientation in the robot frame. The orientation in the world frame is not informative.

Goal

The goal positions are known and their position is fixed in the world frame. It is usually needed to get these positions relative to the robot frame. This example will go through the process of determining the angle of the attacking goal (useful in deciding if you should shoot the ball or not).

First, get the pose of the goal:

> goalPose = wcm.get_goal_attack();
> print(goalPose);
{-3, 0, 0}

and the robot's pose:

> robotPose = wcm.get_pose()
> util.ptable(robotPose);
y    -0.37558883234059
x    -1.5945999399009
a    -2.6230558191345

Now transform the goal pose into the robot's frame:

> goalRelative = util.pose_relative(goalPose, {robotPose.x, robotPose.y, robotPose.a})
> print(goalRelative)
{1.03451, -1.02275, 2.62306}

Compute the angle of the goal relative to the robot:

> angleToGoal = math.atan2(goalRelative[2], goalRelative[1])
> print(angleToGoal*180.0/math.pi)
-44.672423493843

Here the attacking goal is -45 deg from the robot.