search

moveit / moveit_ros

THIS REPO HAS MOVED TO https://github.com/ros-planning/moveit
70 stars 118 forks source link

Current robot state incorrect when using Rviz planning plugin #405

Open shaun-edwards opened 10 years ago

shaun-edwards commented 10 years ago

Setup:

Ubuntu 12.04 (Precies) ROS Hydro (moveit libraries installed from binaries) *Note: this issue does not exist in Groovy ROS-Industrial (installed from source)

Steps to reproduce

  1. Launch MoveIt application (with industrial robot simulator): roslaunch fanuc_m16ib20_moveit_config moveit_planning_execution.launch
  2. Add RobotModel and TF to Rviz visualization
  3. Under Rviz->MotionPlanning->Scene Robot
    1. Set "Robot Alpha" to 0.25
  4. Under Rviz->MotionPlanning->Planning Request
    1. Check "Query Start State"
    2. Check "Query Goal State"
  5. In the Rviz Motion Planning plugin, Click the "Planning" tab
    1. Click "Select Goal State"
    2. Select "random" from the drop down (should already be selected)
    3. Click "Update" until the orange goal state robot has no link in collision (i.e. red links).
    4. Click "Plan", the robot should plan a motion from the current position to the orange state.
    5. Click "Execute", the robot should move from the current position to the orange state. The log shows the following message: [ INFO] [1390185342.859879631]: Execution completed: SUCCEEDED once the motion has completed.

At this point, the Rviz robot visualization should look like the something like the attached image. The following robots are shown:

The expected result is that the translucent and solid robot should overlap. Furthermore, by executing the following it can be seen that MoveIt does not know the true robot state:

  1. In the Rviz Motion Planning plugin, Click the "Planning" tab
    1. Click "Select Start State"
    2. Select "current" from the drop down (should already be selected)
    3. Click "Update"

The expected result is that the green robot jump to the solid yellow robot state, but instead it jumps to the translucent robot state. screenshot from 2014-01-19 21 42 21

shaun-edwards commented 10 years ago

This issue is currently holding up the ROS-Industrial Hydro release of it's MoveIt config packages. If someone can point me to the right files/code, I am more than happy to try to address this.

mikeferguson commented 10 years ago

Shaun, does the planning scene frame for moveit match the selected global frame for RVIZ? I've seen some very funky things happen when the two are not the same. Otherwise, I haven't seen this, but I think I'm still using moveit from about October/November timeframe right now.

shaun-edwards commented 10 years ago

@mikeferguson , is there a way to set the planning scene from the Rviz plugin? I can't seem to find it.

gavanderhoorn commented 10 years ago

@shaun-edwards: does this also happen with a MoveIt cfg pkg generated with the hydro Setup Assistant? The MoveIt pkgs in the fanuc repository are all created using the Groovy version of MoveIt, and I think the Hydro version generates significantly different packages.

isucan commented 10 years ago

Hey Shaun,

I am looking through the launch files you use to start things up. I think your robot_interface_simulator is missing a block along the lines of the one below:

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/>
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
  </node>

If you have something equivalent that publishes on the /joint_states topic, I have missed it (and please point me to it). You should also regenerate your config files and use the fake controller (at least your move_group.launch is out of date). The fake controller is something that pretends to be an actual wrapper for a robot controller and publishes joint state on /move_group/fake_controller_joint_states when an update is received (arm is moved). Then that gets rebroadcasted by the joint_state_publisher and you see the update in rviz. Please let me know how this goes.

shaun-edwards commented 10 years ago

Ioan,

The files I am working with were generated with Groovy, so that could be the cause of my issues. I will try updating them tonight and let you know if that fixes the problem.

ROS-Industrial has it's own simulator that publishes the /joint_states topic. This is added to the stock moveit config package (see here ). The tutorial creates an additional launch file called "moveit_planning_execution.launch"(see here. This launch file allows the user to use either the simulator(by default) or a real controller.

I'm not sure how the ROS-Industrial simulator differs from the built-in simulator, but generally it has been a robust stand-in for a real robot controller. I will also look at the fake_controller_joint_states to see how they differ from the ROS-I joint states.

shaun-edwards commented 10 years ago

I regenerated the moveit config files under hydro. The issue still remains. I tried the fake controller, but it doesn't "execute" the trajectory, it simply jumps to the goal state. The joint_limits.yaml file is loaded, but it appears the fake controller is not using the velocity and acceleration limits (This seems odd, but it''s my best guess).

isucan commented 10 years ago

Hey Shaun,

The fake controller only jumps to the final state, it does not simulate the arm at all. It just jumps back to goal and publishes the final joint state. That is then sent to the joint_state_publisher, so that the current state of the robot (as broadcasted) is the final one reported by the fake controller. It is possible some topics do not match. Can you run rxgraph and check if move_group is publishing to joint_state_publisher? (not just joint_state_publisher to move_group). I can also take a look if you have this checked in somewhere.

isucan commented 10 years ago

Reading your earlier post, I think you have to make your ROS industrial simulator listen to the topic that the fake controller is publishing and update the reported current state as well.

shaun-edwards commented 10 years ago

Ioan,

I checked in my regenerated (hydro version) MoveIt config to a branch on my github fork. You will need the following repos:

Be sure to follow the steps to reproduce above. In particular, call roslaunch fanuc_m16ib20_moveit_config moveit_planning_execution.launch, not demo.launch

I think I understand how the fake controller is working. The fact that it jumps to the final position immediately may be why the issue I am having doesn't present itself. The bug seems to be that MoveIt is capturing some intermediate position as the current pose. With the fake controller, this is always the goal so you would never notice the issue.

The ROS-I simulator simulates the entire trajectory and reports back the required joint states (continuously). It does this using the same topics that the standard ROS-I driver uses. In other words, even though it is a simulator, it behaves exactly as a real robot would. This leads me to believe that the plugin will exhibit the same behavior when used with a real robot (or possibly one simulated in gazebo).

It probably goes without saying, but when ROS-I simulator is being used, the fake controller is not loaded.

gavanderhoorn commented 10 years ago

@shaun-edwards: I get the same results here (MoveIt compiled from latest sources). This reminds me of #278 (see also @JeremyZoss reply in the email linked).

Also: try clicking the plan and execute button, then everything seems to update as expected. But I also get:

[ INFO] [1390500306.087758322]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ WARN] [1390500306.087899751]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as start state in the motion planning request
acornacorn commented 10 years ago

I think https://github.com/ros-planning/moveit_ros/issues/409 will fix this issue. See comments there.