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

Remove robotis_math

Browse Source 
robotis_math is moved to ROBOTIS_Math repository
This commit is contained in:
Jay-Song
2016-08-11 20:07:58 +09:00
parent 47afdecd3e
commit f1f9c88886
10 changed files with 0 additions and 1508 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
robotis_math/CMakeLists.txt
View File

@@ -1,42 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(robotis_math)
find_package(catkin REQUIRED COMPONENTS
roscpp
cmake_modules
)
find_package(Eigen REQUIRED)
###################################
## catkin specific configuration ##
###################################
catkin_package(
INCLUDE_DIRS include
LIBRARIES robotis_math
CATKIN_DEPENDS roscpp
# DEPENDS system_lib
)
###########
## Build ##
###########
include_directories(
include
${catkin_INCLUDE_DIRS}
${Eigen_INCLUDE_DIRS}
)
add_library(robotis_math
src/robotis_math_base.cpp
src/robotis_trajectory_calculator.cpp
src/robotis_linear_algebra.cpp
)
add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(robotis_math
${catkin_LIBRARIES}
)

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

@@ -1,81 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* robotis_linear_algebra.h
*
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_
#define ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_
#include <cmath>
#define EIGEN_NO_DEBUG
#define EIGEN_NO_STATIC_ASSERT
#include <Eigen/Dense>
#include "step_data_define.h"
namespace robotis_framework
{
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 getTranslation4D(double position_x, double position_y, double position_z);
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 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);
Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform);
}
#endif /* ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ */

43
robotis_math/include/robotis_math/robotis_math.h
View File

@@ -1,43 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* robotis_math.h
*
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_MATH_ROBOTIS_MATH_H_
#define ROBOTIS_MATH_ROBOTIS_MATH_H_
#include "robotis_trajectory_calculator.h"
#endif /* ROBOTIS_MATH_ROBOTIS_MATH_H_ */

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

@@ -1,63 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* robotis_math_base.h
*
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_
#define ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_
#include <cmath>
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 DEGREE2RADIAN (M_PI / 180.0)
#define RADIAN2DEGREE (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_ROBOTIS_MATH_BASE_H_ */

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

@@ -1,113 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* robotis_trajectory_calculator.h
*
* Created on: June 7, 2016
* Author: sch
*/
#ifndef ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_
#define ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_
#include "robotis_linear_algebra.h"
#include "robotis_math_base.h"
// minimum jerk trajectory
namespace robotis_framework
{
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 calcMinimumJerkTraPlus(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 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 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);
class FifthOrderPolynomialTrajectory
{
public:
FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc,
double final_time, double final_pos, double final_vel, double final_acc);
FifthOrderPolynomialTrajectory();
~FifthOrderPolynomialTrajectory();
bool changeTrajectory(double final_pos, double final_vel, double final_acc);
bool changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc);
bool changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc,
double final_time, double final_pos, double final_vel, double final_acc);
double getPosition(double time);
double getVelocity(double time);
double getAcceleration(double time);
void setTime(double time);
double getPosition();
double getVelocity();
double getAcceleration();
double initial_time_;
double initial_pos_;
double initial_vel_;
double initial_acc_;
double current_time_;
double current_pos_;
double current_vel_;
double current_acc_;
double final_time_;
double final_pos_;
double final_vel_;
double final_acc_;
Eigen::MatrixXd position_coeff_;
Eigen::MatrixXd velocity_coeff_;
Eigen::MatrixXd acceleration_coeff_;
Eigen::MatrixXd time_variables_;
};
}
#endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */

83
robotis_math/include/robotis_math/step_data_define.h
View File

@@ -1,83 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* step_data_define.h
*
* Created on: 2016. 8. 10.
* Author: Jay Song
*/
#ifndef ROBOTIS_MATH_STEP_DATA_DEFINE_H_
#define ROBOTIS_MATH_STEP_DATA_DEFINE_H_
namespace robotis_framework
{
typedef struct
{
double x, y, z;
} Position3D;
typedef struct
{
double x, y, z, roll, pitch, yaw;
} Pose3D;
typedef struct
{
int moving_foot;
double foot_z_swap, body_z_swap;
double shoulder_swing_gain, elbow_swing_gain;
double waist_roll_angle, waist_pitch_angle, waist_yaw_angle;
Pose3D left_foot_pose;
Pose3D right_foot_pose;
Pose3D body_pose;
} StepPositionData;
typedef struct
{
int walking_state;
double abs_step_time, dsp_ratio;
double start_time_delay_ratio_x, start_time_delay_ratio_y, start_time_delay_ratio_z;
double start_time_delay_ratio_roll, start_time_delay_ratio_pitch, start_time_delay_ratio_yaw;
double finish_time_advance_ratio_x, finish_time_advance_ratio_y, finish_time_advance_ratio_z;
double finish_time_advance_ratio_roll, finish_time_advance_ratio_pitch, finish_time_advance_ratio_yaw;
} StepTimeData;
typedef struct
{
StepPositionData position_data;
StepTimeData time_data;
} StepData;
}
#endif /* ROBOTIS_MATH_STEP_DATA_DEFINE_H_ */

19
robotis_math/package.xml
View File

@@ -1,19 +0,0 @@
<?xml version="1.0"?>
<package>
<name>robotis_math</name>
<version>0.0.0</version>
<description>The robotis_math package</description>
<maintainer email="sch@robotis.com">sch</maintainer>
<license>BSD</license>
<author email="sch@robotis.com">sch</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>

322
robotis_math/src/robotis_linear_algebra.cpp
View File

@@ -1,322 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* robotis_linear_algebra.cpp
*
* Created on: June 7, 2016
* Author: sch
*/
#include "robotis_math/robotis_linear_algebra.h"
namespace robotis_framework
{
Eigen::MatrixXd getTransitionXYZ(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 getTransformationXYZRPY(double position_x, double position_y, double position_z , double roll , double pitch , double yaw)
{
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;
return transformation;
}
Eigen::MatrixXd getInverseTransformation(Eigen::MatrixXd transform)
{
// If T is Transform Matrix A from B, the BOA is translation component coordi. B to coordi. A
Eigen::Vector3d vec_boa;
Eigen::Vector3d vec_x, vec_y, vec_z;
Eigen::MatrixXd inv_t(4,4);
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 getInertiaXYZ(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 getRotationX(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 getRotationY(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 getRotationZ(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 getRotation4d(double roll, double pitch, double yaw )
{
double sr = sin(roll), cr = cos(roll);
double sp = sin(pitch), cp = cos(pitch);
double sy = sin(yaw), cy = cos(yaw);
Eigen::MatrixXd mat_roll(4,4);
Eigen::MatrixXd mat_pitch(4,4);
Eigen::MatrixXd mat_yaw(4,4);
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 getTranslation4D(double position_x, double position_y, double position_z)
{
Eigen::MatrixXd mat_translation(4,4);
mat_translation <<
1, 0, 0, position_x,
0, 1, 0, position_y,
0, 0, 1, position_z,
0, 0, 0, 1;
return mat_translation;
}
Eigen::MatrixXd convertRotationToRPY(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 convertRPYToRotation(double roll, double pitch, double yaw)
{
Eigen::MatrixXd rotation = getRotationZ(yaw)*getRotationY(pitch)*getRotationX(roll);
return rotation;
}
Eigen::Quaterniond convertRPYToQuaternion(double roll, double pitch, double yaw)
{
Eigen::MatrixXd rotation = convertRPYToRotation(roll,pitch,yaw);
Eigen::Matrix3d rotation3d;
rotation3d = rotation.block<3,3>(0,0);
Eigen::Quaterniond quaternion;
quaternion = rotation3d;
return quaternion;
}
Eigen::Quaterniond convertRotationToQuaternion(Eigen::MatrixXd rotation)
{
Eigen::Matrix3d rotation3d;
rotation3d = rotation.block<3,3>(0,0);
Eigen::Quaterniond quaternion;
quaternion = rotation3d;
return quaternion;
}
Eigen::MatrixXd convertQuaternionToRPY(Eigen::Quaterniond quaternion)
{
Eigen::MatrixXd rpy = convertRotationToRPY(quaternion.toRotationMatrix());
return rpy;
}
Eigen::MatrixXd convertQuaternionToRotation(Eigen::Quaterniond quaternion)
{
Eigen::MatrixXd rotation = quaternion.toRotationMatrix();
return rotation;
}
Eigen::MatrixXd calcHatto(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 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));
return rodrigues;
}
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 rot_to_omega(3,1);
if( alpha_dash < eps )
{
rot_to_omega <<
0.0,
0.0,
0.0;
}
else
{
double theta = acos(alpha);
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);
rot_to_omega = 0.5*(theta/sin(theta))*rot_to_omega;
}
return rot_to_omega;
}
Eigen::MatrixXd calcCross(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 calcInner(Eigen::MatrixXd vector3d_a, Eigen::MatrixXd vector3d_b)
{
return vector3d_a.dot(vector3d_b);
}
Pose3D getPose3DfromTransformMatrix(Eigen::MatrixXd transform)
{
Pose3D pose_3d;
pose_3d.x = transform.coeff(0, 3);
pose_3d.y = transform.coeff(1, 3);
pose_3d.z = transform.coeff(2, 3);
pose_3d.roll = atan2( transform.coeff(2,1), transform.coeff(2,2));
pose_3d.pitch = atan2(-transform.coeff(2,0), sqrt(transform.coeff(2,1)*transform.coeff(2,1) + transform.coeff(2,2)*transform.coeff(2,2)) );
pose_3d.yaw = atan2( transform.coeff(1,0), transform.coeff(0,0));
return pose_3d;
}
}

53
robotis_math/src/robotis_math_base.cpp
View File

@@ -1,53 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* robotis_math_base.cpp
*
* Created on: June 7, 2016
* Author: sch
*/
#include "robotis_math/robotis_math_base.h"
namespace robotis_framework
{
double sign(double x)
{
if ( x < 0.0 )
return -1.0;
else if ( x > 0.0)
return 1.0;
else
return 0.0;
}
}

689
robotis_math/src/robotis_trajectory_calculator.cpp
View File

@@ -1,689 +0,0 @@
/*******************************************************************************
* Copyright (c) 2016, ROBOTIS CO., LTD.
* All rights reserved.
*
* 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.
*******************************************************************************/
/*
* robotis_trajectory_calculator.cpp
*
* Created on: June 7, 2016
* Author: sch
*/
#include "robotis_math/robotis_trajectory_calculator.h"
namespace robotis_framework
{
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
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 poly_matrix(3,3);
Eigen::MatrixXd poly_vector(3,1);
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);
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<double,3,1> poly_coeff = poly_matrix.inverse() * poly_vector;
double time_steps = mov_time/smp_time;
int all_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);
for (int step=0; step<all_time_steps; step++)
time.coeffRef(step,0) = step*smp_time;
for (int step=0; step<all_time_steps; step++)
{
minimum_jerk_tra.coeffRef( step , 0 ) =
pos_start +
vel_start*time.coeff(step,0) +
0.5*accel_start*pow(time.coeff(step,0),2) +
poly_coeff.coeff(0,0)*pow(time.coeff(step,0),3) +
poly_coeff.coeff(1,0)*pow(time.coeff(step,0),4) +
poly_coeff.coeff(2,0)*pow(time.coeff(step,0),5);
}
return minimum_jerk_tra;
}
Eigen::MatrixXd calcMinimumJerkTraPlus(
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 poly_matrix(3,3);
Eigen::MatrixXd poly_vector(3,1);
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);
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<double,3,1> poly_coeff = poly_matrix.inverse() * poly_vector;
double time_steps = mov_time/smp_time;
int all_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,3);
for (int step=0; step<all_time_steps; step++)
time.coeffRef(step,0) = step*smp_time;
for (int step=0; step<all_time_steps; step++)
{
minimum_jerk_tra.coeffRef( step , 0 ) =
pos_start +
vel_start*time.coeff(step,0) +
0.5*accel_start*pow(time.coeff(step,0),2) +
poly_coeff.coeff(0,0)*pow(time.coeff(step,0),3) +
poly_coeff.coeff(1,0)*pow(time.coeff(step,0),4) +
poly_coeff.coeff(2,0)*pow(time.coeff(step,0),5);
minimum_jerk_tra.coeffRef( step , 1 ) =
vel_start +
accel_start*time.coeff(step,0) +
3*poly_coeff.coeff(0,0)*pow(time.coeff(step,0),2) +
4*poly_coeff.coeff(1,0)*pow(time.coeff(step,0),3) +
5*poly_coeff.coeff(2,0)*pow(time.coeff(step,0),4);
minimum_jerk_tra.coeffRef( step , 2 ) =
accel_start +
6*poly_coeff.coeff(0,0)*time.coeff(step,0) +
12*poly_coeff.coeff(1,0)*pow(time.coeff(step,0),2) +
20*poly_coeff.coeff(2,0)*pow(time.coeff(step,0),3);
}
return minimum_jerk_tra;
}
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 )
/*
minimum jerk trajectory with via-points
(via-point constraints: position and velocity at each point)
via_num : the number of via-points
pos_start : position at initial state
vel_start : velocity at initial state
accel_start : acceleration at initial state
pos_via : position matrix at via-points state ( size : n x 1 )
vel_via : velocity matrix at via-points state ( size : n x 1 )
accel_via : acceleration matrix at via-points state ( size : n x 1 )
pos_end : position at final state
vel_end : velocity at final state
accel_end : acceleration at final state
smp_time : sampling time
via_time : time matrix passing through via-points state ( size : n x 1 )
via_time : movement time
*/
{
// int i,j,k ;
Eigen::MatrixXd poly_vector = Eigen::MatrixXd::Zero(3*via_num+3,1);
for (int i=1; i<=via_num; i++)
{
poly_vector.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) ;
poly_vector.coeffRef(3*i-2,0) =
vel_via.coeff(i-1,0) -
vel_start -
accel_start*via_time.coeff(i-1,0) ;
poly_vector.coeffRef(3*i-1,0) =
accel_via.coeff(i-1,0) -
accel_start;
}
poly_vector.coeffRef(3*via_num,0) =
pos_end -
pos_start -
vel_start*mov_time -
(accel_start/2)*pow(mov_time,2) ;
poly_vector.coeffRef(3*via_num+1,0) =
vel_end -
vel_start -
accel_start*mov_time ;
poly_vector.coeffRef(3*via_num+2,0) =
accel_end -
accel_start ;
Eigen::MatrixXd poly_matrix_part1 = Eigen::MatrixXd::Zero(3*via_num,3);
for (int i=1; i<=via_num; i++)
{
poly_matrix_part1.coeffRef(3*i-3,0) = pow(via_time.coeff(i-1,0),3) ;
poly_matrix_part1.coeffRef(3*i-3,1) = pow(via_time.coeff(i-1,0),4) ;
poly_matrix_part1.coeffRef(3*i-3,2) = pow(via_time.coeff(i-1,0),5) ;
poly_matrix_part1.coeffRef(3*i-2,0) = 3*pow(via_time.coeff(i-1,0),2) ;
poly_matrix_part1.coeffRef(3*i-2,1) = 4*pow(via_time.coeff(i-1,0),3) ;
poly_matrix_part1.coeffRef(3*i-2,2) = 5*pow(via_time.coeff(i-1,0),4) ;
poly_matrix_part1.coeffRef(3*i-1,0) = 6*via_time.coeff(i-1,0) ;
poly_matrix_part1.coeffRef(3*i-1,1) = 12*pow(via_time.coeff(i-1,0),2) ;
poly_matrix_part1.coeffRef(3*i-1,2) = 20*pow(via_time.coeff(i-1,0),3) ;
}
Eigen::MatrixXd poly_matrix_part2 = Eigen::MatrixXd::Zero(3*via_num,3*via_num);
for (int i=1; i<=via_num; i++)
{
for (int j=1; j<=via_num; j++)
{
int k;
if (i > j)
k = i ;
else
k = j ;
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 ;
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 ;
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 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;
}
FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc,
double final_time, double final_pos, double final_vel, double final_acc)
{
position_coeff_.resize(6, 1);
velocity_coeff_.resize(6, 1);
acceleration_coeff_.resize(6, 1);
time_variables_.resize(1, 6);
position_coeff_.fill(0);
velocity_coeff_.fill(0);
acceleration_coeff_.fill(0);
time_variables_.fill(0);
if(final_time > initial_time)
{
initial_time_ = initial_time;
initial_pos_ = initial_pos;
initial_vel_ = initial_vel;
initial_acc_ = initial_acc;
current_time_ = initial_time;
current_pos_ = initial_pos;
current_vel_ = initial_vel;
current_acc_ = initial_acc;
final_time_ = final_time;
final_pos_ = final_pos;
final_vel_ = final_vel;
final_acc_ = final_acc;
Eigen::MatrixXd time_mat;
Eigen::MatrixXd conditions_mat;
time_mat.resize(6,6);
time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0,
5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0,
20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0;
powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0,
5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0,
20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0;
conditions_mat.resize(6,1);
conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_;
position_coeff_ = time_mat.inverse() * conditions_mat;
velocity_coeff_ << 0.0,
5.0*position_coeff_.coeff(0,0),
4.0*position_coeff_.coeff(0,1),
3.0*position_coeff_.coeff(0,2),
2.0*position_coeff_.coeff(0,3),
1.0*position_coeff_.coeff(0,4);
acceleration_coeff_ << 0.0,
0.0,
20.0*position_coeff_.coeff(0,0),
12.0*position_coeff_.coeff(0,1),
6.0*position_coeff_.coeff(0,2),
2.0*position_coeff_.coeff(0,3);
}
}
FifthOrderPolynomialTrajectory::FifthOrderPolynomialTrajectory()
{
initial_time_ = 0;
initial_pos_ = 0;
initial_vel_ = 0;
initial_acc_ = 0;
current_time_ = 0;
current_pos_ = 0;
current_vel_ = 0;
current_acc_ = 0;
final_time_ = 0;
final_pos_ = 0;
final_vel_ = 0;
final_acc_ = 0;
position_coeff_.resize(6, 1);
velocity_coeff_.resize(6, 1);
acceleration_coeff_.resize(6, 1);
time_variables_.resize(1, 6);
position_coeff_.fill(0);
velocity_coeff_.fill(0);
acceleration_coeff_.fill(0);
time_variables_.fill(0);
}
FifthOrderPolynomialTrajectory::~FifthOrderPolynomialTrajectory()
{
}
bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_pos, double final_vel, double final_acc)
{
final_pos_ = final_pos;
final_vel_ = final_vel;
final_acc_ = final_acc;
Eigen::MatrixXd time_mat;
Eigen::MatrixXd conditions_mat;
time_mat.resize(6,6);
time_mat << powDI(initial_time_, 5), powDI(initial_time_, 4), powDI(initial_time_, 3), powDI(initial_time_, 2), initial_time_, 1.0,
5.0*powDI(initial_time_, 4), 4.0*powDI(initial_time_, 3), 3.0*powDI(initial_time_, 2), 2.0*initial_time_, 1.0, 0.0,
20.0*powDI(initial_time_, 3), 12.0*powDI(initial_time_, 2), 6.0*initial_time_, 2.0, 0.0, 0.0,
powDI(final_time_, 5), powDI(final_time_, 4), powDI(final_time_, 3), powDI(final_time_, 2), final_time_, 1.0,
5.0*powDI(final_time_, 4), 4.0*powDI(final_time_, 3), 3.0*powDI(final_time_, 2), 2.0*final_time_, 1.0, 0.0,
20.0*powDI(final_time_, 3), 12.0*powDI(final_time_, 2), 6.0*final_time_, 2.0, 0.0, 0.0;
conditions_mat.resize(6,1);
conditions_mat << initial_pos_, initial_vel_, initial_acc_, final_pos_, final_vel_, final_acc_;
position_coeff_ = time_mat.inverse() * conditions_mat;
velocity_coeff_ << 0.0,
5.0*position_coeff_.coeff(0,0),
4.0*position_coeff_.coeff(0,1),
3.0*position_coeff_.coeff(0,2),
2.0*position_coeff_.coeff(0,3),
1.0*position_coeff_.coeff(0,4);
acceleration_coeff_ << 0.0,
0.0,
20.0*position_coeff_.coeff(0,0),
12.0*position_coeff_.coeff(0,1),
6.0*position_coeff_.coeff(0,2),
2.0*position_coeff_.coeff(0,3);
return true;
}
bool FifthOrderPolynomialTrajectory::changeTrajectory(double final_time, double final_pos, double final_vel, double final_acc)
{
if(final_time < initial_time_)
return false;
final_time_ = final_time;
return changeTrajectory(final_pos, final_vel, final_acc);
}
bool FifthOrderPolynomialTrajectory::changeTrajectory(double initial_time, double initial_pos, double initial_vel, double initial_acc,
double final_time, double final_pos, double final_vel, double final_acc)
{
if(final_time < initial_time)
return false;
initial_time_ = initial_time;
initial_pos_ = initial_pos;
initial_vel_ = initial_vel;
initial_acc_ = initial_acc;
final_time_ = final_time;
return changeTrajectory(final_pos, final_vel, final_acc);
}
double FifthOrderPolynomialTrajectory::getPosition(double time)
{
if(time >= final_time_)
{
current_time_ = final_time_;
current_pos_ = final_pos_;
current_vel_ = final_vel_;
current_acc_ = final_acc_;
return final_pos_;
}
else if(time <= initial_time_ )
{
current_time_ = initial_time_;
current_pos_ = initial_pos_;
current_vel_ = initial_vel_;
current_acc_ = initial_acc_;
return initial_pos_;
}
else
{
current_time_ = time;
time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0;
current_pos_ = (time_variables_ * position_coeff_).coeff(0,0);
current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0);
current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0);
return current_pos_;
}
}
double FifthOrderPolynomialTrajectory::getVelocity(double time)
{
if(time >= final_time_)
{
current_time_ = final_time_;
current_pos_ = final_pos_;
current_vel_ = final_vel_;
current_acc_ = final_acc_;
return final_vel_;
}
else if(time <= initial_time_ )
{
current_time_ = initial_time_;
current_pos_ = initial_pos_;
current_vel_ = initial_vel_;
current_acc_ = initial_acc_;
return initial_vel_;
}
else
{
current_time_ = time;
time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0;
current_pos_ = (time_variables_ * position_coeff_).coeff(0,0);
current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0);
current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0);
return current_vel_;
}
}
double FifthOrderPolynomialTrajectory::getAcceleration(double time)
{
if(time >= final_time_)
{
current_time_ = final_time_;
current_pos_ = final_pos_;
current_vel_ = final_vel_;
current_acc_ = final_acc_;
return final_acc_;
}
else if(time <= initial_time_ )
{
current_time_ = initial_time_;
current_pos_ = initial_pos_;
current_vel_ = initial_vel_;
current_acc_ = initial_acc_;
return initial_acc_;
}
else
{
current_time_ = time;
time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0;
current_pos_ = (time_variables_ * position_coeff_).coeff(0,0);
current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0);
current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0);
return current_acc_;
}
}
void FifthOrderPolynomialTrajectory::setTime(double time)
{
if(time >= final_time_)
{
current_time_ = final_time_;
current_pos_ = final_pos_;
current_vel_ = final_vel_;
current_acc_ = final_acc_;
}
else if(time <= initial_time_ )
{
current_time_ = initial_time_;
current_pos_ = initial_pos_;
current_vel_ = initial_vel_;
current_acc_ = initial_acc_;
}
else
{
current_time_ = time;
time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0;
current_pos_ = (time_variables_ * position_coeff_).coeff(0,0);
current_vel_ = (time_variables_ * velocity_coeff_).coeff(0,0);
current_acc_ = (time_variables_ * acceleration_coeff_).coeff(0,0);
}
}
double FifthOrderPolynomialTrajectory::getPosition()
{
return current_pos_;
}
double FifthOrderPolynomialTrajectory::getVelocity()
{
return current_vel_;
}
double FifthOrderPolynomialTrajectory::getAcceleration()
{
return current_acc_;
}
}