Add Task's setRobotModel to python binding

[JafarAbdi]

I was testing this with moveit2's python binding and got the following error subsolutions refer to multiple robot models when using a planning scene from MoveItPy.

[rhaschke]

Can you please provide the code causing issues with this? Probably, different RobotModel instances are in use...

[JafarAbdi]

Here is the code-snippet

task = core.Task()
# This fails
# task.loadRobotModel(node)
# This works
task.setRobotModel(planning_scene.robot_model)

task.name = "pick + place"

pipeline = core.PipelinePlanner(node, "ompl", "RRTConnectkConfig")
planners = [(arm, pipeline), (eef, pipeline)]

fixed_state = stages.FixedState("current")
fixed_state.setState(planning_scene)
task.add(fixed_state)

# Connect the two stages
task.add(stages.Connect("connect", planners))

Note that I'm using a planning scene constructed with MoveItPy, I think it's because we only check for the memory location in merge.cpp

[JafarAbdi]

@rhaschke do you mind taking a look at the example I provided? Thanks!

[rhaschke]

Sorry, this got out of my sight. I will try to have a look over the weekend. Thanks for pinging me.

[rhaschke]

Sorry, I only found time to look at this now. Can you please provide the full python script to reproduce the error?
I don't yet see from your snippet, why the task might use different RobotModels. Surely, your PR might fix the issue for you, but I want to understand what is going wrong (and potentially fix the root cause).

[JafarAbdi]

Thanks for looking into this! Here is a full example

from moveit_configs_utils import MoveItConfigsBuilder
from moveit.planning import MoveItPy
import moveit
import moveit.core
import rclcpp
import yaml
from moveit.task_constructor import core, stages


moveit_configs = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()

moveit_cpp_config = """
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: 0.0

planning_pipelines:
  pipeline_names: ["ompl"]

plan_request_params:
  planning_attempts: 1
  planning_time: 1.0
  planning_pipeline: ompl
  planner_id: "RRTConnectkConfig"
  max_velocity_scaling_factor: 1.0
  max_acceleration_scaling_factor: 1.0
"""

moveit_cpp = yaml.load(moveit_cpp_config, Loader=yaml.FullLoader)
configs = moveit_configs.to_dict() | moveit_cpp
moveit_py = MoveItPy("moveitpy", config_dict=configs)


# Create node
node_name = "custom_node"
param_filepath = moveit.utils.create_params_file_from_dict(configs, node_name)
node_options = rclcpp.NodeOptions(
    arguments=["--ros-args", "--params-file", param_filepath]
)
node = rclcpp.Node(node_name, node_options)

task = core.Task()
# This fails
task.loadRobotModel(node)
# This works
# task.setRobotModel(moveit_py.get_robot_model())

task.name = "Task"

planning_scene = moveit.core.planning_scene.PlanningScene(moveit_py.get_robot_model())
planning_scene.current_state.set_to_default_values("panda_arm", "ready")
planning_scene.current_state.set_to_default_values("hand", "open")
fixed_state = stages.FixedState("ready+open")
fixed_state.setState(planning_scene)
task.add(fixed_state)

pipeline = core.PipelinePlanner(node, "ompl", "RRTConnectkConfig")
planners = [("panda_arm", pipeline), ("hand", pipeline)]

task.add(stages.Connect("connect", planners))

planning_scene = moveit.core.planning_scene.PlanningScene(moveit_py.get_robot_model())
planning_scene.current_state.set_to_default_values("panda_arm", "extended")
planning_scene.current_state.set_to_default_values("hand", "close")
fixed_state = stages.FixedState("extended+close")
fixed_state.setState(planning_scene)
task.add(fixed_state)

assert task.plan()

[rhaschke]

Now I see the problem: The FixedState is using MoveItPy's robot model:

planning_scene = moveit.core.planning_scene.PlanningScene(moveit_py.get_robot_model())
...
fixed_state.setState(planning_scene)

If you would use planning_scene = moveit.core.planning_scene.PlanningScene(task.getRobotModel()) instead, the problem should be fixed.

[JafarAbdi]

Now I see the problem: The FixedState is using MoveItPy's robot model:

planning_scene = moveit.core.planning_scene.PlanningScene(moveit_py.get_robot_model())
...
fixed_state.setState(planning_scene)

If you would use planning_scene = moveit.core.planning_scene.PlanningScene(task.getRobotModel()) instead, the problem should be fixed.

That would work for a simple case, but I'm using a modified planning scene from PSM, something like this

with (
    moveit_py.get_planning_scene_monitor().read_only() as planning_scene
):
   # modify the scene
   ...

fixed_state.setState(planning_scene)

[rhaschke]

Sure. I will merge this on the master branch and then on ros2. I just wanted to understand and explain the behavior.

[rhaschke]

Merged via f5a0a54 and 08c1447

[JafarAbdi]

Thanks @rhaschke