Coordinate Systems

GAMS' coordinate system capabilities, contained within the gams::pose namespace, are designed to provide easy conversion between different kinds of coordinate reference frames, embedded within each other. For example, you might represent the Earth using a GPSFrame reference frame, and embed a CartesianFrame within it, with an origin at a given longitude and latitude. This would allow for manipulating positions on a scale of meters near that origin, automatically translating back to latitude and longitudes as needed.

The basic coordinate types, Position, Orientation, and Pose (which combines position and orientation information into one object) bind to a reference frame. GAMS provides a default Cartesian reference frame, suitable if your application does not require any conversion between reference frames.

Reference Frames

Reference Frame objects represent the various coordinate systems coordinates can belong to. Reference frames can embed within other frames, allowing for transformations between those frames. These reference frame objects must live at least as long as the coordinates that reference it (i.e., not be destructed before them), as well as any other reference frames embedded in them.

GAMS supports two kinds of reference frames: CartesianFrame, and GPSFrame. The GPSFrame is a simple spherical model. You may construct a GPSFrame using an optional radius parameter to create frames representing bodies other than Earth. GPSFrame provides constants for Earth, Mars, and the Moon.

There are two default frames available, each accessed through a global function in the pose namespace: default_frame() and gps_frame(). Each returns a const reference to a frame whose lifetime lasts until termination of the program. These frames are parentless, and may serve as roots of a frame tree. A frame may have a parent, by specifying an origin point within another frame. By creating frame objects with parents, coordinates can easily convert between any frames in the resulting tree. For example:

using namespace gams::pose;
const ReferenceFrame &earth_frame = gps_frame(); // Latitude/Longitude frame representing planet Earth

// Cartesian frame representing a house, at the given Longitude and Latitude
CartesianFrame house_frame(Position(earth_frame,-79.9500588, 40.4464481));

// Cartesian frame representing a given room in the house
CartesianFrame room_frame(Position(house_frame, 10, 20));

In this example, coordinates within room_frame may be converted to coordinates with earth_frame. Instead of using Position, Pose may be used instead to specify frames that are rotated with respect to another. Position objects keep a reference to their frame, so you must ensure that a frame object outlives any positions belonging to it.

GAMS currently does not yet support fully converting from GPSFrame to CartesianFrame; specifically, coordinates within a GPSFrame cannot convert to a CartesianFrame that is rotated with respect to it.

Coordinate Types

GAMS supports three coordinate types: Position, Orientation, and Pose. All coordinates bind to a frame (the first argument of any constructor). If you construct a coordinate of any type omitting the frame argument, it will bind to the default reference frame, a Cartesian frame that has no parent.

Position

Position represents position information. It can represent coordinates within any type of 3-dimensional coordinate system (there aren't separate classes for GPS vs Cartesian coordinates). Position provides methods for accessing those coordinates according to various naming conventions:

Position earth_loc(earth_frame, -79.9494575, 40.4465197);
earth_loc.lat(); // Latitude (40.4465197)
earth_loc.lng(); // Longitude (-79.9494575)
earth_loc.alt(); // altitude (0 meters above surface, the default)

Position room_loc(room_frame, 1, 2);
room_loc.x(); // x-coordinate (1 meter from origin)
room_loc.y(); // y-coordinate (2 meters from origin)
room_loc.z(); // z-coordinate (0 meters above origin, the default)

These different functions are for the sake of readable code; there is no underlying difference between, for example, earth_loc.lng() and earth_loc.x().

To set coordinates, simply pass the new value as an argument to the function for that coordinate; e.g., room_loc.z(2).

Orientation

The Orientation class represents a rotation or orientation relative to its frame. There are two primary ways to construct an Orientation. The easiest is to specify one of the fundamental axes, and an Orientation (in degrees, counter clockwise if looking from above) around that axis:

Orientation simple_rot(room_frame, Orientation::Z_axis, 90);

The more expressive form represents the orientation in an angle-axis vector format. For this, the rx, ry, and rz parameters represent a 3-vector which points in the direction of the axis of rotation relative to the origin orientation of the frame (e.g., for GPSFrame, due north along the ground), and whose magnitude is the amount of rotation about that axis in radians. The above orientation could be specified instead as follows:

Orientation simple_rot(room_frame, 0, 0, M_PI / 2);

This notation can represent any possible orientation relative to the frame, while the first notation is limited to simple orientations.

Pose

The Pose class combines both position and orientation information. It may be constructed as follows:

Pose room_pose(room_frame, 3, 1, 0, 0, 0, M_PI / 2);

Where the positional 3-vector is specified first, followed by the rotational 3-vector. A variety of other constructors are avialable (see the Doxygen documentation).

Coordinate Operations

GAMS provides the following methods on all coordinates (Position, Orientaion, and `Pose1):

transform_to creates a copy of a coordinate, but in a new reference frame. This new frame must somehow be related to the coordinate's current frame.

Pose gps_pose = room_pose.transform_to(earth_frame);

transform_this_to transforms the coordinate object, in place, to the new reference frame. Again, it must be related to the current frame.

room_pose.transform_to(earth_frame);

distance_to finds the distance between the two coordinates. For Position and Pose, this is the number of meters between them. For Orientation, this is the number of degrees between them. Note that the coordinates need not be in the same reference frame, but they must be transformable to a common reference frame. Orientation and Pose also provide angle_to. For Orientation, this is synonymous with distance_to. For Poses, this finds the degrees between the orientation parts of the Poses.

room_loc.distance_to(earth_loc); // returns sqrt(5)

Platform Frames

In GAMS, every platform belongs to a reference frame. By default, this frame is the Earth GPS-based frame. In most cases, you will manipulate coordinates belonging to this frame, or to frames embedded in it. For example:

class MyAlgorithm : public BaseAlgorithm {
  ...
  /* Note: BaseAlgorithm provides:
     protected: BasePlatform *platform_; */

  void execute() {
    const ReferenceFrame &platform_frame = platform_->get_frame();
    Position platform_location = platform_->get_location();
    CartesianFrame local_frame(platform_location)
    Pose target(local_frame, 5, 0); /* location 5 meters east of current location */
    Pose target_gps(platform_frame, target); /* converts to platform's frame (GPS) */
    platform_->move(target); /* conversion to platform's frame is automatic;
                                equivalent to platform_->move(target_gps); */

    double distance = target.distance_to(platform_location); /* distance == 5 */
  }
  ...
}

Additionally, platforms which target the VREP simulator (derived from VREPBase) provide a get_vrep_frame() method which returns a CartesianFrame, embedded within the platform's GPSFrame, which corresponds to the reference frame inside VREP. This frame will be a child frame of the platform's frame.

In general, any coordinates you pass to platform methods must belong to that platform's frame, or to a frame that is a child of that frame (or grandchild, etc.).