[DQ_SerialWholeBody_py.cpp] Added two raw_fkm methods to fix #54

[juanjqo]

@dqrobotics/developers

Hi @mmmarinho,

This PR adds the wrappers in Python for the following C++ methods

DQ DQ_SerialWholeBody::raw_fkm(const VectorXd& q) const;
DQ DQ_SerialWholeBody::raw_fkm(const VectorXd& q, const int& to_ith_link) const;

TODO

• [x] Add a unit test.

Best regards,

Juancho

Update:

Minimal example:

#!/bin/python3
from dqrobotics.robot_modeling import DQ_SerialWholeBody
from dqrobotics.robots import FrankaEmikaPandaRobot
from dqrobotics.robots import KukaLw4Robot
import numpy as np

def main() -> None:
    try:
        robot = DQ_SerialWholeBody(FrankaEmikaPandaRobot.kinematics())
        robot.add(KukaLw4Robot.kinematics())
        q = np.zeros(robot.get_dim_configuration_space())
        x_raw = robot.raw_fkm(q)
    except (Exception, KeyboardInterrupt) as e:
        print(e)
        pass

if __name__ == "__main__":
    main()

Current Output (Not working):

'dqrobotics._dqrobotics._robot_modeling.DQ_SerialWh' object has no attribute 'raw_fkm'

Process finished with exit code 0

Expected behavior (using #55):

1i + E*( - 0.06465 - 0.087j)

Process finished with exit code 0