(ROS Fuerte)

Hi all,

I generated a launch file for the Arm Planning Warehouse Viewer, by following this tutorial:
http://www.ros.org/wiki/arm_navigation/Tutorials/tools/Warehouse%20Viewer

Now I'm looking at the Interpolated IK Motion Planner, which is used in the Manipulation Pipeline to move from the "pre-grasp" to "grasp" positions.

It looks like that in Electric, you could testing this interpolated planner from within the Warehouse Viewer.
See previous posts:
http://answers.ros.org/question/37016/planning_scene_warehouse_viewer-crashes-when-using-secondary-ik/
http://answers.ros.org/question/39759/problem-by-using-interpolated-ik-motion-planner/

In Warehouse Viewer, you can select "Planner configuration" --> "use secondary planner"
But when you click on the "Plan New Trajectory" button, it fails with:
Planning Failed!: unknown error code

By modifying the interpolated IK code:
/armnavigationexperimental/interpolatedikmotionplanner/scripts/interpolatedikmotionplanner.py I can see that the planner is getting initialized, but the callback attached to the ROS Service call is never called.

The interpolated IK script was modified to load with arguments "left" and "right", instead of "l" and "r".
I'm launching the interpolated IK planners using suggestions from bit-pirate in the above post (see launch file below) but this secondary planner still fails.

#### So, has anyone managed to use this interpolated planner in Fuerte?
...with a non-PR2 robot?
...in the Warehouse?

Thanks for reading this far. Your help will be rewarded with beer or donuts! cheers, David.

(redeemable at ROSCon 20xx :-)

My launch file for Planning Warehouse:

<launch>
  <param name="/use_sim_time" value="true" />
  <include file="$(find MYROBOT_arm_navigation)/launch/MYROBOT_planning_environment.launch" />
  <node pkg="planning_environment" name="wall_clock_server" type="fake_time.py" />

  <param name="warehouse_host" value="localhost"/>
  <param name="warehouse_port" value="27020"/>
  <node name="mongo" type="wrapper.py" pkg="mongodb">
    <param name="overwrite" value="false"/>
    <param name="database_path" value="arm_navigation_dbs/MYROBOT"/>
  </node>

  <include file="$(find planning_environment)/launch/environment_server.launch">
    <arg name="use_monitor" value="false" />
    <arg name="use_collision_map" value="false" />
  </include>
  <include file="$(find MYROBOT_arm_navigation)/launch/constraint_aware_kinematics.launch" />
  <include file="$(find MYROBOT_arm_navigation)/launch/ompl_planning.launch" />
  <include file="$(find MYROBOT_arm_navigation)/launch/trajectory_filter_server.launch" />
  <node pkg="robot_state_publisher" type="state_publisher" name="rob_st_pub" />
  <node name="planning_scene_warehouse_viewer" pkg="move_arm_warehouse" type="planning_scene_warehouse_viewer"/>



  <node name="interpolated_ik_node_right" pkg="interpolated_ik_motion_planner" 
    type="interpolated_ik_motion_planner.py" args="right" respawn="false" output="screen">
    <param name="robot_prefix" type="string" value="MYROBOT"/>

    <!-- param name="consistent_angle" type="double" value="1.05"/ -->

      <param name="num_steps" type="int" value="0"/>
      <param name="pos_spacing" type="double" value="0.02"/>
      <param name="rot_spacing" type="double" value="0.0349065850399"/>
      <param name="collision_check_resolution" type="int" value="0"/>
      <param name="consistent_angle" type="double" value="0.017"/>
      <param name="collision_aware" type="int" value="1"/>
      <param name="start_from_end" type="int" value="0"/>

  </node>

  <node name="interpolated_ik_node_left" pkg="interpolated_ik_motion_planner" 
    type="interpolated_ik_motion_planner.py" args="left" respawn="false" output="screen">
    <param name="robot_prefix" type="string" value="MYROBOT"/>

    <!-- param name="consistent_angle" type="double" value="1.05"/ -->

      <param name="num_steps" type="int" value="0"/>
      <param name="pos_spacing" type="double" value="0.02"/>
      <param name="rot_spacing" type="double" value="0.0349065850399"/>
      <param name="collision_check_resolution" type="int" value="0"/>
      <param name="consistent_angle" type="double" value="0.017"/>
      <param name="collision_aware" type="int" value="1"/>
      <param name="start_from_end" type="int" value="0"/>

  </node>


  <node pkg="rviz" type="rviz" name='rviz_warehouse_viewer' args="-d $(find MYROBOT_arm_navigation)/config/planning_scene_warehouse_viewer.vcg" />

  <!-- Called when left arm executes trajectory using controllers -->
  <param name="execute_left_trajectory" value="none" />
  <!-- Called when right arm executes trajectory using controllers -->
  <param name="execute_right_trajectory" value="none" />
  <!-- Called to get left arm inverse kinematics with collision checking -->
  <param name="left_ik_name" value="/MYROBOT_left_arm_kinematics/get_constraint_aware_ik" />
  <!-- Called to get right arm inverse kinematics with collision checking -->
  <param name="right_ik_name" value="/MYROBOT_right_arm_kinematics/get_constraint_aware_ik" />
  <!-- Called to get left arm inverse kinematics without collision checking -->
  <param name="non_coll_left_ik_name" value="/MYROBOT_left_arm_kinematics/get_ik" />
  <!-- Called to get right arm inverse kinematics without collision checking -->
  <param name="non_coll_right_ik_name" value="/MYROBOT_right_arm_kinematics/get_ik" />
  <!-- Kinematic chain group name representing the left arm -->
  <param name="left_arm_group" value="left_arm" />
  <!-- Kinematic chain group name representing the right arm -->
  <param name="right_arm_group" value="right_arm" />
  <!-- Degree of freedom representing inverse kinematics redundancy on the left arm -->
  <param name="left_redundancy" value="none" />
  <!-- Degree of freedom representing inverse kinematics redundancy on the right arm -->
  <param name="right_redundancy" value="none" />
  <!-- Link on the left arm to perform inverse kinematics for -->
  <param name="left_ik_link" value="arm_left_7_link" />
  <!-- Link on the right arm to perform inverse kinematics for -->
  <param name="right_ik_link" value="arm_right_7_link" />
  <!-- Called to plan trajectories given motion plan requests -->
  <param name="planner_service_name" value="/ompl_planning/plan_kinematic_path" />



  <!-- Interpolates between end effector positions for the left arm -->
  <param name="left_interpolate_service_name" value="/left_interpolated_ik_motion_plan" />
  <!-- Interpolates between end effector positions for the right arm -->
  <param name="right_interpolate_service_name" value="/right_interpolated_ik_motion_plan" />



  <!-- Called to smooth and optimize a planner trajectory -->
  <param name="trajectory_filter_service_name" value="/trajectory_filter_server/filter_trajectory_with_constraints" />
  <!-- Called to get the proximity space -->
  <param name="proximity_space_service_name" value="none" />
  <!-- Called to get collision validity of robot state -->
  <param name="proximity_space_validity_name" value="none"/>
  <!-- Called to produce a plan that pulls trajectories out of collision -->
  <param name="proximity_space_planner_name" value="none"/>
  <!-- Called to send the planning scene to the environment server -->
  <param name="set_planning_scene_diff_name" value="environment_server/set_planning_scene_diff"/>
  <param name="vis_topic_name" value="planning_scene_visualizer_markers"/>
</launch>