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

[hiikariri]

Jira Link: https://ichiro-its.atlassian.net/browse/PD-121

Description

Implement spline interpolation in akushon for smoother motions.
spline interpolation can be enabled/disabled through actions json.

Type of Change

• Bugfix
• Enhancement
• New feature
• Breaking change (fix or feature that would cause the existing functionality to not work as expected)

How Has This Been Tested?

• New unit tests added.
• Manual tested.

Checklist:

• Using Branch Name Convention
  • feature/JIRA-ID-SHORT-DESCRIPTION if has a JIRA ticket
  • enhancement/SHORT-DESCRIPTION if has/has no JIRA ticket and contain enhancement
  • hotfix/SHORT-DESCRIPTION if the change doesn't need to be tested (urgent)
• I have commented on my code, particularly in hard-to-understand areas.
• I have made the documentation for the corresponding changes.

[threeal]

I'm sorry, there's a bug in here. Please don't follow my suggestion blindly.

in case it is not found, after emplacing it, we should update the it:

Suggested change

	const auto it = joint_splines.find(joint.get_id());
	if (it == joint_splines.end()) {
	joint_splines.emplace(joint.get_id(), std::make_shared<keisan::SmoothSpline>());
	}
	it->second->add_point(joint.get_position(), pose.get_time());
	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());