Pilz Industrial Motion Planner blend generates points with same time_from_start #2741
Description
Description
Using pilz_industrial_motion_planner to generate a motionsequence with a blend radius generates (multiple) points with the same value for RobotTrajectory.joint_trajectory.points[].time_from_start. Looks like it generates two pairs of wrong points per point with a blend.
It does generate and execute the trajectory when blend radius is set to 0.0. Had the problem using the Universal_Robots_ROS2_Driver and was able to reproduce it with the MoveIt example robot (used for this bug report).
Also ran into this issue from MoveIt1 while trying to reproduce. Looks related.
Your environment
- ROS Distro: Iron
- OS Version: Ubuntu 22.04.4 LTS (native)
- Build from source following Getting Started, must be v2.9.0 then.
Steps to reproduce
ros2 launch moveit_resources_panda_moveit_config demo.launch.py
Send MoveGroupSequence goal to /sequence_move_group with a blend radius using the following Python code.
import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from std_msgs.msg import Header
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import Pose, Quaternion
from moveit_msgs.action import MoveGroupSequence, ExecuteTrajectory
from moveit_msgs.msg import MotionPlanRequest, PlanningOptions, Constraints, PositionConstraint, \
RobotState, BoundingVolume, OrientationConstraint, MotionSequenceRequest, MotionSequenceItem
def generate_orient_constraint(x: float, y: float, z: float, w: float, margin: float, z_margin: float = None):
con = [OrientationConstraint()]
con[0].orientation = Quaternion()
con[0].header = Header()
con[0].header.frame_id = 'world'
con[0].weight = 1.0
con[0].link_name = 'panda_hand'
con[0].orientation.x = x
con[0].orientation.y = y
con[0].orientation.z = z
con[0].orientation.w = w
con[0].absolute_x_axis_tolerance = margin
con[0].absolute_y_axis_tolerance = margin
con[0].absolute_z_axis_tolerance = z_margin if z_margin else margin
return con
def generate_pos_constraint(xyz: list, xyz_margin: float, orientation_margin: float) -> Constraints:
con = Constraints()
con.position_constraints = [PositionConstraint()]
con.position_constraints[0].header = Header()
con.position_constraints[0].constraint_region = BoundingVolume()
con.position_constraints[0].constraint_region.primitives = [SolidPrimitive()]
con.position_constraints[0].constraint_region.primitive_poses = [Pose()]
con.position_constraints[0].header.frame_id = 'world'
con.position_constraints[0].link_name = 'panda_hand'
con.position_constraints[0].weight = 1.0
con.position_constraints[0].constraint_region.primitives[0].type = SolidPrimitive.SPHERE
con.position_constraints[0].constraint_region.primitives[0].dimensions = [xyz_margin]
con.position_constraints[0].constraint_region.primitive_poses[0].position.x = xyz[0]
con.position_constraints[0].constraint_region.primitive_poses[0].position.y = xyz[1]
con.position_constraints[0].constraint_region.primitive_poses[0].position.z = xyz[2]
con.position_constraints[0].constraint_region.primitive_poses[0].orientation.x = 0.0
con.position_constraints[0].constraint_region.primitive_poses[0].orientation.y = 0.0
con.position_constraints[0].constraint_region.primitive_poses[0].orientation.z = 0.0
con.position_constraints[0].constraint_region.primitive_poses[0].orientation.w = 1.0
con.orientation_constraints = generate_orient_constraint(0.0, 1.0, 0.0, 0.0, orientation_margin)
return con
class MoveitMover(Node):
def __init__(self):
super().__init__('moveit_mover')
self._plan_action_client = ActionClient(self, MoveGroupSequence, '/sequence_move_group')
self._execute_action_client = ActionClient(self, ExecuteTrajectory, '/execute_trajectory')
self.finished = False
def send_goal(self):
goal_msg = MoveGroupSequence.Goal()
goal_msg.request = MotionSequenceRequest()
goal_msg.planning_options = PlanningOptions()
goal_msg.planning_options.plan_only = False
goal_msg.request.items = [MotionSequenceItem() for _ in range(3)]
x_list = [ 0.3, 0.3, 0.3]
y_list = [-0.2, 0.0, 0.2]
z_list = [ 0.2, 0.3, 0.2]
for index, item in enumerate(goal_msg.request.items):
item.req = MotionPlanRequest()
item.blend_radius = 0.1
item.req.start_state = RobotState()
item.req.goal_constraints = [Constraints()]
item.req.group_name = 'panda_arm'
item.req.pipeline_id = 'pilz_industrial_motion_planner'
item.req.planner_id = 'PTP' # For Pilz PTP, LIN of CIRC
item.req.num_planning_attempts = 1000
item.req.allowed_planning_time = 15.0
item.req.max_velocity_scaling_factor = 0.1
item.req.max_acceleration_scaling_factor = 0.1
item.req.cartesian_speed_limited_link = 'panda_hand'
item.req.max_cartesian_speed = 0.5
xyz = [x_list[index], y_list[index], z_list[index]]
item.req.goal_constraints[0] = generate_pos_constraint(xyz, 0.1, 0.1)
goal_msg.request.items[-1].blend_radius = 0.0
self._plan_action_client.wait_for_server()
self._send_goal_future = self._plan_action_client.send_goal_async(goal_msg)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().info('Goal rejected :(')
return
self.get_logger().info('Goal accepted :)')
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
result = future.result().result
self.get_logger().info(f"Result: {result.response.error_code}")
if result.response.error_code.val != 1:
self.get_logger().warn("Couldn't find a valid solution.")
self.finished = True
return
self.get_logger().info(f"Amount of points: {len(result.response.planned_trajectories[0].joint_trajectory.points)}")
self.finished = True
def main(args = None):
rclpy.init(args=args)
moveit_mover = MoveitMover()
moveit_mover.send_goal()
while not moveit_mover.finished:
rclpy.spin_once(moveit_mover, timeout_sec=1)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
moveit_mover.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()Expected behaviour
Generate and execute a trajectory through these points with them blended.
Actual behaviour
Generates the points and sends them to executor, but that throws an error.
Backtrace or Console output
[move_group-4] [INFO] [1710412387.922100385] [move_group.moveit.pilz_move_group_sequence_action]: Combined planning and execution request received for MoveGroupSequenceAction.
[move_group-4] [WARN] [1710412387.922207759] [move_group.moveit.move_group_capability]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as difference in the planning scene diff
[move_group-4] [INFO] [1710412387.922222398] [move_group.moveit.add_time_optimal_parameterization]: Planning attempt 1 of at most 1
[move_group-4] [INFO] [1710412387.922352625] [move_group.moveit.move_group_capability]: Using planning pipeline 'pilz_industrial_motion_planner'
[move_group-4] [INFO] [1710412387.922388010] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[move_group-4] [WARN] [1710412387.922403733] [move_group.moveit.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[move_group-4] [INFO] [1710412387.922477930] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[move_group-4] [ERROR] [1710412387.922492739] [move_group.moveit.conversions]: Found empty JointState message
[move_group-4] [INFO] [1710412387.922588229] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[move_group-4] [ERROR] [1710412387.922597474] [move_group.moveit.conversions]: Found empty JointState message
[move_group-4] [INFO] [1710412387.922733445] [move_group.moveit.pilz_trajectory_generator_ptp]: Initialized Point-to-Point Trajectory Generator.
[move_group-4] [INFO] [1710412387.922765137] [move_group]: Calling Planner 'Pilz Industrial Motion Planner'
[move_group-4] [INFO] [1710412387.922793603] [move_group.moveit.pilz_trajectory_generator]: Generating PTP trajectory...
[move_group-4] [INFO] [1710412387.924332877] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[move_group-4] [WARN] [1710412387.924351236] [move_group.moveit.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[move_group-4] [INFO] [1710412387.924365066] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[move_group-4] [INFO] [1710412387.924388620] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[move_group-4] [INFO] [1710412387.924440984] [move_group.moveit.pilz_trajectory_generator_ptp]: Initialized Point-to-Point Trajectory Generator.
[move_group-4] [INFO] [1710412387.924452012] [move_group]: Calling Planner 'Pilz Industrial Motion Planner'
[move_group-4] [INFO] [1710412387.924461184] [move_group.moveit.pilz_trajectory_generator]: Generating PTP trajectory...
[move_group-4] [INFO] [1710412387.925071196] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[move_group-4] [WARN] [1710412387.925079721] [move_group.moveit.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[move_group-4] [INFO] [1710412387.925090250] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[move_group-4] [INFO] [1710412387.925105399] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[move_group-4] [INFO] [1710412387.925143550] [move_group.moveit.pilz_trajectory_generator_ptp]: Initialized Point-to-Point Trajectory Generator.
[move_group-4] [INFO] [1710412387.925153480] [move_group]: Calling Planner 'Pilz Industrial Motion Planner'
[move_group-4] [INFO] [1710412387.925161855] [move_group.moveit.pilz_trajectory_generator]: Generating PTP trajectory...
[move_group-4] [INFO] [1710412387.925870114] [move_group.moveit.pilz_trajectory_blender_transition_window]: Start trajectory blending using transition window.
[move_group-4] [INFO] [1710412387.925898933] [move_group.moveit.pilz_trajectory_blender_transition_window]: Search for start and end point of blending trajectory.
[move_group-4] [INFO] [1710412387.925936912] [move_group.moveit.pilz_trajectory_blender_transition_window]: Intersection point of first trajectory found, index: 123
[move_group-4] [INFO] [1710412387.925955168] [move_group.moveit.pilz_trajectory_blender_transition_window]: Intersection point of second trajectory found, index: 20
[move_group-4] [INFO] [1710412387.933391108] [move_group.moveit.pilz_trajectory_blender_transition_window]: Start trajectory blending using transition window.
[move_group-4] [INFO] [1710412387.933431130] [move_group.moveit.pilz_trajectory_blender_transition_window]: Search for start and end point of blending trajectory.
[move_group-4] [INFO] [1710412387.933449892] [move_group.moveit.pilz_trajectory_blender_transition_window]: Intersection point of first trajectory found, index: 6
[move_group-4] [INFO] [1710412387.933464270] [move_group.moveit.pilz_trajectory_blender_transition_window]: Intersection point of second trajectory found, index: 18
[move_group-4] [INFO] [1710412387.939719861] [move_group.moveit.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1710412387.939778719] [move_group.moveit.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1710412387.939873551] [move_group.moveit.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-4] [INFO] [1710412387.949949404] [move_group.moveit.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-4] [INFO] [1710412387.950106225] [move_group.moveit.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1710412387.950140257] [move_group.moveit.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1710412387.950311691] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending continuation for the currently executed trajectory to panda_arm_controller
[ros2_control_node-5] [INFO] [1710412387.951204233] [panda_arm_controller]: Received new action goal
[ros2_control_node-5] [ERROR] [1710412387.951283595] [panda_arm_controller]: Time between points 122 and 123 is not strictly increasing, it is 12.200000 and 12.200000 respectively
[move_group-4] [INFO] [1710412387.953570271] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: panda_arm_controller started execution
[move_group-4] [WARN] [1710412387.953620473] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected
[move_group-4] [ERROR] [1710412387.954978583] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server
[move_group-4] [ERROR] [1710412387.955033649] [move_group.moveit.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller panda_arm_controller
[move_group-4] [INFO] [1710412387.955058316] [move_group.moveit.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
Dirty fix
Set plan_only to true and pass the planned_trajectories to the following function before execution. This shifts the generated points with the same time away from eachother by one ns. Trajectory does execute when doing this.
def shift_robottrajectory(trajectory: RobotTrajectory) -> None:
time_shift_ns = 0
for index, point in enumerate(trajectory.joint_trajectory.points):
if index == 0:
continue
point.time_from_start.nanosec += time_shift_ns
if trajectory.joint_trajectory.points[index-1].time_from_start == point.time_from_start:
point.time_from_start.nanosec += 1
time_shift_ns += 1