25 #ifndef ROBOTIS_MANIPULATOR_MATH_H_ 26 #define ROBOTIS_MANIPULATOR_MATH_H_ 30 #if defined(__OPENCR__) 33 #include <Eigen/Geometry> 35 #include <eigen3/Eigen/Eigen> 36 #include <eigen3/Eigen/LU> 41 #define DEG2RAD 0.01745329252 //(M_PI / 180.0) 42 #define RAD2DEG 57.2957795131 //(180.0 / M_PI) 59 Eigen::Vector3d
vector3(
double v1,
double v2,
double v3);
73 Eigen::Matrix3d
matrix3(
double m11,
double m12,
double m13,
74 double m21,
double m22,
double m23,
75 double m31,
double m32,
double m33);
86 Eigen::Matrix3d
inertiaMatrix(
double ixx,
double ixy,
double ixz ,
double iyy ,
double iyz,
double izz);
238 double sign(
double value);
272 Eigen::Vector3d
positionDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position);
279 Eigen::Vector3d
orientationDifference(Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation);
288 Eigen::VectorXd
poseDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position,
289 Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation);
294 #endif // ROBOTIS_MANIPULATOR_MATH_H_ Eigen::Matrix4d convertRPYToTransformationMatrix(double roll, double pitch, double yaw)
convertRPYToTransformationMatrix
double sign(double value)
sign
Eigen::Matrix3d inertiaMatrix(double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
inertiaMatrix
Eigen::Vector3d convertQuaternionToRPYVector(const Eigen::Quaterniond &quaternion)
convertQuaternionToRPYVector
Eigen::Vector3d convertRPYVelocityToOmega(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity)
convertRPYVelocityToOmega
Eigen::VectorXd poseDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position, Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
poseDifference
Eigen::Matrix3d matrix3(double m11, double m12, double m13, double m21, double m22, double m23, double m31, double m32, double m33)
matrix3
Eigen::Vector3d convertRPYAccelerationToOmegaDot(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d rpy_acceleration)
convertRPYAccelerationToOmegaDot
Eigen::Matrix3d convertQuaternionToRotationMatrix(const Eigen::Quaterniond &quaternion)
convertQuaternionToRotationMatrix
Eigen::Matrix4d inverseTransformationMatrix(const Eigen::MatrixXd &transformation_matrix)
inverseTransformationMatrix
Eigen::Vector3d orientationDifference(Eigen::Matrix3d desired_orientation, Eigen::Matrix3d present_orientation)
orientationDifference
Eigen::Vector3d vector3(double v1, double v2, double v3)
vector3
Eigen::Vector3d convertRotationMatrixToOmega(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToOmega
Eigen::Matrix3d skewSymmetricMatrix(Eigen::Vector3d v)
skewSymmetricMatrix
Eigen::Vector3d positionDifference(Eigen::Vector3d desired_position, Eigen::Vector3d present_position)
positionDifference
Eigen::Vector3d matrixLogarithm(Eigen::Matrix3d rotation_matrix)
matrixLogarithm
Eigen::Matrix3d rodriguesRotationMatrix(Eigen::Vector3d axis, double angle)
rodriguesRotationMatrix
Eigen::Matrix4d convertXYZRPYToTransformationMatrix(double x, double y, double z, double roll, double pitch, double yaw)
convertXYZRPYToTransformationMatrix
Eigen::Vector3d convertOmegaToRPYVelocity(Eigen::Vector3d rpy_vector, Eigen::Vector3d omega)
convertOmegaToRPYVelocity
Eigen::Vector3d convertXYZToVector(double x, double y, double z)
convertXYZToVector
Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw)
convertRPYToQuaternion
Eigen::Matrix3d convertRollAngleToRotationMatrix(double angle)
convertRollAngleToRotationMatrix
Eigen::Vector3d convertRotationMatrixToRPYVector(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToRPYVector
Eigen::Vector3d convertOmegaDotToRPYAcceleration(Eigen::Vector3d rpy_vector, Eigen::Vector3d rpy_velocity, Eigen::Vector3d omega_dot)
convertOmegaDotToRPYAcceleration
Eigen::Matrix3d convertYawAngleToRotationMatrix(double angle)
convertYawAngleToRotationMatrix
Eigen::Matrix3d convertPitchAngleToRotationMatrix(double angle)
convertPitchAngleToRotationMatrix
Eigen::Matrix3d convertRPYToRotationMatrix(double roll, double pitch, double yaw)
convertRPYToRotationMatrix
Eigen::Matrix4d convertXYZToTransformationMatrix(double x, double y, double z)
convertXYZToTransformationMatrix
Eigen::Quaterniond convertRotationMatrixToQuaternion(const Eigen::Matrix3d &rotation_matrix)
convertRotationMatrixToQuaternion