[PI - 260] Implement spline interpolation

.github/workflows/build-and-test-nightly.yml

@@ -7,7 +7,7 @@ on:
     branches: [master]
 jobs:
   build-and-test-nightly:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     steps:
       - name: Checkout this repository
         uses: actions/[email protected]

.github/workflows/build-and-test-stable.yml

@@ -5,7 +5,7 @@ on:
     branches: [master]
 jobs:
   build-and-test-stable:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     steps:
       - name: Checkout this repository
         uses: actions/[email protected]

.github/workflows/build-debian-nightly.yml

@@ -5,7 +5,7 @@ on:
     branches: [master]
 jobs:
   deploy-debian-nightly:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     steps:
       - name: Checkout this repository
         uses: actions/[email protected]

.github/workflows/build-debian-stable.yml

@@ -5,7 +5,7 @@ on:
     types: [created]
 jobs:
   deploy-debian-stable:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-20.04
     steps:
       - name: Checkout this repository
         uses: actions/[email protected]

include/akushon/action/model/action.hpp

@@ -21,11 +21,13 @@
 #ifndef AKUSHON__ACTION__MODEL__ACTION_HPP_
 #define AKUSHON__ACTION__MODEL__ACTION_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
 
 #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

@@ -31,7 +31,7 @@ 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 +108,31 @@ 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>();
+      }
+      joint_splines[joint_id]->add_point(joint.get_position(), pose.get_time());
+    }
+  }
 }
 
 }  // namespace akushon

src/akushon/action/model/pose.cpp

@@ -18,59 +18,40 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
+#include "akushon/action/model/pose.hpp"
+
 #include <string>
 #include <vector>
 
-#include "akushon/action/model/pose.hpp"
-
 #include "tachimawari/joint/model/joint.hpp"
 
 namespace akushon
 {
 
 Pose::Pose(const std::string & pose_name)
-: name(pose_name), speed(0.0), pause(0.0), joints({})
-{
-}
+: name(pose_name), speed(0.0), pause(0.0), joints({}) {}
 
-void Pose::set_speed(float speed)
-{
-  this->speed = speed;
-}
+void Pose::set_speed(float speed) {this->speed = speed;}
 
-float Pose::get_speed() const
-{
-  return speed;
-}
+float Pose::get_speed() const {return speed;}
 
-void Pose::set_pause(float pause)
-{
-  this->pause = pause;
-}
+void Pose::set_pause(float pause) {this->pause = pause;}
 
-float Pose::get_pause() const
-{
-  return pause;
-}
+float Pose::get_pause() const {return pause;}
 
-void Pose::set_name(const std::string & pose_name)
-{
-  name = pose_name;
-}
+float Pose::get_time() const {return time;}
 
-const std::string & Pose::get_name() const
-{
-  return name;
-}
+void Pose::set_time(float time_s) {this->time = time_s * 1000;}
+
+void Pose::set_name(const std::string & pose_name) {name = pose_name;}
+
+const std::string & Pose::get_name() const {return name;}
 
 void Pose::set_joints(const std::vector<tachimawari::joint::Joint> & joints)
 {
   this->joints = joints;
 }
 
-const std::vector<tachimawari::joint::Joint> & Pose::get_joints() const
-{
-  return joints;
-}
+const std::vector<tachimawari::joint::Joint> & Pose::get_joints() const {return joints;}
 
 }  // namespace akushon

src/akushon/action/node/action_manager.cpp

@@ -114,6 +114,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);
           }
@@ -124,8 +125,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/node/action_node.cpp

@@ -123,7 +123,6 @@ bool ActionNode::start(const std::string & action_name)
 bool ActionNode::start(const Action & action)
 {
   Pose pose = this->initial_pose;
-
   if (!pose.get_joints().empty()) {
     action_manager->start(action, pose);
   } else {

src/akushon/action/process/interpolator.cpp

@@ -54,6 +54,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_start_delay() * 1000)) {
@@ -69,6 +73,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 +82,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;
           }
         }
 
@@ -104,7 +110,13 @@ 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();
   }
 }
@@ -119,8 +131,8 @@ void Interpolator::next_pose()
   auto current_pose = get_current_pose();
   for (const auto & joint : current_pose.get_joints()) {
     if (joint_processes.find(joint.get_id()) != joint_processes.end()) {
-      joint_processes.at(joint.get_id()).set_target_position(
-        joint.get_position(), current_pose.get_speed());
+      joint_processes.at(joint.get_id())
+      .set_target_position(joint.get_position(), current_pose.get_speed());
     }
   }
   ++current_pose_index;

src/akushon/action/process/joint_process.cpp

@@ -18,12 +18,11 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
+#include <cmath>
 #include <string>
 #include <vector>
-#include <cmath>
 
 #include "akushon/action/process/joint_process.hpp"
-
 #include "tachimawari/joint/model/joint.hpp"
 
 namespace akushon
@@ -45,6 +44,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;
@@ -53,10 +57,10 @@ void JointProcess::set_initial_position(float initial_position)
 void JointProcess::interpolate()
 {
   bool target_position_is_reached = false;
-  target_position_is_reached |= (additional_position >= 0 &&
-    (joint.get_position() + additional_position >= target_position));
-  target_position_is_reached |= (additional_position <= 0 &&
-    (joint.get_position() + additional_position < target_position));
+  target_position_is_reached |=
+    (additional_position >= 0 && (joint.get_position() + additional_position >= target_position));
+  target_position_is_reached |=
+    (additional_position <= 0 && (joint.get_position() + additional_position < target_position));
 
   if (target_position_is_reached) {
     joint.set_position(target_position);
@@ -67,11 +71,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;