diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt index 4d6421d..7ce8d9a 100644 --- a/robotis_math/CMakeLists.txt +++ b/robotis_math/CMakeLists.txt @@ -1,9 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(robotis_math) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp cmake_modules @@ -11,16 +8,10 @@ find_package(catkin REQUIRED COMPONENTS find_package(Eigen REQUIRED) - ################################### ## catkin specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( INCLUDE_DIRS include LIBRARIES robotis_math @@ -32,34 +23,20 @@ catkin_package( ## Build ## ########### -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include ${catkin_INCLUDE_DIRS} ${Eigen_INCLUDE_DIRS} ) -## Declare a C++ library add_library(robotis_math src/robotis_math_base.cpp src/robotis_trajectory_calculator.cpp src/robotis_linear_algebra.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -# add_executable(robotis_math_node src/robotis_math_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -#add_dependencies(robotis_math_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against target_link_libraries(robotis_math ${catkin_LIBRARIES} ) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 84b3e11..6751b2b 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -31,8 +31,8 @@ /* * robotis_linear_algebra.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_LINEAR_ALGEBRA_H_ @@ -48,32 +48,27 @@ 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 getTransitionXYZ(double position_x, double position_y, double position_z); +Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll, double pitch, double yaw); +Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform); +Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz); +Eigen::MatrixXd getRotationX(double angle); +Eigen::MatrixXd getRotationY(double angle); +Eigen::MatrixXd getRotationZ(double angle); +Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw); -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ); +Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation); +Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw); +Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw); +Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation); +Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion); +Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion); -Eigen::MatrixXd rotationX( double angle ); -Eigen::MatrixXd rotationY( double angle ); -Eigen::MatrixXd rotationZ( double angle ); - -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ); -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ); - -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ); -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ); - -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ); -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 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 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); } diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h index 61c33e5..caa6dc9 100644 --- a/robotis_math/include/robotis_math/robotis_math.h +++ b/robotis_math/include/robotis_math/robotis_math.h @@ -31,8 +31,8 @@ /* * robotis_math.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_MATH_H_ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h index 8088051..14c4bc3 100644 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ b/robotis_math/include/robotis_math/robotis_math_base.h @@ -31,8 +31,8 @@ /* * robotis_math_base.h * - * Created on: 2016. 3. 28. - * Author: JaySong + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_MATH_BASE_H_ @@ -54,7 +54,7 @@ inline double powDI(double a, int b) return (b == 0 ? 1 : (b > 0 ? a * powDI(a, b - 1) : 1 / powDI(a, -b))); } -double sign( double x ); +double sign(double x); } diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index a1c3807..79857df 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -31,14 +31,13 @@ /* * robotis_trajectory_calculator.h * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_ #define ROBOTIS_TRAJECTORY_CALCULATOR_H_ - #include "robotis_linear_algebra.h" #include "robotis_math_base.h" @@ -47,21 +46,20 @@ namespace robotis_framework { -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ); +Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time); -Eigen::MatrixXd minimum_jerk_tra_via_n_qdqddq( int via_num, - double pos_start , double vel_start , double accel_start , - Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, - double pos_end, double vel_end, double accel_end, - double smp_time, Eigen::MatrixXd via_time, double mov_time ); +Eigen::MatrixXd calcMinimumJerkTraWithViaPoints(int via_num, + double pos_start, double vel_start, double accel_start, + Eigen::MatrixXd pos_via, Eigen::MatrixXd vel_via, Eigen::MatrixXd accel_via, + double pos_end, double vel_end, double accel_end, + double smp_time, Eigen::MatrixXd via_time, double mov_time); -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ); +Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio); } - #endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml index 506f261..29d618b 100644 --- a/robotis_math/package.xml +++ b/robotis_math/package.xml @@ -4,28 +4,10 @@ 0.0.0 The robotis_math package - - - - jay - - - - - - BSD - - - - - - - - - - - - + sch + BSD + + sch catkin roscpp diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 60cdf90..d85d744 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -31,291 +31,265 @@ /* * robotis_linear_algebra.cpp * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ - #include "robotis_math/robotis_linear_algebra.h" namespace robotis_framework { -Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z ) +Eigen::MatrixXd getTransitionXYZ(double position_x, double position_y, double position_z) { - Eigen::MatrixXd _position(3,1); + Eigen::MatrixXd position(3,1); - _position << position_x, - position_y, - position_z; + position << + position_x, + position_y, + position_z; - return _position; + return position; } -Eigen::MatrixXd transformationXYZRPY ( double position_x, double position_y, double position_z , double roll , double pitch , double yaw ) +Eigen::MatrixXd getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll , double pitch , double yaw) { -// Eigen::MatrixXd _position(3,1); -// -// _position << position_x, -// position_y, -// position_z; -// -// Eigen::MatrixXd _rotation = rpy2rotation( roll , pitch , yaw ); -// -// Eigen::MatrixXd _transformation(4,4); -// -// _transformation << _rotation , _position, -// 0 , 0 , 0 , 1; + Eigen::MatrixXd transformation = getRotation4d(roll, pitch, yaw); + transformation.coeffRef(0,3) = position_x; + transformation.coeffRef(1,3) = position_y; + transformation.coeffRef(2,3) = position_z; - Eigen::MatrixXd _transformation = rotation4d(roll, pitch, yaw); - _transformation.coeffRef(0,3) = position_x; - _transformation.coeffRef(1,3) = position_y; - _transformation.coeffRef(2,3) = position_z; - - return _transformation; + return transformation; } -Eigen::MatrixXd inverseTransformation(Eigen::MatrixXd transform) +Eigen::MatrixXd getInverseTransformation(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; - Eigen::MatrixXd _invT(4,4); + // If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A - vecBOA.coeffRef(0) = -transform(0,3); vecBOA(1) = -transform(1,3); vecBOA(2) = -transform(2,3); - vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); - vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); - vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); -// -// -// // inv = [ x' | -AtoB??x ] -// // [ y' | -AtoB??y ] -// // [ z' | -AtoB??z ] -// // [ 0 0 0 | 1 ] -// - _invT << vec_x(0), vec_x(1), vec_x(2), vecBOA.dot(vec_x), - vec_y(0), vec_y(1), vec_y(2), vecBOA.dot(vec_y), - vec_z(0), vec_z(1), vec_z(2), vecBOA.dot(vec_z), - 0, 0, 0, 1; + Eigen::Vector3d vec_boa; + Eigen::Vector3d vec_x, vec_y, vec_z; + Eigen::MatrixXd inv_t(4,4); - return _invT; + vec_boa(0) = -transform(0,3); + vec_boa(1) = -transform(1,3); + vec_boa(2) = -transform(2,3); + + vec_x(0) = transform(0,0); vec_x(1) = transform(1,0); vec_x(2) = transform(2,0); + vec_y(0) = transform(0,1); vec_y(1) = transform(1,1); vec_y(2) = transform(2,1); + vec_z(0) = transform(0,2); vec_z(1) = transform(1,2); vec_z(2) = transform(2,2); + + inv_t << + vec_x(0), vec_x(1), vec_x(2), vec_boa.dot(vec_x), + vec_y(0), vec_y(1), vec_y(2), vec_boa.dot(vec_y), + vec_z(0), vec_z(1), vec_z(2), vec_boa.dot(vec_z), + 0, 0, 0, 1; + + return inv_t; } -Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz ) +Eigen::MatrixXd getInertiaXYZ(double ixx, double ixy, double ixz , double iyy , double iyz, double izz) { - Eigen::MatrixXd _inertia(3,3); + Eigen::MatrixXd inertia(3,3); - _inertia << ixx , ixy , ixz, - ixy , iyy , iyz, - ixz , iyz , izz ; + inertia << + ixx, ixy, ixz, + ixy, iyy, iyz, + ixz, iyz, izz; - return _inertia; + return inertia; } -Eigen::MatrixXd rotationX( double angle ) +Eigen::MatrixXd getRotationX(double angle) { - Eigen::MatrixXd _rotation( 3 , 3 ); + Eigen::MatrixXd rotation(3,3); - _rotation << 1.0, 0.0, 0.0, - 0.0, cos( angle ), -sin( angle ), - 0.0, sin( angle ), cos( angle ); + rotation << + 1.0, 0.0, 0.0, + 0.0, cos(angle), -sin(angle), + 0.0, sin(angle), cos(angle); - return _rotation; + return rotation; } -Eigen::MatrixXd rotationY( double angle ) +Eigen::MatrixXd getRotationY(double angle) { - Eigen::MatrixXd _rotation( 3 , 3 ); + Eigen::MatrixXd rotation(3,3); - _rotation << cos( angle ), 0.0, sin( angle ), - 0.0, 1.0, 0.0, - -sin( angle ), 0.0, cos( angle ); + rotation << + cos(angle), 0.0, sin(angle), + 0.0, 1.0, 0.0, + -sin(angle), 0.0, cos(angle); - return _rotation; + return rotation; } -Eigen::MatrixXd rotationZ( double angle ) +Eigen::MatrixXd getRotationZ(double angle) { - Eigen::MatrixXd _rotation(3,3); + Eigen::MatrixXd rotation(3,3); - _rotation << cos( angle ), -sin( angle ), 0.0, - sin( angle ), cos( angle ), 0.0, - 0.0, 0.0, 1.0; + rotation << + cos(angle), -sin(angle), 0.0, + sin(angle), cos(angle), 0.0, + 0.0, 0.0, 1.0; - return _rotation; + return rotation; } -Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation ) +Eigen::MatrixXd getRotation4d(double roll, double pitch, double yaw ) { - Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 ); + double sr = sin(roll), cr = cos(roll); + double sp = sin(pitch), cp = cos(pitch); + double sy = sin(yaw), cy = cos(yaw); - _rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) ); - _rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) ); - _rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) ); + Eigen::MatrixXd mat_roll(4,4); + Eigen::MatrixXd mat_pitch(4,4); + Eigen::MatrixXd mat_yaw(4,4); - return _rpy; + mat_roll << + 1, 0, 0, 0, + 0, cr, -sr, 0, + 0, sr, cr, 0, + 0, 0, 0, 1; + + mat_pitch << + cp, 0, sp, 0, + 0, 1, 0, 0, + -sp, 0, cp, 0, + 0, 0, 0, 1; + + mat_yaw << + cy, -sy, 0, 0, + sy, cy, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + + Eigen::MatrixXd mat_rpy = (mat_yaw*mat_pitch)*mat_roll; + + return mat_rpy; } -Eigen::MatrixXd rpy2rotation( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertRotationToRPY(Eigen::MatrixXd rotation) { - Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); + Eigen::MatrixXd rpy = Eigen::MatrixXd::Zero(3,1); - return _rotation; + rpy.coeffRef(0,0) = atan2(rotation.coeff(2,1), rotation.coeff(2,2)); + rpy.coeffRef(1,0) = atan2(-rotation.coeff(2,0), sqrt(pow(rotation.coeff(2,1), 2) + pow(rotation.coeff(2,2),2))); + rpy.coeffRef(2,0) = atan2 (rotation.coeff(1,0), rotation.coeff(0,0)); + + return rpy; } -Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertRPYToRotation(double roll, double pitch, double yaw) { - Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw ); + Eigen::MatrixXd rotation = getRotationZ(yaw)*getRotationY(pitch)*getRotationX(roll); - Eigen::Matrix3d _rotation3d; - _rotation3d = _rotation.block<3,3>( 0 , 0); - - Eigen::Quaterniond _quaternion; - - _quaternion = _rotation3d; - - return _quaternion; + return rotation; } -Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation ) +Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw) { - Eigen::Matrix3d _rotation3d; + Eigen::MatrixXd rotation = convertRPYToRotation(roll,pitch,yaw); - _rotation3d = rotation.block<3,3>( 0 , 0); + Eigen::Matrix3d rotation3d; + rotation3d = rotation.block<3,3>(0,0); - Eigen::Quaterniond _quaternion; - _quaternion = _rotation3d; + Eigen::Quaterniond quaternion; + quaternion = rotation3d; - return _quaternion; + return quaternion; } -Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion ) +Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation) { - Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() ); + Eigen::Matrix3d rotation3d; + rotation3d = rotation.block<3,3>(0,0); - return _rpy; + Eigen::Quaterniond quaternion; + quaternion = rotation3d; + + return quaternion; } -Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion ) +Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion) { - Eigen::MatrixXd _rotation = quaternion.toRotationMatrix(); + Eigen::MatrixXd rpy = convertRotationToRPY(quaternion.toRotationMatrix()); - return _rotation; + return rpy; } -Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw ) +Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion) { -// Eigen::MatrixXd _rotation4d = Eigen::MatrixXd::Zero( 4 , 4 ); -// _rotation4d.coeffRef( 3 , 3 ) = 1.0; -// -// Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll ); -// -//// _rotation4d.block<3,3>(0,0) = _rotation; -// _rotation4d.coeffRef(0,0) = _rotation.coeff(0,0); -// _rotation4d.coeffRef(0,1) = _rotation.coeff(0,1); -// _rotation4d.coeffRef(0,2) = _rotation.coeff(0,2); -// _rotation4d.coeffRef(1,0) = _rotation.coeff(1,0); -// _rotation4d.coeffRef(1,1) = _rotation.coeff(1,1); -// _rotation4d.coeffRef(1,2) = _rotation.coeff(1,2); -// _rotation4d.coeffRef(2,0) = _rotation.coeff(2,0); -// _rotation4d.coeffRef(2,1) = _rotation.coeff(2,1); -// _rotation4d.coeffRef(2,2) = _rotation.coeff(2,2); + Eigen::MatrixXd rotation = quaternion.toRotationMatrix(); -// return _rotation4d; - double sr = sin(roll), cr = cos(roll); - double sp = sin(pitch), cp = cos(pitch); - double sy = sin(yaw), cy = cos(yaw); - - Eigen::MatrixXd matRoll(4,4); - Eigen::MatrixXd matPitch(4,4); - Eigen::MatrixXd matYaw(4,4); - - matRoll << 1, 0, 0, 0, - 0, cr, -sr, 0, - 0, sr, cr, 0, - 0, 0, 0, 1; - - matPitch << cp, 0, sp, 0, - 0, 1, 0, 0, - -sp, 0, cp, 0, - 0, 0, 0, 1; - - matYaw << cy, -sy, 0, 0, - sy, cy, 0, 0, - 0, 0, 1, 0, - 0, 0, 0, 1; - - - return (matYaw*matPitch)*matRoll; + return rotation; } - - -Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d ) +Eigen::MatrixXd hatto(Eigen::MatrixXd matrix3d) { - Eigen::MatrixXd _hatto(3,3); + Eigen::MatrixXd hatto(3,3); - _hatto << 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), - matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), - -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; + hatto << + 0.0, -matrix3d.coeff(2,0), matrix3d.coeff(1,0), + matrix3d.coeff(2,0), 0.0, -matrix3d.coeff(0,0), + -matrix3d.coeff(1,0), matrix3d.coeff(0,0), 0.0; - return _hatto; + 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 ) ); + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); + Eigen::MatrixXd rodrigues = identity+hat_matrix*sin(angle)+hat_matrix*hat_matrix*(1-cos(angle)); - return _Rodrigues; + return rodrigues; } -Eigen::MatrixXd rot2omega( Eigen::MatrixXd rotation ) +Eigen::MatrixXd rot2omega(Eigen::MatrixXd rotation) { - double _eps = 1e-12; + double eps = 1e-12; -// Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); + 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 ); - double _alpha = ( rotation.coeff( 0 , 0 ) + rotation.coeff( 1 , 1 ) + rotation.coeff( 2 , 2 ) - 1.0 ) / 2.0 ; + Eigen::MatrixXd rot2omega(3,1); - double _alpha_plus = fabs( _alpha - 1.0 ); + if( alpha_dash < eps ) + { + rot2omega << + 0.0, + 0.0, + 0.0; + } + else + { + double theta = acos(alpha); - Eigen::MatrixXd _rot2omega( 3 , 1 ); + rot2omega << + rotation.coeff(2,1)-rotation.coeff(1,2), + rotation.coeff(0,2)-rotation.coeff(2,0), + rotation.coeff(1,0)-rotation.coeff(0,1); - if( _alpha_plus < _eps ) - { - _rot2omega << 0.0, - 0.0, - 0.0; - } - else - { - double _theta = acos( _alpha ); + rot2omega = 0.5*(theta/sin(theta))*rot2omega; + } - _rot2omega << 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; - } - - return _rot2omega; + return rot2omega; } -Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { - Eigen::MatrixXd _cross( 3 , 1 ); + Eigen::MatrixXd cross(3,1); - _cross << vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 2 , 0 ) - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 1 , 0 ), - vector3d_a.coeff( 2 , 0 ) * vector3d_b.coeff( 0 , 0 ) - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 2 , 0 ), - vector3d_a.coeff( 0 , 0 ) * vector3d_b.coeff( 1 , 0 ) - vector3d_a.coeff( 1 , 0 ) * vector3d_b.coeff( 0 , 0 ); + cross << + vector3d_a.coeff(1,0)*vector3d_b.coeff(2,0)-vector3d_a.coeff(2,0)*vector3d_b.coeff(1,0), + vector3d_a.coeff(2,0)*vector3d_b.coeff(0,0)-vector3d_a.coeff(0,0)*vector3d_b.coeff(2,0), + vector3d_a.coeff(0,0)*vector3d_b.coeff(1,0)-vector3d_a.coeff(1,0)*vector3d_b.coeff(0,0); - return _cross; + return cross; } -double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b ) +double dot(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b) { - return vector3d_a.dot(vector3d_b); - //return vector3d_a.coeff(0,0)*vector3d_b.coeff(0,0) + vector3d_a.coeff(1,0)*vector3d_b.coeff(1,0) + vector3d_a.coeff(2,0)*vector3d_b.coeff(2,0); + return vector3d_a.dot(vector3d_b); } } diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp index b5c43d3..60eb1f4 100644 --- a/robotis_math/src/robotis_math_base.cpp +++ b/robotis_math/src/robotis_math_base.cpp @@ -31,24 +31,23 @@ /* * robotis_math_base.cpp * - * Created on: Mar 18, 2016 - * Author: jay + * Created on: June 6, 2016 + * Author: sch */ #include "robotis_math/robotis_math_base.h" - namespace robotis_framework { -double sign( double x ) +double sign(double x) { - if ( x < 0.0 ) - return -1.0; - else if ( x > 0.0) - return 1.0; - else - return 0.0; + if ( x < 0.0 ) + return -1.0; + else if ( x > 0.0) + return 1.0; + else + return 0.0; } } diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 4d91a2c..4b776aa 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -1,30 +1,48 @@ -/* - * RobotisTrajectoryCalculator.cpp +/******************************************************************************* + * Copyright (c) 2016, ROBOTIS CO., LTD. + * All rights reserved. * - * Created on: Mar 18, 2016 - * Author: jay - */ - - - -#include "../include/robotis_math/robotis_trajectory_calculator.h" - + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of ROBOTIS nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *******************************************************************************/ /* - * trajectory.cpp + * robotis_trajectory_calculator.cpp * - * Created on: Jul 13, 2015 + * Created on: June 6, 2016 * Author: sch */ - +#include "../include/robotis_math/robotis_trajectory_calculator.h" namespace robotis_framework { -Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double accel_start, - double pos_end , double vel_end , double accel_end, - double smp_time , double mov_time ) +Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double accel_start, + double pos_end, double vel_end, double accel_end, + double smp_time, double mov_time) /* simple minimum jerk trajectory @@ -37,288 +55,275 @@ Eigen::MatrixXd minimum_jerk_tra( double pos_start , double vel_start , double a accel_end : acceleration at final state smp_time : sampling time - mov_time : movement time -*/ + */ { - Eigen::MatrixXd ___C( 3 , 3 ); - Eigen::MatrixXd __C( 3 , 1 ); + Eigen::MatrixXd poly_matrix(3,3); + Eigen::MatrixXd poly_vector(3,1); - ___C << pow( mov_time , 3 ) , pow( mov_time , 4 ) , pow( mov_time , 5 ), - 3 * pow( mov_time , 2 ) , 4 * pow( mov_time , 3 ) , 5 * pow( mov_time , 4 ), - 6 * mov_time , 12 * pow( mov_time , 2 ) , 20 * pow( mov_time , 3 ); + poly_matrix << + pow(mov_time,3), pow(mov_time,4), pow(mov_time,5), + 3*pow(mov_time,2), 4*pow(mov_time,3), 5*pow(mov_time,4), + 6*mov_time, 12*pow(mov_time,2), 20*pow(mov_time,3); - __C << pos_end - pos_start - vel_start * mov_time - accel_start * pow( mov_time , 2 ) / 2, - vel_end - vel_start - accel_start * mov_time, - accel_end - accel_start ; + poly_vector << + pos_end-pos_start-vel_start*mov_time-accel_start*pow(mov_time,2)/2, + vel_end-vel_start-accel_start*mov_time, + accel_end-accel_start ; - Eigen::Matrix _A = ___C.inverse() * __C; + Eigen::Matrix poly_coeff = poly_matrix.inverse() * poly_vector; - double _time_steps; + double time_steps = mov_time/smp_time; + int all_time_steps = round(time_steps+1); - _time_steps = mov_time / smp_time; - int __time_steps = round( _time_steps + 1 ); + Eigen::MatrixXd time = Eigen::MatrixXd::Zero(all_time_steps,1); + Eigen::MatrixXd minimum_jerk_tra = Eigen::MatrixXd::Zero(all_time_steps,1); - Eigen::MatrixXd _time = Eigen::MatrixXd::Zero( __time_steps , 1 ); - Eigen::MatrixXd _tra = Eigen::MatrixXd::Zero( __time_steps , 1 ); + for (int step=0; step j){ - k = i ; - }else{ - k = j ; - } - A2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - A2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - A2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - A2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - - A2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; - A2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; - A2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; - } - } - - - Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero(3,3*via_num+3); - - A3.coeffRef(0,0) = pow(mov_time,3); - A3.coeffRef(0,1) = pow(mov_time,4); - A3.coeffRef(0,2) = pow(mov_time,5); - - A3.coeffRef(1,0) = 3*pow(mov_time,2); - A3.coeffRef(1,1) = 4*pow(mov_time,3); - A3.coeffRef(1,2) = 5*pow(mov_time,4); - - A3.coeffRef(2,0) = 6*mov_time; - A3.coeffRef(2,1) = 12*pow(mov_time,2); - A3.coeffRef(2,2) = 20*pow(mov_time,3); - - for (i=1; i<=via_num; i++){ - A3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - A3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; - - A3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - A3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; - - A3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; - A3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; - A3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; - } - - Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); - - A.block(0,0,3*via_num,3) = A1 ; - A.block(0,3,3*via_num,3*via_num) = A2 ; - A.block(3*via_num,0,3,3*via_num+3) = A3 ; - - /* Calculation Matrix C (coefficient of polynomial function) */ - - Eigen::MatrixXd C(3*via_num+3,1); - //C = A.inverse()*B; - C = A.colPivHouseholderQr().solve(B); - - /* Time */ - - int NN; - double N; - - N = mov_time/smp_time ; - NN = round(N) ; - - Eigen::MatrixXd Time = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - Time.coeffRef(i-1,0) = (i-1)*smp_time; - } - - /* Time_via */ - - Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero(via_num,1); - - for (i=1; i<=via_num; i++){ - Time_via.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; - } - - /* Minimum Jerk Trajectory with Via-points */ - - Eigen::MatrixXd _tra_jerk_via = Eigen::MatrixXd::Zero(NN+1,1); - - for (i=1; i<=NN+1; i++){ - _tra_jerk_via.coeffRef(i-1,0) = - pos_start + - vel_start*Time.coeff(i-1,0) + - 0.5*accel_start*pow(Time.coeff(i-1,0),2) + - C.coeff(0,0)*pow(Time.coeff(i-1,0),3) + - C.coeff(1,0)*pow(Time.coeff(i-1,0),4) + - C.coeff(2,0)*pow(Time.coeff(i-1,0),5) ; - } - - for (i=1; i<=via_num; i++){ - for (j=Time_via.coeff(i-1,0); j<=NN+1; j++){ - _tra_jerk_via.coeffRef(j-1,0) = - _tra_jerk_via.coeff(j-1,0) + - C.coeff(3*i,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + - C.coeff(3*i+1,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + - C.coeff(3*i+2,0)*pow((Time.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; - - } - } - - return _tra_jerk_via; - -} - -Eigen::MatrixXd arc3d_tra( double smp_time, double mov_time, - Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, - double rotation_angle, double cross_ratio ) -{ - int _all_time_steps = int ( round( mov_time / smp_time ) ) + 1 ; - - Eigen::MatrixXd _angle_tra = minimum_jerk_tra( 0.0 , 0.0 , 0.0 , - rotation_angle , 0.0 , 0.0 , - smp_time , mov_time ); - - Eigen::MatrixXd _pt = Eigen::MatrixXd::Zero( 3 , _all_time_steps ); - - for (int i = 0; i < _all_time_steps; i++ ) + for (int i=1; i<=via_num; i++) + { + for (int j=1; j<=via_num; j++) { - double _t = ( ( double ) i ) * smp_time ; + int k; - double _th = _angle_tra.coeff( i , 0 );//( _t / mov_time ) * rotation_angle; + if (i > j) + k = i ; + else + k = j ; - Eigen::MatrixXd _w_wedge( 3 , 3 ); + poly_matrix_part2.coeffRef(3*j-3,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + poly_matrix_part2.coeffRef(3*j-3,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; + poly_matrix_part2.coeffRef(3*j-3,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),5)/120 ; - _w_wedge << 0.0 , -normal_vector.coeff(2,0), normal_vector.coeff(1,0), - normal_vector.coeff(2,0), 0.0 , -normal_vector.coeff(0,0), - -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-3) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; + poly_matrix_part2.coeffRef(3*j-2,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),4)/24 ; - Eigen::MatrixXd _E = Eigen::MatrixXd::Identity( 3 , 3 ); - - Eigen::MatrixXd _R = _E + _w_wedge * sin( _th ) + _w_wedge * _w_wedge * ( 1 - cos( _th ) ); - - double _cross = cross_ratio * ( 1.0 - 2.0 * ( abs ( 0.5 - _t/mov_time ) ) ); - - _pt.block( 0 , i , 3 , 1 ) = ( 1 + _cross ) * ( _R * ( start_point - center_point ) ) + center_point; + poly_matrix_part2.coeffRef(3*j-1,3*i-3) = via_time.coeff(k-1,0)-via_time.coeff(i-1,0) ; + poly_matrix_part2.coeffRef(3*j-1,3*i-2) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),2)/2 ; + poly_matrix_part2.coeffRef(3*j-1,3*i-1) = pow((via_time.coeff(k-1,0)-via_time.coeff(i-1,0)),3)/6 ; } + } - Eigen::MatrixXd _pt_trans = _pt.transpose(); - return _pt_trans; + Eigen::MatrixXd poly_matrix_part3 = Eigen::MatrixXd::Zero(3,3*via_num+3); + + poly_matrix_part3.coeffRef(0,0) = pow(mov_time,3); + poly_matrix_part3.coeffRef(0,1) = pow(mov_time,4); + poly_matrix_part3.coeffRef(0,2) = pow(mov_time,5); + + poly_matrix_part3.coeffRef(1,0) = 3*pow(mov_time,2); + poly_matrix_part3.coeffRef(1,1) = 4*pow(mov_time,3); + poly_matrix_part3.coeffRef(1,2) = 5*pow(mov_time,4); + + poly_matrix_part3.coeffRef(2,0) = 6*mov_time; + poly_matrix_part3.coeffRef(2,1) = 12*pow(mov_time,2); + poly_matrix_part3.coeffRef(2,2) = 20*pow(mov_time,3); + + for (int i=1; i<=via_num; i++) + { + poly_matrix_part3.coeffRef(0,3*i) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + poly_matrix_part3.coeffRef(1,3*i) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + poly_matrix_part3.coeffRef(2,3*i) = mov_time-via_time.coeff(i-1,0) ; + + poly_matrix_part3.coeffRef(0,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + poly_matrix_part3.coeffRef(1,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + poly_matrix_part3.coeffRef(2,3*i+1) = pow(mov_time-via_time.coeff(i-1,0),2)/2 ; + + poly_matrix_part3.coeffRef(0,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),5)/120 ; + poly_matrix_part3.coeffRef(1,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),4)/24 ; + poly_matrix_part3.coeffRef(2,3*i+2) = pow(mov_time-via_time.coeff(i-1,0),3)/6 ; + } + + Eigen::MatrixXd poly_matrix = Eigen::MatrixXd::Zero(3*via_num+3,3*via_num+3); + + poly_matrix.block(0,0,3*via_num,3) = poly_matrix_part1 ; + poly_matrix.block(0,3,3*via_num,3*via_num) = poly_matrix_part2 ; + poly_matrix.block(3*via_num,0,3,3*via_num+3) = poly_matrix_part3 ; + + Eigen::MatrixXd poly_coeff(3*via_num+3,1); + //C = A.inverse()*B; + poly_coeff = poly_matrix.colPivHouseholderQr().solve(poly_vector); + + int all_time_steps; + all_time_steps = round(mov_time/smp_time) ; + + Eigen::MatrixXd time_vector = Eigen::MatrixXd::Zero(all_time_steps+1,1); + + for (int i=1; i<=all_time_steps+1; i++) + time_vector.coeffRef(i-1,0) = (i-1)*smp_time; + + Eigen::MatrixXd via_time_vector = Eigen::MatrixXd::Zero(via_num,1); + + for (int i=1; i<=via_num; i++) + via_time_vector.coeffRef(i-1,0) = round(via_time.coeff(i-1,0)/smp_time)+2; + + Eigen::MatrixXd minimum_jerk_tra_with_via_points = Eigen::MatrixXd::Zero(all_time_steps+1,1); + + for (int i=1; i<=all_time_steps+1; i++) + { + minimum_jerk_tra_with_via_points.coeffRef(i-1,0) = + pos_start + + vel_start*time_vector.coeff(i-1,0) + + 0.5*accel_start*pow(time_vector.coeff(i-1,0),2) + + poly_coeff.coeff(0,0)*pow(time_vector.coeff(i-1,0),3) + + poly_coeff.coeff(1,0)*pow(time_vector.coeff(i-1,0),4) + + poly_coeff.coeff(2,0)*pow(time_vector.coeff(i-1,0),5) ; + } + + for (int i=1; i<=via_num; i++) + { + for (int j=via_time_vector.coeff(i-1,0); j<=all_time_steps+1; j++) + { + minimum_jerk_tra_with_via_points.coeffRef(j-1,0) = + minimum_jerk_tra_with_via_points.coeff(j-1,0) + + poly_coeff.coeff(3*i,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),3)/6 + + poly_coeff.coeff(3*i+1,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),4)/24 + + poly_coeff.coeff(3*i+2,0)*pow((time_vector.coeff(j-1,0)-via_time.coeff(i-1,0)),5)/120 ; + + } + } + + return minimum_jerk_tra_with_via_points; +} + +Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, + Eigen::MatrixXd center_point, Eigen::MatrixXd normal_vector, Eigen::MatrixXd start_point, + double rotation_angle, double cross_ratio ) +{ + int all_time_steps = int (round(mov_time/smp_time))+1; + + Eigen::MatrixXd angle_tra = calcMinimumJerkTra(0.0, 0.0, 0.0, + rotation_angle, 0.0, 0.0, + smp_time, mov_time); + + Eigen::MatrixXd arc_tra = Eigen::MatrixXd::Zero(3,all_time_steps); + + for (int step = 0; step < all_time_steps; step++ ) + { + double time = ((double)step)*smp_time; + double theta = angle_tra.coeff(step,0); + + Eigen::MatrixXd weight_wedge(3,3); + + weight_wedge << + 0.0, -normal_vector.coeff(2,0), normal_vector.coeff(1,0), + normal_vector.coeff(2,0), 0.0, -normal_vector.coeff(0,0), + -normal_vector.coeff(1,0), normal_vector.coeff(0,0), 0.0; + + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(3,3); + Eigen::MatrixXd rotation = identity + weight_wedge*sin(theta) + weight_wedge*weight_wedge*(1-cos(theta)); + double cross = cross_ratio*(1.0-2.0*(abs(0.5-(time/mov_time)))); + + arc_tra.block(0,step,3,1) = (1+cross)*(rotation*(start_point-center_point))+center_point; + } + + Eigen::MatrixXd act_tra_trans = arc_tra.transpose(); + + return act_tra_trans; }