modified.
This commit is contained in:
ROBOTIS-zerom
65
robotis_math/CMakeLists.txt
Normal file
65
robotis_math/CMakeLists.txt
Normal file
@ -0,0 +1,65 @@
|
||||
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
|
||||
)
|
||||
|
||||
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
|
||||
CATKIN_DEPENDS roscpp
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## 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/RobotisMathBase.cpp
|
||||
src/RobotisTrajectoryCalculator.cpp
|
||||
src/RobotisLinearAlgebra.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}
|
||||
)
|
||||
52
robotis_math/include/robotis_math/RobotisLinearAlgebra.h
Normal file
52
robotis_math/include/robotis_math/RobotisLinearAlgebra.h
Normal file
@ -0,0 +1,52 @@
|
||||
/*
|
||||
* RobotisLinearAlgebra.h
|
||||
*
|
||||
* Created on: Mar 18, 2016
|
||||
* Author: jay
|
||||
*/
|
||||
|
||||
#ifndef ROBOTIS_LINEAR_ALGEBRA_H_
|
||||
#define ROBOTIS_LINEAR_ALGEBRA_H_
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#define EIGEN_NO_DEBUG
|
||||
#define EIGEN_NO_STATIC_ASSERT
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
namespace ROBOTIS
|
||||
{
|
||||
|
||||
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 inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz );
|
||||
|
||||
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 );
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* ROBOTIS_LINEAR_ALGEBRA_H_ */
|
||||
13
robotis_math/include/robotis_math/RobotisMath.h
Normal file
13
robotis_math/include/robotis_math/RobotisMath.h
Normal file
@ -0,0 +1,13 @@
|
||||
/*
|
||||
* robotis_math.h
|
||||
*
|
||||
* Created on: Mar 18, 2016
|
||||
* Author: jay
|
||||
*/
|
||||
|
||||
#ifndef ROBOTIS_MATH_H_
|
||||
#define ROBOTIS_MATH_H_
|
||||
|
||||
#include "RobotisTrajectoryCalculator.h"
|
||||
|
||||
#endif /* ROBOTIS_MATH_H_ */
|
||||
33
robotis_math/include/robotis_math/RobotisMathBase.h
Normal file
33
robotis_math/include/robotis_math/RobotisMathBase.h
Normal file
@ -0,0 +1,33 @@
|
||||
/*
|
||||
* RobotisMathBase.h
|
||||
*
|
||||
* Created on: 2016. 3. 28.
|
||||
* Author: JaySong
|
||||
*/
|
||||
|
||||
#ifndef ROBOTIS_MATH_BASE_H_
|
||||
#define ROBOTIS_MATH_BASE_H_
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace ROBOTIS
|
||||
{
|
||||
|
||||
#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)
|
||||
|
||||
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 );
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* ROBOTIS_MATH_BASE_H_ */
|
||||
@ -0,0 +1,37 @@
|
||||
/*
|
||||
* RobotisTrajectoryCalculator.h
|
||||
*
|
||||
* Created on: Mar 18, 2016
|
||||
* Author: jay
|
||||
*/
|
||||
|
||||
#ifndef ROBOTIS_TRAJECTORY_CALCULATOR_H_
|
||||
#define ROBOTIS_TRAJECTORY_CALCULATOR_H_
|
||||
|
||||
|
||||
#include "RobotisMathBase.h"
|
||||
#include "RobotisLinearAlgebra.h"
|
||||
|
||||
// minimum jerk trajectory
|
||||
|
||||
namespace ROBOTIS
|
||||
{
|
||||
|
||||
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 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 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 );
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif /* ROBOTIS_TRAJECTORY_CALCULATOR_H_ */
|
||||
37
robotis_math/package.xml
Normal file
37
robotis_math/package.xml
Normal file
@ -0,0 +1,37 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>robotis_math</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The robotis_math package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="jay@todo.todo">jay</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/robotis_math</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintianers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>cmake_modules</build_depend>
|
||||
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>cmake_modules</run_depend>
|
||||
|
||||
</package>
|
||||
291
robotis_math/src/RobotisLinearAlgebra.cpp
Normal file
291
robotis_math/src/RobotisLinearAlgebra.cpp
Normal file
@ -0,0 +1,291 @@
|
||||
/*
|
||||
* RobotisLinearAlgebra.cpp
|
||||
*
|
||||
* Created on: Mar 18, 2016
|
||||
* Author: jay
|
||||
*/
|
||||
|
||||
|
||||
#include "robotis_math/RobotisLinearAlgebra.h"
|
||||
|
||||
namespace ROBOTIS
|
||||
{
|
||||
|
||||
Eigen::MatrixXd transitionXYZ ( double position_x, double position_y, double position_z )
|
||||
{
|
||||
Eigen::MatrixXd _position(3,1);
|
||||
|
||||
_position << position_x,
|
||||
position_y,
|
||||
position_z;
|
||||
|
||||
return _position;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd transformationXYZRPY ( 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 = rotation4d(roll, pitch, yaw);
|
||||
_transformation.coeffRef(0,3) = position_x;
|
||||
_transformation.coeffRef(1,3) = position_y;
|
||||
_transformation.coeffRef(2,3) = position_z;
|
||||
|
||||
return _transformation;
|
||||
}
|
||||
|
||||
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;
|
||||
Eigen::MatrixXd _invT(4,4);
|
||||
|
||||
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;
|
||||
|
||||
return _invT;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd inertiaXYZ( double ixx, double ixy, double ixz , double iyy , double iyz, double izz )
|
||||
{
|
||||
Eigen::MatrixXd _inertia(3,3);
|
||||
|
||||
_inertia << ixx , ixy , ixz,
|
||||
ixy , iyy , iyz,
|
||||
ixz , iyz , izz ;
|
||||
|
||||
return _inertia;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd rotationX( double angle )
|
||||
{
|
||||
Eigen::MatrixXd _rotation( 3 , 3 );
|
||||
|
||||
_rotation << 1.0, 0.0, 0.0,
|
||||
0.0, cos( angle ), -sin( angle ),
|
||||
0.0, sin( angle ), cos( angle );
|
||||
|
||||
return _rotation;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd rotationY( double angle )
|
||||
{
|
||||
Eigen::MatrixXd _rotation( 3 , 3 );
|
||||
|
||||
_rotation << cos( angle ), 0.0, sin( angle ),
|
||||
0.0, 1.0, 0.0,
|
||||
-sin( angle ), 0.0, cos( angle );
|
||||
|
||||
return _rotation;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd rotationZ( double angle )
|
||||
{
|
||||
Eigen::MatrixXd _rotation(3,3);
|
||||
|
||||
_rotation << cos( angle ), -sin( angle ), 0.0,
|
||||
sin( angle ), cos( angle ), 0.0,
|
||||
0.0, 0.0, 1.0;
|
||||
|
||||
return _rotation;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd rotation2rpy( Eigen::MatrixXd rotation )
|
||||
{
|
||||
Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 );
|
||||
|
||||
_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::MatrixXd rpy2rotation( double roll, double pitch, double yaw )
|
||||
{
|
||||
Eigen::MatrixXd _rotation = rotationZ( yaw ) * rotationY( pitch ) * rotationX( roll );
|
||||
|
||||
return _rotation;
|
||||
}
|
||||
|
||||
Eigen::Quaterniond rpy2quaternion( double roll, double pitch, double yaw )
|
||||
{
|
||||
Eigen::MatrixXd _rotation = rpy2rotation( roll, pitch, yaw );
|
||||
|
||||
Eigen::Matrix3d _rotation3d;
|
||||
_rotation3d = _rotation.block<3,3>( 0 , 0);
|
||||
|
||||
Eigen::Quaterniond _quaternion;
|
||||
|
||||
_quaternion = _rotation3d;
|
||||
|
||||
return _quaternion;
|
||||
}
|
||||
|
||||
Eigen::Quaterniond rotation2quaternion( Eigen::MatrixXd rotation )
|
||||
{
|
||||
Eigen::Matrix3d _rotation3d;
|
||||
|
||||
_rotation3d = rotation.block<3,3>( 0 , 0);
|
||||
|
||||
Eigen::Quaterniond _quaternion;
|
||||
_quaternion = _rotation3d;
|
||||
|
||||
return _quaternion;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd quaternion2rpy( Eigen::Quaterniond quaternion )
|
||||
{
|
||||
Eigen::MatrixXd _rpy = rotation2rpy( quaternion.toRotationMatrix() );
|
||||
|
||||
return _rpy;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd quaternion2rotation( Eigen::Quaterniond quaternion )
|
||||
{
|
||||
Eigen::MatrixXd _rotation = quaternion.toRotationMatrix();
|
||||
|
||||
return _rotation;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd rotation4d( double roll, double pitch, double yaw )
|
||||
{
|
||||
// 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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Eigen::MatrixXd hatto( Eigen::MatrixXd matrix3d )
|
||||
{
|
||||
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;
|
||||
|
||||
return _hatto;
|
||||
}
|
||||
|
||||
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 ) );
|
||||
|
||||
return _Rodrigues;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd rot2omega( Eigen::MatrixXd rotation )
|
||||
{
|
||||
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_plus = fabs( _alpha - 1.0 );
|
||||
|
||||
Eigen::MatrixXd _rot2omega( 3 , 1 );
|
||||
|
||||
if( _alpha_plus < _eps )
|
||||
{
|
||||
_rot2omega << 0.0,
|
||||
0.0,
|
||||
0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
double _theta = acos( _alpha );
|
||||
|
||||
_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;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd cross(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b )
|
||||
{
|
||||
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 );
|
||||
|
||||
return _cross;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
26
robotis_math/src/RobotisMathBase.cpp
Normal file
26
robotis_math/src/RobotisMathBase.cpp
Normal file
@ -0,0 +1,26 @@
|
||||
/*
|
||||
* RobotisMathBase.cpp
|
||||
*
|
||||
* Created on: Mar 18, 2016
|
||||
* Author: jay
|
||||
*/
|
||||
|
||||
#include "robotis_math/RobotisMathBase.h"
|
||||
|
||||
|
||||
|
||||
|
||||
namespace ROBOTIS
|
||||
{
|
||||
|
||||
double sign( double x )
|
||||
{
|
||||
if ( x < 0.0 )
|
||||
return -1.0;
|
||||
else if ( x > 0.0)
|
||||
return 1.0;
|
||||
else
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
}
|
||||
325
robotis_math/src/RobotisTrajectoryCalculator.cpp
Normal file
325
robotis_math/src/RobotisTrajectoryCalculator.cpp
Normal file
@ -0,0 +1,325 @@
|
||||
/*
|
||||
* RobotisTrajectoryCalculator.cpp
|
||||
*
|
||||
* Created on: Mar 18, 2016
|
||||
* Author: jay
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#include "robotis_math/RobotisTrajectoryCalculator.h"
|
||||
|
||||
|
||||
/*
|
||||
* trajectory.cpp
|
||||
*
|
||||
* Created on: Jul 13, 2015
|
||||
* Author: sch
|
||||
*/
|
||||
|
||||
|
||||
|
||||
namespace ROBOTIS
|
||||
{
|
||||
|
||||
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 )
|
||||
/*
|
||||
simple minimum jerk trajectory
|
||||
|
||||
pos_start : position at initial state
|
||||
vel_start : velocity at initial state
|
||||
accel_start : acceleration at initial state
|
||||
|
||||
pos_end : position at final state
|
||||
vel_end : velocity at final state
|
||||
accel_end : acceleration at final state
|
||||
|
||||
smp_time : sampling time
|
||||
|
||||
mov_time : movement time
|
||||
*/
|
||||
|
||||
{
|
||||
Eigen::MatrixXd ___C( 3 , 3 );
|
||||
Eigen::MatrixXd __C( 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 );
|
||||
|
||||
__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 ;
|
||||
|
||||
Eigen::Matrix<double,3,1> _A = ___C.inverse() * __C;
|
||||
|
||||
double _time_steps;
|
||||
|
||||
_time_steps = mov_time / smp_time;
|
||||
int __time_steps = round( _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 < __time_steps; step++ )
|
||||
_time.coeffRef( step , 0 ) = step * smp_time;
|
||||
|
||||
for ( int step = 0; step < __time_steps; step++ )
|
||||
{
|
||||
_tra.coeffRef( step , 0 ) =
|
||||
pos_start +
|
||||
vel_start * _time.coeff( step , 0 ) +
|
||||
0.5 * accel_start * pow( _time.coeff( step , 0 ) , 2 ) +
|
||||
_A.coeff( 0 , 0 ) * pow( _time.coeff( step , 0 ) , 3 ) +
|
||||
_A.coeff( 1 , 0 ) * pow( _time.coeff( step , 0 ) , 4 ) +
|
||||
_A.coeff( 2 , 0 ) * pow( _time.coeff( step , 0 ) , 5 );
|
||||
}
|
||||
|
||||
return _tra;
|
||||
}
|
||||
|
||||
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 )
|
||||
/*
|
||||
minimum jerk trajectory with via-points
|
||||
(via-point constraints: position and velocity at each point)
|
||||
|
||||
n : the number of via-points
|
||||
|
||||
x0 : position at initial state
|
||||
v0 : velocity at initial state
|
||||
a0 : acceleration at initial state
|
||||
|
||||
x : position matrix at via-points state ( size : n x 1 )
|
||||
dx : velocity matrix at via-points state ( size : n x 1 )
|
||||
ddx : acceleration matrix at via-points state ( size : n x 1 )
|
||||
|
||||
xf : position at final state
|
||||
vf : velocity at final state
|
||||
af : acceleration at final state
|
||||
|
||||
smp : sampling time
|
||||
|
||||
t : time matrix passing through via-points state ( size : n x 1 )
|
||||
tf : movement time
|
||||
*/
|
||||
|
||||
{
|
||||
int i,j,k ;
|
||||
|
||||
/* Calculation Matrix B */
|
||||
|
||||
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3*via_num+3,1);
|
||||
|
||||
for (i=1; i<=via_num; i++){
|
||||
B.coeffRef(3*i-3,0) =
|
||||
pos_via.coeff(i-1,0) -
|
||||
pos_start -
|
||||
vel_start*via_time.coeff(i-1,0) -
|
||||
(accel_start/2)*pow(via_time.coeff(i-1,0),2) ;
|
||||
|
||||
B.coeffRef(3*i-2,0) =
|
||||
vel_via.coeff(i-1,0) -
|
||||
vel_start -
|
||||
accel_start*via_time.coeff(i-1,0) ;
|
||||
|
||||
B.coeffRef(3*i-1,0) =
|
||||
accel_via.coeff(i-1,0) -
|
||||
accel_start ;
|
||||
}
|
||||
|
||||
B.coeffRef(3*via_num,0) =
|
||||
pos_end -
|
||||
pos_start -
|
||||
vel_start*mov_time -
|
||||
(accel_start/2)*pow(mov_time,2) ;
|
||||
|
||||
B.coeffRef(3*via_num+1,0) =
|
||||
vel_end -
|
||||
vel_start -
|
||||
accel_start*mov_time ;
|
||||
|
||||
B.coeffRef(3*via_num+2,0) =
|
||||
accel_end -
|
||||
accel_start ;
|
||||
|
||||
|
||||
/* Calculation Matrix A */
|
||||
|
||||
Eigen::MatrixXd A1 = Eigen::MatrixXd::Zero(3*via_num,3);
|
||||
|
||||
for (i=1; i<=via_num; i++){
|
||||
A1.coeffRef(3*i-3,0) = pow(via_time.coeff(i-1,0),3) ;
|
||||
A1.coeffRef(3*i-3,1) = pow(via_time.coeff(i-1,0),4) ;
|
||||
A1.coeffRef(3*i-3,2) = pow(via_time.coeff(i-1,0),5) ;
|
||||
|
||||
A1.coeffRef(3*i-2,0) = 3*pow(via_time.coeff(i-1,0),2) ;
|
||||
A1.coeffRef(3*i-2,1) = 4*pow(via_time.coeff(i-1,0),3) ;
|
||||
A1.coeffRef(3*i-2,2) = 5*pow(via_time.coeff(i-1,0),4) ;
|
||||
|
||||
A1.coeffRef(3*i-1,0) = 6*via_time.coeff(i-1,0) ;
|
||||
A1.coeffRef(3*i-1,1) = 12*pow(via_time.coeff(i-1,0),2) ;
|
||||
A1.coeffRef(3*i-1,2) = 20*pow(via_time.coeff(i-1,0),3) ;
|
||||
}
|
||||
|
||||
|
||||
Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num);
|
||||
|
||||
for (i=1; i<=via_num; i++){
|
||||
for (j=1; j<=via_num; j++){
|
||||
if (i > 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++ )
|
||||
{
|
||||
double _t = ( ( double ) i ) * smp_time ;
|
||||
|
||||
double _th = _angle_tra.coeff( i , 0 );//( _t / mov_time ) * rotation_angle;
|
||||
|
||||
Eigen::MatrixXd _w_wedge( 3 , 3 );
|
||||
|
||||
_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 ;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
Eigen::MatrixXd _pt_trans = _pt.transpose();
|
||||
|
||||
return _pt_trans;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user