Transforms.h

Go to the documentation of this file.

#ifndef CONTROLS_UTILS_TRANSFORMS_H
#define CONTROLS_UTILS_TRANSFORMS_H
 
#include <utils/NodeUtils.h>
#include <utils/Types.h>
 
double wrap(double r);
 
Vector6d pose2n(const Pose& pose);
 
Pose n2pose(const Vector6d& pose);
 
Vector3d rad2deg(const Vector3d& rpy);
 
Vector3d deg2rad(const Vector3d& rpy);
 
Matrix3d rpy2rot(const Vector3d& rpy);
 
Matrix3d rpy2rot(const double roll, const double pitch, const double yaw);
 
Matrix2d ang2rot(const double angle);
 
double rot2ang(const Matrix2d& m);
 
Vector2d getT(const Matrix3d& r);
 
Matrix2d getR(const Matrix3d& r);
 
Matrix3d pose2tf_2d(const Pose& p);
 
Pose tf2pose_2d(const Matrix3d& m);
 
Matrix4d pose2tf(const Pose& p);
 
Pose tf2pose(const Matrix4d& m);
 
Vector3d getT(const Matrix4d& r);
 
Matrix3d getR(const Matrix4d& r);
 
Matrix3d tf_3dto2d(const Matrix4d& m);
 
Matrix4d tf_2dto3d(const Matrix3d& m);
 
Vector3d rot2rpy(const Matrix3d& r);
 
double rot2yaw(const Matrix3d& r);
 
Matrix3d quat2rot(const Quaterniond& q);
 
Quaterniond rot2quat(const Matrix3d& r);
 
Vector3d quat2rpy(const Quaterniond& q);
 
double quat2yaw(const Quaterniond& q);
 
Quaterniond rpy2quat(const Vector3d& rpy);
 
Quaterniond rpy2quat(const double r, const double p, const double y);
 
Quaterniond extractYaw(const Quaterniond& q);
 
Vector3d extract2d(const Vector6d& v);
 
Vector3d extract2d(const State& state);
 
Matrix3d jacobian(const Vector3d& rpy);
 
Matrix3d jacobian_inv(const Vector3d& rpy);
 
Matrix6d getdJ(const Vector3d& rpy, const Vector3d& n2dot);
 
Matrix6d transformation(const Vector3d& rpy);
 
Matrix6d transformation(const Quaterniond& q);
 
Matrix6d transformation(const Matrix3d& r);
 
Matrix6d transformation_inv(const Vector3d& rpy);
 
Matrix6d transformation_inv(const Quaterniond& q);
 
Matrix6d transformation_inv(const Matrix3d& r);
 
Matrix3d skew(const Vector3d& v);
 
Vector3d skew(const Matrix3d& v);
 
VectorXd error(const VectorXd& d, const VectorXd& y);
 
Vector3d error(const Quaterniond& qd, const Quaterniond& q);
 
Vector6d error(const Pose& d, const Pose& y);
 
Vector3d error2d(const Pose& d, const Pose& y);
 
Vector18d error(const State& d, const State& y);
 
Vector3d errorrpy(const Vector3d& rpyd, const Vector3d& rpy);
 
Vector3d convert(const geometry_msgs::msg::Vector3::ConstSharedPtr& v);
 
Vector3d convert(const geometry_msgs::msg::Vector3& v);
 
Vector3d convert(const geometry_msgs::msg::Point::ConstSharedPtr& v);
 
Vector3d convert(const geometry_msgs::msg::Point& v);
 
Vector6d convert(const geometry_msgs::msg::Twist::ConstSharedPtr& v);
 
Vector6d convert(const geometry_msgs::msg::Twist& v);
 
Vector6d convert(const geometry_msgs::msg::TwistStamped& v);
 
Vector6d convert(const geometry_msgs::msg::TwistStamped::ConstSharedPtr& v);
 
Quaterniond convert(const geometry_msgs::msg::Quaternion::ConstSharedPtr& v);
 
Quaterniond convert(const geometry_msgs::msg::Quaternion& v);
 
Pose convert(const geometry_msgs::msg::Pose::ConstSharedPtr& p);
 
Pose convert(const geometry_msgs::msg::Pose& p);
 
State convert(const nav_msgs::msg::Odometry::ConstSharedPtr& odom);
 
State convert(const nav_msgs::msg::Odometry& odom);
 
geometry_msgs::msg::Pose convert(const Pose& p);
 
nav_msgs::msg::Odometry convert(const State& state);
 
geometry_msgs::msg::Twist convert(const Vector6d& v);
 
geometry_msgs::msg::WrenchStamped convert(const Vector6d& v, const std::string& frame_id, double t);
 
MatrixXd pseudoInverse(const MatrixXd& m);
 
#endif  // CONTROLS_UTILS_TRANSFORMS_H