update joint_trjectory controller with mimic joint support

[k-okada]

@mintar I have tested your mimic_joint_gazebo_tutorial and it works fine for the small example, but when I tried to apply that plugins to a more complex model, for example, humanoid hand model, it did not work well. I thank this because when we calculate

value = multiplier * other_joint_value + offset.

where the mimic joint plugin uses other_joint_value as current Position of the joint in gazebo simulation, but I think we have to use current Desired(Commanded) Position in the controller, because the current position in gazebo simulation oscillates and that makes mimic joint unstable.

Here is my workaround for supporting mimic joint in joint trajectory controller and it works well on your tutorial code. How do you think?

see https://github.com/k-okada/mimic_joint_gazebo_tutorial.git for example

gripper_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - right_finger_joint
  mimic_joints:
    - left_finger_joint
  gains:
    right_finger_joint: {p: 20.0, i: 0.1, d: 0.2}
    left_finger_joint: {p: 20.0, i: 0.1, d: 0.2}

we added mimic_joints tag for the list of the mimic joint, and you have to add gains section. Mimic joint params (muliplier/offset) is taken from urdf model

[mintar]

+1

I think your analysis is spot on. I have also observed this instability with even slightly more complex models in Gazebo, such as the Robotiq 2 Finger Gripper. The instability is even bigger when there are external forces, such as trying to grasp an object.

Most of the mimic joints in that model could be replaced by properly modeling the parallel kinematics as a kinematic loop in SDF, but there needs stil to be one mimic joint (to ensure that the right and left fingers move in sync). I haven't tested this PR, but it sounds like it could enable us to get rid of the mimic joint plugin entirely.

[mathias-luedtke]

IMHO mimic joints should not be controlled actively.
Instead, mimic joints should be handled in RobotHW, e.g. with transmission support.
This has the advantage that it would work for all controllers.