diff --git a/robotis_math/include/robotis_math/robotis_linear_algebra.h b/robotis_math/include/robotis_math/robotis_linear_algebra.h index 9c62268..f8cd69e 100644 --- a/robotis_math/include/robotis_math/robotis_linear_algebra.h +++ b/robotis_math/include/robotis_math/robotis_linear_algebra.h @@ -44,6 +44,7 @@ #define EIGEN_NO_STATIC_ASSERT #include +#include "step_data_define.h" namespace robotis_framework { @@ -71,6 +72,8 @@ 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); + } diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h index 56bf644..09eaa36 100644 --- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h +++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h @@ -60,6 +60,50 @@ 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 new file mode 100644 index 0000000..135d445 --- /dev/null +++ b/robotis_math/include/robotis_math/step_data_define.h @@ -0,0 +1,83 @@ +/******************************************************************************* + * 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/src/robotis_linear_algebra.cpp b/robotis_math/src/robotis_linear_algebra.cpp index 7683810..293fef3 100644 --- a/robotis_math/src/robotis_linear_algebra.cpp +++ b/robotis_math/src/robotis_linear_algebra.cpp @@ -305,4 +305,18 @@ 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_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp index 5f66ffc..9c8554d 100644 --- a/robotis_math/src/robotis_trajectory_calculator.cpp +++ b/robotis_math/src/robotis_trajectory_calculator.cpp @@ -327,4 +327,288 @@ Eigen::MatrixXd calcArc3dTra(double smp_time, double mov_time, } + +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_; +} + }