-
Notifications
You must be signed in to change notification settings - Fork 0
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
demo_gazebo.launch with standard ROS packages #9
Conversation
@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 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 |
@rhaschke , yes I can confirm the aim was to implement a controller that uses the same interface as |
@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 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) |
Yes, thanks. That's exactly what I was looking for. I will continue on this PR later... |
ac5ab55
to
d5e6feb
Compare
d5e6feb
to
e8b40ab
Compare
While franka_gazebo.launch uses franka_gazebo from franka_ros, demo_gazebo.launch just uses standard ROS packages.
franka_gazebo run the controller_manager in namespace panda. Thus joint_states and controller_manager services run in this namespace too, which requires some remapping for move_group to work.
35728f0
to
d2b09ea
Compare
e8b40ab
to
1356e3b
Compare
We essentially just need to include demo.launch and gazebo.launch as well as translate arguments appropriately.
As we don't have valid PID gains for an effort controller on the real robot, we resort to position control on the real robot. For Gazebo, Franka provided PID gains in frankaemika/franka_ros#161 (comment)
All its functionality is provided by panda_moveit.launch
- Launch MoveIt in namespace `panda` enforced by franka_gazebo/panda.launch - Load trajectory controllers - Provide controller argument with cartesian_impedance_example_controller as default
0290a6f
to
d76af19
Compare
Now, the following operating modes are available:
|
@rhaschke I reviewed your new code-base and I think it is a great improvement! I will test the following options:
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
[/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 panda_moveit_config/config/hand.xacro Line 26 in d76af19
Franka_gazebo
[/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
[/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. |
Didn't MoveIt load the controllers automatically? I'm sure, it can at least start and stop them.
I noticed as well, that
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.
I don't get this yet. Can you please refer to the
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? |
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 |
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 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. |
Are you sure that Franka relies on the fact that the mimic joint is also declared as passive? |
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
I think this message was chosen since MoveIt supports it and the other actions provided by Franka also use the 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 The passive joint, therefore, is mainly a problem that arises at the MoveIt side. The conflicts are the following:
Please let me know if something is unclear. |
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 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 The problem I was referring to above was that when the [/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:
- 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 |
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. |
@rhaschke Adding the pause logic from the panda.launch file inside the |
Let me answer a few of your questions:
The MoveIt grasp message you are referring to is used to trigger a pick action. This is not related to trajectory execution.
#9 (comment) will be resolved by using Thanks for checking #9 (comment). |
@rhaschke Thanks for your answer. I think I now have a clear picture of the whole planning and control pipeline.
We can also keep using the After this change is applied, the 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. |
The controller_list parameter is simply ignored by |
@rhaschke Thanks for pointing that out. I added a POC to #13. You are right the Anyway, it looks that the |
That's what I argued for since our meeting 😄 |
@rhaschke you argued correctly 😄. I however wanted to understand what caused all these errors and warnings when the |
This is an attempt to assemble a
demo_gazebo.launch
file without usingfranka_ros/franka_gazebo
.TODOs:
franka_gazebo
related stuff from the URDF. Thus there is a pre-processedpanda_hand.urdf
.We should add an option in
franka_description
to create both versions of the Gazebo-capable URDF.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 asros_control
.Can you confirm both assumptions, @mohmadAyman?
If so, I think the name
ros_controllers
was a bad choice for the name.I suggestfake_ros_controllers
.moveit_sim_controller
config@rickstaa, can you help on this?
@guihomework, can you point me to appropriate ROS controller configs of our robots?