Feature: Custom python functions for dependent joints

[Rogerpi]

Right now, dependent joints can be set using a simple formula: joint = parent_joint * factor + offset

However, in a lot of cases this relationship is not enough, especially for grippers. For example, it is common to deal with different grippers actuated by prismatic pistons.

This pull request allows to write custom python functions that are evaluated in runtime. Inside the dependent_joints map, by tag "fun". The parent joint value has the reserved variable name "parent_position". The final value must be stored in the reserved variable "position". Variables are allowed to be set inside the map too.

Example:

dependent_joints:
finger_1: {parent: piston_joint,
z1: 0.03636756796927724,
z2: 0.025,
q_off: -0.820403314,
fun: "position = (-2.0 * math.atan2((z1 - math.sqrt(math.pow(z1, 2) + math.pow(z2, 2) - math.pow(parent_position, 2))), (z2 - parent_position))) + q_off"
}
finger_2: {parent: finger_1, fun: "position = -1 * parent_position" }

This example is added as a test.

To avoid breaking the current functionality, if no function is provided, it uses the standard joint = parent_joint * factor + offset