ros-planning / moveit_ros Goto Github PK

The 'Planning Time' field in the rviz plugin is not used. Right now it is hard configured inside move_group.cpp to 5.0s. I'm finding many plans require 10 to 15 seconds, so being able to configure this value will be essential.

I heard that the default for a non-solution is 5 seconds per point.

If a user is not familiar with their arm, the initial guesses for Workspace size can be defined with many non-solution points.

Allowing the user to change this value would speed preliminary searches and allow much faster searching if the user knows their arm does not contain many degrees of freedom.

We want to define a benchmark using:

• a start state which is robot specific, and specified in joint space
• a set of target poses, specified in Cartesian space, over a set of planning scenes. This set must be usable by all robots that we want to run the benchmark against.
• the name of a link used to achieve the target poses

The benchmark tool should allow us to define and run this benchmark.

Note that this is different from the current notion of saved "query" as:

• only the start state is in joint space and robot specific
• the goal states are in Cartesian space (IK is performed as part of the benchmark) and they are shared by all robots we run the benchmark against

In parallel, in a different ticket, we are adding the ability to save the target poses using the rviz plugin.

helping: @isucan

I loaded a simple 3 dof arm into the tool and allowed it to compute FK for around 1 second. This generated 600,000 points that immediately locked up my computer when RVIZ attempted to visualize that number.

It should be possible to define the stopping point of the computation in terms of either overall 'Allowed Time for Computation' or 'Maximum Output Samples' (default around 1000?). This should help control an unexpectedly taxing output for users with simpler arms.

It's surprising easy to accidentally overwrite scenes. When you click save scene, it doesn't make you confirm that you are overwriting a scene.

RVIZ also saves the last "Scene Name", so you can easily overwrite your last session's scenes with new scene.

The following would help this situation:

• Confirm saving
• move 'Scene Name' out of the 'Planning Scene' section.
• Empty 'Scene Name' each time the plugin starts.

If moveit debs are installed, some packages fail to build because they find the wrong headers. I believe this is caused by how the find_package and include_directories macros are used in the CMakeLists.txt files for various packages.

I believe the "best practices" for the CMake file is detailed here:
http://www.ros.org/wiki/catkin/CMakeLists.txt
and you will notice that things like include_directories should come late, if at all.
I think putting them before the catkin stuff is causing stuff to be found in the wrong order.

Allow insertion of scene collision objects by specifying location via URL

Ioan and I have discussed this as a lower-priority item, but I wanted to note it here so we don't forget.
At some point, it would be good to make the planning_scene_display its own rviz display plugin, and then have the motion planning plugin just instantiate one internally.

This has the benefit of allowing someone to display a planning scene without needing the whole motion planning plugin, and could also easily allow multiple planning scenes with different topics to be displayed by just creating multiple displays.

It would be nice to be able to load different robots into the same scene.

Currently, if you try to load a different robot or URDF, it makes RVIZ crash complaining that the root link was not found.

There should be a feature to load without robot details, and there should be a check to ensure that if the robot is not compatible with the queries that RVIZ does not crash.

When operating on planning scenes outside the rviz plugin it is possible to cause errors due to synchronization.

When updates are received for groups other than the current group, and those updates include attached objects, the attached objects should be preserved

Currently, joint_limits.yaml allows overriding the max_velocity and max_acceleration for a joint.

For programmatic reasons, it would be convenient to also be specify joint limits for position that would take precedence over those in the URDF.

These could be named 'upper_position_limit' and 'lower_position_limit'. These would be ignored for continuous and fixed joints.

An example configuration would be as follows:
joint_limits:
revolute_joint:
lower_position_limit: 0.0
upper_position_limit: 1.0
has_velocity_limits: true
max_velocity: 1.0
has_acceleration_limits: true
max_acceleration: 2.0
prismatic_joint:
lower_position_limit: -0.2
upper_position_limit: 0.2
has_velocity_limits: true
max_velocity: 0.05
has_acceleration_limits: true
max_acceleration: 10.0
continuous_joint:
has_velocity_limits: true
max_velocity: 2.0
has_acceleration_limits: true
max_acceleration: 2.0

I added the arm to a robot base using a fixed frame.

The tree looks like this (all fixed frames):
/base_link ->torso_link->arm_base_link

I set the kinematics reachability to use /base_link as the '~frame_id", but it complained about:
/moveit/planning_models/src/transforms.cpp:123(Transforms::getTransform ...
Unable to transform from frame 'torso_link' to frame 'base_link'

This transform was a fixed joint specifying a z offset of around 20cm. When run, it would compute reachability with an offset of 20cm, as if it did not take this link into account.

The fix was to add the fixed joint into the srdf's 'arm' group, along with the rest of the arm. This means that the arm group has to have an immediate parent of the frame_id or the reachability tool does not produce the right output.

we need to decide what to do with messages -- conversion from ones generated by gencpp to ones generated by genpy and reverse.

In order to run the benchmark defined in #36, we need to create and save multiple desired gripper poses for each planning scene.

• we would like to save gripper poses from the rviz plugin
• gripper poses should be saved in Cartesian space (position and orientation, arm-independent).
• we need the ability to name gripper poses, so we can refer to them from the benchmark

helping: @isucan @marioprats

It would be helpful to be able to control whether to show both success (green spheres and arrows) and failures (red dots and arrows) separately.

If it's at all possible to latch these arrays, it would be helpful too. When you unclick the Marker Array or namespace box in RVIZ, you lose the data and need to rerun the script. I'm not sure if latching is possible with marker arrays though.

I create a scene and a constraint with the moveit plugin. Inside the plugin double-clicking the constraint computes the IK and a solution is found, so the goal is reachable. However, when running the benchmark tool on this scene I get this error:

[ WARN] [1354844470.079475944]: It looks like the planning volume was not specified.
[ INFO] [1354844470.086848920]: Planner configuration 'arm[BKPIECEkConfigDefault]' will use planner 'geometric::BKPIECE'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1354844470.088222357]: Attempting to use default projection.
[ INFO] [1354844470.106765389]: Starting with 1 states
[ERROR] [1354844470.596659101]: IK solver failed with error -6
[ERROR] [1354844471.102957452]: IK solver failed with error -6
[ERROR] [1354844471.603711269]: IK solver failed with error -6
[ERROR] [1354844472.097742587]: Unable to sample any valid states for goal tree
[ INFO] [1354844472.097790450]: Created 1 (1 start + 0 goal) states in 1 cells (1 start (1 on boundary) + 0 goal (0 on boundary))
[ INFO] [1354844472.097817136]: No solution found after 2.009304 seconds
[ERROR] [1354844472.109146345]: IK solver failed with error -6
[ INFO] [1354844472.109397973]: Unable to solve the planning problem

Currently, move_group_interface does not support pose goals for multiple end-effectors. It should.

move_group_interface should support multiple pose goals:
(1) for one end-effector
(2) for multiple end-effectors

The RVIZ Plugin has a "Planned Path" section that will playback plans between the start and end position.

The current timing is done according to the "State Display Time" parameter. However, since states are irregular in time and there can be large gaps between successive states, you cannot use this to evaluate the performance of an arm considering velocities and accelerations.

Having a checkbox or a special value for "State Display Time" (maybe 0), that animates the plans as they would happen on the physical arms would resolve this issue.

check what happens when objects with an existing name but on a different link are received. it seems shapes are merged when they should actually be replaced

It would be nice to be able to see (and maybe also set) the pose coordinate values for the goal pose being moved around. This can help verify if the orientation is the one apparent visually or not (for example, if a rotation is exactly 90 degrees or not)

The KDL IK based behavior is a good substitute for seeing what a Jinv or Jt controller would do, as long as you're in the reachable workspace. Once you move out, it is unresponsive and hard to use. I (think I) understand why it's doing that: it queues up all the poses you go through, tries IK for all of them and fails, and the queue must clear up before it regains responsiveness.

A real controller though would behave differently - it would just constantly try to move in a straight line (in the direction of the gradient) towards the current goal, and just push against a joint limit if it can not get there. It never looses responsiveness trying to go to where the goal used to be. Can we get similar behavior out of the KDL IK based version? Sorry if this is unclear, would be happy to chat more in person.

I'm getting the following error on the benchmark server side:

The planner id 'BKPIECEkConfigDefault' is not known to the planning interface 'ompl_interface_ros/OMPLPlanner'
Planning interface 'ompl_interface_ros/OMPLPlanner' has no planners defined

The client side shows:

[ INFO] [1354662258.176541272]: Calling benchmark for goal constraints 'shelf table tight 1_pose_0000' for scene 'shelf table tight 1' ...
[ERROR] [1354662258.253907686]: Exception thrown while while deserializing service call: Buffer Overrun

Tried with different planners
Thx!

Not sure how much related this is with the issue of how planning scenes are stored in the database, but the following will not work:

1. Create a planning scene for a robot
2. Update the robot description (e.g add a gripper), and regenerate the moveit package
3. Load the planning scene of the first step

All the new links appear always in collision. However, it works with new scenes.

I'd like to refactor the plugin to preserve more separation of the front end and backend.

I propose to do this by making the plugin itself just a wrapper that maps menu and button events to send messages to a backend node that implements all the functionality currently in planning_display, planning_link_updater, etc. (This is the way it is done in pr2_interactive_manipulation.)

Since I know you want the user of Rviz to just have to load a single plugin, we can preserve the fact that the plugin currently instantiates other display types within itself. As part of that, I could break the planning_scene_render into a new type of display plugin that the planning plugin instantiates, with the added bonus that one could use a planning scene display separately.

I'm happy to do this, because I think it will be critical for the teleop stuff I am doing. Jon also wants it so that he can instantiate more of the backend functionality from his other code.

Title says it, though I am not sure if this is the right TRAC for this. Please close / reassign as appropriate...

Not sure if this is a bug, or just not implemented for any reason: when you move the end-effector around and some links of the robot are in collision, I would expect the IK solver to find another collision-free solution in the space of redundant configurations.

In the kinematics tool, would be very informative to see which joints are hitting their limits. If a joint is within epsilon of the limit, can we put an informative marker there? Maybe also color coded going from yellow to red as you get closer and closer.

The IK tool can take a long time to run. It could be convenient to add a cancellation button to the GUI to stop solving for new points in case the user made a mistake setting up the parameters (despite the Visualize Points button).

This way, the user wouldn't have to close and restart the GUI to restart.

The PlanningSceneDisplay has support for rendering simple shapes and meshes. However, rendering octomaps is not supported. We should use the existing octomap rviz plugin to render the octomap part of a plannig scene as well.
Anyone up for this?

People who can probably help:
@marioprats @jkammerl @dgossow @isucan @aleeper

Can we get started on a few representative environments for the Kinematics Tool? I would think a good start would be:

• a table top, potentially cluttered
• an open fridge, realistic (e.g. thick doors) with a few things inside
• a closed fridge. Stretch goal: a hinged door, so we can actually open it?

I finally figured out why interactive markers seemed to appear sometimes and sometimes not.

To reproduce this issue do the following:

1. Launch moveit_apps/moveit_visualizer.launch for whichever arm/robot.
2. Check for interactive markers (should be present)
3. Change Fixed Frame to "/asdf"
4. Press Ctrl + S to save RVIZ configuration.
5. Relaunch everything
6. Interactive Markers won't appear
7. Update RVIZ Fixed Frame to be base link of arm/robot.
8. Interactive Markers still won't appear
9. Ctrl + S to save RVIZ
10. Relaunch everythign
11. Interactive markers should appear

It looks like something in the interactive markers fails to initialize if the fixed frame at launch does not exist.

For another data point, if the fixed frame at launch exists, but is not necessarily the fixed frame, it still works. For example, with my robot + arm, I have /base_link and an /arm_base_link. Launching with fixed frame set to /arm_base_link did NOT break the interactive markers.

Not sure if this makes sense or if it belongs here, but just in case:

In the MoveIt RVIZ Plug-in (and corresponding warehouse back-end) it would be nice to be able to put a name on queries so that they can be better identified on what each of them does.

Probably there is something wrong in my environment, but in groovy the moveit_rviz_plugin cannot find the resource 'access-denied.dae' when the eef is outside the reachable workspace. I see it only exists in the src robot_interaction package, and never installed. Could it be installed under install/share/moveit_ros_planning_interface? I did that as a temporary solution. If it is correct I can submit a pull request.

This tool should also support joint_limits.yaml if it has been uploaded into the 'robot_description_planning' namespace.

Marker topic /arm_workspace_tests/workspace_markers/reachable creates large arrows which make it hard to see anything in the rviz screen.

Just like we can run benchmarks for planning plugins, we should be able to benchmark a plannig pipeline.
This might require another node, similar to run_benchmark, or another service for run_benchmark. I prefer the latter.

This shows that we can run the benchmark defined in #36 end-to-end. It needs:

• the appropriate features in the benchmarking tool (#36)
• the appropriate feature in the rviz plugin (#37)
• one demo set of panning scenes (potentially containing a single scene) and a set of gripper poses defined in it. Do not worry too much about the exact scene, this is just to show that the end-to-end pipeline is operational.

Success criteria:

• we can run the benchmark and get numerical scores for at least two different arms.

helping: @adamantivm @isucan

Currently you can remove regular unattached collision objects from the planning scene from the "Scene Collision Objects" tab of the moveit_rviz_plugin. It would be good to be able to remove attached collision objects as well.

it's almost unbearably difficult to properly position separate scene objects using the X Y Z and R P Y fields. The center of rotation is not always on the object and the rotations cause the object to translate around.

Interactive markers would help tremendously with this problem and allow for faster scene creation.

It took me about 5 minutes of trial and error to realize that the orientation R,P, Y are set in degrees and not radians. At the very least, a note should be made to express the units here.

Ideally, there would be a selection box to allow a user to input orientation in their preferred method: degrees, radians, or quaternion.

The "Set Start to Current" button in the rviz plugin sets the start to be the "home" position with the arm by the robot's side. "Set Goal to Current" does the same. The robot model is updating and tracking the arm but the plugin isn't taking into account the current state.

It would be helpful if one of the final ROS_INFO calls were some sort of summary or statistics about the reachability exercise just performed.

For instance, something as simple as: "Successfully reached 670 of 1000 poses". This should help as a simple quantitative metric that's easy to output.

Other averages or standard deviations may also be useful, such as "Average maneuverability index/deviation", but I do not specifically have a use case for other metrics at this time.

It would be nice to be able to create a duplicate of an already imported and positioned object in a planning scene.

If I run the plugin for a robot and create a planning scene, I cannot import the scene geometry for another robot. I'm getting the following message, but no geometry appears:

Scene 'scene' was saved for robot 'my_robot' but we are using robot 'my_other_robot'. Using scene geometry only.