Integrate with Gazebo - Githubissues

tahsinkose commented 4 years ago

~Needs a bit more work to have the minimal intrusion.~

Now it should be ready. The bulk of the change is due to now filled moveit.rviz. The version in the base repo is an almost empty -- default Rviz config file, whereas now all the juicy MoveIt stuff is loaded within.

tahsinkose commented 3 years ago

@davetcoleman PTAL.

tahsinkose commented 3 years ago

Needs https://github.com/frankaemika/franka_ros/pull/126 to be merged.

simonbogh commented 3 years ago

Thanks @tahsinkose for putting the effort into getting Panda available in Gazebo, hope to see this merged soon.

I tested both your PRs together and everything works on my end, though the gripper fingers are a bit "soft" so maybe the gains need to be adjusted, see at 0:32 https://youtu.be/JrAq8LK5l1M.

Any suggestions?

simonbogh commented 3 years ago

Also, panda_controllers.yaml is still present but seems to never be used/loaded.

tahsinkose commented 3 years ago

@simonbogh thank you very much for the video. I didn't actually test grippers for grasping. I'm not an expert on gripper dynamics, so cannot give any suggestions. Try to increase P gains of the fingers to have a stiffer grasp.

tahsinkose commented 3 years ago

Also, panda_controllers.yaml is still present but seems to never be used/loaded.

I'm not cleaning up all the auto-generated files, so there may be artifacts that are not used at all. Please ignore them in this PR.

simonbogh commented 3 years ago

Also, panda_controllers.yaml is still present but seems to never be used/loaded.

I'm not cleaning up all the auto-generated files, so there may be artifacts that are not used at all. Please ignore them in this PR.

Alright, as long as it is working 😊 (just a disclaimer, I am not an officiel reviewer, just here as a user).

I also changed my local version to use effort controllers instead as I need that.

Do you remember what version of moveit setup asssistant the package was generated with? I made a quick test importing it into the current 1.0.7 (Melodic), and besides some manual settings (e.g. PID gains), there seems to be some changes. According to the timestamp in .setup_assistant it was last generated in Feb. 2018, but I don't know if this is accurate. It could maybe make sense to regenerate the package with the latest setup assistant.

tahsinkose commented 3 years ago

Do you remember what version of moveit setup asssistant the package was generated with?

Unfortunately not :disappointed:

fwalch commented 3 years ago

I didn't look into this in detail yet, just wanted to report some results on Noetic. I'm trying to run roslaunch franka_example_controllers move_to_start.launch robot_ip:=<ip>.

With d99e64f (current melodic-devel of panda_moveit_config) and https://github.com/frankaemika/franka_ros/pull/126, this executes without issues.

With this branch and https://github.com/frankaemika/franka_ros/pull/126, I'm getting the following:

[ WARN] [1616070427.320851483]: Link 'panda_hand' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322043080]: Link 'panda_hand' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322078747]: Link 'panda_hand' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322097331]: Link 'panda_hand' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322114854]: Link 'panda_hand' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322132436]: Link 'panda_hand' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322150106]: Link 'panda_hand' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322167521]: Link 'panda_leftfinger' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322185301]: Link 'panda_leftfinger' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322202800]: Link 'panda_leftfinger' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322219829]: Link 'panda_leftfinger' is not known to URDF. Cannot disable collisons.
[ WARN] [1616070427.322236838]: Link 'panda_leftfinger' is not known to URDF. Cannot disable collisons.
[ INFO] [1616070427.322355330]: Loading robot model 'panda'...
[ WARN] [1616070427.322384402]: Skipping virtual joint 'virtual_joint' because its child frame 'panda_link0' does not match the URDF frame 'world'
[ INFO] [1616070427.322402792]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1616070427.375235427]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1616070427.377156874]: MoveGroup debug mode is OFF
Starting planning scene monitors...
[ INFO] [1616070427.377204891]: Starting planning scene monitor
[ INFO] [1616070427.378503155]: Listening to '/planning_scene'
[ INFO] [1616070427.378538420]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1616070427.379716602]: Listening to '/collision_object'
[ INFO] [1616070427.380909800]: Listening to '/planning_scene_world' for planning scene world geometry
[ INFO] [1616070427.388197131]: Listening to '/camera/depth_registered/points' using message filter with target frame 'world '
[ INFO] [1616070427.394604908]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1616070427.433130766]: Using planning interface 'OMPL'
[ INFO] [1616070427.435098071]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1616070427.435418519]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1616070427.435714579]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1616070427.436015285]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1616070427.436297072]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1616070427.436582894]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1616070427.436642656]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1616070427.436676221]: Using planning request adapter 'Resolve constraint frames to robot links'
[ INFO] [1616070427.436701223]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1616070427.436724797]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1616070427.436748357]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1616070427.436764898]: Using planning request adapter 'Fix Start State Path Constraints'
[INFO] [1616070427.562306]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1616070427.571215]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1616070427.575872]: Controller Spawner: Waiting for service controller_manager/load_controller
[INFO] [1616070427.581625]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1616070427.584948]: Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [1616070427.590181]: Loading controller: position_joint_trajectory_controller
[INFO] [1616070427.592954]: Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [1616070427.602346]: Loading controller: franka_state_controller
[INFO] [1616070427.624754]: Controller Spawner: Loaded controllers: position_joint_trajectory_controller
[INFO] [1616070427.631478]: Controller Spawner: Loaded controllers: franka_state_controller
[ INFO] [1616070427.633593568]: FrankaHW: Prepared switching controllers to joint_position with parameters limit_rate=1, cutoff_frequency=100, internal_controller=joint_impedance
[INFO] [1616070427.634894]: Started controllers: position_joint_trajectory_controller
[INFO] [1616070427.638682]: Started controllers: franka_state_controller
[ WARN] [1616070432.449204725]: Waiting for panda_arm_controller/follow_joint_trajectory to come up
[ WARN] [1616070438.449375320]: Waiting for panda_arm_controller/follow_joint_trajectory to come up
[ERROR] [1616070444.449532523]: Action client not connected: panda_arm_controller/follow_joint_trajectory

The warning [ WARN] [1616070427.322384402]: Skipping virtual joint 'virtual_joint' because its child frame 'panda_link0' does not match the URDF frame 'world' has already been there, the other warnings/errors are new.

tahsinkose commented 3 years ago

The warning [ WARN] [1616070427.322384402]: Skipping virtual joint 'virtual_joint' because its child frame 'panda_link0' does not match the URDF frame 'world' has already been there, the other warnings/errors are new.

@fwalch Probably there is a namespace mismatch. I'll try to look into it.

tahsinkose commented 3 years ago

Action client not connected: panda_arm_controller/follow_joint_trajectory

This was because ros_controllers.launch was included only in gazebo.launch. Now I moved it to move_group.launch. Please try again @fwalch.

rickstaa commented 3 years ago

@tahsinkose Is this pull request ready to be merged? I added https://github.com/tahsinkose/panda_moveit_config/pull/7, which can also be included in this pull request. This pull request gives users the ability to specify the hardware interface that is used. It needs to be merged together with https://github.com/tahsinkose/franka_ros/pull/1.

tahsinkose commented 3 years ago

@rickstaa it requires approval from @fwalch.

rickstaa commented 3 years ago

@tahsinkose I see. No problem, shal we do https://github.com/tahsinkose/panda_moveit_config/pull/7 & https://github.com/tahsinkose/franka_ros/pull/1. In a seperate pull request after #68 has been merged? Then @fwalch can decide if they want to have that feature in the main repository.

rickstaa commented 3 years ago

@tahsinkose Now that https://github.com/frankaemika/franka_ros/releases/tag/0.8.0 has been released #68 might need some changes. In v0.8.0 the robot urdf were modified such that they contain all the components needed for creating a gazebo simulation. Further, Franka Emika also added the franka_gazebo package which contains a more stable/accurate simulation.

For ROS noetic, I implemented two versions of the panda_moveit_config:

https://github.com/rickstaa/panda_moveit_config/pull/40 uses the gazebo_demo.launch file that is supplied by the moveit_setup_assistant to create a gazebo simulation. This is similar to how it is done in this pull request.
https://github.com/rickstaa/panda_moveit_config/pull/39 uses the franka_gazebo package to create a more stable/accurate simulation.

These pull requests are currently on an orphan branch. I did this so that it is easier to track the changes needed after the 'moveit_setup_assistant' was run. I am however happy to rebase them onto the noetic-devel and melodic-devel branches. I however think that the @ros-planning team first needs to decide which version they want to include inside the https://github.com/ros-planning/panda_moveit_config repository and which version they want to add to the https://github.com/ros-planning/moveit/tree/master/moveit_setup_assistant.

To give my two cents. I think adding version 2 would make the most sense since it is more closely related to the real robot. The downside is that it is different from the implementation that is created when running the moveit_setup_assistant. In this case, we therefore also need to decide which files we want the moveit_setup_assistant to create. Maybe, since the moveit_setup_assistnat is meant to be more general, let's use version 1 there and use version 2 for the panda_moveit_config.

tahsinkose commented 3 years ago

Unfortunately, I'm unable to take any action on this branch as I can't allocate time for this PR as of now @rickstaa. It needs to wait a couple of more weeks. Sorry for the inconvenience.

rickstaa commented 3 years ago

@tahsinkose No problem, let's wait for some feedback from the @ros-planning team before we proceed with finalizing this feature.

rickstaa commented 3 years ago

I created this issue to alert the Franka team about the issues we are having with the JointPositionInterface.

rickstaa commented 2 years ago

The following steps need to be performed when we want to use the new franka_gazebo package for the Gazebo simulation (see version 2 above):

[ ] https://github.com/frankaemika/franka_ros/pull/173 needs to be merged. The current implementation supplied by frankaemika contains a small bug that prevents users from setting specific gripper widths.
[ ] We need to find PID gains for the effort_controllers/JointTrajectoryController. The current PID gains lead to several issues (see https://github.com/frankaemika/franka_ros/issues/161#issuecomment-927969666). I created https://github.com/frankaemika/franka_ros/issues/161 to see if frankaemika is able to provide a better solution.

rickstaa commented 2 years ago

The gazebo simulation is working on the noetic branch. We can backport it to melodic-devel when https://github.com/rhaschke/panda_moveit_config/pull/8 is merged.

rickstaa commented 2 years ago

@rhaschke I think @tahsinkose can safely backport c46c81c8a1e0da1288d8ef4462b8daa10df37213 into the melodic branch? To my knowledge, franka_ros v0.8.1 works with melodic (see https://github.com/frankaemika/franka_ros-release).

rhaschke commented 2 years ago

@rhaschke I think @tahsinkose can safely backport c46c81c into the melodic branch?

If it was tested on Melodic, sure.

tahsinkose commented 2 years ago

Tested and not working. I think https://github.com/ros-planning/panda_moveit_config/commit/c46c81c8a1e0da1288d8ef4462b8daa10df37213 alone doesn't suffice.

rickstaa commented 2 years ago

@tahsinkose did you install franka_ros from source and pull all the upstream changes. You can use my branch to test it out. If so what are the problems you encounter?

tahsinkose commented 2 years ago

@tahsinkose did you install franka_ros from source and pull all the upstream changes

Yes sure. I'm already using your branch. My configuration is as follows:

https://github.com/rickstaa/franka_ros/tree/noetic-devel-panda_gazebo
https://github.com/tahsinkose/panda_moveit_config + c46c81c8a1e0da1288d8ef4462b8daa10df37213

The issue is No such file or directory: /home/tahsincan/franka_ws/src/franka_ros/franka_description/robots/panda_arm_hand.urdf.xacro when I run roslaunch panda_moveit_config demo_gazebo.launch.

tahsinkose commented 2 years ago

@rhaschke, @rickstaa we should stop targetting the latest release of franka_ros in panda_moveit_config:melodic-devel. This PR now sufficiently works with https://github.com/rickstaa/franka_ros/commit/f9128cd0de60db60fd9814c4f1f8672b4e5b3575, which adds position and velocity motion generators.

I suggest merging https://github.com/frankaemika/franka_ros/pull/181 ASAP and fixating the updated franka_ros version with version_eq in this PR with a final commit. I'm deliberately suggesting version_eq -and not version_gte- because launch file (now this repo depends on franka_gazebo/panda.launch) changes are much harder to detect and there is no integration testing pipeline between here and franka_ros. Just one unused argument change without notice and everything would be broken. Melodic EOL is 1.5 years from now on and I say this should remain stable till then. There is simply no point in having all the newest stuff for this version.

TL;DR: If someone needs more sim2real alignment in panda robot, then relatively more unstable Noetic is the way to go :P melodic-devel should stay just as a PoC for MoveIt-Gazebo integration, that's it.

Docs todo:

[ ] Copy https://github.com/ros-planning/moveit_tutorials/tree/master/doc/gazebo_simulation to moveit_tutorials:melodic-devel and update according to the latest changes enforced here and franka_ros dependency.

rickstaa commented 2 years ago

@tahsinkose Sounds good to me.

rhaschke commented 2 years ago

All this strongly depends on how franka_ros evolves. This repo needs to adapt to franka_ros. Honestly, I suggest that panda_moveit_config should be handled by Franka again. Having responsibilities shared didn't turn out to be fruitful. @davetcoleman: What is your take on this?

tahsinkose commented 2 years ago

@rhaschke I'm not saying that this repo should not evolve. The latest version -in this case noetic-devel- should of course evolve simultaneously with franka_ros ideally with the same ROS versions (although they still seem to actively bump melodic and noetic with the same tags). Even more, they should have a simple integration pipeline that would at least inform maintainers of both sides about the possible breaks. However, this is just a dream 🙂

Again, the idea behind this PR was to provide a proof of concept MoveIt-Gazebo integration, which was the subject of https://ros-planning.github.io/moveit_tutorials/doc/gazebo_simulation/gazebo_simulation.html. Yet it inadvertently evolved to full support of all the new changes from franka_ros. This very effort is being invested in panda_moveit_config:noetic-devel already. I can't see a strong reason for doubling it for Melodic that will EOL in 17 months. Would it be the last ROS1 version, it might be continued until that time, but that guy is Noetic. It is not ROS2 after all 🤷🏼‍♂️

My point is focusing dev efforts of this repo on Noetic. So that means stopping Melodic with what we have now in this PR - an integration PoC- with a trace to a sufficiently working version of franka_ros.

rhaschke commented 2 years ago

I completely agree with you regarding focussing our efforts on Noetic (or even better ROS2). However, when you state to merge this PR as a PoC as is, I cannot agree. As you wrote, it relies on some unmerged and unreleased features (https://github.com/frankaemika/franka_ros/pull/181). Hence, it's not (yet) working with a set of standard debian packages. Further, we don't have control over the evolution franka_ros. If they decide to release breaking changes, this repo needs to be adapted accordingly.

tahsinkose commented 2 years ago

However, when you state to merge this PR as a PoC as is, I cannot agree. As you wrote, it relies on some unmerged and unreleased features (frankaemika/franka_ros#181)

I already suggested to wait for the immediately next minor of franka_ros after https://github.com/frankaemika/franka_ros/pull/181 🙂. Assuming that is done, why would any breaking changes on newer releases break melodic-devel as long as we make a version_eq dependency to that minor?

rhaschke commented 2 years ago

Ah. Now I understand your reasoning. The problem is: ROS apt repos don't maintain old versions. Hence you cannot lock to a specific version.

tahsinkose commented 2 years ago

Hmm, that's very sad. Then this will definitely break sometime in the future, it is just a matter of when 🙂 I got your reasoning too 👍🏼

v4hn commented 2 years ago

Then this will definitely break sometime in the future, it is just a matter of when 🙂 I got your reasoning too 👍🏼

Again, that depends on upstream. noetic is under active development and melodic matured a lot and is considered mostly stable these days. I fail to understand why they still push breaking changes there. They really shouldn't.

Either way it would definitely be a good idea to add a paragraph somewhere stating the exact commit/version for the franka repository the demo was tested with. Everything after that might be supported, but could fail.

rickstaa commented 2 years ago

@tahsinkose Thanks again for back-porting the noetic-devel simulation into your pull request. Can you maybe also apply the changes made in https://github.com/ros-planning/panda_moveit_config/pull/91#pullrequestreview-787482484 when it is merged.

@rhaschke is also working on a panda simulation that doesn't require the franka_gazebo package see https://github.com/rhaschke/panda_moveit_config/pull/9. I think we should also back-port this into your pull request when it is finalized. After that, I think we can merge #68 into the melodic-devel branch.

rhaschke commented 2 years ago

I'm tempted to close this issue in favour of #89 and https://github.com/rhaschke/panda_moveit_config/pull/9.

rickstaa commented 2 years ago

@rhaschke Do you mean back-porting those pull requests into the melodic-devel or updating the melodic-devel branch with all the code found in the noetic-devel branch?

I think both cases are fine and greatly improve maintainability. It looks like there were no breaking changes regarding the XML and xacro syntax between ros-noetic and ros-melodic but I'm not 100% sure (see http://wiki.ros.org/noetic/Migration).

rhaschke commented 2 years ago

I was thinking of updating/overring melodic-devel by merging noetic-devel. As long as franka_ros is the same on Melodic and Noetic, I don't expect any incompatibilities.

rickstaa commented 2 years ago

@rhaschke Sound like a good idea. Makes the melodic-devel branch easier to maintain.

rhaschke commented 2 years ago

Superseded by new noetic-devel branch.

moveit / panda_moveit_config

Integrate with Gazebo #68