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 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.
Public Static Functions
-
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.
-
using Translation = Eigen::Translation3d¶
-
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.
-
Twist()¶