Python API tutorial launch file not working with default parameters

dghw commented 1 year ago

Description

Python API tutorial fails, using default configuration/script/launch files

Your environment

ROS Distro: Iron
OS Version: e.g. Ubuntu 22.04
Source or Binary build? Source
If binary, which release version? N/A
If source, which git commit or tag? f226955

Steps to reproduce

Install/source ROS2 Iron & MoveIt 2 following instructions on website https://moveit.picknik.ai/main/doc/tutorials/getting_started/getting_started.html
Launch Python API tutorial using ros2 launch moveit2_tutorials motion_planning_python_api_tutorial.launch.py as from https://moveit.picknik.ai/main/doc/examples/motion_planning_python_api/motion_planning_python_api_tutorial.html
Robot model loads but does nothing on RViz

Note: have verified that the Pick and Place with MoveIt Task Constructor tutorial works, and so does the Jupyter Notebook Prototyping tutorial, although I get some errors e.g.:

[WARN] [1690796515.991981572] [moveit_py]: Parameter 'plan_request_params.planner_id' not found in config use default value instead, check parameter type and namespace in YAML file

so I guess the Jupyter Notebook tutorial doesn't use the same YAML config file as the standard Python one

Expected behaviour

Panda should carry out motion plans as defined in the motion_planning_python_api_tutorial.py script

Backtrace or Console output

(Relevant excerpt)

[motion_planning_python_api_tutorial.py-1] Traceback (most recent call last): [motion_planning_python_api_tutorial.py-1] File "/home/external/moveit2/iron/install/moveit2_tutorials/lib/moveit2_tutorials/motion_planning_python_api_tutorial.py", line 176, in [motion_planning_python_api_tutorial.py-1] main() [motion_planning_python_api_tutorial.py-1] File "/home/external/moveit2/iron/install/moveit2_tutorials/lib/moveit2_tutorials/motion_planning_python_api_tutorial.py", line 62, in main [motion_planning_python_api_tutorial.py-1] panda = MoveItPy(node_name="moveit_py") [motion_planning_python_api_tutorial.py-1] RuntimeError: Planning plugin name is empty. Please choose one of the available plugins: chomp_interface/CHOMPPlanner, ompl_interface/OMPLPlanner, pilz_industrial_motion_planner/CommandPlanner, stomp_moveit/StompPlanner [ERROR] [motion_planning_python_api_tutorial.py-1]: process has died [pid 379489, exit code 1, cmd '/home/external/moveit2/iron/install/moveit2_tutorials/lib/moveit2_tutorials/motion_planning_python_api_tutorial.py --ros-args -r __node:=moveit_py --params-file /tmp/launch_params_utfrvk4b'].

dghw commented 1 year ago

If I remove chomp and ompl_rrt_star from the pipeline_names parameter in the motion_planning_python_api_tutorial.yaml file, and also from the multi_pipeline_plan_request_params variable in motion_planning_python_api_tutorial.py, then the tutorial runs as expected for the remaining planners.

pinakjani commented 1 year ago

I face the same issue. Do we need to add the planner plugin in the YAML? Is it because the MoveitConfigBuilder is unable to find the the planner plugin in the default path?

pinakjani commented 1 year ago

@peterdavidfagan @henningkayser Stuck on this for a while. Are we missing something here?

peterdavidfagan commented 1 year ago

Hi @pinakjani,

Apologies for delay in response I am working towards a research deadline and as a result I haven't been very active in maintaining the Python library. The tutorials were only tested against Humble at the time of their release.

If I get time to I will look into this issue in greater detail.

nbkn865 commented 1 year ago

Hi, @pinakjani and @dghw, I had the same issue, and I think I partially solved it.

My setup is ROS2 Humble on Ubuntu 22.04.2 LTS

I believe that these instructions as well as some of the other settings in the tutorial are incorrect or incomplete.

Instead, here's what I have in terms of code settings:

First, the launch file in my ROS2 package:

def generate_launch_description():

    # Figured out the full and correct way to specify the below by looking at
    # moveit2/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py
    moveit_config = (
        MoveItConfigsBuilder(
            robot_name="spot_with_arm", package_name="spot_moveit_config_no_namespace"
        )
        .robot_description(file_path="config/spot_with_arm.urdf.xacro")
        .robot_description_semantic(file_path="config/spot_with_arm.srdf")
        .robot_description_kinematics(file_path="config/kinematics.yaml")
        .planning_pipelines()  # Uses defaults from package moveit_configs_utils default_config folder, see moveit_configs_builder.py 
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .planning_scene_monitor()  # Uses default flags, see moveit_configs_builder.py
        .sensors_3d(file_path="config/sensors_3d.yaml")
        .joint_limits(file_path="config/joint_limits.yaml")
        .pilz_cartesian_limits(file_path="config/pilz_cartesian_limits.yaml")
        .moveit_cpp(
            file_path=get_package_share_directory("spot_arm_moveit")
            + "/config/spot_arm_moveit.yaml"
        )
        .to_moveit_configs()
    )

Here's my YAML file for .moveit_cpp (spot_arm_moveit.yaml):

planning_scene_monitor_options:
  name: "planning_scene_monitor"
  robot_description: "robot_description"
  joint_state_topic: "/joint_states"
  attached_collision_object_topic: "/planning_scene_monitor"
  publish_planning_scene_topic: "/publish_planning_scene"
  monitored_planning_scene_topic: "/monitored_planning_scene"
  wait_for_initial_state_timeout: 10.0

planning_pipelines:
  # !! NOTE: pipeline_names seem to be causing conflicts with package moveit_configs_utils default
  #          config files in the default_config folder, see NOTE in next section below for solution
  pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "stomp"]  #, "ompl_rrt_star"]

# Default
plan_request_params:
  planning_attempts: 1
  planning_pipeline: ompl
  planner_id: BiTRRT
  max_velocity_scaling_factor: 1.0
  max_acceleration_scaling_factor: 1.0
  planning_time: 1.0

# !! NOTE: Make sure these namespaces are not the same names as what are in
#          package moveit_configs_utils default config files in the default_config folder
ompl_rrtc:  # Namespace for individual plan request
  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
    planning_pipeline: ompl  # Name of the pipeline that is being used
    planner_id: RRTConnect  # Name of the specific planner to be used by the pipeline
    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
    planning_time: 1.0  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned

pilz_lin:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: pilz_industrial_motion_planner
    planner_id: PTP
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 0.8

chomp_b:  # This was changed because it conflicts with the chomp default config in moveit_configs_utils
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: chomp
    planner_id: chomp
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 1.5

# Second OMPL pipeline
ompl_rrt_star:
  plan_request_params:
    planning_attempts: 1
    # planning_pipeline: ompl_rrt_star # Different OMPL pipeline name!  # Original, but gave errors in runtime
    planning_pipeline: ompl
    planner_id: RRTstar
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 1.5

stomp_b:  # Added this
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: stomp
    planner_id: stomp
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 1.5

What seems to be happening is that this part pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp", "stomp"] #, "ompl_rrt_star"] needs to be named so that the list corresponds to the config files created by package moveit_configs_utils default *_planning.yaml config files in the default_config folder, because MoveItConfigsBuilder looks for those files.

In contrast, this part in the tutorial multi_pipeline_plan_request_params = MultiPipelinePlanRequestParameters( spot, ["ompl_rrtc", "pilz_lin", "chomp_b", "ompl_rrt_star", "stomp_b"] ) refers to those definitions in the moveit_cpp YAML file (spot_arm_moveit.yaml)

In other words, I think there seems to be a namespace conflict in the tutorial example.

I can get my package to work with the above changes.

Having said that, I still get this warning when running my code:

[spot_arm_with_obstacles.py-1] [WARN] [1693013942.132647049] [moveit.ompl_planning.planning_context_manager]: Cannot find planning configuration for group 'arm' using planner 'BiTRRT'. Will use defaults instead.
[spot_arm_with_obstacles.py-1] [INFO] [1693013942.132859989] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[spot_arm_with_obstacles.py-1] [WARN] [1693013942.212629192] [moveit.ros_planning.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.

In other words, I'm still having trouble correctly specifying my moveit_cpp YAML file (spot_arm_moveit.yaml) so that MoveItPy can find the pipelines I specified.

But, at least my code now works with default solvers, and I can get my robot to do planning and rendered in rviz.

Let me know if that above works for you, and if you can also figure out how to fix the warnings I mentioned above.

pinakjani commented 1 year ago

Hi @nbkn865,

Yes, you are right it looks like a namespace issue, I think @dghw also suggested that the default planner with OMPL does work fine but we still can't get the CHOMP and STOMP planners working as per the tutorial. Were you able to work around with the CHOMP and STOMP planners after changing the names? Also wouldn't we need to specify the new namespace in the planning pipeline to refer to the new specified config rather than the default config?

I am currently working on a different project for now but will give it a shot as soon as I get a chance. :man_technologist:

peterdavidfagan commented 1 year ago

I face the same issue. Do we need to add the planner plugin in the YAML? Is it because the MoveitConfigBuilder is unable to find the the planner plugin in the default path?

@pinakjani this is correct, you need to include the plugin in the config for the planner, this should be included by default in the resources package used by MoveItConfigsBuilder. For CHOMP here is an example config where this plugin has been defined (here). This gets loaded by default by the MoveItConfigsBuilder as we passed moveit_resources_panda_moveit_config.

If you wanted to use CHOMP as the default instead of OMPL set the plan_request_parameters in your config as follows:

plan_request_params:
planning_attempts: 1
planning_pipeline: chomp
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0

Where importantly you list your planning_pipeline as chomp.

If you want to run a plan with a specific planner or perform parallel planning you can use the Python API to specify PlanRequestParameters or MultiPipelinePlanRequestParameters. For the latter you want to define namespaces for difference sets of plan request parameters (important: ensure these namespaces don't conflict with planner names, this was what was causing the issue in the tutorial config), one example is given:

ompl_rrtc:  # Namespace for individual plan request
  plan_request_params:  # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp
    planning_attempts: 1  # Number of attempts the planning pipeline tries to solve a given motion planning problem
    planning_pipeline: ompl  # Name of the pipeline that is being used
    planner_id: "RRTConnectkConfigDefault"  # Name of the specific planner to be used by the pipeline
    max_velocity_scaling_factor: 1.0  # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
    max_acceleration_scaling_factor: 1.0  # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
    planning_time: 1.0  # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned

peterdavidfagan commented 1 year ago

I believe that these instructions as well as some of the other settings in the tutorial are incorrect or incomplete.

@nbkn865 you are correct the tutorial in its current form needs updating. The issue relates to mismatches between *_planning.yaml files in the default config and the pipeline names used in the tutorial config file. I will send a fix for this shortly, thank you for identifying this.

In particular the cause of the error is this line. This namespace needs to be different from the planning_pipeline_id for the chomp planner. Likely this should be documented somewhere, if the pipeline name is defined under pipeline_names it should have a corresponding namespace with the planner config. You don't want conflicts between these parameters and plan_request_parameters.

Below is an example of how these parameters should be separated for CHOMP:

  # chomp planner config (read automatically from resources package)
  chomp.add_randomness
  chomp.animate_endeffector
  chomp.animate_endeffector_segment
  chomp.animate_path
  chomp.collision_clearance
  chomp.collision_threshold
  chomp.default_workspace_bounds
  chomp.enable_failure_recovery
  chomp.hmc_annealing_factor
  chomp.hmc_discretization
  chomp.hmc_stochasticity
  chomp.jiggle_fraction
  chomp.joint_update_limit
  chomp.learning_rate
  chomp.max_iterations
  chomp.max_iterations_after_collision_free
  chomp.max_recovery_attempts
  chomp.max_sampling_attempts
  chomp.min_angle_change
  chomp.obstacle_cost_weight
  chomp.path_tolerance
  chomp.planning_plugin
  chomp.planning_time_limit
  chomp.pseudo_inverse_ridge_factor
  chomp.random_jump_amount
  chomp.request_adapters
  chomp.resample_dt
  chomp.ridge_factor
  chomp.smoothness_cost_acceleration
  chomp.smoothness_cost_jerk
  chomp.smoothness_cost_velocity
  chomp.smoothness_cost_weight
  chomp.start_state_max_bounds_error
  chomp.start_state_max_dt
  chomp.trajectory_initialization_method
  chomp.use_hamiltonian_monte_carlo
  chomp.use_pseudo_inverse
  chomp.use_stochastic_descent
  # plan request parameters (defined in tutorial config file)
  chomp_planner.plan_request_params.max_acceleration_scaling_factor
  chomp_planner.plan_request_params.max_velocity_scaling_factor
  chomp_planner.plan_request_params.planning_attempts
  chomp_planner.plan_request_params.planning_pipeline
  chomp_planner.plan_request_params.planning_time

peterdavidfagan commented 1 year ago

Addressed in https://github.com/ros-planning/moveit2_tutorials/pull/792.

peterdavidfagan commented 1 year ago

@nbkn865 following up on your additional questions.

[spot_arm_with_obstacles.py-1] [WARN] [1693013942.132647049] [moveit.ompl_planning.planning_context_manager]: Cannot find planning configuration for group 'arm' using planner 'BiTRRT'. Will use defaults instead. [spot_arm_with_obstacles.py-1] [INFO] [1693013942.132859989] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.

You likely want to set your config to BiTRRTkConfigDefault as this is defined here. Currently you have this set to BiTRRT.

[spot_arm_with_obstacles.py-1] [WARN] [1693013942.212629192] [moveit.ros_planning.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.

This warning is due to the plugin implementation not setting the response value, I don't think this warning is a major concern.

peterdavidfagan commented 1 year ago

Closing for now as addressed in the open pull request, please follow up with questions here or through opening a another issue and tagging me in this issue.

moveit / moveit2_tutorials