Tune effort joint trajectory PID gains

[rickstaa]

The current PID gains found in the effort_joint_trajectory_controllers.yaml are not correct. With these PID gains.

• The real robot shows dangerous behaviour (i.e. lead to very high velocities).
• The simulation has small oscillations (i.e. non-zero velocities).

This is because this PID controller was tuned for frankaemika/franka_ros#126. These wrongly tuned PID gains lead to a number of problems.

Joint trajectory controller GOAL_TOLERANCE_VOILATED error

With the current PID gains the robot has small oscillations when the velocity should be zero (see the gif and the console log below). As a result, during trajectory control, the JointTrajectoryController throws a GOAL_TOLERANCE_VIOLATED error. As explained in [this ROS answers question](https://answers.ros.org/question/312123/goal_tolerance_violated-gazebomoveit] a quick fix would be to set the stopped_velocity_tolerance to 0. However, to really solve this issue, the PID gains should be retuned for the new franka_gazebo simulation.

velocity: [-2.3700327160491814, -0.07111169060622406, 2.42011259637029, -0.1602890653322738, -0.5338011186773755, 0.25708581160465127, 1.144875244723069, 0.04420516084897766, -0.044204294583440176]

As can be seen from the picture above, the velocity of the first panda joint is very high, while we do not see this movement in the simulation. This is likely because the PID gains are very high, causing it to constantly switch the direction of the control. This assumption strengthened by looking at the velocity between time steps. One time step it is -2.36 then the next 2.36.

MoveIt gripper control execution timeout

In addition to the problem above, these wrongly tuned PID gains also prohibit MoveIt from correctly using the franka_gripper/gripper_action action service. This is because under the hood, this action uses the GRASP state of the controller state machine (See https:/frankaemika/franka_ros/blob/054af531f43f3fc1ddb439cc4102a41f87a8e0da/franka_gazebo/src/franka_gripper_sim.cpp#L435-L439). This state only transitions to the HOLDING state when the gripper finger velocities are close to zero (see https:/frankaemika/franka_ros/blob/054af531f43f3fc1ddb439cc4102a41f87a8e0da/franka_gazebo/src/franka_gripper_sim.cpp#L171-L191). Due to the badly tuned PID gains these velocities are never under the velocity threshold of 0.001 (see https:/frankaemika/franka_ros/blob/054af531f43f3fc1ddb439cc4102a41f87a8e0da/franka_gazebo/include/franka_gazebo/franka_gripper_sim.h#L36), meaning that the controller never transitions to the HOLDING state (See https:/frankaemika/franka_ros/blob/054af531f43f3fc1ddb439cc4102a41f87a8e0da/franka_gazebo/src/franka_gripper_sim.cpp#L441). As a result, the franka_gripper/gripper_action does not return a result before MoveIt times out.

Related issues

• [feature_request] Adds ability to switch gazebo hardware interface frankaemika/franka_ros#161

[rickstaa]

Solved when frankaemika/franka_ros#161 is merged. The code then has to be updated to use the effort_joint_trajectory_controller found in the franka_ros package.

[rickstaa]

Closed for now since using a seperate local controller configuration file makes more sense. We can however improve the PID gains if better gains are proposed in frankaemika/franka_ros#161.