[feature_request] Adds ability to switch gazebo hardware interface

[rickstaa]

I think it would be handy to have the ability to switch the hardware_interface that is used in the gazebo simulation for controlling the panda arm. This feature would allow users to test control the robot using different hardware interfaces, similar to the real robot.

Currently, the hardware_interface/JointEffortInterface is hardcoded into the panda_arm.urdf.xacro file. Because of this, the release of the Noetic panda_moveit_config package, which targets franka_ros v0.8.0, is blocked, as we would like to both use the JointPositionInterface in the gazebo simulation and on the real robot (see moveit/panda_moveit_config#74 and moveit/panda_moveit_config#68).

I also tried switching to the effort_controllers/JointTrajectoryController while using the following pid gains:

effort_joint_trajectory_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - panda_joint1
    - panda_joint2
    - panda_joint3
    - panda_joint4
    - panda_joint5
    - panda_joint6
    - panda_joint7
  gains:
    panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
    panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
    panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
    panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
    panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
    panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
    panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }

Unfortunately, although these gains seem decent on the simulation robot (i.e. control has a slight offset), they lead to dangerous situations (i.e. very high velocities and acceleration) on the real robot, which causes the software emergency stop to be invoked).

I created #156 and #157 to add this feature.

[rickstaa]

@gollth I think for releasing the Noetic version of the panda_moveit_config it would be enough if you could provide add a joint_trajectory_example_controller with PID gains that work in the simulation but can also be safely used on the real robot.

Example controllers

Effort trajectory controller

The PID gains below do not work on the real robot (i.e. will lead to very big velocities) and cause small oscillations (i.e. non-zero velocities) in the simulation. This is because this PID controller was tuned for #126.

joint_position_example_controller:
  type: franka_example_controllers/JointPositionExampleController
  arm_id: $(arg arm_id)
  joint_names:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7

+joint_effort_trajectory_example_controller:
+  type: effort_controllers/JointTrajectoryController
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+   constraints:
+     goal_time: 0.5
+     panda_joint1:
+       goal: 0.05
+     panda_joint2:
+       goal: 0.05
+     panda_joint3:
+       goal: 0.05
+     panda_joint4:
+       goal: 0.05
+     panda_joint5:
+       goal: 0.05
+     panda_joint6:
+       goal: 0.05
+     panda_joint7:
+       goal: 0.05
+  gains:
+    panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
+    panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
+    panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
+    panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
+    panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
+    panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
+    panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }

Position trajectory controller

The controller below works on the real robot but does not work in simulation as the gazebo_ros_control.pid_gains need to be set (see #161 (comment)).

joint_position_example_controller:
  type: franka_example_controllers/JointPositionExampleController
  arm_id: $(arg arm_id)
  joint_names:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7

+joint_position_trajectory_example_controller:
+  type: position_controllers/JointTrajectoryController
+  joints:
+    - panda_joint1
+    - panda_joint2
+    - panda_joint3
+    - panda_joint4
+    - panda_joint5
+    - panda_joint6
+    - panda_joint7
+   constraints:
+     goal_time: 0.5
+     panda_joint1:
+       goal: 0.05
+     panda_joint2:
+       goal: 0.05
+     panda_joint3:
+       goal: 0.05
+     panda_joint4:
+       goal: 0.05
+     panda_joint5:
+       goal: 0.05
+     panda_joint6:
+       goal: 0.05
+     panda_joint7:
+       goal: 0.05
+  gains:
+    panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
+    panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
+    panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
+    panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
+    panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
+    panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
+    panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
+
+ gazebo_ros_control:
+   pid_gains:
+     panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
+     panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
+     panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
+     panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
+     panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
+     panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
+     panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
+     panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
+     panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }

[rickstaa]

Problems with the earlier mentioned PID gains

Effort trajectory controller PID gains

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 #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. With the fixed franka_gripper/gripper_action (i.e. after #173 has been merged) the state machine sometimes gets stuck in the GRASP state.

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 435 to 439 in 054af53

	transition(State::GRASPING,
	Config{.width_desired = goal->command.position * 2.0 < width ? 0 : kMaxFingerWidth,
	.speed_desired = this->speed_default_,
	.force_desired = goal->command.max_effort,
	.tolerance = eps});

This state only transitions to the HOLDING state when the gripper finger velocities are close to zero.

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 171 to 191 in 054af53

	if (state == State::GRASPING) {
	// Since the velocity signal is noisy it can easily happen that one sample is below the
	// threshold To avoid abortion because of noise, we have to read at least N consecutive number
	// of samples before interpreting something was grasped (or not)
	static int speed_threshold_counter = 0;
	double speed = this->finger1_.getVelocity() + this->finger2_.getVelocity();
	if (std::abs(speed) <= this->speed_threshold_) {
	speed_threshold_counter++;
	} else {
	speed_threshold_counter = 0;
	}
	if (speed_threshold_counter >= this->speed_samples_) {
	// Done with grasp motion, switch to holding, i.e. keep position & force
	transition(State::HOLDING, Config{.width_desired = width,
	.speed_desired = 0,
	.force_desired = this->config_.force_desired,
	.tolerance = this->config_.tolerance});
	speed_threshold_counter = 0;
	}
	}

Due to the badly tuned PID gains these velocities are never under the velocity threshold of 0.001.

franka_ros/franka_gazebo/include/franka_gazebo/franka_gripper_sim.h

Line 36 in 054af53

const double kGraspRestingThreshold = 0.001;

.

As a result, the controller never transitions to the HOLDING state.

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Line 441 in 054af53

waitUntilStateChange();

Consequently, the third party services like MoveIt, which depend on the franka_gripper/gripper_action does not receive a action result before they timeout.

I think this is caused by the fact that the finger_joint velocities are a bit unstable (i.e., slightly increase) when the other joints are moving, or that the finger controller PID gains should be improved.

Investigation

Let's verify this hypothesis by inspecting the finger velocity commands while moving the arm arround. We can use the pick & place example that is given in the documentation. Below I added a gif of how the velocities behave when I move the arm arround. Keep in mind that the velocity threshold is set to 0.001.

Resting velocities

velocity: [-0.0008394972722331565, 0.002920198607601596, -0.0004664629045941222, 0.0018506860899074268, -0.008040965487597593, -0.0017851274417448255, 0.0021114798536561847, -5.6208884257551426e-05, 5.6490498751197266e-05]

From this we can see that the finger velocities are indeed a little bit influenced by the movement of the robot arm. I'm unsure if this is because of finger PID gains that can be improved or that this is due to instaiblities inside the gazebo simulation.

I however think that this should not cause to much problems as the velocity is only shortly above the threshold. I only becomes a problem when the arm controllers are tuned badly (see gif below).

Resting velocities

velocity: [-2.3617382151641126, 0.00779549310441726, 2.398145283557499, 0.004950285110020838, -0.5145020878195646, -0.21680003827061464, -0.7459508604510294, 0.031055436989710014, -0.031054684808693785]
velocity: [2.3645298595526008, -0.007416133146815294, -2.4055287574456763, -0.0077949747761998645, 0.5515721516252698, 0.22718758256158672, 0.7415553930080468, -0.030523518801102368, 0.03052436581763448]

Position trajectory controller PID gains

For this controller both the gazebo_ros_control.pid_gains and joint_position_trajectory_example_controller.gains are not tuned (see GIFS below).

With only controller gains and no gazebo PID gains

With controller gains and gazebo PID gains

[gollth]

Wow, many things happening at once here 😲

After diving deeper into the gazebo_ros_control repo, I think I now understand why @frankaemika doesn't provide the position and velocity hardware interfaces in the franka_hw_sim.

The gazebo_ros_control currently only directly support the gazebo effort hardware interface. When a robot specifies a hardware_interface::PositionJointInterface or hardware_interface::PositionVelocityInterface in its RobotHWSim interface, the gazebo SetAngle and SetVelocity functions are used to set the position and velocity (see ros-simulation/gazebo_ros_pkgs#135 (comment)). As these functions don't use any dynamics, they often fail when a robot has friction in the joints or interactions with the environment.

To solve this, the gazebo_ros_control gives users the ability to use a PID controller to control the robot position/velocity through the gazebo effort hardware interface. Users can enable this by setting gazebo_ros_control.pid_gains in the ROS parameter server (see default_robot_hw_sim.cpp#L203).

As the Panda robot only has effort controlled joints (see https://frankaemika.github.io/docs/libfranka.html#realtime-commands), it, therefore, doesn't make sense to ship the position and velocity joints in the franka_hw_sim.h interface.

As a result, I think merging #156 might not be the best solution. To keep the simulation as close as possible to the real simulation, the best thing to do would be to add the motion generators found in the FCI or equivalent PID controllers to the RobotHWSim interface. By doing this, people can specify joint position controller and joint velocity controllers, but under the hood, the Gazebo effort controllers would be used.

Merging #156 would only allow users to simulate joint position and joint velocity control using the earlier described logic of the gazebo_ros_control. As this does not seem to work out of the box (the robot has joint frictions), people need to tune the gazebo_ros_control.pid_gains gains and/or switch to using joint_effort_controllers/{joint_position_controller, joint_velocity_controller} controller. For both these options, they need to tune PID gains.

@gollth, @frankaemika Please correct me if my analysis is wrong.

To answer this first part: You are right, this was one of the reasons we didn't provide positions and velocity interfaces. I totally agree with keeping the simulation as close to the real system as possible. However due to proprietary reasons, unfortunately we are not able to release the internal workings of the FCI. This includes open-sourcing the motion generators.

Since as you say user would need to tune gains in both cases, maybe the solution proposed in #156 is the next best solution, still?

[gollth]

Hi @rickstaa,

I had a look at the problem with the alternating high velocities of joint 1. On first glance it seems that your gains are way too high. I tested with the following gains and got rid of the oscillations:

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]](https://answers.ros.org/question/312123/goal_tolerance_violated-gazebomoveit%5D) 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.

joint_effort_trajectory_example_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - $(arg arm_id)_joint1
    - $(arg arm_id)_joint2
    - $(arg arm_id)_joint3
    - $(arg arm_id)_joint4
    - $(arg arm_id)_joint5
    - $(arg arm_id)_joint6
    - $(arg arm_id)_joint7
  gains:
    $(arg arm_id)_joint1: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint2: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint3: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint4: { p: 600, d: 30, i: 0 }
    $(arg arm_id)_joint5: { p: 250, d: 10, i: 0 }
    $(arg arm_id)_joint6: { p: 150, d: 10, i: 0 }
    $(arg arm_id)_joint7: { p:  50, d:  5, i: 0 }
  constraints:
    goal_time: 0.5
    $(arg arm_id)_joint1: { goal: 0.05 }
    $(arg arm_id)_joint2: { goal: 0.05 }
    $(arg arm_id)_joint3: { goal: 0.05 }
    $(arg arm_id)_joint4: { goal: 0.05 }
    $(arg arm_id)_joint5: { goal: 0.05 }
    $(arg arm_id)_joint6: { goal: 0.05 }
    $(arg arm_id)_joint7: { goal: 0.05 }

With this rough tuning the robot is still able to track the joint trajectories okayish in Gazebo. See position error in right plot is around 1.2 degrees peak. Also the jittering in RViz is (almost) gone. One could tune further of course. I will test on the real robot as well. Looks stable on the real hardware as well.

If this is useful we can think about adding the tuned gains into the sim_controllers.yaml

[rickstaa]

Wow, many things happening at once here astonished

After diving deeper into the gazebo_ros_control repo, I think I now understand why @frankaemika doesn't provide the position and velocity hardware interfaces in the franka_hw_sim.
The gazebo_ros_control currently only directly support the gazebo effort hardware interface. When a robot specifies a hardware_interface::PositionJointInterface or hardware_interface::PositionVelocityInterface in its RobotHWSim interface, the gazebo SetAngle and SetVelocity functions are used to set the position and velocity (see ros-simulation/gazebo_ros_pkgs#135 (comment)). As these functions don't use any dynamics, they often fail when a robot has friction in the joints or interactions with the environment.
To solve this, the gazebo_ros_control gives users the ability to use a PID controller to control the robot position/velocity through the gazebo effort hardware interface. Users can enable this by setting gazebo_ros_control.pid_gains in the ROS parameter server (see default_robot_hw_sim.cpp#L203).
As the Panda robot only has effort controlled joints (see https://frankaemika.github.io/docs/libfranka.html#realtime-commands), it, therefore, doesn't make sense to ship the position and velocity joints in the franka_hw_sim.h interface.
As a result, I think merging #156 might not be the best solution. To keep the simulation as close as possible to the real simulation, the best thing to do would be to add the motion generators found in the FCI or equivalent PID controllers to the RobotHWSim interface. By doing this, people can specify joint position controller and joint velocity controllers, but under the hood, the Gazebo effort controllers would be used.
Merging #156 would only allow users to simulate joint position and joint velocity control using the earlier described logic of the gazebo_ros_control. As this does not seem to work out of the box (the robot has joint frictions), people need to tune the gazebo_ros_control.pid_gains gains and/or switch to using joint_effort_controllers/{joint_position_controller, joint_velocity_controller} controller. For both these options, they need to tune PID gains.
@gollth, @frankaemika Please correct me if my analysis is wrong.

To answer this first part: You are right, this was one of the reasons we didn't provide positions and velocity interfaces. I totally agree with keeping the simulation as close to the real system as possible. However due to proprietary reasons, unfortunately we are not able to release the internal workings of the FCI. This includes open-sourcing the motion generators.

Since as you say user would need to tune gains in both cases, maybe the solution proposed in #156 is the next best solution, still?

@gollth Thanks for your answer!

No problem, I think this is closely related to the discussion held in #143. On one side, you have to hide the internal workings of the robot and the controllers from your competitors, but also want to provide the best experience for researchers. Since even a .dll or .so can eventually be reverse engineered, I understand your decision not to share the motion controllers.

I agree with you that #156 would already be a significant improvement. This gives users the ability also to test position and velocity controllers in the simulation if they need to. In that case, it would be beneficial if you could provide working gazebo_ros_control.pid_gains. Nevertheless, in most of my research and my colleagues, the effort controllers are used.

The main reason why I created this pull request is that the new franka_gazebo simulation doesn't work anymore with the joint_position_example_controller that is provided by the franka_example_controllers package. I think this controller is used in the panda_moveit_config since it is easy to use for beginners, as on the real robot, it doesn't require the tuning of PID gains. I think for releasing the Noetic branch, merging #156, while finding gazebo_ros_control.pid_gains that makes the joint_position_example_controller work on the simulation would be enough. I quickly tried tuning them manually, but did not yet have the correct values.

Hi @rickstaa,

I had a look at the problem with the alternating high velocities of joint 1. On first glance it seems that your gains are way too high. I tested with the following gains and got rid of the oscillations:

Great, thanks a lot! I already found them suspiciously high, but like the gazebo_ros_control.pid_gains didn't yet find the time to investigate. Did you use an optimizer or also tuned them manually? I think adding the PID gains you found to https://github.com/frankaemika/franka_ros/blob/develop/franka_gazebo/config/sim_controllers.yaml might help people out.

For adjusting the gazebo_ros_control.pid_gains, we could add a dynamic reconfigure server to the https://github.com/ros-simulation/gazebo_ros_pkgs/blob/231a7219b36b8a6cdd100b59f66a3df2955df787/gazebo_ros_control/src/default_robot_hw_sim.cpp#L220 so that we can manually tune them. Or do you have another idea?

[rickstaa]

@gollth I just noticed that the gazebo_ros_control.pid_gains are not used in our case since we are not using the DefaultRobotHWSim but the franka_gazebo/FrankaHWSim simulation hardware interface.

As, a result my comments above are incorrect. To fix the position and velocity control, we need to implement the PID logic inside the franka_gazebo/FrankaHWSim class.

[rickstaa]

@gollth I managed to add motion generators into the FrankaHWSim see #181.

[rickstaa]

This feature was implemented in 2514ae1.