From 525ab6cf756ea4340333be00169bc35e790bfeb4 Mon Sep 17 00:00:00 2001
From: s-changhyun <sch@robotis.com>
Date: Wed, 10 Aug 2016 14:40:32 +0900
Subject: [PATCH] add function

---
 .../robotis_controller/robotis_controller.h   |  2 +-
 .../robotis_trajectory_calculator.h           |  4 +
 .../src/robotis_trajectory_calculator.cpp     | 80 ++++++++++++++++++-
 3 files changed, 82 insertions(+), 4 deletions(-)

diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h
index ae91370..a24f870 100644
--- a/robotis_controller/include/robotis_controller/robotis_controller.h
+++ b/robotis_controller/include/robotis_controller/robotis_controller.h
@@ -93,7 +93,7 @@ private:
   void initializeSyncWrite();
 
 public:
-  static const int  CONTROL_CYCLE_MSEC  = 8;    // 8 msec
+  static const int  CONTROL_CYCLE_MSEC  = 4;    // 8 msec
 
   bool              DEBUG_PRINT;
   Robot            *robot_;
diff --git a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h
index 56bf644..1d1f532 100644
--- a/robotis_math/include/robotis_math/robotis_trajectory_calculator.h
+++ b/robotis_math/include/robotis_math/robotis_trajectory_calculator.h
@@ -50,6 +50,10 @@ Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double ac
                                    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,
diff --git a/robotis_math/src/robotis_trajectory_calculator.cpp b/robotis_math/src/robotis_trajectory_calculator.cpp
index 5f66ffc..bddb6c1 100644
--- a/robotis_math/src/robotis_trajectory_calculator.cpp
+++ b/robotis_math/src/robotis_trajectory_calculator.cpp
@@ -40,9 +40,11 @@
 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 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
 
@@ -97,6 +99,78 @@ Eigen::MatrixXd calcMinimumJerkTra(double pos_start, double vel_start, double ac
   return minimum_jerk_tra;
 }
 
+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
+    )
+/*
+   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,3);
+
+  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);
+
+    minimum_jerk_tra.coeffRef( step , 1 ) =
+        vel_start +
+        accel_start*time.coeff(step,0) +
+        3*poly_coeff.coeff(0,0)*pow(time.coeff(step,0),2) +
+        4*poly_coeff.coeff(1,0)*pow(time.coeff(step,0),3) +
+        5*poly_coeff.coeff(2,0)*pow(time.coeff(step,0),4);
+
+    minimum_jerk_tra.coeffRef( step , 2 ) =
+        accel_start +
+        6*poly_coeff.coeff(0,0)*time.coeff(step,0) +
+        12*poly_coeff.coeff(1,0)*pow(time.coeff(step,0),2) +
+        20*poly_coeff.coeff(2,0)*pow(time.coeff(step,0),3);
+  }
+
+  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,