#include <utils/Transforms.h>
#include <cmath>
#include <iomanip>
#include <sstream>

Functions
double	wrap (double r)
	Wraps an angle to the range (-pi, pi]. More...

Vector6d	pose2n (const Pose &pose)
	Converts a Pose to a 6-element XYZRPY vector. More...

Pose	n2pose (const Vector6d &pose)
	Converts a 6-element XYZRPY vector to a Pose. More...

Vector3d	rad2deg (const Vector3d &rpy)
	Converts a 3-element RPY vector from radians to degrees. More...

Vector3d	deg2rad (const Vector3d &rpy)
	Converts a 3-element RPY vector from degrees to radians. More...

Matrix3d	rpy2rot (const Vector3d &rpy)
	Builds a 3x3 rotation matrix from roll-pitch-yaw angles. More...

Matrix3d	rpy2rot (const double roll, const double pitch, const double yaw)
	Builds a 3x3 rotation matrix from individual roll-pitch-yaw angles. More...

Matrix2d	ang2rot (const double angle)
	Builds a 2x2 rotation matrix from a single planar angle. More...

double	rot2ang (const Matrix2d &m)
	Extracts the rotation angle from a 2x2 rotation matrix. More...

Vector2d	getT (const Matrix3d &r)
	Extracts the 2D translation vector from a 3x3 homogeneous transform. More...

Matrix2d	getR (const Matrix3d &r)
	Extracts the 2x2 rotation block from a 3x3 homogeneous transform. More...

Matrix3d	pose2tf_2d (const Pose &p)
	Converts a Pose to a 3x3 homogeneous 2D transformation matrix. More...

Pose	tf2pose_2d (const Matrix3d &m)
	Converts a 3x3 homogeneous 2D transformation matrix to a Pose. More...

Matrix4d	pose2tf (const Pose &p)
	Converts a Pose to a 4x4 homogeneous 3D transformation matrix. More...

Pose	tf2pose (const Matrix4d &m)
	Converts a 4x4 homogeneous 3D transformation matrix to a Pose. More...

Vector3d	getT (const Matrix4d &r)
	Extracts the 3D translation vector from a 4x4 homogeneous transform. More...

Matrix3d	getR (const Matrix4d &r)
	Extracts the 3x3 rotation block from a 4x4 homogeneous transform. More...

Matrix3d	tf_3dto2d (const Matrix4d &m)
	Projects a 4x4 3D homogeneous transform down to a 3x3 2D transform. More...

Matrix4d	tf_2dto3d (const Matrix3d &m)
	Embeds a 3x3 2D homogeneous transform into a 4x4 3D transform. More...

Vector3d	rot2rpy (const Matrix3d &m)
	Extracts roll-pitch-yaw angles from a rotation matrix. More...

double	rot2yaw (const Matrix3d &m)
	Extracts the yaw angle from a rotation matrix. More...

Matrix3d	quat2rot (const Quaterniond &q)
	Converts a quaternion to a 3x3 rotation matrix. More...

Quaterniond	rot2quat (const Matrix3d &r)
	Converts a 3x3 rotation matrix to a quaternion. More...

Vector3d	quat2rpy (const Quaterniond &q)
	Converts a quaternion to roll-pitch-yaw angles. More...

double	quat2yaw (const Quaterniond &q)
	Extracts the yaw angle from a quaternion. More...

Quaterniond	rpy2quat (const Vector3d &rpy)
	Converts a roll-pitch-yaw vector to a quaternion. More...

Quaterniond	rpy2quat (const double roll, const double pitch, const double yaw)
	Converts individual roll-pitch-yaw angles to a quaternion. More...

Quaterniond	extractYaw (const Quaterniond &q)
	Strips roll and pitch from a quaternion, retaining only yaw. More...

Vector3d	extract2d (const Vector6d &v)
	Extracts the X, Y, and yaw components from a 6-element state vector. More...

Vector3d	extract2d (const State &state)
	Extracts the X, Y, and yaw components from a vehicle State. More...

Matrix3d	jacobian (const Vector3d &rpy)
	Builds the 3x3 Jacobian mapping body angular rates to Euler-angle rates. More...

Matrix3d	jacobian_inv (const Vector3d &rpy)
	Builds the 3x3 inverse Jacobian mapping Euler-angle rates to body angular rates. More...

Matrix6d	getdJ (const Vector3d &rpy, const Vector3d &n2dot)
	Computes the time derivative of the 6x6 transformation Jacobian. More...

Matrix6d	transformation (const Vector3d &rpy)
	Builds the 6x6 world-to-body transformation matrix from RPY angles. More...

Matrix6d	transformation (const Quaterniond &q)
	Builds the 6x6 world-to-body transformation matrix from a quaternion. More...

Matrix6d	transformation (const Matrix3d &r)
	Builds the 6x6 world-to-body transformation matrix from a rotation matrix. More...

Matrix6d	transformation_inv (const Vector3d &rpy)
	Builds the 6x6 body-to-world inverse transformation matrix from RPY angles. More...

Matrix6d	transformation_inv (const Quaterniond &q)
	Builds the 6x6 body-to-world inverse transformation matrix from a quaternion. More...

Matrix6d	transformation_inv (const Matrix3d &r)
	Builds the 6x6 body-to-world inverse transformation matrix from a rotation matrix. More...

Matrix3d	skew (const Vector3d &v)
	Builds a 3x3 skew-symmetric matrix from a 3-vector. More...

Vector3d	skew (const Matrix3d &s)
	Extracts the axial vector from a 3x3 skew-symmetric matrix. More...

VectorXd	error (const VectorXd &d, const VectorXd &y)
	Computes the signed error between two vectors as (y - d) More...

Vector3d	error (const Quaterniond &qd, const Quaterniond &q)
	Computes the orientation error between two quaternions as (y - d) More...

Vector6d	error (const Pose &d, const Pose &y)
	Computes the 6DOF pose error between two Poses as (y - d) More...

Vector3d	error2d (const Pose &d, const Pose &y)
	Computes the planar (X-Y-yaw) pose error between two Poses as (y - d) More...

Vector18d	error (const State &d, const State &y)
	Computes the full 18DOF state error (pose + twist + acceleration) as (y - d) More...

Vector3d	errorrpy (const Vector3d &rpyd, const Vector3d &rpy)
	Computes the RPY orientation error between two RPY angle vectors. More...

Vector3d	convert (const geometry_msgs::msg::Vector3::ConstSharedPtr &v)
	Converts a ROS Vector3 shared-pointer message to an Eigen Vector3d. More...

Vector3d	convert (const geometry_msgs::msg::Vector3 &v)
	Converts a ROS Vector3 message to an Eigen Vector3d. More...

Vector3d	convert (const geometry_msgs::msg::Point::ConstSharedPtr &v)
	Converts a ROS Point shared-pointer message to an Eigen Vector3d. More...

Vector3d	convert (const geometry_msgs::msg::Point &v)
	Converts a ROS Point message to an Eigen Vector3d. More...

Vector6d	convert (const geometry_msgs::msg::TwistStamped::ConstSharedPtr &v)
	Converts a ROS TwistStamped shared-pointer message to a 6-element Eigen vector. More...

Vector6d	convert (const geometry_msgs::msg::TwistStamped &v)
	Converts a ROS TwistStamped message to a 6-element Eigen vector. More...

Vector6d	convert (const geometry_msgs::msg::Twist::ConstSharedPtr &v)
	Converts a ROS Twist shared-pointer message to a 6-element Eigen vector. More...

Vector6d	convert (const geometry_msgs::msg::Twist &v)
	Converts a ROS Twist message to a 6-element Eigen vector. More...

Quaterniond	convert (const geometry_msgs::msg::Quaternion::ConstSharedPtr &v)
	Converts a ROS Quaternion shared-pointer message to an Eigen Quaterniond. More...

Quaterniond	convert (const geometry_msgs::msg::Quaternion &v)
	Converts a ROS Quaternion message to an Eigen Quaterniond. More...

Pose	convert (const geometry_msgs::msg::Pose::ConstSharedPtr &p)
	Converts a ROS Pose shared-pointer message to a Pose struct. More...

Pose	convert (const geometry_msgs::msg::Pose &p_msg)
	Converts a ROS Pose message to a Pose struct. More...

State	convert (const nav_msgs::msg::Odometry::ConstSharedPtr &odom)
	Converts a ROS Odometry shared-pointer message to a State struct. More...

State	convert (const nav_msgs::msg::Odometry &odom)
	Converts a ROS Odometry message to a State struct. More...

geometry_msgs::msg::Pose	convert (const Pose &p)
	Converts a Pose struct to a ROS Pose message. More...

nav_msgs::msg::Odometry	convert (const State &state)
	Converts a State struct to a ROS Odometry message. More...

geometry_msgs::msg::Twist	convert (const Vector6d &v)
	Converts a 6-element Eigen vector to a ROS Twist message. More...

geometry_msgs::msg::WrenchStamped	convert (const Vector6d &v, const std::string &frame_id, double t)
	Converts a 6-element force/torque vector to a ROS WrenchStamped message. More...

MatrixXd	pseudoInverse (const MatrixXd &m)
	Computes the Moore-Penrose pseudo-inverse of a matrix. More...

Function Documentation

◆ ang2rot()

Matrix2d ang2rot ( const double angle )

Builds a 2x2 rotation matrix from a single planar angle.

Parameters

angle [in] Rotation angle in radians

Returns: 2D rotation matrix

◆ convert() [1/18]

Vector3d convert ( const geometry_msgs::msg::Point & v )

Converts a ROS Point message to an Eigen Vector3d.

Parameters

v	[in] geometry_msgs::msg::Point message

Returns: Equivalent Eigen 3-vector

◆ convert() [2/18]

Vector3d convert ( const geometry_msgs::msg::Point::ConstSharedPtr & v )

Converts a ROS Point shared-pointer message to an Eigen Vector3d.

Parameters

v	[in] Shared pointer to a geometry_msgs::msg::Point

Returns: Equivalent Eigen 3-vector

◆ convert() [3/18]

Pose convert ( const geometry_msgs::msg::Pose & p )

Converts a ROS Pose message to a Pose struct.

Parameters

p	[in] geometry_msgs::msg::Pose message

Returns: Equivalent Pose struct

◆ convert() [4/18]

Pose convert ( const geometry_msgs::msg::Pose::ConstSharedPtr & p )

Converts a ROS Pose shared-pointer message to a Pose struct.

Parameters

p	[in] Shared pointer to a geometry_msgs::msg::Pose

Returns: Equivalent Pose struct

◆ convert() [5/18]

Quaterniond convert ( const geometry_msgs::msg::Quaternion & v )

Converts a ROS Quaternion message to an Eigen Quaterniond.

Parameters

v	[in] geometry_msgs::msg::Quaternion message

Returns: Normalized Eigen quaternion

◆ convert() [6/18]

Quaterniond convert ( const geometry_msgs::msg::Quaternion::ConstSharedPtr & v )

Converts a ROS Quaternion shared-pointer message to an Eigen Quaterniond.

Parameters

v	[in] Shared pointer to a geometry_msgs::msg::Quaternion

Returns: Normalized Eigen quaternion

◆ convert() [7/18]

Vector6d convert ( const geometry_msgs::msg::Twist & v )

Converts a ROS Twist message to a 6-element Eigen vector.

Parameters

v	[in] geometry_msgs::msg::Twist message

Returns: 6-element vector [vx, vy, vz, wx, wy, wz]

◆ convert() [8/18]

Vector6d convert ( const geometry_msgs::msg::Twist::ConstSharedPtr & v )

Converts a ROS Twist shared-pointer message to a 6-element Eigen vector.

Parameters

v	[in] Shared pointer to a geometry_msgs::msg::Twist

Returns: 6-element vector [vx, vy, vz, wx, wy, wz]

◆ convert() [9/18]

Vector6d convert ( const geometry_msgs::msg::TwistStamped & v )

Converts a ROS TwistStamped message to a 6-element Eigen vector.

Parameters

v	[in] geometry_msgs::msg::TwistStamped message

Returns: 6-element vector [vx, vy, vz, wx, wy, wz]

◆ convert() [10/18]

Vector6d convert ( const geometry_msgs::msg::TwistStamped::ConstSharedPtr & v )

Converts a ROS TwistStamped shared-pointer message to a 6-element Eigen vector.

Parameters

v	[in] Shared pointer to a geometry_msgs::msg::TwistStamped

Returns: 6-element vector [vx, vy, vz, wx, wy, wz]

◆ convert() [11/18]

Vector3d convert ( const geometry_msgs::msg::Vector3 & v )

Converts a ROS Vector3 message to an Eigen Vector3d.

Parameters

v	[in] geometry_msgs::msg::Vector3 message

Returns: Equivalent Eigen 3-vector

◆ convert() [12/18]

Vector3d convert ( const geometry_msgs::msg::Vector3::ConstSharedPtr & v )

Converts a ROS Vector3 shared-pointer message to an Eigen Vector3d.

Parameters

v	[in] Shared pointer to a geometry_msgs::msg::Vector3

Returns: Equivalent Eigen 3-vector

◆ convert() [13/18]

State convert ( const nav_msgs::msg::Odometry & odom )

Converts a ROS Odometry message to a State struct.

Parameters

odom	[in] nav_msgs::msg::Odometry message

Returns: Equivalent State struct with pose and twist populated

◆ convert() [14/18]

State convert ( const nav_msgs::msg::Odometry::ConstSharedPtr & odom )

Converts a ROS Odometry shared-pointer message to a State struct.

Parameters

odom	[in] Shared pointer to a nav_msgs::msg::Odometry message

Returns: Equivalent State struct with pose and twist populated

◆ convert() [15/18]

geometry_msgs::msg::Pose convert ( const Pose & p )

Converts a Pose struct to a ROS Pose message.

Parameters

p	[in] Pose struct

Returns: Equivalent geometry_msgs::msg::Pose message

◆ convert() [16/18]

nav_msgs::msg::Odometry convert ( const State & state )

Converts a State struct to a ROS Odometry message.

Parameters

state [in] Vehicle state

Returns: Equivalent nav_msgs::msg::Odometry message

◆ convert() [17/18]

geometry_msgs::msg::Twist convert ( const Vector6d & v )

Converts a 6-element Eigen vector to a ROS Twist message.

Parameters

v	[in] 6-element vector [vx, vy, vz, wx, wy, wz]

Returns: Equivalent geometry_msgs::msg::Twist message

◆ convert() [18/18]

geometry_msgs::msg::WrenchStamped convert	(	const Vector6d &	v,
		const std::string &	frame_id,
		double	t
	)

Converts a 6-element force/torque vector to a ROS WrenchStamped message.

Parameters

v	[in] 6-element wrench [Fx, Fy, Fz, Tx, Ty, Tz] in N and Nm
frame_id	[in] TF frame ID to embed in the message header
t	[in] Timestamp in seconds; uses current ROS time if <= 0

Returns: Equivalent geometry_msgs::msg::WrenchStamped message

◆ deg2rad()

Vector3d deg2rad ( const Vector3d & rpy )

Converts a 3-element RPY vector from degrees to radians.

Parameters

rpy	[in] Angles in degrees

Returns: Angles in radians

◆ error() [1/4]

Vector6d error	(	const Pose &	d,
		const Pose &	y
	)

Computes the 6DOF pose error between two Poses as (y - d)

Parameters

d	[in] Desired pose
y	[in] Current pose

Returns: 6-element pose error [Δx, Δy, Δz, Δroll, Δpitch, Δyaw]

◆ error() [2/4]

Vector3d error	(	const Quaterniond &	qd,
		const Quaterniond &	q
	)

Computes the orientation error between two quaternions as (y - d)

Parameters

qd	[in] Desired quaternion
q	[in] Current quaternion

Returns: 3-element rotation-error vector (imaginary part of qd⁻¹ * q)

◆ error() [3/4]

Vector18d error	(	const State &	d,
		const State &	y
	)

Computes the full 18DOF state error (pose + twist + acceleration) as (y - d)

Parameters

d	[in] Desired state
y	[in] Current state

Returns: 18-element error vector [pose_err(6), twist_err(6), accel_err(6)]

◆ error() [4/4]

VectorXd error	(	const VectorXd &	d,
		const VectorXd &	y
	)

Computes the signed error between two vectors as (y - d)

Parameters

d	[in] Desired (goal) vector
y	[in] Current (actual) vector

Returns: Element-wise error (y - d)

◆ error2d()

Vector3d error2d	(	const Pose &	d,
		const Pose &	y
	)

Computes the planar (X-Y-yaw) pose error between two Poses as (y - d)

Parameters

d	[in] Desired pose
y	[in] Current pose

Returns: 3-element error [Δx, Δy, Δyaw]

◆ errorrpy()

Vector3d errorrpy	(	const Vector3d &	rpyd,
		const Vector3d &	rpy
	)

Computes the RPY orientation error between two RPY angle vectors.

Parameters

rpyd	[in] Desired roll-pitch-yaw in radians
rpy	[in] Current roll-pitch-yaw in radians

Returns: 3-element RPY error vector

◆ extract2d() [1/2]

Vector3d extract2d ( const State & state )

Extracts the X, Y, and yaw components from a vehicle State.

Parameters

state [in] Full 6DOF vehicle state

Returns: 3-element vector [x, y, yaw]

◆ extract2d() [2/2]

Vector3d extract2d ( const Vector6d & v )

Extracts the X, Y, and yaw components from a 6-element state vector.

Parameters

v	[in] 6-element vector [x, y, z, roll, pitch, yaw]

Returns: 3-element vector [x, y, yaw]

◆ extractYaw()

Quaterniond extractYaw ( const Quaterniond & q )

Strips roll and pitch from a quaternion, retaining only yaw.

Parameters

q	[in] Input quaternion

Returns: Quaternion representing only the yaw component of q

◆ getdJ()

Matrix6d getdJ	(	const Vector3d &	rpy,
		const Vector3d &	n2dot
	)

Computes the time derivative of the 6x6 transformation Jacobian.

Parameters

rpy	[in] Current roll, pitch, yaw in radians
n2dot	[in] World-frame angular velocity [ṙoll, ṗitch, ẏaw] in rad/s

Returns: 6x6 Jacobian time derivative matrix J̇

◆ getR() [1/2]

Matrix2d getR ( const Matrix3d & r )

Extracts the 2x2 rotation block from a 3x3 homogeneous transform.

Parameters

r	[in] 3x3 homogeneous transformation matrix

Returns: 2x2 rotation matrix

◆ getR() [2/2]

Matrix3d getR ( const Matrix4d & r )

Extracts the 3x3 rotation block from a 4x4 homogeneous transform.

Parameters

r	[in] 4x4 homogeneous transformation matrix

Returns: 3x3 rotation matrix

◆ getT() [1/2]

Vector2d getT ( const Matrix3d & r )

Extracts the 2D translation vector from a 3x3 homogeneous transform.

Parameters

r	[in] 3x3 homogeneous transformation matrix

Returns: 2-element translation vector

◆ getT() [2/2]

Vector3d getT ( const Matrix4d & r )

Extracts the 3D translation vector from a 4x4 homogeneous transform.

Parameters

r	[in] 4x4 homogeneous transformation matrix

Returns: 3-element translation vector

◆ jacobian()

Matrix3d jacobian ( const Vector3d & rpy )

Builds the 3x3 Jacobian mapping body angular rates to Euler-angle rates.

Parameters

rpy	[in] Current roll, pitch, yaw in radians

Returns: 3x3 Jacobian matrix J such that ṅ_ang = J * ω

◆ jacobian_inv()

Matrix3d jacobian_inv ( const Vector3d & rpy )

Builds the 3x3 inverse Jacobian mapping Euler-angle rates to body angular rates.

Parameters

rpy	[in] Current roll, pitch, yaw in radians

Returns: 3x3 inverse Jacobian matrix J⁻¹

◆ n2pose()

Pose n2pose ( const Vector6d & pose )

Converts a 6-element XYZRPY vector to a Pose.

Parameters

pose	[in] 6-element vector [x, y, z, roll, pitch, yaw]

Returns: Pose with position and quaternion orientation

◆ pose2n()

Vector6d pose2n ( const Pose & pose )

Converts a Pose to a 6-element XYZRPY vector.

Parameters

pose	[in] Pose containing position and quaternion orientation

Returns: 6-element vector [x, y, z, roll, pitch, yaw]

◆ pose2tf()

Matrix4d pose2tf ( const Pose & p )

Converts a Pose to a 4x4 homogeneous 3D transformation matrix.

Parameters

p	[in] Pose with 3D position and quaternion orientation

Returns: 4x4 homogeneous transform

◆ pose2tf_2d()

Matrix3d pose2tf_2d ( const Pose & p )

Converts a Pose to a 3x3 homogeneous 2D transformation matrix.

Parameters

p	[in] Pose (only XY position and yaw are used)

Returns: 3x3 2D homogeneous transform

◆ pseudoInverse()

MatrixXd pseudoInverse ( const MatrixXd & m )

Computes the Moore-Penrose pseudo-inverse of a matrix.

Parameters

m	[in] Input matrix

Returns: Right pseudo-inverse mᵀ (m mᵀ)⁻¹

◆ quat2rot()

Matrix3d quat2rot ( const Quaterniond & q )

Converts a quaternion to a 3x3 rotation matrix.

Parameters

q	[in] Unit quaternion

Returns: Equivalent 3x3 rotation matrix

◆ quat2rpy()

Vector3d quat2rpy ( const Quaterniond & q )

Converts a quaternion to roll-pitch-yaw angles.

Parameters

q	[in] Unit quaternion

Returns: 3-element vector [roll, pitch, yaw] in radians

◆ quat2yaw()

double quat2yaw ( const Quaterniond & q )

Extracts the yaw angle from a quaternion.

Parameters

q	[in] Unit quaternion

Returns: Yaw angle in radians

◆ rad2deg()

Vector3d rad2deg ( const Vector3d & rpy )

Converts a 3-element RPY vector from radians to degrees.

Parameters

rpy	[in] Angles in radians

Returns: Angles in degrees

◆ rot2ang()

double rot2ang ( const Matrix2d & m )

Extracts the rotation angle from a 2x2 rotation matrix.

Parameters

m	[in] 2D rotation matrix

Returns: Rotation angle in radians

◆ rot2quat()

Quaterniond rot2quat ( const Matrix3d & r )

Converts a 3x3 rotation matrix to a quaternion.

Parameters

r	[in] 3x3 rotation matrix

Returns: Equivalent unit quaternion

◆ rot2rpy()

Vector3d rot2rpy ( const Matrix3d & r )

Extracts roll-pitch-yaw angles from a rotation matrix.

Parameters

r	[in] 3x3 rotation matrix

Returns: 3-element vector [roll, pitch, yaw] in radians

◆ rot2yaw()

double rot2yaw ( const Matrix3d & r )

Extracts the yaw angle from a rotation matrix.

Parameters

r	[in] 3x3 rotation matrix

Returns: Yaw angle in radians

◆ rpy2quat() [1/2]

Quaterniond rpy2quat	(	const double	r,
		const double	p,
		const double	y
	)

Converts individual roll-pitch-yaw angles to a quaternion.

Parameters

r	[in] Roll angle in radians
p	[in] Pitch angle in radians
y	[in] Yaw angle in radians

Returns: Equivalent unit quaternion

◆ rpy2quat() [2/2]

Quaterniond rpy2quat ( const Vector3d & rpy )

Converts a roll-pitch-yaw vector to a quaternion.

Parameters

rpy	[in] Roll, pitch, yaw in radians

Returns: Equivalent unit quaternion

◆ rpy2rot() [1/2]

Matrix3d rpy2rot	(	const double	roll,
		const double	pitch,
		const double	yaw
	)

Builds a 3x3 rotation matrix from individual roll-pitch-yaw angles.

Parameters

roll	[in] Roll angle in radians
pitch	[in] Pitch angle in radians
yaw	[in] Yaw angle in radians

Returns: ZYX rotation matrix

◆ rpy2rot() [2/2]

Matrix3d rpy2rot ( const Vector3d & rpy )

Builds a 3x3 rotation matrix from roll-pitch-yaw angles.

Parameters

rpy	[in] Roll, pitch, yaw in radians

Returns: ZYX rotation matrix

◆ skew() [1/2]

Vector3d skew ( const Matrix3d & v )

Extracts the axial vector from a 3x3 skew-symmetric matrix.

Parameters

v	[in] 3x3 skew-symmetric matrix

Returns: 3-element axial vector

◆ skew() [2/2]

Matrix3d skew ( const Vector3d & v )

Builds a 3x3 skew-symmetric matrix from a 3-vector.

Parameters

v	[in] 3-element vector

Returns: Skew-symmetric matrix [v]×

◆ tf2pose()

Pose tf2pose ( const Matrix4d & m )

Converts a 4x4 homogeneous 3D transformation matrix to a Pose.

Parameters

m	[in] 4x4 homogeneous transform

Returns: Pose with 3D position and quaternion orientation

◆ tf2pose_2d()

Pose tf2pose_2d ( const Matrix3d & m )

Converts a 3x3 homogeneous 2D transformation matrix to a Pose.

Parameters

m	[in] 3x3 2D homogeneous transform

Returns: Pose with XY position and yaw orientation

◆ tf_2dto3d()

Matrix4d tf_2dto3d ( const Matrix3d & m )

Embeds a 3x3 2D homogeneous transform into a 4x4 3D transform.

Parameters

m	[in] 3x3 2D homogeneous transform

Returns: 4x4 3D homogeneous transform with identity Z and rotation blocks

◆ tf_3dto2d()

Matrix3d tf_3dto2d ( const Matrix4d & m )

Projects a 4x4 3D homogeneous transform down to a 3x3 2D transform.

Parameters

m	[in] 4x4 3D homogeneous transform

Returns: 3x3 2D homogeneous transform using only the XY plane

◆ transformation() [1/3]

Matrix6d transformation ( const Matrix3d & r )

Builds the 6x6 world-to-body transformation matrix from a rotation matrix.

Parameters

r	[in] 3x3 rotation matrix

Returns: 6x6 block-diagonal transformation [R, 0; 0, J]

◆ transformation() [2/3]

Matrix6d transformation ( const Quaterniond & q )

Builds the 6x6 world-to-body transformation matrix from a quaternion.

Parameters

q	[in] Unit quaternion

Returns: 6x6 block-diagonal transformation [R, 0; 0, J]

◆ transformation() [3/3]

Matrix6d transformation ( const Vector3d & rpy )

Builds the 6x6 world-to-body transformation matrix from RPY angles.

Parameters

rpy	[in] Roll, pitch, yaw in radians

Returns: 6x6 block-diagonal transformation [R, 0; 0, J]

◆ transformation_inv() [1/3]

Matrix6d transformation_inv ( const Matrix3d & r )

Builds the 6x6 body-to-world inverse transformation matrix from a rotation matrix.

Parameters

r	[in] 3x3 rotation matrix

Returns: 6x6 block-diagonal inverse transformation [Rᵀ, 0; 0, J⁻¹]

◆ transformation_inv() [2/3]

Matrix6d transformation_inv ( const Quaterniond & q )

Builds the 6x6 body-to-world inverse transformation matrix from a quaternion.

Parameters

q	[in] Unit quaternion

Returns: 6x6 block-diagonal inverse transformation [Rᵀ, 0; 0, J⁻¹]

◆ transformation_inv() [3/3]

Matrix6d transformation_inv ( const Vector3d & rpy )

Builds the 6x6 body-to-world inverse transformation matrix from RPY angles.

Parameters

rpy	[in] Roll, pitch, yaw in radians

Returns: 6x6 block-diagonal inverse transformation [Rᵀ, 0; 0, J⁻¹]

◆ wrap()

double wrap ( double r )

Wraps an angle to the range (-pi, pi].

Parameters

r	[in] Angle in radians

Returns: Wrapped angle in radians

Functions

Function Documentation

◆ ang2rot()

◆ convert() [1/18]

◆ convert() [2/18]

◆ convert() [3/18]

◆ convert() [4/18]

◆ convert() [5/18]

◆ convert() [6/18]

◆ convert() [7/18]

◆ convert() [8/18]

◆ convert() [9/18]

◆ convert() [10/18]

◆ convert() [11/18]

◆ convert() [12/18]

◆ convert() [13/18]

◆ convert() [14/18]

◆ convert() [15/18]

◆ convert() [16/18]

◆ convert() [17/18]

◆ convert() [18/18]

◆ deg2rad()

◆ error() [1/4]

◆ error() [2/4]

◆ error() [3/4]

◆ error() [4/4]

◆ error2d()

◆ errorrpy()

◆ extract2d() [1/2]

◆ extract2d() [2/2]

◆ extractYaw()

◆ getdJ()

◆ getR() [1/2]

◆ getR() [2/2]

◆ getT() [1/2]

◆ getT() [2/2]

◆ jacobian()

◆ jacobian_inv()

◆ n2pose()

◆ pose2n()

◆ pose2tf()

◆ pose2tf_2d()

◆ pseudoInverse()

◆ quat2rot()

◆ quat2rpy()

◆ quat2yaw()

◆ rad2deg()

◆ rot2ang()

◆ rot2quat()

◆ rot2rpy()

◆ rot2yaw()

◆ rpy2quat() [1/2]

◆ rpy2quat() [2/2]

◆ rpy2rot() [1/2]

◆ rpy2rot() [2/2]

◆ skew() [1/2]

◆ skew() [2/2]

◆ tf2pose()

◆ tf2pose_2d()

◆ tf_2dto3d()

◆ tf_3dto2d()

◆ transformation() [1/3]

◆ transformation() [2/3]

◆ transformation() [3/3]

◆ transformation_inv() [1/3]

◆ transformation_inv() [2/3]

◆ transformation_inv() [3/3]

◆ wrap()