fixed urdf<->rcs inconsistency for mimic joints

[smanschi]

Hi,

I tried to fix the inconsistency between mimic joints from urdf's and Rcs's coupledJoints.

In short, the urdf definition of a mimic joint is:
q_slave = c*q_master + o
(q = joint value, c = coupling factor, o = offset)
while Rcs computes the joint value according to:
q_slave = q_slave_init + c*(q_master - q_master_init)

Hence, we can use the Rcs computation if we set q_slave_init to
q_slave_init = c*q_master_init + o
When parsing the urdf node, I temporarily store the offset in the jnt->q_init variable. After the parsing is finished and all joints
and bodies have been created, I correct jnt->q_init such that the normal Rcs computation can be used for computing the value of a coupled joint.