[Sprint 22 / PD-121] - [WIP] Implement Spline Interpolation on Akushon

data/action/left_kick.json

@@ -27,7 +27,8 @@
       },
       "name": "walkready",
       "pause": 0,
-      "speed": 0.003
+      "speed": 0.003,
+      "time": 1.0
     },
     {
       "joints": {
@@ -54,7 +55,8 @@
       },
       "name": "feet",
       "pause": 1,
-      "speed": 0.01
+      "speed": 0.01,
+      "time": 1.0
     },
     {
       "joints": {
@@ -81,9 +83,11 @@
       },
       "name": "kick",
       "pause": 0,
-      "speed": 0.3
+      "speed": 0.3,
+      "time": 1.0
     }
   ],
   "start_delay": 0,
-  "stop_delay": 0
+  "stop_delay": 0,
+  "use_spline": false
 }

data/action/right_kick.json

@@ -27,7 +27,8 @@
       },
       "name": "r_walkready",
       "pause": 0,
-      "speed": 1
+      "speed": 1,
+      "time": 1.0
     },
     {
       "joints": {
@@ -54,7 +55,8 @@
       },
       "name": "r_feet",
       "pause": 1,
-      "speed": 0.01
+      "speed": 0.01,
+      "time": 1.0
     },
     {
       "joints": {
@@ -81,9 +83,11 @@
       },
       "name": "r_kick",
       "pause": 0,
-      "speed": 0.3
+      "speed": 0.3,
+      "time": 1.0
     }
   ],
   "start_delay": 0,
-  "stop_delay": 0
+  "stop_delay": 0,
+  "use_spline": false
 }

data/action/sit_down.json

@@ -27,7 +27,8 @@
       },
       "name": "down",
       "pause": 0,
-      "speed": 1
+      "speed": 1,
+      "time": 1.0
     },
     {
       "joints": {
@@ -54,7 +55,8 @@
       },
       "name": "sit_feet",
       "pause": 1,
-      "speed": 0.01
+      "speed": 0.01,
+      "time": 1.0
     },
     {
       "joints": {
@@ -81,9 +83,11 @@
       },
       "name": "sit_up",
       "pause": 0,
-      "speed": 0.3
+      "speed": 0.3,
+      "time": 1.0
     }
   ],
   "start_delay": 0,
-  "stop_delay": 0
+  "stop_delay": 0,
+  "use_spline": false
 }

data/action/walk_ready.json

@@ -27,9 +27,11 @@
       },
       "name": "walkready",
       "pause": 0,
-      "speed": 0.003
+      "speed": 0.003,
+      "time": 1.0
     }
   ],
   "start_delay": 0,
-  "stop_delay": 0
+  "stop_delay": 0,
+  "use_spline": false
 }

data/action/walkready.json

@@ -27,9 +27,11 @@
       },
       "name": "walkready",
       "pause": 0,
-      "speed": 0.0031
+      "speed": 0.0031,
+      "time": 1.0
     }
   ],
   "start_delay": 0,
-  "stop_delay": 0
+  "stop_delay": 0,
+  "use_spline": false
 }

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::unordered_map<uint8_t, std::shared_ptr<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()) {
+      auto it = joint_splines.find(joint.get_id());
+      if (it == joint_splines.end()) {
+        it = joint_splines.emplace(joint.get_id(), std::make_shared<keisan::SmoothSpline>()).first;
+      }
+      it->second->add_point(joint.get_position(), pose.get_time());
+    }
+  }
+}
 }  // namespace akushon

src/akushon/action/model/pose.cpp

@@ -53,6 +53,16 @@ float Pose::get_pause() const
   return pause;
 }
 
+float Pose::get_time() const 
+{
+  return time;
+}
+
+void Pose::set_time(float time_s) 
+{
+  this->time = time_s * 1000;
+}
+
 void Pose::set_name(const std::string & pose_name)
 {
   name = pose_name;

src/akushon/action/node/action_manager.cpp

@@ -76,7 +76,7 @@ void ActionManager::load_config(const std::string & path)
     }
 
     // remove "/" from the start of the name string
-    name.erase(0, 1);
+    // name.erase(0, 1);
     try {
       std::ifstream file(file_name);
       nlohmann::json action_data = nlohmann::json::parse(file);
@@ -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(val);
       }
     }
+    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;