Adds position and velocity motion generators to FrankaHWSim

[rickstaa]

This pull request adds the hardware_interface/PositionJointInterface'and hardware_interface/VelocityJointInterface interfaces to the FrankaHWSim module. When a user chooses these interfaces, motion generators (i.e. PID controllers) are used for controlling the robot.

• This pull request requires Adds the 'transmission' arg to the 'panda_arm.urdf.xacro' file #157 to be merged first.
• I did not extensively tune the PID gains of the motion generators, but they seem to work. Feel free to adjust them further.
• There is one minor bug in the current implementation that you might want to solve in future versions. The arm moves slightly at the beginning of the simulation when the position and velocity motion generators are used. This is probably because the joint commands are not initialized to the starting joint angles but to 0. As I'm a bit short on time, I did not investigate this issue further. Adds position and velocity motion generators to FrankaHWSim #181 is, in my opinion, good enough for releasing the Noetic version of the panda_moveit_config.

TODOS

• Tune PID gains of the motion generators if you want smoother behaviour.
• Adjust the trajectory controller goal tolerance (see [feature_request] Decrease joint trajectory controller goal tollerance #201).

Other TODOS

• Update documentation.

[rhaschke]

Just a few remarks. Generally looks good to me.

[rickstaa]

@gollth I think this pull request provides the best solution since users can now work with the simulation in the same way as they work with the real robot. I think it is ready to be merged after we improved the PID gains of the motion planners (see GIF below).

[rickstaa]

As explained in #135, I based the code in this pull request on the implementation found in the gazebo_ros_control. There they also use the control_toolbox PID's instead of the PID controllers that are implemented by Gazebo (i.e. SetPositionTarget and SetVelocityTarget). The reason is given here:

Gazebo already has built-in PID controllers for these two control modes

I believe that the reason why control_toolbox's PIDs are used instead of Gazebo's implementation is that loading the controller configuration (gains and clampings) is straightforward and already implemented for the former (via ROS params).

For the FrankaHWSim we could therefore also directly use the Gazebo PID controllers. This will, however, make tuning a bit harder.

[rhaschke]

I'm confused. #183 doesn't deal with motion generators in FrankaHWSim. However, the contributions here are required for [position|velocity]_controllers/JointTrajectoryController to be used in simulation, aren't they?

[rickstaa]

@rhaschke Ah your right! Based on our meeting I assumed your pull request was fully based on #181. I now see that it only includes the commits from #157.

[rickstaa]

@rpapallas The pull request found in #224 do currently not configure any position/effort controllers. They only add the required code changes to use position controllers. You are still expected to load your own position/velocity controllers. For an example, see e60cf08. Also see #181 (comment) for all the steps required for using the new (simulated) position and velocity motion generators.

I guess @frankaemika could include a position and velocity example controller in the https://github.com/frankaemika/franka_ros/blob/develop/franka_gazebo/config/sim_controllers.yaml. I, however, did not do this since the position and effort motion generators are not 100% equal to the motion generators on the real robot. I, therefore, left this decision to @frankaemika.

I did include a position_joint_trajectory_controller because this controller is required by MoveIt tutorials and these tutorials are often used in combination with the Panda robot.

franka_ros/franka_gazebo/config/sim_controllers.yaml

Line 70 in 765cacd

position_joint_trajectory_controller:

[rpapallas]

Thank you, @rickstaa. I was able to get it working.

[marcbone]

Ok guys. We heavily modified this PR and merged it into develop.

The feature works now a bit different. You do not have to specify at launch time which type of interface you want to use. All three are available. So you can start a position controller, stop it and then run an effort controller. Just like on the real robot. However, you cannot have a controller to control only one joint as @rickstaa did for his demo, where he had 7 controllers (one for each joint). You need to use one controller to control all joints, as you would on the real robot.
Also when you launch the robot in gazebo without any controller, the robot will now keep its position.

So any controller that does not use the cartesian pose or cartesian velocity interface and runs on the real robot should now also work in gazebo.

I am going to close this PR , #157 and #224

[rpapallas]

Thank you for this; much appreciated.

@marcbone, To get this simulated position controller working in gazebo, I use the following:

roslaunch franka_gazebo panda.launch controller:=joint_position_example_controller

But I can't seem to find where the controller is expecting the new joint positions, is there a ros topic / actionlib that I need to use to send joint positions to the robot?

Thank you.

[gollth]

Hi @rpapallas,

you can for example start the Position Joint Trajectory Controller and use its action or topic interface

$ roslaunch franka_gazebo panda.launch controller:=position_joint_trajectory_controller

$ rostopic list /position_joint_trajectory_controller
/position_joint_trajectory_controller/command                                # <-  Topic interface
/position_joint_trajectory_controller/follow_joint_trajectory/cancel         # -.
/position_joint_trajectory_controller/follow_joint_trajectory/feedback       #  |
/position_joint_trajectory_controller/follow_joint_trajectory/goal           #  }- Action interface
/position_joint_trajectory_controller/follow_joint_trajectory/result         #  |
/position_joint_trajectory_controller/follow_joint_trajectory/status         # -`
/position_joint_trajectory_controller/state

[rpapallas]

Hi @gollth thank you for the details. I don't think I can run that successfully despite having ros-noetic-ros-controllers, ros-noetic-trajectory-controller and ros-noetic-joint-trajectory-controller installed.

Running the controller spawner alone also suggests that it can't find the controller:

rosrun controller_manager spawner position_joint_trajectory_controller

with the following output:

[INFO] [1647252909.642155]: Controller Spawner: Waiting for service controller_manager/load_controller
[WARN] [1647252939.845601]: Controller Spawner couldn't find the expected controller_manager ROS interface.

[marcbone]

Try to install ros-noetic-position-controllers, but actually they should be part of ros-controllers.

Does the controller show up in the rqt_controller_manager?

[rpapallas]

Hi @marcbone indeed the position-controllers is installed. The rqt_controller_manager opens and has an empty dropdown menu when I run it through rosrun rqt_controller_manager rqt_controller_manager

[marcbone]

That is strange. Could you post the output of roslaunch franka_gazebo panda.launch please?

[gollth]

Sometimes RQT does not reload newly installed plugins automatically. Does

rqt --force-discover

help?

[rpapallas]

Hi @gollth thank you for your support. I found the problem. I am using a Singularity container to have the system working (it's similar to a Docker container). I installed ROS Noetic and Ubuntu 20.04 but only Python3. Your delayed_controller_spawner.py's shebang points to Python 2 and it couldn't find it. Since Python 2 is not supported, I changed the shebang to #!/usr/bin/env python3 and it now works.

[rpapallas]

If you think that shebang should be Python 3, I can create a PR, if you want.

[gollth]

Changing the shebang to python3 would make it so that delayed_controller_spanwer.py wouldn't work on melodic anymore.. I think the script is simple enough to work on both python2 and python3. Can you just install python-is-python3 in your docker container?

[rpapallas]

Perfect; didn't know about this! Thank you very much for the help!

[rhaschke]

@gollth, you can install python scripts via catkin_install_python() as explained here and have the shebang rewritten correctly.

[gollth]

have the shebang rewritten correctly.

In the link you sent it seems the correct way is #!/usr/bin/env python what we already have in the script. Could you elaborate what you mean with "correct" @rhaschke ?

[rhaschke]

The cmake function catkin_install_python() will "correctly" rewrite the shebang during the installation process to use either python2 or python3. Thus, python-is-python3 or python-is-python2 is not needed when running from an install space.
The question is how you install the python script(s). However, I couldn't find delayed_controller_spanwer.py.

[marcbone]

@rhaschke Thanks for the hint. I fixed it

[rpapallas]

Many new things learnt today! Glad I am in this thread 😊 thank you, @marcbone and @rhaschke!

[rickstaa]

Ok guys. We heavily modified this PR and merged it into develop.

The feature works now a bit different. You do not have to specify at launch time which type of interface you want to use. All three are available. So you can start a position controller, stop it and then run an effort controller. Just like on the real robot. However, you cannot have a controller to control only one joint as @rickstaa did for his demo, where he had 7 controllers (one for each joint). You need to use one controller to control all joints, as you would on the real robot. Also when you launch the robot in gazebo without any controller, the robot will now keep its position.

So any controller that does not use the cartesian pose or cartesian velocity interface and runs on the real robot should now also work in gazebo.

I am going to close this PR , #157 and #224

@marcbone Thanks for getting this implemented. Your implementation makes a lot of sense! I will perform some tests today to see which of my issues can be closed.

[rickstaa]

Test results

@marcbone I reviewed your pull request, and all of the controllers below seem to work as expected (see table below). Thanks for improving on my PR.

	Gazebo Tests	Comment	franka_control	franka_example_controllers	franka_gazebo	Loaded by default
joint_velocity_example_controller	✔️		❌	✔️	✔️	✔️
joint_position_example_controller	✔️		❌	✔️	✔️	✔️
cartesian_pose_example_controller	❌	Interface not yet implemented.	❌	✔️	❌	❌
elbow_example_controller	❌	Interface not yet implemented.	❌	✔️	❌	❌
cartesian_velocity_example_controller	❌	Interface not yet implemented.	❌	✔️	❌	❌
model_example_controller	✔️		❌	✔️	✔️	✔️
force_example_controller	✅	Works but drift due to #160.	❌	✔️	✔️	✔️
joint_impedance_example_controller	❌	Interface not yet implemented.	❌	✔️	❌	❌
cartesian_impedance_example_controller	✔️		❌	✔️	✔️	✔️
dual_arm_cartesian_impedance_example_controller	-	Not tested.	❌	✔️	❌	❌
teleop_joint_pd_example_controller	-	Not tested.	❌	✔️	❌	❌
franka_state_controller	✔️		✔️	❌	✔️	✔️
position_joint_trajectory_controller	✔️		✔️	❌	✔️	✔️
effort_joint_trajectory_controller	✔️		✔️	❌	✔️	✔️
joint_effort_controller	✔️		❌	❌	❌	❌
joint_position_controller	✔️		❌	❌	❌	❌
joint_velocity_controller	✔️		❌	❌	❌	❌
franka_gripper	✔️		❌	❌	❌	✔️

Steps to test these controllers

1. Build libfranka from source according to the documentation.
2. Create a catkin workspace.
3. Clone rickstaa/test-effort-position-velocity-controllers into this workspace.
4. Install the ros dependencies using rosdep install --from-path src --ignore-src -y -r --skip-keys libfranka .
5. Build the catkin workspace catkin build -DCMAKE_BUILD_TYPE=Debug -DFranka_DIR:PATH=<PATH_TO_INSTALL>/libfranka/build.
6. Source the workspace.
7. Start the panda simulation.

roslaunch franka_gazebo panda.launch 

8. Load the rqt_controller_manager and start a position/effort/velocity/trajectory controller.

rosrun rqt_controller_manager rqt_controller_manager

9. Start a position/effort/velocity or trajectory dynamic reconfigure server:

 rosrun franka_gazebo joint_position_dynamic_reconfigure_server.py

rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller

11. For the position/effort/velocity control also start the dynamic reconfigure GUI:

rosrun rqt_gui rqt_gui -s reconfigure

12. Control the robot by changing the joint positions/efforts/velocities in the joint_position_reconfig_server tab.
13. See that it works.

[rickstaa]

@marcbone, @gollth I just reviewed that all new controllers work as expected (see #181 (comment)). I, however, have several minor remarks:
- If people want to use simulated position controls in research, PID controller gains might still need to be improved. People, however, can do this in the future.
- Gravity compensation is still broken when working with effort or velocity control (see #160). The current solution only masks the problem because position control is enabled by default. I will continue my discussion on this in #160.
- Maybe it is an idea to add the configurations for the ros_control position/effort/velocity group controllers to the controller configurations (i.e. https://github.com/frankaemika/franka_ros/blob/develop/franka_gazebo/config/sim_controllers.yaml and https://github.com/frankaemika/franka_ros/blob/develop/franka_control/config/default_controllers.yaml). I think most people are going to use these controllers when they start instead of writing C++ controllers themselves.

joint_position_group_example_controller:
  type: effort_controllers/JointGroupEffortController
  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