diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 9fecb82..84b3e11 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -50,7 +50,7 @@ namespace robotis_framework Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ); Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ); -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform); +Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform); Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); @@ -70,7 +70,7 @@ Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ); Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ); Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ); -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ); +Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ); Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation ); Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ); diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 436b0e4..8088051 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -46,8 +46,8 @@ namespace robotis_framework #define PRINT_VAR(X) std::cout << #X << " : " << X << std::endl #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define deg2rad (M_PI / 180.0) -#define rad2deg (180.0 / M_PI) +#define DEGREE2RADIAN (M_PI / 180.0) +#define RADIAN2DEGREE (180.0 / M_PI) inline double powDI(double a, int b) { diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 569a695..60cdf90 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -75,7 +75,7 @@ Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, dou return _transformation; } -Eigen::MatrixXd InverseTransformation(Eigen::MatrixXd transform) +Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform) { Eigen::Vector3d vecBOA; //If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A Eigen::Vector3d vec_x, vec_y, vec_z; @@ -261,7 +261,7 @@ Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) return _hatto; } -Eigen::MatrixXd Rodrigues( Eigen::MatrixXd hat_matrix , double angle ) +Eigen::MatrixXd rodrigues( Eigen::MatrixXd hat_matrix , double angle ) { Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); Eigen::MatrixXd _Rodrigues = _E + hat_matrix * sin( angle ) + hat_matrix * hat_matrix * ( 1 - cos( angle ) );