Planning for moving the PR2 base in a scene with OMPL and planar virtual joint

[Peter Schueller]

Dear All,

I would like to use MoveIt for motion planning of the PR2 (only moving
the base in a scene).
I use the sample SRDF in moveit_resources/test/srdf/robot.xml
I would like to use the virtual planar joint "world_joint" and use OMPL.

Currently I set the robot start position as a value of this planar joint:

req.start_state.multi_dof_joint_state.header.frame_id = "odom_combined";
req.start_state.multi_dof_joint_state.joint_names[0] = "world_joint";
req.start_state.multi_dof_joint_state.joint_transforms[0].translation ...
req.start_state.multi_dof_joint_state.joint_transforms[0].rotation ...

* Is this the right way to specify the start position?

Then I provide a position constraint for the base in the scene

req.goal_constraints[0].position_constraints[0].header.frame_id =
"odom_combined";
req.goal_constraints[0].position_constraints[0].link_name = "base_footprint";
req.goal_constraints[0].position_constraints[0].target_point_offset.{x,y,z}
= 0 ...
req.goal_constraints[0].position_constraints[0].constraint_region.primitives[0].type
= SPHERE
req.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses[0].position
= ...
req.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses[0].orientation
= ...

I also provide joint constraints (x/y/theta) for the world_joint
because otherwise no ConstraintSampler can be selected.

* Is this the way to do it?

Then I do planning with

req.group_name = "base";
ompl_interface::OMPLInterface ompl(rmodel);
planning_interface::MotionPlanResponse res;
bool bres = ompl.solve(ps, req, res);

The result is

[ INFO] [1369990588.870046380]: No planner specified. Using default.
Debug: base: Solving the planning problem once...
[ WARN] [1369990588.872574339]: Skipping invalid start state (invalid state)
[ERROR] [1369990588.872668316]: Motion planning start tree could not
be initialized!
[ INFO] [1369990588.872707961]: No solution found after 0.002420 seconds
[ WARN] [1369990588.879718332]: Goal sampling thread never did any work.
Debug: There were 0 valid motions and 0 invalid motions.
[ INFO] [1369990588.879921334]: Unable to solve the planning problem

* How can I find out why my start state is invalid? I removed all
scene geometries so collisions with the scene cannot be the reason.
Also I set the workspace to a very large area. Do I need to specify
joint values/constraints for all joints even if I plan only for the
base? (rviz shows that my robot has a certain joint state, but I am
not sure if MoveIt uses a default state for the joints of the robot)

Some further questions that are related:
* Do I need to do something with ompl.setPlanningConfigurations(configurations)?
* If i specify odom_combined as frame_id's in the planning request
there are no complaints by moveit but if this is a good idea I do not
know - odom_combined is not broadcast over TF because I want to do
hypothetical reasoning relative to the fixed frame of the scene.
Should I specify odom_combined for all frame_id's in the planning
request or only for some?
* Which information on /tf or on /joint_states is relevant for MoveIt?
Can I do motion planning without ROS? (this would be ideal)
* I found no examples of planning with the "base" virtual joint in the
moveit git source code. (Is there?)
* How can I specify to use RRT or RRTstar? If I say planner_name =
"geometric::RRTstar" then I get a warning message about missing
configuration for group "base".

Best,
Peter

[Ioan Sucan]

Hello Peter,

Answers inline:

On Fri, May 31, 2013 at 12:56 PM, Peter Schueller <schue...@gmail.com> wrote:

Dear All,

I would like to use MoveIt for motion planning of the PR2 (only moving
the base in a scene).
I use the sample SRDF in moveit_resources/test/srdf/robot.xml
I would like to use the virtual planar joint "world_joint" and use OMPL.

Currently I set the robot start position as a value of this planar joint:

req.start_state.multi_dof_joint_state.header.frame_id = "odom_combined";
req.start_state.multi_dof_joint_state.joint_names[0] = "world_joint";
req.start_state.multi_dof_joint_state.joint_transforms[0].translation ...
req.start_state.multi_dof_joint_state.joint_transforms[0].rotation ...

* Is this the right way to specify the start position?

Yes, this looks fine.
 

Then I provide a position constraint for the base in the scene

req.goal_constraints[0].position_constraints[0].header.frame_id =
"odom_combined";
req.goal_constraints[0].position_constraints[0].link_name = "base_footprint";
req.goal_constraints[0].position_constraints[0].target_point_offset.{x,y,z}
= 0 ...
req.goal_constraints[0].position_constraints[0].constraint_region.primitives[0].type
= SPHERE
req.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses[0].position
= ...
req.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses[0].orientation
= ...

I also provide joint constraints (x/y/theta) for the world_joint
because otherwise no ConstraintSampler can be selected.

* Is this the way to do it?

For efficiency, I would specify the goal as 3 JointConstraint messages. The base has 3 degrees of freedom, and they are named:
<joint_name>/x
<joint_name>/y
<joint_name>/theta
So you can specify the goal pose this way.

Things should work with specifying a position constraint only, but we need a specialized sampler for that (and that is not yet implemented)

Why do you specify a position constraint as well?

You mention lower in the message that the workspace is set, so that is probably ok.

 

Then I do planning with

req.group_name = "base";
ompl_interface::OMPLInterface ompl(rmodel);
planning_interface::MotionPlanResponse res;
bool bres = ompl.solve(ps, req, res);

This looks fine.

How is your planning scene initialized?

Is the allowed collision matrix loaded?
 

The result is

[ INFO] [1369990588.870046380]: No planner specified. Using default.
Debug:   base: Solving the planning problem once...
[ WARN] [1369990588.872574339]: Skipping invalid start state (invalid state)
[ERROR] [1369990588.872668316]: Motion planning start tree could not
be initialized!
[ INFO] [1369990588.872707961]: No solution found after 0.002420 seconds
[ WARN] [1369990588.879718332]: Goal sampling thread never did any work.
Debug:   There were 0 valid motions and 0 invalid motions.
[ INFO] [1369990588.879921334]: Unable to solve the planning problem

* How can I find out why my start state is invalid? I removed all
scene geometries so collisions with the scene cannot be the reason.
Also I set the workspace to a very large area.

If you have things running from source, I hack ompl_interface/src/detail/state_validity_checker.cpp and set verbose to true.

I suspect the problem you are having is that a joint is slightly out of bounds.

 

Do I need to specify
joint values/constraints for all joints even if I plan only for the
base?

No you do not. The rest of the joints are taken from the planning scene current state.

 

(rviz shows that my robot has a certain joint state, but I am
not sure if MoveIt uses a default state for the joints of the robot)
 

Some further questions that are related:
* Do I need to do something with ompl.setPlanningConfigurations(configurations)?

You should do that if you want to use the additional configurations. The ones from ompl_planning.yaml should be loaded automatically.
 

* If i specify odom_combined as frame_id's in the planning request
there are no complaints by moveit but if this is a good idea I do not
know - odom_combined is not broadcast over TF because I want to do
hypothetical reasoning relative to the fixed frame of the scene.
Should I specify odom_combined for all frame_id's in the planning
request or only for some?

All data is in theory converted to one reference frame: the planning frame, which is usually the parent frame of the virtual_joint of your robot. In your case, odom_combined. It is ok to specify the request in this frame.

* Which information on /tf or on /joint_states is relevant for MoveIt?
Can I do motion planning without ROS? (this would be ideal)

Both are used. The frames of reference for the links of the robot are computed by doing FK  using joint states. The rest of the frames are taken from /tf, but only transforms from known frames to the planning frame are stored.

* I found no examples of planning with the "base" virtual joint in the
moveit git source code. (Is there?)

There is no example unfortunately.

* How can I specify to use RRT or RRTstar? If I say planner_name =
"geometric::RRTstar" then I get a warning message about missing
configuration for group "base".

The name of the configuration is one specified in ompl_planning.yaml; You need to add a planner configuration for rrt* (one is probably there already); The only requirement is that "type:" is set to geometric::RRTstar.

Best,
Peter

[Peter Schueller]

Hello Ioan,

Thank you for your fast reply, I was able to generate my first
successful motion!

(See inline below for some comments.)

I started defining a position constraint only, which seemed to me the
thing that I need.

Then I found out that this does not even attempt planning because no
viable Constraints Sampler is found as the planar joint is not
bounded. Then I added the constraints on the planar joint.

>
> You mention lower in the message that the workspace is set, so that is
> probably ok.
>
>
>>
>> Then I do planning with
>>
>> req.group_name = "base";
>> ompl_interface::OMPLInterface ompl(rmodel);
>> planning_interface::MotionPlanResponse res;
>> bool bres = ompl.solve(ps, req, res);
>>
> This looks fine.
> How is your planning scene initialized?
> Is the allowed collision matrix loaded?

I am not sure if I have such a thing. It should be in the SRDF but in
the sample SRDF for PR2 there is only one disallowed collision.

To make things easier I removed everything from the URDF/SRDF except
base_link and base_bellow_link. (see below) Now the robot looks
different ;) but there are fewer possibilities to do something wrong.

>
>
>> The result is
>>
>> [ INFO] [1369990588.870046380]: No planner specified. Using default.
>> Debug: base: Solving the planning problem once...
>> [ WARN] [1369990588.872574339]: Skipping invalid start state (invalid
>> state)
>> [ERROR] [1369990588.872668316]: Motion planning start tree could not
>> be initialized!
>> [ INFO] [1369990588.872707961]: No solution found after 0.002420 seconds
>> [ WARN] [1369990588.879718332]: Goal sampling thread never did any work.
>> Debug: There were 0 valid motions and 0 invalid motions.
>> [ INFO] [1369990588.879921334]: Unable to solve the planning problem
>>
>> * How can I find out why my start state is invalid? I removed all
>> scene geometries so collisions with the scene cannot be the reason.
>> Also I set the workspace to a very large area.
>
> If you have things running from source, I hack
> ompl_interface/src/detail/state_validity_checker.cpp and set verbose to
> true.
>
> I suspect the problem you are having is that a joint is slightly out of
> bounds.

I deinstalled my ubuntu moveit packages and built from source.

I followed http://moveit.ros.org/wiki/index.php/Groovy/Installation

Until it worked I had to fix the following:
* moveit_resources was not contained in the moveit.rosinstall package,
so i manually cloned it into a workspace
* moveit_resources gave the following error

-- +++ processing catkin package: 'moveit_resources'
-- ==> add_subdirectory(moveit_resources)
CMake Error at /opt/ros/groovy/share/catkin/cmake/catkin_package.cmake:314
(message):
catkin_package() include dir
'/home/ps/_nobackup/builddirs/moveit_src/build/moveit_resources/include' is
neither an absolute directory nor exists relative to
'/home/ps/_nobackup/builddirs/moveit_src/src/moveit_resources'
Call Stack (most recent call first):
/opt/ros/groovy/share/catkin/cmake/catkin_package.cmake:98 (_catkin_package)
moveit_resources/CMakeLists.txt:6 (catkin_package)

I could solve the problem with "mkdir build/moveit_resources/include"
but it seems like a bug to me.


The result of setting verbose = true is the message

Found a contact between 'base_link' (type 'Robot link') and
'base_bellow_link' (type 'Robot link'), which constitutes a collision.
Contact information is not stored.

I defined that these collisions shall not be checked in the SRDF.

Now I can find plans!

Best,
Peter

[Ioan Sucan]

Sounds good!
I am incorporating fixes to the issues you found as we speak.

Ioan

[Peter Schueller]

Hi Ioan,

Unfortunately I was not able to select different planners so far.

On Fri, May 31, 2013 at 1:39 PM, Ioan Sucan <isu...@willowgarage.com> wrote:

>> Some further questions that are related:
>> * Do I need to do something with
>> ompl.setPlanningConfigurations(configurations)?
>
> You should do that if you want to use the additional configurations. The
> ones from ompl_planning.yaml should be loaded automatically.

Should this happen in a launch file? Or in another way automatically?
Is there a documentation for this format?

I think "rosparam load ompl_planning.yaml" is the right thing. I did
this after modifying ompl_planning.yaml to contain a "base" group and
several configurations as follows:

base:
planner_configs:
- PRM
- RRTk
- RRTConnectk
- RRTstark
longest_valid_segment_fraction: 0.1
projection_evaluator: joints(world_joint)

planner_configs:
PRM:
type: geometric::PRM
RRTk:
type: geometric::RRT
range: 100000000
RRTConnectk:
type: geometric::RRTConnect
range: 100000000
RRTstark:
type: geometric::RRTstar

>> * How can I specify to use RRT or RRTstar? If I say planner_name =
>> "geometric::RRTstar" then I get a warning message about missing
>> configuration for group "base".
>>
> The name of the configuration is one specified in ompl_planning.yaml; You
> need to add a planner configuration for rrt* (one is probably there
> already); The only requirement is that "type:" is set to geometric::RRTstar.

Then I set "planner_name" to a name which i set as part of the
"planner_configs" list, e.g., "RRTstark".

Unfortunately with all this I still get

Warning: Cannot find planning configuration for group 'base' using
planner 'RRTstark'. Will use defaults instead.
at line 342 in
/home/ps/_nobackup/builddirs/moveit_src/src/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp

I want to use RRTConnectk and RRTstark and later also SBPL on the same
scene ... probably with different configurations.

What should I do?

Best,
Peter

[Ioan Sucan]

Hello Peter,

You are loading the launch file correctly, but probably not in the namespace in the node you are using OMPLInterface. The library assumes you have the parameters in the local namespace. Try to load the params in the expected ns, and let me know if you still have problems.

Ioan

[Peter Schueller]

Hello Ioan,

I did not use a launchfile, I executed "rosparam load <yamlfile>" on
the shell and expected my other binary to use these values afterwards.
Now I have the following launchfile

<launch>
<node name="planner" pkg="housekeeping" type="plan_motion"
respawn="false" output="screen">
<rosparam command="load" file="$(find housekeeping)/ompl_planning.yaml"/>
</node>
</launch>

("housekeeping" is the name of my package and "plan_motion" the name
of my binary)

And not it works! (at least something different from before works: it
reports to use the PRM configuration)

Thank you!
Peter