Gazebo initial joint configuration

[matteodv99tn]

Currently, with PR #2, @Hydran00 set the initial configuration for the second and third joint with a value that differs from the homing condition (which is the zero vector).

This is to avoid the known issue of robot self compenetration; indeed, if starting the robot from his homing configuration, the visualisation in RViz, after few seconds of using torque-control, becomes the following:

Possible reasons to investigate:

• due to numerical errors, the meshes used for the collision checking are already compenetrating at robot spawn in gazebo;
• collisions are not correctly setup in the gazebo simulation environment.

If someone is willing to tackle this problem, let me know!