Question: is it possible to fail planning if pose is not possible?

[kongoncharuk]

Hi,

I was wondering if it's possible to split the move into two stages: planning and execution, and fail planning if the pose is not possible before starting the execution phase.

It is the way how MoveIt C++ interface works and is very convenient.

Currenly move_to_pose will start moving straight away and hit itself if the move is not attainable.

Thanks.

[AndrejOrsula]

Hello,

You can do something similar to what MoveIt2.move_to_pose() does internally and just separate the MoveIt2.plan() and MoveIt2.execute() calls.

pymoveit2/pymoveit2/moveit2.py

Lines 274 to 285 in 60eb340

	self.execute(
	self.plan(
	position=position,
	quat_xyzw=quat_xyzw,
	frame_id=frame_id,
	tolerance_position=tolerance_position,
	tolerance_orientation=tolerance_orientation,
	weight_position=weight_position,
	weight_orientation=weight_orientation,
	cartesian=cartesian,
	)
	)

Such as:

from pymoveit2 import MoveIt2
moveit2 = MoveIt2(...)
planned_trajectory = moveit2.plan(...)
if planned_trajectory is not None:
    moveit2.execute(planned_trajectory)

[AndrejOrsula]

If the example works for you with the panda arm, then it could have something to do with the moveit_config that you are using (e.g. could be timeouts - but that seems unlikely).

What version of MoveIt 2 (ROS 2) are you using? I have not tested humble/rolling for a long time now, so I don't know if there were some changes that would need to be addressed.

[kongoncharuk]

Good point, let me check it with Panda arm. I'm using Foxy.

[AndrejOrsula]

Oh, I have never tested this on foxy. As README.md says, it was only tested on galactic, humble, and rolling (June 2022).

[kongoncharuk]

ah, ok makes sense then. Let me try to upgrade my setup to humble then, wanted to do it anyway, will let you know if it resolves the issue.

[kongoncharuk]

Quick update regarding the above -- it does work on humble and doesn't freeze as on foxy, but it looks like plan() returns a trajectory message no matter what (even if planning fails), but adding the following condition seems like doing the proper check:

if len(planned_trajectory.points) > 1 or planned_trajectory.points[0].time_from_start.nanosec > 0:
  # planning successful

If planning fails trajectory will contain only one point (based on what I've seen) with nanosec = 0.