Geometry

Geometry#

Two dimensional#

#include "navground/core/common.h"
using navground::core::Radians = ng_float_t#

Angle in radians.

Radians navground::core::PI = static_cast<Radians>(M_PI)#
Radians navground::core::TWO_PI = static_cast<Radians>(2 * M_PI)#
Radians navground::core::HALF_PI = static_cast<Radians>(M_PI_2)#
enum class navground::core::Frame#

Values:

enumerator relative#

agent-fixed frame

enumerator absolute#

world-fixed frame

using navground::core::Vector2 = Eigen::Vector2<ng_float_t>#

A two-dimensional vector, see Eigen

inline Radians navground::core::orientation_of(const Vector2 &vector)#

The orientation of a two dimensional vector.

Parameters:

vector – [in] The vector

Returns:

The orientation of the vector

inline Radians navground::core::normalize_angle(Radians value)#

Normalize an angle to a value in \([-\pi, \pi]\). Apply it when computing angular differences.

Parameters:

value – [in] The original unbounded angle

Returns:

The equivalent angle in \([-\pi, \pi]\)

inline Vector2 navground::core::to_relative(const Vector2 &value, const Pose2 &reference)#

Transform an absolute to a relative vector.

Equivalent to reference.inverse().transform_vector(value)

Parameters:

  • value – [in] The absolute vector

  • reference – [in] The reference pose

Returns:

The absolute vector

inline Vector2 navground::core::to_absolute(const Vector2 &value, const Pose2 &reference)#

Transform a relative to an absolute vector.

Equivalent to reference.transform_vector(value)

Parameters:

  • value – [in] The relative vector

  • reference – [in] The reference pose

Returns:

The relative vector

inline Vector2 navground::core::to_relative_point(const Vector2 &value, const Pose2 &reference)#

Transform an absolute to a relative vector.

Equivalent to reference.inverse().transform_point(value)

Parameters:

  • value – [in] The absolute vector

  • reference – [in] The reference pose

Returns:

The absolute point

inline Vector2 navground::core::to_absolute_point(const Vector2 &value, const Pose2 &reference)#

Transform a relative to an absolute point.

Equivalent to reference.transform_point(value)

Parameters:

  • value – [in] The relative point

  • reference – [in] The reference pose

Returns:

The relative point

inline Vector2 navground::core::unit(ng_float_t angle)#

Unit vector with a desired orientated.

Parameters:

angle – [in] The desired orientation

Returns:

Vector of norm one and desired orientation

inline Vector2 navground::core::rotate(const Vector2 vector, ng_float_t angle)#

Rotate a two-dimensional vector.

Parameters:

  • vector – [in] The original vector

  • angle – [in] The rotation angle in radians

Returns:

The rotated vector

inline Vector2 navground::core::clamp_norm(const Vector2 &vector, ng_float_t max_length)#

Clamp the norm of a vector.

Parameters:

  • vector – [in] The original vector

  • max_length – [in] The maximum length

Returns:

A vector with the original orientation but norm clamped to max_length.

struct Pose2#

Two-dimensional pose composed of planar position and orientation. When not specified, poses are assumed to be in the world-fixed frame.

Poses are also associated to rigid transformations in SE(2) translation(pose.position) . rotation(pose.orientation). We define the group operations as a multiplication and add methods like transform_pose for a pose to operate as a rigid transformation.

Public Functions

inline Pose2 rotate(Radians angle) const#

Rotate the pose by an angle.

Parameters:

angle – [in] The rotation angle in radians.

Returns:

The rotated pose.

Pose2 integrate(const Twist2 &twist, ng_float_t dt)#

Integrate a pose.

Parameters:

  • twist – [in] The twist (in agent or world frame)

  • dt – [in] The time step

Returns:

The result of pose + dt * twist (in world frame)

inline bool operator==(const Pose2 &other) const#

Equality operator.

Parameters:

other – [in] The other pose

Returns:

The result of the equality

inline bool operator!=(const Pose2 &other) const#

Inequality operator.

Parameters:

other – [in] The other pose

Returns:

The result of the inequality

inline Pose2 absolute(const Pose2 &reference) const#

Transform a relative pose to an absolute pose.

Equivalent to reference.transform_pose(self) = reference * self

Parameters:

reference – [in] The reference pose

Returns:

The absolute pose

inline Pose2 relative(const Pose2 &reference) const#

Transform an absolute pose to a relative pose.

Equivalent to reference.inverse().transform_pose(self) = reference.inverse() * self

Parameters:

reference – [in] The reference pose

Returns:

The relative pose

inline Pose2 operator*(const Pose2 &other) const#

Concatenate two transformations, i.e. the SE(2) operation.

Parameters:

other – [in] The other pose

Returns:

The composite transformation resulting from applying first the other transformation and then this transformation.

inline Pose2 operator/(const Pose2 &other) const#

SE(2) division of two transformations.

Parameters:

other – [in] The other pose

Returns:

self * other.inverse()

inline Pose2 inverse() const#

Invert a transformation, i.e., SE(2) inversion.

Returns:

The inverse transformation

inline Pose2 transform_pose(const Pose2 &pose) const#

Applies this rigid transformation to another pose.

Equivalent to pose.absolute(self)

Parameters:

pose – [in] The pose to transform

Returns:

self * pose

inline Vector2 transform_point(const Vector2 &point) const#

Applies this rigid transformation to a point.

Equivalent to to_absolute_point(point, self)

Parameters:

point – [in] The point to transform

Returns:

The roto-translated point

inline Vector2 transform_vector(const Vector2 &vector) const#

Applies the rigid transformation to a vector.

Equivalent to to_absolute(vector, self)

Parameters:

vector – [in] The vector to transform

Returns:

The rotated vector

inline Pose2 get_transformation_in_frame(const Pose2 &frame) const#

Returns this transformation in the desired frame of reference.

The returned transformation is defined as frame.inverse() * self * frame, so that, for any frame f, the following expressions are equivalent:

  • pose.transform(other).relative(f)

  • pose.get_transformation_in_frame(frame).transform(other.relative(f))

Parameters:

frame – [in] A posed defining the desired frame of reference for the transformation.

Returns:

The transformation with respect to frame

Public Members

Vector2 position#

Position in world frame

Radians orientation#

Orientation in world frame

struct Twist2#

Two-dimensional twist composed of planar velocity and angular speed.

Twist coordinates are in the frame specified by frame.

Public Functions

inline Twist2 rotate(Radians angle) const#

Rotate the twist by an angle.

Parameters:

angle – [in] The rotation angle in radians.

Returns:

The rotated twist.

inline bool operator==(const Twist2 &other) const#

Equality operator.

Parameters:

other – [in] The other twist

Returns:

The result of the equality

inline bool operator!=(const Twist2 &other) const#

Inequality operator.

Parameters:

other – [in] The other twist

Returns:

The result of the inequality

Twist2 absolute(const Pose2 &reference) const#

Transform a twist to Frame::absolute.

Parameters:

reference – [in] The pose of reference of this relative twist

Returns:

The same twist in Frame::absolute (unchanged if already in Frame::absolute)

Twist2 relative(const Pose2 &reference) const#

Transform a twist to Frame::relative relative to a pose.

Parameters:

reference – [in] The desired pose of reference

Returns:

The same twist in Frame::relative (unchanged if already in Frame::relative)

Twist2 to_frame(Frame frame, const Pose2 &reference) const#

Convert a twist to a reference frame.

Parameters:

  • frame – [in] The desired frame of reference

  • reference – [in] The pose of reference

Returns:

The twist in the desired frame of reference.

inline bool is_almost_zero(ng_float_t epsilon_speed = 1e-6, ng_float_t epsilon_angular_speed = 1e-6) const#

Determines if almost zero.

Parameters:

  • epsilon_speed – [in] The epsilon speed

  • epsilon_angular_speed – [in] The epsilon angular speed

Returns:

True if almost zero, False otherwise.

void snap_to_zero(ng_float_t epsilon = 1e-6)#

Snap components to zero if their module is smaller than epsilon.

Parameters:

epsilon – [in] The tolerance

Twist2 interpolate(const Twist2 &target, ng_float_t time_step, ng_float_t max_acceleration, ng_float_t max_angular_acceleration) const#

Interpolates a twist towards a target respecting acceleration limits.

Parameters:

  • target – [in] The target twist, must be in the same frame

  • time_step – [in] The time step

  • max_acceleration – [in] The maximum acceleration

  • max_angular_acceleration – [in] The maximum angular acceleration

Returns:

The interpolate twist

Public Members

Vector2 velocity#

Velocity

Radians angular_speed#

Angular speed

Frame frame#

Frame of reference.

Three dimensional#

#include "navground/core/controller_3d.h"
using navground::core::Vector3 = Eigen::Vector3<ng_float_t>#

A three-dimensional vector, see Eigen

struct Pose3#

Three-dimensional pose composed of position and orientation.

Poses are assumed to be a world fixed frame.

Public Functions

inline Pose3 integrate(const Twist3 &twist, ng_float_t dt)#

Integrate a pose.

Parameters:

  • twist – [in] The twist

  • dt – [in] The time step

Returns:

pose + dt * twist

inline Pose2 project() const#

Project to the two dimensional plane.

Returns:

The projected pose

Public Members

Vector3 position#

Position in world frame

Radians orientation#

Orientation in world frame

struct Twist3#

Three-dimensional twist composed of velocity and angular speed.

Twist3 coordinates may be in a fixed frame or in the agent’s own frame, as specified by frame.

Public Functions

inline Twist2 project() const#

Project to the two dimensional plane.

Returns:

The projected twist

Public Members

Vector3 velocity#

Velocity

Radians angular_speed#

Angular speed

Frame frame#

The frame of reference.