(Physics) OptimCartesianConverter is bugged

[kdesnos]

When trying to compute the position of the hand of the Widowx robotic arm with the armlearn::kinematics::OptimCartesianConverter, the returned results are sometime wrong for an unknown reason.

To replicate this bug, simply compile and run this code:

for (int deg = 0; deg <= 1; deg += 1) {

        std::vector<uint16_t> positions = {0, 2048, 3072, 1250 +deg, 256, 256};

        armlearn::kinematics::OptimCartesianConverter converter;
        converter.addServo("base", armlearn::kinematics::rotZ, 0, 0, 125, 0, 0, M_PI);
        converter.addServo("shoulder", armlearn::kinematics::rotX, 0, -49, 142, M_PI / 2, 0,
                           M_PI); // Add 90° because of the orientation of the elbow servomotor
        converter.addServo("elbow", armlearn::kinematics::rotX, 0, 0, 142);
        converter.addServo("wristAngle", armlearn::kinematics::rotX, 0, 0, 74);
        converter.addServo("wristRotate", armlearn::kinematics::rotZ, 0, 0, 41);
        converter.addServo("gripper", armlearn::kinematics::rotZ, 0, 0, 40);

        std::cout << "Positions of servomotors : ";
        for (auto &&v : positions) std::cout << v << " ";
        std::cout << std::endl;

        std::vector<double> cartesianCoords = converter.computeServoToCoord(positions)->getCoord();
        std::cout << "Cartesian coordinates equivalent : ";
        for (auto &&v : cartesianCoords) std::cout << v << " ";
        std::cout << std::endl;
}

This produces

Positions of servomotors : 0 2048 3072 1250 256 256 
Cartesian coordinates equivalent : 1.19267e-14 -194.778 461.665 
Positions of servomotors : 0 2048 3072 1251 256 256 
Cartesian coordinates equivalent : -3.57653e-14 194.697 461.889 

The correct output, obtained by replacing armlearn::kinematics::OptimCartesianConverter with armlearn::kinematics::BasicCartesianConverter is:

Positions of servomotors : 0 2048 3072 1250 256 256 
Cartesian coordinates equivalent : 2.38396e-14 -194.778 461.665 
Positions of servomotors : 0 2048 3072 1251 256 256 
Cartesian coordinates equivalent : 2.3867e-14 -194.697 461.889