Description

As far as I understand:

Background:

• The sequence planning capability (plan sequence service, plan sequence action etc.) plans a series of connected motions.
• The end state from each motion is used to seed the start state of the next motion.
• This update should really only update the joint state, but in practice the whole robot state is regenerated each time, including a conversion from robot_state::RobotState to moveit_msgs::RobotState.

The problem:

• The poses of the attached collision objects in the start state (which should be constant) change between each waypoint in the sequence. This change is initially minuscule, but the rate of change grows and eventually (waypoint ~30 in one path we've seen, later in other paths) this error becomes very large (~several degrees).

My suspicions:

• The attached collision object poses are being converted back and forth between different representations. This conversion involves FK and so is slightly lossy, and these errors compound exponentially each time the poses are re-calculated. I haven't been able to track down where this might be happening.

Your environment

• ROS Distro: Noetic
• OS Version: Ubuntu 20.04
• Source build - 1.1.13-c09acf72487071593c9a60edae0357e7ceb87210

Steps to reproduce

Call pilz' plan_sequence_path service with a toolpath containing many waypoints, using an attached collision object as the planning frame.

The motion plan will drift away from the toolpath, with increasing drift as the waypoints increase.

This can be observed more directly by putting a print statement in TrajectoryGenerator::generate in trajectory_generator.cpp, to look at the poses in req.start_state.attached_collision_objects[i].pose for some i.

Expected behaviour

The poses of attached collision objects should remain rigid, and not vary along a sequence plan.

Actual behaviour

The poses of attached collision objects gain numerical error with each step along a sequence plan. This error compounds exponentially.

Backtrace or Console output

N/A