Do not reconstruct joint velocity from link velocities when it is static

[eisoku9618]

link velocities after connected body dummy joint can be non 0 value even when we set 0 to all the active joint velocities.

for i in range(100):
   robot.SetDOFVelocities(vel1)
   robot.SetDOFVelocities(vel2)
robot.SetDOFVelocities([0] * 6)
robot.GetLinkVelocities() # returns non 0 velocities!

This is because

1. prismatic joint is weak to rounding error when joint velocity is 0 while revolute joint does not suffer from it.
   1. when a joint velocity is set to 0, the child link velocity is computed like the following
      • revolute joint: wchild = wparent
      • prismatic joint: vchild = vparent + wparent.cross(pchild - pparent)
   2. when we reconstruct a joint velocity based on its parent link position / velocity and its child link position / velocity, the computation is like the following
      • revolute joint: joint velocity = wchild - wparent
      • prismatic joint: joint velocity = vchild - vparent - wparent.cross(pchild - pparent)
   3. so we can reconstruct 0 for revolute joint but not for prismatic joint due to rounding error.
      • revolute joint: wparent - wparent is 0
      • prismatic joint: vparent + wparent.cross(pchild - pparent) - vparent - wparent.cross(pchild - pparent) might not be 0
2. In SetDOFVelocities(), we consider passive joint velocity even when it is static, so computed link velocities are affected by passive static joints velocity because of forward kinematics.
   • first we compute passive joint velocities, then do forward kinematics with specified joint velocities for non passive joints to compute link velocities, and then store link velocities.
3. joints for connected bodies are not revolute joints but prismatic joints

We don't need to compute static joint velocity from adjacent links and we can just set it to 0 because it is static. And also we use revolute joint for static joint when we create a model on our pendant, so better to use revolute joint for connected body joint?

[eisoku9618]

pipelineid=329861

[yoshikikanemoto]

looks good to me. @rdiankov can you review and merge? this is necessary for the force torque sensor feature that eisoku is currently working on.

[rdiankov]

thanks~