diff --git a/include/akushon/action/model/action.hpp b/include/akushon/action/model/action.hpp index 409d48e..d3d2b41 100644 --- a/include/akushon/action/model/action.hpp +++ b/include/akushon/action/model/action.hpp @@ -24,8 +24,10 @@ #include #include #include +#include #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 joint_splines; + private: std::string name; std::vector poses; + bool use_spline; + int stop_delay; int start_delay; diff --git a/include/akushon/action/model/pose.hpp b/include/akushon/action/model/pose.hpp index 9c6af2f..4b87bf2 100644 --- a/include/akushon/action/model/pose.hpp +++ b/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; diff --git a/include/akushon/action/process/interpolator.hpp b/include/akushon/action/process/interpolator.hpp index e47631e..87eee12 100644 --- a/include/akushon/action/process/interpolator.hpp +++ b/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; diff --git a/include/akushon/action/process/joint_process.hpp b/include/akushon/action/process/joint_process.hpp index 9393238..fe6dfcf 100644 --- a/include/akushon/action/process/joint_process.hpp +++ b/include/akushon/action/process/joint_process.hpp @@ -24,6 +24,7 @@ #include #include +#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 diff --git a/src/akushon/action/model/action.cpp b/src/akushon/action/model/action.cpp index 70da59f..1099bdf 100644 --- a/src/akushon/action/model/action.cpp +++ b/src/akushon/action/model/action.cpp @@ -24,14 +24,13 @@ #include #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().get(); + } + joint_splines[joint_id]->add_point(joint.get_position(), pose.get_time()); + } + } +} } // namespace akushon diff --git a/src/akushon/action/node/action_manager.cpp b/src/akushon/action/node/action_manager.cpp index 6919c3b..e12e3cf 100644 --- a/src/akushon/action/node/action_manager.cpp +++ b/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; diff --git a/src/akushon/action/process/interpolator.cpp b/src/akushon/action/process/interpolator.cpp index 7822e91..918524b 100644 --- a/src/akushon/action/process/interpolator.cpp +++ b/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(); } } diff --git a/src/akushon/action/process/joint_process.cpp b/src/akushon/action/process/joint_process.cpp index 5e237a3..15aa836 100644 --- a/src/akushon/action/process/joint_process.cpp +++ b/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;