From f1f9c888867f58ef0f48f3671730e1e77e662a79 Mon Sep 17 00:00:00 2001 From: Jay-Song Date: Thu, 11 Aug 2016 20:07:58 +0900 Subject: [PATCH] Remove robotis_math robotis_math is moved to ROBOTIS_Math repository --- robotis_math/CMakeLists.txt | 42 -- .../robotis_math/robotis_linear_algebra.h | 81 -- .../include/robotis_math/robotis_math.h | 43 -- .../include/robotis_math/robotis_math_base.h | 63 -- .../robotis_trajectory_calculator.h | 113 --- .../include/robotis_math/step_data_define.h | 83 --- robotis_math/package.xml | 19 - robotis_math/src/robotis_linear_algebra.cpp | 322 -------- robotis_math/src/robotis_math_base.cpp | 53 -- .../src/robotis_trajectory_calculator.cpp | 689 ------------------ 10 files changed, 1508 deletions(-) delete mode 100644 robotis_math/CMakeLists.txt delete mode 100644 robotis_math/include/robotis_math/robotis_linear_algebra.h delete mode 100644 robotis_math/include/robotis_math/robotis_math.h delete mode 100644 robotis_math/include/robotis_math/robotis_math_base.h delete mode 100644 robotis_math/include/robotis_math/robotis_trajectory_calculator.h delete mode 100644 robotis_math/include/robotis_math/step_data_define.h delete mode 100644 robotis_math/package.xml delete mode 100644 robotis_math/src/robotis_linear_algebra.cpp delete mode 100644 robotis_math/src/robotis_math_base.cpp delete mode 100644 robotis_math/src/robotis_trajectory_calculator.cpp diff --git a/robotis_math/CMakeLists.txt b/robotis_math/CMakeLists.txt deleted file mode 100644 index 7ce8d9a..0000000 --- a/robotis_math/CMakeLists.txt +++ /dev/null @@ -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} -) diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h deleted file mode 100644 index f8cd69e..0000000 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ /dev/null @@ -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 - -#define EIGEN_NO_DEBUG -#define EIGEN_NO_STATIC_ASSERT - -#include -#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_ */ diff --git a/robotis_math/include/robotis_math/robotis_math.h b/robotis_math/include/robotis_math/robotis_math.h deleted file mode 100644 index f78f4be..0000000 --- a/robotis_math/include/robotis_math/robotis_math.h +++ /dev/null @@ -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_ */ diff --git a/robotis_math/include/robotis_math/robotis_math_base.h b/robotis_math/include/robotis_math/robotis_math_base.h deleted file mode 100644 index e54caca..0000000 --- a/robotis_math/include/robotis_math/robotis_math_base.h +++ /dev/null @@ -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 - -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_ */ diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h deleted file mode 100644 index 670264f..0000000 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ /dev/null @@ -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_ */ diff --git a/robotis_math/include/robotis_math/step_data_define.h b/robotis_math/include/robotis_math/step_data_define.h deleted file mode 100644 index 135d445..0000000 --- a/robotis_math/include/robotis_math/step_data_define.h +++ /dev/null @@ -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_ */ diff --git a/robotis_math/package.xml b/robotis_math/package.xml deleted file mode 100644 index 29d618b..0000000 --- a/robotis_math/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - robotis_math - 0.0.0 - The robotis_math package - - sch - BSD - - sch - - catkin - roscpp - cmake_modules - - roscpp - cmake_modules - - \ No newline at end of file diff --git a/robotis_math/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp deleted file mode 100644 index 293fef3..0000000 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ /dev/null @@ -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; -} - -} diff --git a/robotis_math/src/robotis_math_base.cpp b/robotis_math/src/robotis_math_base.cpp deleted file mode 100644 index bd37d51..0000000 --- a/robotis_math/src/robotis_math_base.cpp +++ /dev/null @@ -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; -} - -} diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp deleted file mode 100644 index c436310..0000000 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ /dev/null @@ -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 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 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 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_; -} - -}