[BUG] Bad DQ_SerialManipulatorDH(dh_matrix) call

Bug description

Hi @mmmarinho!

The fifth line in the DH matrix is missing in YouBotVrepRobot.cpp when used the DQ_SerialManipulatorDH class.

To Reproduce

Run the YouBotVrepRobot.cpp example

Code

DQ_robotics::DQ_WholeBody YouBotVrepRobot::kinematics()  #line 58 of master/src/dqrobotics/interfaces/vrep/robots/YouBotVrepRobot.cpp
{
    const double pi2 = pi/2.0;

    Matrix<double,4,5> dh(4,5);
    dh <<    0,      pi2,       0,      pi2,    0,
            0.147,    0,       0,        0,    0.218,
            0.033,    0.155,   0.135,    0,    0,
            pi2,      0,       0,      pi2,    0; 
          # ----------------------------------------------------------- missing fifth line of the dh matrix

    auto arm = std::make_shared<DQ_SerialManipulatorDH>(DQ_SerialManipulatorDH(dh));
    auto base = std::make_shared<DQ_HolonomicBase>(DQ_HolonomicBase());

Output

─ ./vrep_interface_move_kuka_and_youbot                                     ─╯
Starting V-REP simulation...
There was an error connecting to V-REP, please check that it is open and that the Kuka Robot is in the scene.
Bad DQ_SerialManipulatorDH(dh_matrix) call: dh_matrix should be 5xn

Environment:

OS: [Ubuntu 18.04]
dqrobotics version (20.04.0-0~202203090039~ubuntu20.04.1).
libdqrobotics-interface-vrep is already the newest version (19.10.0-0~202203090042~ubuntu20.04.1).

Cheers,

Juancho

dqrobotics / cpp-interface-vrep

[BUG] Bad DQ_SerialManipulatorDH(dh_matrix) call #5