Merge branch 'develop' of https://github.com/ROBOTIS-GIT/ROBOTIS-Framework into develop
This commit is contained in:
ROBOTIS-zerom
@ -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}
|
||||
)
|
||||
@ -1,78 +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>
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* ROBOTIS_MATH_ROBOTIS_LINEAR_ALGEBRA_H_ */
|
||||
@ -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_ */
|
||||
@ -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_ */
|
||||
@ -1,65 +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 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);
|
||||
|
||||
}
|
||||
|
||||
#endif /* ROBOTIS_MATH_ROBOTIS_TRAJECTORY_CALCULATOR_H_ */
|
||||
@ -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>
|
||||
@ -1,308 +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);
|
||||
}
|
||||
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@ -1,330 +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 "../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 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;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user