demo_gazebo.launch with standard ROS packages

[rhaschke]

This is an attempt to assemble a demo_gazebo.launch file without using franka_ros/franka_gazebo.
TODOs:

• I had to manually remove all franka_gazebo related stuff from the URDF. Thus there is a pre-processed panda_hand.urdf.
  We should add an option in franka_description to create both versions of the Gazebo-capable URDF.
• The files ros_controllers.launch / ros_controllers.yaml introduced via Setup assistant automating generation of controllers configs moveit/moveit#908 seem to be related to https://github.com/PickNikRobotics/moveit_sim_controller. As far as I understand, this project aims to implement a fake controller that uses the same interface as ros_control.
  Can you confirm both assumptions, @mohmadAyman?
  If so, I think the name ros_controllers was a bad choice for the name. I suggest fake_ros_controllers.
• Define PID parameters for the joint controllers
• Verify that joint trajectory controllers work
• Verify moveit_sim_controller config

@rickstaa, can you help on this?
@guihomework, can you point me to appropriate ROS controller configs of our robots?

[rickstaa]

@rhaschke I think you are right that providing both options is best. I guess I already implemented the solution you are asking for in my attempt to create a fix for the MSA (see TODO 4 under the Moveit_setup_assistant todos section).

Can you take a look at rickstaa#40 and 1c47532 and check if that is what you had in mind? I'm happy to rebase it or change it to separate it from the other franka_gazebo. I'm happy to help with #9 if it is not what you had in mind.

[rickstaa]

@rhaschke I also added #9 to our todos.

[mayman99]

@rhaschke , yes I can confirm the aim was to implement a controller that uses the same interface as ros_control to be used for simulation purposes.

[guihomework]

@rhaschke we use a non standard RobotHWSim for the Shadow hands ( https://github.com/ubi-agni/sr_core/tree/ubi-melodic-devel/sr_gazebo_sim ), so the config is maybe not relevant for your Default RobotHWSim

I could find which of our robots use gazebo_ros_control using the keyword in a search in our repo, and we mostly use the DefaultRobotHWSim on the pa10 robot sim (but they are not used in sim so often and not particularly well tuned when a heavy payload as the Shadow hand is attached to them I would say)

https://github.com/ubi-agni/pa10_7a_description/blob/master/config/gazebo_controller.yaml

if you still want to use the one of the Shadow hands they are there
https://github.com/ubi-agni/agni_robots/blob/noetic-devel/agni_description/config/shadow_motor_right_position_controller_gazebo.yaml

Our kuka sim do not use ros_control, but some other sim of kuka use a their own RobotHWSim and their own controllers (https://github.com/CentroEPiaggio/kuka-lwr)

[rhaschke]

Can you take a look at rickstaa#40 and 1c47532 and check if that is what you had in mind?

Yes, thanks. That's exactly what I was looking for. I will continue on this PR later...
Thanks, @guihomework, for looking up our refs.

[rhaschke]

Now, the following operating modes are available:

• real robot (position-controlled): roslaunch panda_moveit.launch
• gazebo_ros sim (effort-controlled): roslaunch demo_gazebo.launch
  The effort-based hardware interface is configured by default, but other interfaces (I tested position) work as well.
  However, this requires changes in two locations: gazebo_ros_controllers.yaml and demo_gazebo.launch.
• franka_gazebo sim (effort-controlled or Franka controllers): roslaunch panda_moveit_config franka_gazebo.launch
  To use position- or velocity-based hardware interfaces we need Adds position and velocity motion generators to FrankaHWSim frankaemika/franka_ros#181
• moveit_sim_controller (fake ROS control, no Gazebo):

  roslaunch panda_moveit_config demo.launch moveit_controller_manager:=ros_control
  roslaunch panda_moveit_config moveit_sim_controller.launch
  

• The commits 3a6f056 and 3a6f056 depend on Cosmetic changes to xacro and launch files frankaemika/franka_ros#183 to enable generation of appropriate URDFs.

[rickstaa]

@rhaschke I reviewed your new code-base and I think it is a great improvement! I will test the following options:

• real robot (position-controlled): roslaunch panda_moveit.launch: I have a timeslot with the robot next Monday.
• ✔️ gazebo_ros sim (effort-controlled): roslaunch demo_gazebo.launch: Works there are however some things I found (see sections below).
• ❌ franka_gazebo sim (effort-controlled or Franka controllers): roslaunch panda_moveit_config franka_gazebo.launch: Has several warnings and errors (see sections below).
• ✔️ moveit_sim_controller (fake ROS control, no Gazebo): Works Fix shared pointer conversion bug PickNikRobotics/moveit_sim_controller#9 is merged.

You are probably already aware of most points I mentioned below but allow me to briefly state the problems I encountered when testing this pull request.

Gazebo_ros sim

• Currently, the panda_hand_controller has to be loaded manually. Do we want to do this automatically?
• The position controller works but does throw a failed with error GOAL_TOLERANCE_VIOLATED warnings. I think this is because the [gazebo_ros_control/pid_gains] should be improved.
• When trying to control the panda hand using the panda_hand_controller I get the following error:

[/gazebo] [ERROR] [WallTime: 1635254600.108482341, 216.149000000]: Joints on incoming goal don't match the controller joints.
[/move_group] [ WARN] [WallTime: 1635254600.109364018, 216.150000000]: Controller /panda_hand_controller failed with error INVALID_JOINTS:
[/move_group] [ WARN] [WallTime: 1635254600.109494627, 216.150000000]: Controller handle /panda_hand_controller reports status FAILED

This is solved by commenting on the passive joint in the hand.xacro file. If I remember it correctly this line is however needed for the franka_gazebo simulation since it uses the franka_gripper/gripper_command action server.

panda_moveit_config/config/hand.xacro

Line 26 in d76af19

<passive_joint name="panda_finger_joint2"/>

Franka_gazebo

• When I launch the simulation using the roslaunch panda_moveit_config franka_gazebo.launch command both the cartesian_impedance_example_controller and panda_arm_controller are loaded. This leads to conflicts:

[/gazebo] [ WARN] [WallTime: 1635257268.049186574, 0.795000000]: Resource conflict on [panda_joint1].  Controllers = [cartesian_impedance_example_controller, panda_arm_controller, ]
[/gazebo] [ WARN] [WallTime: 1635257268.049361774, 0.795000000]: Resource conflict on [panda_joint2].  Controllers = [cartesian_impedance_example_controller, panda_arm_controller, ]
[/gazebo] [ WARN] [WallTime: 1635257268.049390041, 0.796000000]: Resource conflict on [panda_joint3].  Controllers = [cartesian_impedance_example_controller, panda_arm_controller, ]
[/gazebo] [ WARN] [WallTime: 1635257268.049405802, 0.796000000]: Resource conflict on [panda_joint4].  Controllers = [cartesian_impedance_example_controller, panda_arm_controller, ]
[/gazebo] [ WARN] [WallTime: 1635257268.049421574, 0.796000000]: Resource conflict on [panda_joint5].  Controllers = [cartesian_impedance_example_controller, panda_arm_controller, ]
[/gazebo] [ WARN] [WallTime: 1635257268.049444384, 0.796000000]: Resource conflict on [panda_joint6].  Controllers = [cartesian_impedance_example_controller, panda_arm_controller, ]
[/gazebo] [ WARN] [WallTime: 1635257268.049462411, 0.796000000]: Resource conflict on [panda_joint7].  Controllers = [cartesian_impedance_example_controller, panda_arm_controller, ]
[/gazebo] [ERROR] [WallTime: 1635257268.049508482, 0.796000000]: Could not switch controllers, due to resource conflict
[/panda/panda_controller_spawner] [INFO] [WallTime: 1635257268.049268, 0.795000]: Started controllers: franka_state_controller, cartesian_impedance_example_controller
[/panda/trajectory_controller_spawner] [ERROR] [WallTime: 1635257268.049735, 0.795000]: Failed to start controllers: panda_arm_controller

Maybe we should set the controller arg default to " " and prevent the panda_arm_controller from loading in the ros_controllers.launch file when the controller arg is specified?

• If I use controller:=panda_arm_controller the panda_arm_controller is loaded through the controller_manager that is inside the franka_ros/panda.launch file. In that case I need to specify the controller_list parameter.
• If I change the controller arg default to " " the moveit_ros_control_interface::MoveItControllerManager is used and the controllers are loaded successfully. I however do receive several warnings because panda_joint7 starts out of bounds:

[/panda/move_group] [ WARN] [WallTime: 1635258153.990667834, 47.367000000]: Joint 'panda_joint7' from the starting state is outside bounds by a significant margin: [ -3.31565 ] should be in the range [ -2.8973 ], [ 2.8973 ] but the error above the ~start_state_max_bounds_error parameter (currently set to 0.1)
[/panda/move_group] [ WARN] [WallTime: 1635258153.992843253, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ WARN] [WallTime: 1635258153.992908280, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ERROR] [WallTime: 1635258153.992977843, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ WARN] [WallTime: 1635258153.993028591, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ERROR] [WallTime: 1635258153.993080566, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ERROR] [WallTime: 1635258153.993138518, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ WARN] [WallTime: 1635258153.993188608, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ERROR] [WallTime: 1635258153.993227315, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ WARN] [WallTime: 1635258153.993280301, 47.370000000]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000692 seconds
[/panda/move_group] [ WARN] [WallTime: 1635258153.993407445, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ WARN] [WallTime: 1635258153.993455087, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ERROR] [WallTime: 1635258153.993481836, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ WARN] [WallTime: 1635258153.993523189, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ERROR] [WallTime: 1635258153.993567527, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ERROR] [WallTime: 1635258153.993609622, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ WARN] [WallTime: 1635258153.993672380, 47.370000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ERROR] [WallTime: 1635258153.993710516, 47.370000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ WARN] [WallTime: 1635258153.993759099, 47.370000000]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000388 seconds
[/panda/move_group] [ WARN] [WallTime: 1635258153.993858961, 47.371000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ WARN] [WallTime: 1635258153.993889621, 47.371000000]: panda_arm/panda_arm: Skipping invalid start state (invalid bounds)
[/panda/move_group] [ERROR] [WallTime: 1635258153.993917793, 47.371000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ERROR] [WallTime: 1635258153.993946258, 47.371000000]: panda_arm/panda_arm: Motion planning start tree could not be initialized!
[/panda/move_group] [ WARN] [WallTime: 1635258153.993984646, 47.371000000]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.000173 seconds
[/panda/move_group] [ WARN] [WallTime: 1635258154.001882762, 47.379000000]: Goal sampling thread never did any work.

Please, let me know if you want want to me to look into some of these issues.

[rhaschke]

Gazebo_ros sim

• Currently, the panda_hand_controller has to be loaded manually. Do we want to do this automatically?

Didn't MoveIt load the controllers automatically? I'm sure, it can at least start and stop them.
Maybe we should load, but not start the controllers by default.

• The position controller works but does throw a failed with error GOAL_TOLERANCE_VIOLATED warnings. I think this is because the [gazebo_ros_control/pid_gains] should be improved. 👍
• When trying to control the panda hand using the panda_hand_controller I get errors.
  This is solved by commenting on the passive joint in the hand.xacro file. If I remember it correctly this line is however needed for the franka_gazebo simulation since it uses the franka_gripper/gripper_command action server.

I noticed as well, that gazebo_ros sim operates both finger joints independently from each other and doesn't seem to consider the <mimic> tag, but I didn't investigate further.
I'm not sure about the semantics of a passive joint. As far as I remember that's simply an uncontrolled joint.
My guess is, that franka_gripper doesn't care about this. Did you already try?

Franka_gazebo

• When I launch the simulation using the roslaunch panda_moveit_config franka_gazebo.launch command both the cartesian_impedance_example_controller and panda_arm_controller are loaded. This leads to conflicts.
  Maybe we should set the controller arg default to " " and prevent the panda_arm_controller from loading in the ros_controllers.launch file when the controller arg is specified?

As stated above, we should load the controller(s), but not yet start them. Otherwise, I like your proposal. I just didn't have time to put more effort resolving the conflicting controllers.

• If I use controller:=panda_arm_controller the panda_arm_controller is loaded through the controller_manager that is inside the franka_ros/panda.launch file. In that case I need to specify the controller_list parameter.

I don't get this yet. Can you please refer to the controller_list parameter to be specified?

• If I change the controller arg default to " " the moveit_ros_control_interface::MoveItControllerManager is used and the controllers are loaded successfully. I however do receive several warnings because panda_joint7 starts out of bounds

The initial joint positions for Gazebo are set in gazebo.launch resp. franka_gazebo/panda.launch. I guess, we see these violations because the default Franka controller is drifting away. What happens if your start a controller that doesn't drift?

[rickstaa]

I noticed as well, that gazebo_ros sim operates both finger joints independently from each other and doesn't seem to consider the tag, but I didn't investigate further.

I ran into this issue some months ago and found that Gazebo does not yet support mimic joints (see gazebo_ros_pkgs/issues/251 and how-to-do-mimic-joints-that-work-in-gazebo. Judging from other issues and ROS answers the most used solution is to use the roboticsgroup/roboticsgroup_upatras_gazebo_plugins for solving this issue.

Franka does something similar in the franka_gripper/gripper_action action. They expect the gripper_width from the moveit_simple_controller_manager and then use PID controllers to apply the corresponding joint commands.

[rickstaa]

To solve this issue, I think we, therefore, have multiple options. I think the easiest, fix, for now, would be to use a xacro conditional block to enable and disable this passive joint. This however will clutter the code.

The other solution is to use the roboticsgroup_upatras_gazebo_plugins. This however creates an extra dependency that is not directly maintained by ros-planning.

I think solving ros-simulation/gazebo_ros_pkgs#251 (comment) would be the best option but this will likely require significant development effort.

We can discuss this later. I will first try to look into the other issues.

[rhaschke]

Are you sure that Franka relies on the fact that the mimic joint is also declared as passive?

[rickstaa]

My understanding of the topic is as follows. Please correct me on the wrong parts.

On the real robot, @frankaemika implements several gripper action servers (see the documentation). The franka_control.gripper_action action was created as an interface for MoveIt. This action server expects the control_msgs.GripperCommand message. This message only contains the gripper_width and the max_effort. The comment made by Franka in the code could be improved to explain this behaviour better:

// HACK: As one gripper finger is , MoveIt!'s trajectory execution manager
// only sends us the width of one finger. Multiply by 2 to get the intended width.
double width = this->finger1_.getPosition() + this->finger2_.getPosition();

I think this message was chosen since MoveIt supports it and the other actions provided by Franka also use the gripper_width (see the available actions). This can be further seen by the fact that Franka uses the XML mimic tag in the hand.xacro robot description. They removed this tag from the gazebo robot description because Gazebo and gazebo_ros_control do not yet support it.

Now from the MoveIt side, we have three options for controlling parallel jaw grippers. We can use a joint_trajectory_controller, a MoveIt grasp message or the GripperCommand. Franka chose to use the GripperCommand message in the franka_gazebo simulation because of the reasons explained above.

The passive joint, therefore, is mainly a problem that arises at the MoveIt side. The conflicts are the following:

• When we want to use the joint_trajectory_controller, as you do in the gazebo_ros, we need to remove the passive joint since otherwise, MoveIt can not control all the joints listed in the panda_hand_controller parameter (see the warning in demo_gazebo.launch with standard ROS packages #9 (comment)).
• When we want to use the GripperCommand action in the franka_gazebo simulations, we can safely remove the passive joint. MoveIt can control the gripper since the mimic tag is not found in the hand robot description.
• If I remember correctly, we need the passive joint for the real robot since the hand.xacro robot description file contains the mimic tag. I will check this on Monday. If the real robot control works without the passive joint, we can remove it from the hand.xacro file, and we are done.

Please let me know if something is unclear.

[rickstaa]

I don't get this yet. Can you please refer to the controller_list parameter to be specified?

In all my previous pull request since I was using the moveit_fake_controller_manager/MoveItFakeControllerManager and the moveit_simple_controller_manager I needed to set the controller_list parameter. This was needed for MoveIt such that it is aware of the controller configuration of the panda robot (see te docs).

Based on the documentation (i.e. Low Level Controllers tutorial and ros_planning/moveit_plugins/moveit_ros_control_interface#setup) I was under the impression that moveit_ros_control_interface::MoveItControllerManager also uses this mechanism.

You, however, do not set this parameter for the gazebo_ros simulation. To my surprise, MoveIt can control both the robot arm and hand after I load the hand controller. I guess this has something to do with an undocumented feature of the moveit_ros_control_interface::MoveItControllerManager or MoveIt. I will check the code to see how MoveIt finds the controllers.

The problem I was referring to above was that when the panda_arm_controller is loaded through the franka_gazebo.launch file I get the following error message:

[/panda/move_group] [ WARN] [WallTime: 1635334863.833228399, 86.015000000]: Failed to read controllers from /controller_manager/list_controllers
[/panda/move_group] [ERROR] [WallTime: 1635334863.833493876, 86.015000000]: Unable to identify any set of controllers that can actuate the specified joints: [ panda_finger_joint1 panda_finger_joint2 ]
[/panda/move_group] [ERROR] [WallTime: 1635334863.833543861, 86.015000000]: Known controllers and their joints:

[/panda/move_group] [ERROR] [WallTime: 1635334863.833618211, 86.015000000]: Apparently trajectory initialization failed

To solve this, we, need to add the controller_list variable when we use the franka_gazebo.launch file:

 controller_list:
  - name: position_joint_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7
  - name: franka_gripper
    action_ns: gripper_action
    type: GripperCommand
    default: true
    joints:
      - panda_finger_joint1
      - panda_finger_joint2

[rickstaa]

The initial joint positions for Gazebo are set in gazebo.launch resp. franka_gazebo/panda.launch. I guess, we see these violations because the default Franka controller is drifting away. What happens if your start a controller that doesn't drift?

It looks like these errors and warning were caused by d21d5c2a07faa5318fe63bbd4740612154fb60a5. I think this is an example of the behaviour @gollth explained in the meeting.

[rickstaa]

@rhaschke Adding the pause logic from the panda.launch file inside the gazebo.launch file improves the starting position of the robot.

[rhaschke]

Let me answer a few of your questions:

Now from the MoveIt side, we have three options for controlling parallel jaw grippers. We can use a joint_trajectory_controller, a MoveIt grasp message or the GripperCommand.

The MoveIt grasp message you are referring to is used to trigger a pick action. This is not related to trajectory execution.
Essentially, we have two options for execution:

• Sending a GripperCommand via moveit_simple_controller_manager/MoveItSimpleControllerManager and
• Sending a joint trajectory via moveit_simple_controller_manager/MoveItSimpleControllerManager or moveit_ros_control_interface::MoveItControllerManager - both interface a FollowJointTrajectory action server.

#9 (comment) will be resolved by using MoveItSimpleControllerManager and configuring the controller_list as you pointed out.

Thanks for checking #9 (comment).

[rickstaa]

@rhaschke Thanks for your answer. I think I now have a clear picture of the whole planning and control pipeline.

#9 (comment) will be resolved by using MoveItSimpleControllerManager and configuring the controller_list as you pointed out.

We can also keep using the moveit_ros_control_interface::MoveItControllerManager interface since it also works with the controller_list parameter (see the documentation). An example of this can be found in my previous pull request (see this commit. I think we could add this code to the panda_gazebo file.

After this change is applied, the franka_gazebo simulation is working. We then only have to change the franka_gazebo controller argument default to " " and make sure the panda_arm_controller is not loaded when a user specifies the controller argument.

Please let me know if I have to check other things or create a pull request. I will perform some tests on the real robot on Monday.

[rhaschke]

We can also keep using the moveit_ros_control_interface::MoveItControllerManager interface since it also works with the controller_list parameter (see the documentation).

The controller_list parameter is simply ignored by moveit_ros_control_interface::MoveItControllerManager.

[rickstaa]

@rhaschke Thanks for pointing that out. I added a POC to #13.

You are right the controller_list parameter is ignored. This can be seen when we provide incorrect values for the controller_list. I guess the README.md and documentation need to get updated (i.e. Low-Level Controllers tutorial and ros_planning/moveit_plugins/moveit_ros_control_interface#setup).

Anyway, it looks that the moveit_ros_control_interface::MoveItControllerManager only works with Trajectory controllers and does not yet work with the GripperCommand action server. We therefore need to switch back to using the moveit_simple_controller_manager/MoveItSimpleControllerManager or implement this feature in the moveit_ros_control_interface.

[rhaschke]

It looks like that the moveit_ros_control_interface::MoveItControllerManager only works with trajectory controllers and does not yet work with the GripperCommand action server. We therefore need to switch back to using the moveit_simple_controller_manager/MoveItSimpleControllerManager

That's what I argued for since our meeting 😄
See #14 for a corresponding config.

[rickstaa]

@rhaschke you argued correctly 😄. I however wanted to understand what caused all these errors and warnings when the moveit_ros_control_interface is used 🤓 .