Make Panda robot Gazebo compatible

[tahsinkose]

Continues from #52. I've rebased existing work onto melodic-devel. Changes can be listed as:

• Introduces handy macros for joints&links in arm and handa .xacro files. Although one may find this structure slightly complicated, I believe it makes any future additions trivial-ish. Besides overall file lengths dropped slightly.
• Brings back collision STLs. Adds xacro arg use_cylinder_collision_model for the .launch-time specification of collision model. It defaults to true. In MoveIt we need meshes. See usage at https://github.com/tahsinkose/panda_moveit_config/blob/c88412d75651e66dcfecae5b877e6d3c910d2c7c/launch/gazebo.launch#L16-L17.
• Adds another xacro arg use_gazebo_sim. There is one major and one minor reason. Major one is that I did not want the xacro process to waste time on loading irrelevant sections. Minor one is the fact that mimic joints are not properly treated by Gazebo. Hence that field needs to be absent when launching robot for Gazebo.

[simonbogh]

I think it would make sense to add ${arm_id}_ to the virtual_joint, else you can get an error that virtual_joint is not unique when using multiple robots in the same environment or importing into moveit setup assistant (MSA). The dual arm example works on its own because of namespace, but if you import dual_panda_example.urdf.xacro into MSA you will get an error.

[tahsinkose]

@simonbogh, are you authorized to merge PRs in this repo?

[fwalch]

Hi, sorry for the late response, and thanks for the contribution! I tested Gazebo on Noetic, and everything worked fine. The robot is moving quite slowly however, which is causing ABORTED: Solution found but controller failed during execution warnings.

On Melodic, we ran into an issue with panda_moveit_config seemingly not being compatible with the MoveIt packages from the Ubuntu binary repos (moveit/panda_moveit_config#76). Are you compiling Moveit from source on Melodic?

[tahsinkose]

The robot is moving quite slowly however, which is causing ABORTED: Solution found but controller failed during execution

It probably needs fine-tuning. The idea is to provide a baseline that could be also adapted by the other robots. After merging this PR, "Panda in Gazebo" becomes a dedicated use-case that people can solve in particular.

Are you compiling Moveit from source on Melodic?

Yes. moveit/panda_moveit_config#68 is the PR that applies the changes suggested in https://ros-planning.github.io/moveit_tutorials/doc/gazebo_simulation/gazebo_simulation.html. You may want to use that PR or manually apply the changes on a freshly generated panda_moveit_config.

[rickstaa]

@tahsinkose Thanks a lot for creating this pull request and the accompanying documentation. When it merges this pull request allows me to switch back from my local forks to the upstream franka_ros and panda_moveit_config branches in my projects. However, when testing out your pull request, I noticed that some dependencies are missing in the package.xml of the panda_moveit_config branch. I created a pull request onto your branch. I further also removed several redundant PID gains from the ros_controllers.yaml file. The pull request for this change can be found here.

[tahsinkose]

@fwalch probably you didn't receive notification on my reply at #126 (comment) since I didn't explicitly mention you. I believe PID tuning in Noetic needs to be separately addressed after enabling native MoveIt support. What do you think?

[tahsinkose]

The robot is moving quite slowly however,

@fwalch, this was because I adjusted velocity and acceleration scalars to be 0.1 😆 I made them 1.0 again in moveit/panda_moveit_config@c3eed9c#diff-0b89fdfad571d8e4315bc8654941cd7bed8155ed97b9a71756716619333566efR250.

[gollth]

Hello and thanks for your contribution on this topic. We had a close look at your changes and found them very valuable in simulating the robot with Gazebo.

However due to some internal processes we are not able to merge this directly. Rather we integrated your changes together with some other features in a complete Gazebo integration of franka_ros. Next to the necessary URDF changes this includes also:

• Gravity Compensation in Gazebo, similar to how the real Panda does it
• Offering the same FrankaState- & FrankaModelInterface as franka_control does
• Offering the same action interfaces like franka_gripper does
• Offering similar service (non-realtime) interfaces similar to how franka_control does it

Hopefully this will allow you to simulate existing ROS controllers now in Gazebo as well.

Check out our documentation for more information.

Since this feature is now implemented I will close this PR now. Thanks for your effort!

[rickstaa]

@gollth Thanks a lot for this feature improvement! It takes many things on my to-do list that I planned to implement for my research (i.e. gravity composition, better collision meshes, etc.). Can I maybe ask you some questions about your implementation?

• It looks you use collision files similar to the ones created by @AndrejOrsula. However, I see that you did use a less accurate stl for the finger collision description (https://github.com/frankaemika/franka_ros/blob/noetic-devel/franka_description/meshes/collision/finger.stl vs. https://github.com/AndrejOrsula/panda_ign/blob/master/panda/meshes/collision/finger.stl). Was this done because you required an extra safety boundary for the fingers? I created Adds accurate finger collision mesh #146 to show what I mean.
• Why are geometries + an extra safety boundary used instead of meshes? You appeared to have used meshes in the past (see aa2f01e), but these were replaced by geometries by @MohamadrezaSabaghian in 616055e. My guess is that by doing this, you can provide users a way to dynamically specify the safety boundary? Maybe you can provide a xacro argument to let users decide which approach they want to use.
  

  franka_ros/franka_description/robots/hand.xacro

  Lines 50 to 52 in 356ec11

  	<geometry>
  	<sphere radius="${0.02+safety_distance}" />
  	</geometry>

• Why did you choose not to set a collision geometry for the hand/fingers, in the hand.xacro file (like was one in this repository?). As a result of not specifying collision geometries for the fingers i get several warnings when using the urdf in the panda_moveit_config/demo.launch launch file (see the warnings below). Here again you appeared to have used meshes in the past, but these were removed in cecb1b7.
  

  franka_ros/franka_description/robots/hand.xacro

  Lines 70 to 84 in 356ec11

  	<joint name="${ns}_finger_joint1" type="prismatic">
  	<parent link="${ns}_hand"/>
  	<child link="${ns}_leftfinger"/>
  	<origin xyz="0 0 0.0584" rpy="0 0 0"/>
  	<axis xyz="0 1 0"/>
  	<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
  	</joint>
  	<joint name="${ns}_finger_joint2" type="prismatic">
  	<parent link="${ns}_hand"/>
  	<child link="${ns}_rightfinger"/>
  	<origin xyz="0 0 0.0584" rpy="0 0 0"/>
  	<axis xyz="0 -1 0"/>
  	<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
  	<mimic joint="${ns}_finger_joint1" />
  	</joint>

• Why did you choose to use a geometry instead of a mesh in the panda_gazebo.xacro file? I created Improve gazebo hand/finger collision behavoir #147 to show how we can improve the gazebo collision behavoir.
  

  franka_ros/franka_description/robots/panda_gazebo.xacro

  Lines 305 to 324 in 356ec11

  	<link name="${ns}_leftfinger">
  	<xacro:inertia-cylinder mass="15e-3" radius="0.01" h="0.04"/>
  	<visual>
  	<geometry>
  	<mesh filename="package://franka_description/meshes/visual/finger.dae"/>
  	</geometry>
  	</visual>
  	<collision>
  	<origin xyz="0 0.015 0.03" rpy="0 0 0"/>
  	<geometry>
  	<box size="0.03 0.005 0.047" />
  	</geometry>
  	</collision>
  	<collision>
  	<origin xyz="0 0.007 0.045" rpy="0 0 0"/>
  	<geometry>
  	<box size="0.03 0.015 0.017" />
  	</geometry>
  	</collision>
  	</link>

Demo.launch warnings

[ WARN] [1628672207.286446541]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.
[ WARN] [1628672207.286477598]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry.

[rickstaa]

@gollth Can I ask you one small other question? I was wondering why you are publishing the /joint_state_desired topic when controlling the real robot but not for the simulated robot? I think adding '/joint_states_desired' topic both in simulation and the real robot would improve translating between the two. I created pull request #145 to show what I mean.

[gollth]

Thanks you @rickstaa for your interest in this topic.

• Your suggestion with better finger collision meshes definitely makes sense. Reason was that we started testing with convex collision shapes in the beginning and never changed them since then. We will have a more in depth look to your PR in near future. I think that would also solve your third point, not?
• Regarding the mesh vs. geometry differences: Geometries plus a safety distance are used for the real hardware since this relates to its internal collision model. However in simulation convex cylinders (plus a safety distance) can easily overlap with other links causing weird effects in Gazebo, which is why we chose to go with simple meshes here. Remember: You can always assemble and specify your own URDF according to what collision model you want right now.
• Would be solved by first point, not?
• Answered by first point

Regarding the /joint_state_desired topic, we will have a deeper look as well, thanks in advance already

[rickstaa]

@gollth Thanks a lot for the fast response and the clear explanation.

• Adding the gazebo collision meshes that are found in Improve gazebo hand/finger collision behavoir #147 will solve the problems I encountered when working with the franka_gazebo package.

• My third point however is related to how other third party packages use the collision meshes/geometries in the robot_description. MoveIt for example expects each controllable joint to have a collision mesh/geometry. If these are not found, it will throw warnings. To solve my third point, therefore, not only collision meshes have to be added to the gazebo urdf but also the urdf that is used with the real robot. I created several pull requests to show what I mean. I will leave it up to you to decide whether any of these solutions are compatible with the Robot control software.

  • Improves hand/finger collision geometries #150 In this pull request, I created the collision geometries that are defined for the fingers but found in the hand joint. I replaced these with more accurate geometries in the fingers.
  • Moves finger collision geometries to the finger joints #151 Similar to Improves hand/finger collision geometries #150 but in this pull request, I simply moved the geometries that you use for the fingers from the hand joint to the finger.
  • move finger collision geometry sparse #152 Similar to Moves finger collision geometries to the finger joints #151 but now the cylinder geometry between the finger was removed.
  • In any of these cases, I think it might be beneficial to give users the ability to specify the safety_distance. A separate pull request for this feature request can be found in Adds the ability to specify the safety_distance #153.

[rickstaa]

@gollth I added one last pull request that is related to point 2 of my initial post:

Why are geometries + an extra safety boundary used instead of meshes? You appeared to have used meshes in the past (see aa2f01e), but these were replaced by geometries by @MohamadrezaSabaghian in 616055e. My guess is that by doing this, you can provide users a way to dynamically specify the safety boundary? Maybe you can provide a xacro argument to let users decide which approach they want to use.

This pull request #154 gives users the ability to switch between a mesh based robot-description and a geometry based robot_description. This allows users to use mesh based collision properties for an external planner like MoveIt. I use this pull request in my own projects, but I am unsure if it is desired to give users the ability to do this. I will leave that decision up to you.

[rickstaa]

@gollth can I ask you one last question about the urdf.

I looked through both the libfranka and franka_ros code to see where you used the collision geometries defined in the urdf. I however saw no use of the urdf::Link::collision method. I only saw usage of other methods of the urdf library.

After this search, I came to the conclusion that the (when working with the real robot) collision properties in the urdf are there to be used by third party planners like MoveIt. You replaced the simple meshes that were used in earlier version by geometries to make sure that movements that are computed by these third party planners are guaranteed to be executable on the real robot. Preferably, you would also like to use these geometries in the gazebo simulation, but due to instabilities in the Gazebo simulation this was not possible. As a result, for the gazebo simulation, you use simple meshes.

If I understood the above correctly, that means that the motions that are allowed are different when working with the simulated one compared to the real one. This means that if I train a RL algorithm in simulation, it might learn motions that are restricted in the real robot. The only way to solve this discrepancy is to create collision geometries, similar to those used in the FCI, that do not cause earlier mentioned instabilities in the gazebo simulation.

[rickstaa]

In that case I think merging #137, #139, #145 and #147 would possibly improve user experience.

For #153, it currently only changes the safety_distance of the urdf not the one used in your FCI. Is the safety_distance in the FCI set to 0.0 or also to 0.03?

Additionally, you could merge #154 to give users the ability to switch to simple meshes instead of collisions. This however is up to you, as users can also just make these changes themselves if they really need them.

I am not sure about #150, #151 and #152 which I created to solve warnings thrown by MoveIt when no collision geometry is found in any of the joints that are controlled. I do not know which solution is the best, as I don't know the exact collision geometries that are used in the FCI. Maybe you can the team can take a look if solving these warning is desired.

[rickstaa]

@gollth I created #155, #156 and #157 since I ran into some problems when switching between controlling the real and simulated robot using MoveIt.

[rickstaa]

@gollth I created feature request #161, #162, #163, #164 to make the issues I explained it easier to track. We can continue our discussion on any of these feature requests.