How to set a specific start state of the robot in a task

[matthias88]

I want to implement an offline planning task, so I cannot use CurrentState stage for fetching the robot state and have to manually set the start state of the robot. How can this be implemented in Python/Python bindings? The idea was to use a FixedState stage and modify the PlanningScene in it (set current robot state) - is that easily possible?

cartesian, jointspace, pipeline, multiPlanner = self.init_planners(static_params)

planners = [(static_params.get('group_name_arm'), pipeline)]

# Define task
self.task = core.Task()
self.task.name = f'MoveTo {task_request.get('unique_id')}'
self.task.loadRobotModel(self.mtc_node)

# start from current robot state
# self.task.add(stages.CurrentState('current state'))

# start from desired start state
planning_scene = PlanningScene(self.task.getRobotModel())
# planning_scene.setCurrentState(start_dict)
self.logger.info(f'Starting state: {planning_scene.current_state}')

# Create a FixedState stage and pass the created PlanningScene as its state
fixed_state = stages.FixedState("fixed state")
fixed_state.setState(planning_scene)

# Add the stage to the task hierarchy
self.task.add(fixed_state)

# moveTo named posture, using joint-space interplation
move = stages.MoveTo(
    f'MoveTo {task_request.get('target')} {task_request.get('target_index')}',
    pipeline,
)
move.group = static_params.get('group_name_arm')
move.timeout = 0.1
self.logger.info(f'Target: {target_dict}')
move.setGoal(target_dict)
self.task.add(move)

if self.task.plan():
    self.task.enableIntrospection()
    self.task.publish(self.task.solutions[0])

[rhaschke]

Yes, that's exactly the purpose of FixedState. Here is a running example:

moveit_task_constructor/demo/scripts/fixed_state.py

Lines 18 to 24 in 0cc3987

	# Initialize a PlanningScene for use in a FixedState stage
	task.loadRobotModel() # load the robot model (usually done in init())
	planningScene = PlanningScene(task.getRobotModel())
	# Create a FixedState stage and pass the created PlanningScene as its state
	fixedState = stages.FixedState("fixed state")
	fixedState.setState(planningScene)

As this is identical to your code, what are the issue(s) in your case?
To set the PlanningScene's current state, two methods are wrapped:

.def("setCurrentState", py::overload_cast<const moveit_msgs::RobotState&>(&PlanningScene::setCurrentState))
.def("setCurrentState", py::overload_cast<const robot_state::RobotState&>(&PlanningScene::setCurrentState))

The dictionary approach you tried, is not available.

[matthias88]

Thank you very much for your fast reply. My issue is, that i get an unexpected error: 'moveit.core.planning_scene.PlanningScene' object has no attribute 'setCurrentState'. start_dict is of type moveit_msgs.msg.RobotState - sorry for that incorrect variable name. I am using latest docker.io/moveit/moveit2:jazzy-release image and jazzy/ros2 (both tested) branch of moveit_task_constructor repo.

[rhaschke]

Indeed, MoveIt2's python wrapper of PlanningScene doesn't yet implement setCurrentState() (my code sample was for ROS1).
However, you can easily add that function in the wrapper here. Please file a PR.

[matthias88]

Thank you a lot, i am gonna file a PR. Here you can see the extension in the wrapper

      .def("set_current_state",
           py::overload_cast<const moveit_msgs::msg::RobotState&>(&planning_scene::PlanningScene::setCurrentState),
           py::arg("robot_state"),
           R"(
           Set the current state using a moveit_msgs::msg::RobotState message.

           Args:
           robot_state (moveit_msgs.msg.RobotState): The robot state message to set as current state.
           )")

      .def("set_current_state",
           py::overload_cast<const moveit::core::RobotState&>(&planning_scene::PlanningScene::setCurrentState),
           py::arg("robot_state"),
           R"(
           Set the current state using a moveit::core::RobotState object.

           Args:
           robot_state (moveit_py.core.RobotState): The robot state object to set as current state.
           )")

[matthias88]

@rhaschke Could you kindly provide me with a hint as to why creating the planning scene (line 1147) takes so long, approximately 1 second?

[rhaschke]

Setting up a planning scene involves setting up the collision models as well. Are your robot meshes particularly complex?
They need to be transformed into the collision plugin's representation. Use simpler collision models.

[matthias88]

Thank you so much - i forgot to simplify the gripper 👍