General Implementation Strategy

[mstoelzle]

This issue shall concisely describe the general, initial implementation strategy. The repository shall use the improved state parametrisation for Piecewise Constant Curvature (PCC) as proposed by (Della Santina, 2020):

https://ieeexplore.ieee.org/document/8961972

It consists of the configuration variables Delta_x, Delta_y and delta L for each segment.

This repository, shall contain the following ROS2 packages:

• pcc_kinematics_interfaces to contain all ROS2 msg definitions for the kinematics
• pcc_kinematics_cpp a C++ package containing both libraries and nodes.
• pcc_kinematics_py a Python package containing both libraries and nodes.

The python package could be either (a) providing python interfaces for the C++ library using pybind11 or (b) be the analogue implementing in numpy of the C++ code

The reason that we aim to provide a C++ implementation is that this repository will likely be used in a variety of research projects, and it also will be running live on the robot. Accordingly, we need to push the computational time to be as fast as possible, to reach high sampling rates.

The library shall provide the following functions / or classes, which can be imported and used in other ROS packages:

• forward_kinematics(q, s): This function shall receive the configuration variables of the robot q and the running-variable s \in [0,1] and return the pose (e.g. position and orientation) of the center-line at point s. For example, if our robot consists of two segments, forward_kinematics(q, 0.5) -> chi would return the pose of the tip of the first segment in the global reference frame.
• transformation_segment(q_i) -> T: given the configuration q_i of segment i, this function returns the relative transformation matrix between the base and the tip of the segment
• transformation_chain(q, s_1, s_2) -> T This function returns the relative transformation matrix between the point s_1 and the point s_2 where s \in [0,1].
• inverse_kinematics_segment(T) -> q_i: For a given relative transformation between the base and the tip of the segment, returns the estimated configuration of the segment q_i
• inverse_kinematics(s_list, chi_list) -> q: This function uses inverse_kinematics_segment repetitively to estimate the entire configuration of the robot q from a list of measured poses in the global frame chi at known points s

Then, there are at least two nodes which advertise the forward_kinematics and inverse_kinematics implementations by subscribing to topics and then subsequently publishing the result.

To implement the inverse kinematics in C++, you can take some inspiration from here:
https://github.com/tud-cor-sr/ros2-mocap_optitrack/blob/main/mocap_optitrack_inv_kin/src/InverseKinematics3D.cpp