Geometry

using Config = std::vector<double>

Type alias for std::vector<double> as a joint configuration of a robot.

struct Frame : public Eigen::Isometry3d

Represents a transformation or pose in 3D Cartesian space.

Encapsulates translation and rotation in a unified manner for 3D poses.

Public Types

using Translation = Eigen::Translation3d

Translation component.

using EulerAngles = Eigen::EulerAngles<double, Eigen::EulerSystemXYZ>

Euler angles representation.

using EulerParameter = std::array<double, 6>

Euler angles and translation.

using QuaternionParameter = std::array<double, 7>

Quaternion and translation.

Public Functions

Frame() = default

Default constructor.

Frame(const Eigen::Isometry3d &frame)

Construct a Frame from an Eigen Isometry3d object.

Parameters:

frame – An Eigen Isometry3d object representing the transformation.

double translational_distance(const Frame &other) const

Calculate the Euclidean distance between two Frames’ positions.

Parameters:

other – The other Frame to compare against.

Returns:

double The Euclidean distance.

double angular_distance(const Frame &other) const

Calculate the angular distance between two Frames’ orientations.

Parameters:

other – The other Frame to compare against.

Returns:

double The angular distance.

Frame interpolate(double t, const Frame &other) const

Interpolate between this Frame and another Frame.

Parameters:

  • t – The interpolation parameter (0.0 to 1.0).

  • other – The other Frame to interpolate towards.

Returns:

Frame The interpolated Frame.

std::array<double, 16> to_matrix() const

Convert the Frame to a 4x4 matrix.

Returns:

std::array<double, 16> The Frame as a 4x4 matrix.

std::array<double, 6> to_euler() const

Convert the Frame to Euler angles and translation.

Returns:

std::array<double, 6> The Frame as Euler angles and translation.

bool operator==(const Frame &other) const

Check if two Frames are equal.

Parameters:

other – The other Frame to compare against.

Returns:

bool True if the Frames are equal, false otherwise.

Public Static Functions

static Frame Identity()

Get the identity transformation.

Returns:

Frame The identity Frame.

static Frame from_matrix(const std::array<double, 16> &data)

Create a Frame from a matrix.

Parameters:

data – A 16-element array representing a 4x4 matrix.

Returns:

Frame The Frame constructed from the matrix.

static Frame from_translation(double x, double y, double z)

Create a Frame from translation values.

Parameters:

  • x – The x translation.

  • y – The y translation.

  • z – The z translation.

Returns:

Frame The Frame with the specified translation.

static Frame from_quaternion(double x, double y, double z, double qw, double qx, double qy, double qz)

Create a Frame from quaternion values.

Parameters:

  • x – The x component of the quaternion.

  • y – The y component of the quaternion.

  • z – The z component of the quaternion.

  • qw – The scalar component of the quaternion.

  • qx – The x component of the quaternion.

  • qy – The y component of the quaternion.

  • qz – The z component of the quaternion.

Returns:

Frame The Frame with the specified quaternion.

static Frame from_euler(double x, double y, double z, double a, double b, double c)

Create a Frame from Euler angles.

Parameters:

  • x – The x translation.

  • y – The y translation.

  • z – The z translation.

  • a – The rotation angle around the x-axis.

  • b – The rotation angle around the y-axis.

  • c – The rotation angle around the z-axis.

Returns:

Frame The Frame with the specified translation and Euler angles.

static Frame x(double x)

Create a Frame representing a translation along the x-axis.

Parameters:

x – The translation value along the x-axis.

Returns:

Frame The Frame with the x-axis translation.

static Frame y(double y)

Create a Frame representing a translation along the y-axis.

Parameters:

y – The translation value along the y-axis.

Returns:

Frame The Frame with the y-axis translation.

static Frame z(double z)

Create a Frame representing a translation along the z-axis.

Parameters:

z – The translation value along the z-axis.

Returns:

Frame The Frame with the z-axis translation.

static Frame Rx(double c, double s)

Create a Frame representing a rotation around the x-axis.

Parameters:

  • c – The cosine of the rotation angle.

  • s – The sine of the rotation angle.

Returns:

Frame The Frame with the rotation around the x-axis.

static Frame Ry(double c, double s)

Create a Frame representing a rotation around the y-axis.

Parameters:

  • c – The cosine of the rotation angle.

  • s – The sine of the rotation angle.

Returns:

Frame The Frame with the rotation around the y-axis.

static Frame Rz(double c, double s)

Create a Frame representing a rotation around the z-axis.

Parameters:

  • c – The cosine of the rotation angle.

  • s – The sine of the rotation angle.

Returns:

Frame The Frame with the rotation around the z-axis.

static Frame Rx(double a)

Create a Frame representing a rotation around the x-axis.

Parameters:

a – The rotation angle around the x-axis.

Returns:

Frame The Frame with the rotation around the x-axis.

static Frame Ry(double a)

Create a Frame representing a rotation around the y-axis.

Parameters:

a – The rotation angle around the y-axis.

Returns:

Frame The Frame with the rotation around the y-axis.

static Frame Rz(double a)

Create a Frame representing a rotation around the z-axis.

Parameters:

a – The rotation angle around the z-axis.

Returns:

Frame The Frame with the rotation around the z-axis.

struct Twist : public Eigen::Vector<double, 6>

Represents a velocity in 3D Cartesian space.

The Twist struct represents a 6-dimensional vector used to describe velocity in 3D Cartesian space. It consists of linear velocities in the x, y, and z directions, and angular velocities around these axes.

Public Functions

Twist()

Default constructor.

Twist(double x, double y, double z, double rx, double ry, double rz)

Construct a Twist with specified linear and angular velocities.

Parameters:

  • x – The linear velocity along the x-axis.

  • y – The linear velocity along the y-axis.

  • z – The linear velocity along the z-axis.

  • rx – The angular velocity around the x-axis.

  • ry – The angular velocity around the y-axis.

  • rz – The angular velocity around the z-axis.

Twist(const std::array<double, 6> &twist)

Construct a Twist from an array.

Parameters:

twist – An array containing the 6-dimensional twist values.

Twist(const Eigen::Vector<double, 6> &twist)

Construct a Twist from an Eigen vector.

Parameters:

twist – An Eigen vector containing the 6-dimensional twist values.

Twist operator+(const Twist &other)

Add two Twist objects.

Parameters:

other – The Twist object to add.

Returns:

Twist The result of the addition.

Twist transform(const Eigen::Matrix3d &rotation, const Eigen::Vector3d &translation) const

Transform the Twist by a rotation and translation.

Parameters:

  • rotation – A 3x3 rotation matrix.

  • translation – A 3D translation vector.

Returns:

Twist The transformed Twist.

Twist cross(const Twist &other) const

Compute the cross product of two Twists.

Parameters:

other – The Twist object to cross with this one.

Returns:

Twist The result of the cross product.

std::array<double, 6> to_array() const

Convert the Twist to an array.

Converts the Twist object into a 6-element array, with the components arranged as [x, y, z, rx, ry, rz].

Returns:

std::array<double, 6> The Twist represented as an array.