feat: implement spline interpolation

include/akushon/action/model/action.hpp

@@ -24,8 +24,10 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <map>
 
 #include "akushon/action/model/pose.hpp"
+#include "keisan/spline/smooth_spline.hpp"
 
 namespace akushon
 {
@@ -57,11 +59,19 @@ class Action
 
   void reset();
 
+  void enable_spline(bool enable);
+  bool is_using_spline() const;
+  void generate_splines();
+
+  std::map<uint8_t, keisan::SmoothSpline *> joint_splines;
+
 private:
   std::string name;
 
   std::vector<Pose> poses;
 
+  bool use_spline;
+
   int stop_delay;
   int start_delay;
 

include/akushon/action/model/pose.hpp

@@ -40,6 +40,9 @@ class Pose
   void set_pause(float pause);
   float get_pause() const;
 
+  void set_time(float time_s);
+  float get_time() const;
+
   void set_name(const std::string & pose_name);
   const std::string & get_name() const;
 
@@ -51,6 +54,7 @@ class Pose
 private:
   float speed;
   float pause;
+  int time;
 
   std::string name;
 

include/akushon/action/process/interpolator.hpp

@@ -67,6 +67,7 @@ class Interpolator
   int start_stop_time;
   bool init_pause;
   int pause_time;
+  int prev_time;
 
   int current_action_index;
   int current_pose_index;

include/akushon/action/process/joint_process.hpp

@@ -24,6 +24,7 @@
 #include <string>
 #include <vector>
 
+#include "keisan/spline/spline.hpp"
 #include "tachimawari/joint/model/joint.hpp"
 
 namespace akushon
@@ -35,20 +36,26 @@ class JointProcess
   explicit JointProcess(uint8_t joint_id, float position = 0.0);
 
   void set_target_position(float target_position, float speed = 1.0);
+  void set_spline(const keisan::Spline &spline);
   void set_initial_position(float initial_position);
 
   void interpolate();
+  void interpolate_spline(float t);
 
   bool is_finished() const;
 
+  void reset_time();
+
   operator tachimawari::joint::Joint() const;
 
 private:
   tachimawari::joint::Joint joint;
+  keisan::Spline position_spline;
 
   float target_position;
   float initial_position;
   float additional_position;
+  float current_time;
 };
 
 }  // namespace akushon

src/akushon/action/model/action.cpp

@@ -24,14 +24,13 @@
 #include <vector>
 
 #include "akushon/action/model/action.hpp"
-
 #include "akushon/action/model/pose.hpp"
 
 namespace akushon
 {
 
 Action::Action(const std::string & action_name)
-: name(action_name), poses({}), start_delay(0), stop_delay(0), next_action("")
+: name(action_name), poses({}), start_delay(0), stop_delay(0), next_action(""), use_spline(false)
 {
 }
 
@@ -108,6 +107,30 @@ const std::string & Action::get_next_action() const
 void Action::reset()
 {
   poses.clear();
+  joint_splines.clear();
 }
 
+void Action::enable_spline(bool enable)
+{
+  use_spline = enable;
+}
+
+bool Action::is_using_spline() const
+{
+  return use_spline;
+}
+
+void Action::generate_splines()
+{
+  joint_splines.clear();
+  for (auto pose : poses) {
+    for (auto & joint : pose.get_joints()) {
+      uint8_t joint_id = joint.get_id();
+      if (joint_splines.find(joint_id) == joint_splines.end()) {
+        joint_splines[joint_id] = std::make_shared<keisan::SmoothSpline>().get();
+      }
+      joint_splines[joint_id]->add_point(joint.get_position(), pose.get_time());
+    }
+  }
+}
 }  // namespace akushon

src/akushon/action/node/action_manager.cpp

@@ -117,6 +117,7 @@ Action ActionManager::load_action(
 
             pose.set_pause(raw_pose["pause"]);
             pose.set_speed(raw_pose["speed"]);
+            pose.set_time(raw_pose["time"]);
             pose.set_joints(joints);
             action.add_pose(pose);
           }
@@ -127,8 +128,11 @@ Action ActionManager::load_action(
         action.set_stop_delay(val);
       } else if (key == "next") {
         action.set_next_action(val);
+      } else if (key == "use_spline") {
+        action.enable_spline(true);
       }
     }
+    action.generate_splines();
   } catch (nlohmann::json::parse_error & ex) {
     // TODO(any): will be used for logging
     // std::cerr << "parse error at byte " << ex.byte << std::endl;

src/akushon/action/process/interpolator.cpp

@@ -69,6 +69,7 @@ void Interpolator::process(int time)
           if (init_pause) {
             init_pause = false;
             pause_time = time;
+            prev_time = time;
           }
 
           if (current_pose_index == get_current_action().get_pose_count()) {
@@ -77,6 +78,7 @@ void Interpolator::process(int time)
           } else if ((time - pause_time) > (get_current_pose().get_pause() * 1000)) {
             next_pose();
             init_pause = true;
+            prev_time = time;
           }
         }
 
@@ -88,6 +90,10 @@ void Interpolator::process(int time)
         if (init_state) {
           init_state = false;
           start_stop_time = time;
+          for (const auto & [id, spline] : get_current_action().joint_splines) {
+            joint_processes.at(id).set_spline(*spline);
+            joint_processes.at(id).reset_time();
+          }
         }
 
         if ((time - start_stop_time) > (get_current_action().get_stop_delay() * 1000)) {
@@ -104,7 +110,14 @@ void Interpolator::process(int time)
       }
   }
 
+  bool is_using_spline = get_current_action().is_using_spline();
+  int delta_time = time - prev_time;
+  prev_time = time;
+
   for (const auto & [id, joint] : joint_processes) {
+    if(is_using_spline) {
+      joint_processes.at(id).interpolate_spline(delta_time);
+    }
     joint_processes.at(id).interpolate();
   }
 }

src/akushon/action/process/joint_process.cpp

@@ -45,6 +45,11 @@ void JointProcess::set_target_position(float target_position, float speed)
   additional_position = (fabs(additional_position) < 0.1) ? 0.0 : additional_position;
 }
 
+void JointProcess::set_spline(const keisan::Spline & spline)
+{
+  this->position_spline = spline;
+}
+
 void JointProcess::set_initial_position(float initial_position)
 {
   this->initial_position = initial_position;
@@ -67,11 +72,22 @@ void JointProcess::interpolate()
   }
 }
 
+void JointProcess::interpolate_spline(float t)
+{
+  current_time += t;
+  joint.set_position(position_spline.interpolate_value(current_time, keisan::Polynom::POSITION));
+}
+
 bool JointProcess::is_finished() const
 {
   return (initial_position == target_position) || (additional_position == 0.0);
 }
 
+void JointProcess::reset_time()
+{
+  current_time = 0;
+}
+
 JointProcess::operator tachimawari::joint::Joint() const
 {
   return joint;