Merge pull request #15 from ROBOTIS-GIT/master

merge for sync kinetic-devel and master branch

.travis.yml

@@ -0,0 +1,32 @@
+# This config file for Travis CI utilizes ros-industrial/industrial_ci package.
+# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst
+
+sudo: required
+dist: trusty
+services:
+  - docker
+language: generic
+python:
+  - "2.7"
+compiler:
+  - gcc
+notifications:
+  email:
+    on_success: always
+    on_failure: always
+    recipients:
+      - [email protected]
+env:
+  matrix:
+    - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian
+#   - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie
+branches:
+  only:
+    - master
+    - develop
+    - kinetic-devel
+install:
+  - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
+script:
+  - source .ci_config/travis.sh
+

robotis_math/CMakeLists.txt

@@ -43,11 +43,14 @@ include_directories(
 add_library(robotis_math
    src/robotis_math_base.cpp
    src/robotis_trajectory_calculator.cpp
+   src/minimum_jerk_trajectory.cpp
+   src/minimum_jerk_trajectory_with_via_point.cpp
    src/fifth_order_polynomial_trajectory.cpp
    src/simple_trapezoidal_velocity_profile.cpp
    src/robotis_linear_algebra.cpp
    src/bezier_curve.cpp
    src/step_data_define.cpp
+   src/preview_control.cpp
 )
 
 add_dependencies(robotis_math ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

robotis_math/include/robotis_math/minimum_jerk_trajectory.h

@@ -0,0 +1,78 @@
+/*******************************************************************************
+ * 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.
+ *******************************************************************************/
+
+#ifndef ROBOTIS_MATH_MINIMUM_JERK_TRAJECTORY_H_
+#define ROBOTIS_MATH_MINIMUM_JERK_TRAJECTORY_H_
+
+#define EIGEN_NO_DEBUG
+#define EIGEN_NO_STATIC_ASSERT
+
+#include "robotis_linear_algebra.h"
+#include "robotis_math_base.h"
+
+#include <ros/ros.h>
+#include <stdint.h>
+#include <vector>
+
+namespace robotis_framework
+{
+
+class MinimumJerk
+{
+public:
+  MinimumJerk(double ini_time, double fin_time,
+              std::vector<double_t> ini_pos, std::vector<double_t> ini_vel, std::vector<double_t> ini_acc,
+              std::vector<double_t> fin_pos, std::vector<double_t> fin_vel, std::vector<double_t> fin_acc);
+  virtual ~MinimumJerk();
+
+  std::vector<double_t> getPosition(double time);
+  std::vector<double_t> getVelocity(double time);
+  std::vector<double_t> getAcceleration(double time);
+
+  double cur_time_;
+  std::vector<double_t> cur_pos_;
+  std::vector<double_t> cur_vel_;
+  std::vector<double_t> cur_acc_;
+
+  Eigen::MatrixXd position_coeff_;
+  Eigen::MatrixXd velocity_coeff_;
+  Eigen::MatrixXd acceleration_coeff_;
+  Eigen::MatrixXd time_variables_;
+
+private:
+  int number_of_joint_;
+  double ini_time_, fin_time_;
+  std::vector<double_t> ini_pos_, ini_vel_, ini_acc_;
+  std::vector<double_t> fin_pos_, fin_vel_, fin_acc_;
+};
+
+}
+
+#endif /* ROBOTIS_MATH_MINIMUM_JERK_TRAJECTORY_H_ */

robotis_math/include/robotis_math/minimum_jerk_trajectory_with_via_point.h

@@ -0,0 +1,82 @@
+/*******************************************************************************
+ * 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.
+ *******************************************************************************/
+
+#ifndef ROBOTIS_MATH_MINIMUM_JERK_TRAJECTORY_WITH_VIA_POINT_H_
+#define ROBOTIS_MATH_MINIMUM_JERK_TRAJECTORY_WITH_VIA_POINT_H_
+
+#define EIGEN_NO_DEBUG
+#define EIGEN_NO_STATIC_ASSERT
+
+#include "robotis_linear_algebra.h"
+#include "robotis_math_base.h"
+
+#include <ros/ros.h>
+#include <stdint.h>
+#include <vector>
+
+namespace robotis_framework
+{
+
+class MinimumJerkViaPoint
+{
+public:
+  MinimumJerkViaPoint(double ini_time, double fin_time, double via_time, double ratio,
+                      std::vector<double_t> ini_pos, std::vector<double_t> ini_vel, std::vector<double_t> ini_acc,
+                      std::vector<double_t> fin_pos, std::vector<double_t> fin_vel, std::vector<double_t> fin_acc,
+                      std::vector<double_t> via_pos, std::vector<double_t> via_vel, std::vector<double_t> via_acc);
+  virtual ~MinimumJerkViaPoint();
+
+  std::vector<double_t> getPosition(double time);
+  std::vector<double_t> getVelocity(double time);
+  std::vector<double_t> getAcceleration(double time);
+
+  double cur_time_;
+  std::vector<double_t> cur_pos_;
+  std::vector<double_t> cur_vel_;
+  std::vector<double_t> cur_acc_;
+
+  Eigen::MatrixXd position_coeff_;
+  Eigen::MatrixXd velocity_coeff_;
+  Eigen::MatrixXd acceleration_coeff_;
+  Eigen::MatrixXd time_variables_;
+
+private:
+  int number_of_joint_;
+  double ratio_;
+  double input_ini_time_, input_fin_time_;
+  double ini_time_, fin_time_, via_time_;
+  std::vector<double_t> ini_pos_, ini_vel_, ini_acc_;
+  std::vector<double_t> fin_pos_, fin_vel_, fin_acc_;
+  std::vector<double_t> via_pos_, via_vel_, via_acc_;
+};
+
+}
+
+#endif /* ROBOTIS_MATH_MINIMUM_JERK_TRAJECTORY_WITH_VIA_POINT_H_ */

robotis_math/include/robotis_math/preview_control.h

@@ -0,0 +1,63 @@
+/*******************************************************************************
+ * 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.
+ *******************************************************************************/
+
+#ifndef ROBOTIS_MATH_PREVIEW_CONTROL_H_
+#define ROBOTIS_MATH_PREVIEW_CONTROL_H_
+
+#define EIGEN_NO_DEBUG
+#define EIGEN_NO_STATIC_ASSERT
+
+#include "robotis_linear_algebra.h"
+#include "robotis_math_base.h"
+
+#include <ros/ros.h>
+#include <stdint.h>
+#include <vector>
+
+namespace robotis_framework
+{
+
+class PreviewControl
+{
+public:
+  PreviewControl();
+  virtual ~PreviewControl();
+
+  Eigen::MatrixXd calcPreviewParam(double preview_time, double control_cycle,
+                                   double lipm_height,
+                                   Eigen::MatrixXd K, Eigen::MatrixXd P);
+
+private:
+
+};
+
+}
+
+#endif /* ROBOTIS_MATH_MINIMUM_JERK_TRAJECTORY_H_ */

robotis_math/include/robotis_math/robotis_math.h

@@ -40,5 +40,8 @@
 
 #include "robotis_trajectory_calculator.h"
 #include "bezier_curve.h"
+#include "preview_control.h"
+#include "minimum_jerk_trajectory.h"
+#include "minimum_jerk_trajectory_with_via_point.h"
 
 #endif /* ROBOTIS_MATH_ROBOTIS_MATH_H_ */

robotis_math/include/robotis_math/robotis_math_base.h

@@ -38,6 +38,7 @@
 #ifndef ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_
 #define ROBOTIS_MATH_ROBOTIS_MATH_BASE_H_
 
+#include <iostream>
 #include <cmath>
 
 namespace robotis_framework

robotis_math/include/robotis_math/robotis_trajectory_calculator.h

@@ -46,6 +46,7 @@
 #include "fifth_order_polynomial_trajectory.h"
 #include "simple_trapezoidal_velocity_profile.h"
 
+
 namespace robotis_framework
 {
 // minimum jerk trajectory

robotis_math/src/bezier_curve.cpp

@@ -40,10 +40,14 @@
 using namespace robotis_framework;
 
 BezierCurve::BezierCurve()
-{ }
+{
+
+}
 
 BezierCurve::~BezierCurve()
-{  }
+{
+
+}
 
 void BezierCurve::setBezierControlPoints(const std::vector<Point2D>& points)
 {