BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ROS C++ coding style is applied to robotis_math -version2

Browse Source
This commit is contained in:
sch
2016-06-07 16:54:01 +09:00
parent 2e7b2c6c35
commit 36f627f035
7 changed files with 34 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
robotis_math/include/robotis_math/robotis_linear_algebra.h
View File

@@ -31,12 +31,12 @@
/*
* robotis_linear_algebra.h
*
* Created on: June 6, 2016
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_LINEAR_ALGEBRA_H_
#define ROBOTIS_LINEAR_ALGEBRA_H_
#ifndef ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_
#define ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_
#include <cmath>
@@ -64,14 +64,14 @@ Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation);
Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion);
Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion);
Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d);
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);
Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d);
Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle);
Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation);
Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b);
double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b);
}
#endif /* ROBOTIS_LINEAR_ALGEBRA_H_ */
#endif /* ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ */

8
robotis_math/include/robotis_math/robotis_math.h
View File

@@ -31,13 +31,13 @@
/*
* robotis_math.h
*
* Created on: June 6, 2016
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_MATH_H_
#define ROBOTIS_MATH_H_
#ifndef ROBOTIS_MATH_ROBOTIS_MATH_H_
#define ROBOTIS_MATH_ROBOTIS_MATH_H_
#include "robotis_trajectory_calculator.h"
#endif /* ROBOTIS_MATH_H_ */
#endif /* ROBOTIS_MATH_ROBOTIS_MATH_H_ */

8
robotis_math/include/robotis_math/robotis_math_base.h
View File

@@ -31,12 +31,12 @@
/*
* robotis_math_base.h
*
* Created on: June 6, 2016
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_MATH_BASE_H_
#define ROBOTIS_MATH_BASE_H_
#ifndef ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_
#define ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_
#include <cmath>
@@ -60,4 +60,4 @@ double sign(double x);
#endif /* ROBOTIS_MATH_BASE_H_ */
#endif /* ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_ */

8
robotis_math/include/robotis_math/robotis_trajectory_calculator.h
View File

@@ -31,12 +31,12 @@
/*
* robotis_trajectory_calculator.h
*
* Created on: June 6, 2016
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_
#define ROBOTIS_TRAJECTORY_CALCULATOR_H_
#ifndef ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_
#define ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_
#include "robotis_linear_algebra.h"
#include "robotis_math_base.h"
@@ -62,4 +62,4 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time,
}
#endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */
#endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */

22
robotis_math/src/robotis_linear_algebra.cpp
View File

@@ -31,7 +31,7 @@
/*
* robotis_linear_algebra.cpp
*
* Created on: June 6, 2016
* Created on: June 7, 2016
* Author: sch
*/
@@ -224,7 +224,7 @@ Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion)
return rotation;
}
Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d)
Eigen::MatrixXd calcHatto(Eigen::MatrixXd matrix3d)
{
Eigen::MatrixXd hatto(3,3);
@@ -236,7 +236,7 @@ Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d)
return hatto;
}
Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle)
Eigen::MatrixXd calcRodrigues(Eigen::MatrixXd hat_matrix, double angle)
{
Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3);
Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle));
@@ -244,18 +244,18 @@ Eigen::MatrixXd rodrigues(Eigen::MatrixXd hat_matrix, double angle)
return rodrigues;
}
Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation)
Eigen::MatrixXd convertRotToOmega(Eigen::MatrixXd rotation)
{
double eps = 1e-12;
double alpha = (rotation.coeff(0,0)+rotation.coeff(1,1)+rotation.coeff(2,2)-1.0)/2.0;
double alpha_dash = fabs( alpha - 1.0 );
Eigen::MatrixXd rot2omega(3,1);
Eigen::MatrixXd rot_to_omega(3,1);
if( alpha_dash < eps )
{
rot2omega <<
rot_to_omega <<
0.0,
0.0,
0.0;
@@ -264,18 +264,18 @@ Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation)
{
double theta = acos(alpha);
rot2omega <<
rot_to_omega <<
rotation.coeff(2,1)-rotation.coeff(1,2),
rotation.coeff(0,2)-rotation.coeff(2,0),
rotation.coeff(1,0)-rotation.coeff(0,1);
rot2omega = 0.5*(theta/sin(theta))*rot2omega;
rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega;
}
return rot2omega;
return rot_to_omega;
}
Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
Eigen::MatrixXd calcCross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
{
Eigen::MatrixXd cross(3,1);
@@ -287,7 +287,7 @@ Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
return cross;
}
double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
double calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
{
return vector3d_a.dot(vector3d_b);
}

2
robotis_math/src/robotis_math_base.cpp
View File

@@ -31,7 +31,7 @@
/*
* robotis_math_base.cpp
*
* Created on: June 6, 2016
* Created on: June 7, 2016
* Author: sch
*/

2
robotis_math/src/robotis_trajectory_calculator.cpp
View File

@@ -31,7 +31,7 @@
/*
* robotis_trajectory_calculator.cpp
*
* Created on: June 6, 2016
* Created on: June 7, 2016
* Author: sch
*/