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

Add new functions to robotis_math

Browse Source 
step data define
fifth polynomial trajectory
This commit is contained in:
Jay-Song
2016-08-10 10:48:42 +09:00
parent 6df646ae09
commit 54d93f872c
5 changed files with 428 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -44,6 +44,7 @@
#define EIGEN_NO_STATIC_ASSERT
#include <Eigen/Dense>
#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);
}

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

@ -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_ */

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

@ -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_ */

14
robotis_math/src/robotis_linear_algebra.cpp
View File

@ -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;
}
}

284
robotis_math/src/robotis_trajectory_calculator.cpp
View File

@ -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_;
}
}