search

moveit / moveit2

:robot: MoveIt for ROS 2
https://moveit.ai/
BSD 3-Clause "New" or "Revised" License
982 stars 491 forks source link

Pilz Industrial Motion Planner blend generates points with same time_from_start #2741

Open TheSpeedM opened 3 months ago

TheSpeedM commented 3 months ago

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

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
AGuthmann commented 3 months ago

Hey, Had the same problem and had to fixed it first with a small function like yours. Further looking into it showed me, that the problematic points always were the first and last point inside the blend radius.

!!BE CAREFUL before you test this script with a real robot, In my case not only the time was off, but the velocity and acceleration at these points too, so the robot moves in an abrupt and jittery way when transitioning!!

In the MoveIt Humble Docs I found 'Time Parameterization' and the Time-Optimal Parameter Generation method which fixed time, acceleration and velocity for me. Unfortunately I only found a C++ version of this. Alternatively you can look into Ruckig-Smoothing of the trajectory. Hope I could point you in the right direction

TheSpeedM commented 3 months ago

First of all thank your proposed solution and heads-up. I haven't tested this on a real robot yet, though this is something that I will make a priority because of your experience.

In the MoveIt Humble Docs I found 'Time Parameterization' and the Time-Optimal Parameter Generation method which fixed time, acceleration and velocity for me.

The problem with this solution is that adding the AddTimeOptimalParameterization request adapter to pilz_industrial_motion_planner.yaml doesn't work in my case. This is probably the same problem this commenter in ros-planning/moveit#2905 was having; I can't use a blend with this request adapter enabled.

I might have to look into other ways of smoothing the trajectory.

AGuthmann commented 3 months ago

Oh yeah, forgot to mention that the adapter didn't work for me either.

I had to manually create a class object of TimeOptimalTrajectoryGeneration and recalculate the trajectory with the computeTimeStamps function. For this to work I had to change the moveit-RobotTrajectory into a robot_trajectory-RobotTrajectory, get the new waypoints and convert back. But this was all in C++ and unfortunately I don't know if there's a python class for the TOTG :/

ilijaljubicic commented 3 months ago

@AGuthmann could you be so kind to share a snippet of your code showcasing the approach you took?

I am facing same problem, in my case , the code is in C++.

AGuthmann commented 3 months ago

sure, here you go:

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_core/moveit/trajectory_processing/time_optimal_trajectory_generation.h>

void optimize_trajectory(moveit_msgs::msg::RobotTrajectory &trajectory, double max_velocity_scaling_factor, double max_acceleration_scaling_factor){
  moveit::core::RobotState robot_state(move_group_interface_->getRobotModel());
  robot_trajectory::RobotTrajectory robot_trajectory(move_group_interface_->getRobotModel(), robot_state.getJointModelGroup(PLANNING_GROUP)->getName());

  robot_trajectory.setRobotTrajectoryMsg(robot_state, trajectory); // convert moveit_msgs::msg::RobotTrajectory into robot_trajectory::RobotTrajectory

  trajectory_processing::TimeOptimalTrajectoryGeneration totg; // Create Optimization Object
  totg.computeTimeStamps(robot_trajectory, 0.1, 0.1); // First optimization with default scalings
  totg.computeTimeStamps(robot_trajectory,max_velocity_scaling_factor, max_acceleration_scaling_factor); // Second optimization with user chosen scalings
  RCLCPP_INFO(logger_, "First Iteration: %d, Second Iteration: %d", first, second);

  robot_trajectory.getRobotTrajectoryMsg(trajectory); // convert robot_trajectory::RobotTrajectory back to moveit_msgs::msg::RobotTrajectory
}

I had to optimize twice because sometimes longer trajectories weren't optimized at all. Couldn't figure out where this comes from...

Additionally if you have 180° turns in your trajectory the optimization will probably fail, because these aren't implemented / supported yet. For my tests it was sufficient to offset the Points before or after the 180° turn by 1mm perpendicular to the movement of the trajectory.

TheSpeedM commented 3 months ago

I have tested the 'dirty solution' on a real robot and it indeed jitters a lot at these points. I also found that Pilz normally creates points with an interval of 0.1 seconds, so i opted to offset the point with 0.1 seconds instead of 1 ns and it works very smoothly.

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:
            # Not sure if this works for seconds rollover but I didn't have any problem
            point.time_from_start.nanosec += int(1e8)
            time_shift_ns += int(1e8)
ilijaljubicic commented 3 months ago

Thank you @AGuthmann .

AGuthmann commented 3 months ago

@TheSpeedM just so I understand correctly: offsetting the point with identical time_from_start and all following points by 0.1 seconds fixed the jittery motion? That's nice to hear!

@ilijaljubicic you're welcome!

TheSpeedM commented 3 months ago

@AGuthmann yes you're correct. The Pilz planner generates points at a 0.1s interval, except at the start and end of a blend. So the 'wrong' points - with the same time as the previous points - of a blend have to be offset by 0.1s and all of the following points have to be shifted too (so they won't overlap). This shifting adds up the more points there have to be corrected, see the diagram below.

Shifting points

github-actions[bot] commented 1 month ago

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

gaspatxo commented 3 weeks ago

Hi @TheSpeedM. Im trying to reproduce your steps but the code gets stuck on self._plan_action_client.wait_for_server()

Indeed, a ros2 action list shows there is no /sequence_move_group being advertised after launching the panda demo file.

I am using humble, could this be the issue?

gaspatxo commented 3 weeks ago

Yes, it is https://github.com/moveit/moveit2/issues/2707

Danilrivero commented 1 week ago

I have encountered the same issue as well. There is an ongoing issue at Moveit1 here https://github.com/moveit/moveit/issues/3583.

I could solve it by doing the bypass commented here https://github.com/moveit/moveit/issues/3583#issuecomment-2035540755 and making some changes for ROS2:

  // Ensure time_from_start is strictly increasing
  double epsilon = 0.01; 
  for (size_t i = 1; i < plan.trajectory_.joint_trajectory.points.size(); ++i)
  {
    auto& current_point = plan.trajectory_.joint_trajectory.points[i];
    auto& prev_point = plan.trajectory_.joint_trajectory.points[i - 1];

    if (current_point.time_from_start == prev_point.time_from_start)
    {
      // Shift current point by 0.1 seconds (100 milliseconds)
      current_point.time_from_start.sec = prev_point.time_from_start.sec;
      current_point.time_from_start.nanosec += 100000000; 
      RCLCPP_WARN(this->get_logger(), "Adjusted time_from_start to be strictly increasing at point %zu", i);
    }
  }

I do still rarely encounter the issues described in this https://github.com/moveit/moveit/issues/3553 with accelerations when working on the real robot only.

Definitely solving that issue and being able to backport to MoveIt2 would be appreciated.