search

UniversalRobots / Universal_Robots_ROS_Driver

Universal Robots ROS driver supporting CB3 and e-Series
Apache License 2.0
738 stars 396 forks source link

Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 0.887946 seconds). Stopping trajectory. #708

Closed zp2546265641 closed 4 weeks ago

zp2546265641 commented 1 month ago

Affected ROS Driver version(s)

ur5

Used ROS distribution.

Noetic

Which combination of platform is the ROS driver running on.

Linux

How is the UR ROS Driver installed.

From binary packets

Which robot platform is the driver connected to.

Real robot

Robot SW / URSim version(s)

i do not know which

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 0.887946 seconds). Stopping trajectory.

Issue details

#! /usr/bin/python3
import moveit_commander
import sys
from geometry_msgs.msg import Pose, PoseStamped
import rospy

def main():
    # 初始化ROS节点
    rospy.init_node('moveit_test_trajectory', anonymous=True)

    # 初始化MoveIt
    moveit_commander.roscpp_initialize(sys.argv)

    # 初始化机械臂的运动组
    arm_group = moveit_commander.MoveGroupCommander("manipulator")
    # 设置参考坐标系
    arm_group.set_pose_reference_frame('base_link') 
    # 当运动规划失败后，允许重新规划
    arm_group.allow_replanning(True)

    # 设置位置(单位：米)和姿态（单位：弧度）的允许误差
    # arm_group.set_goal_position_tolerance(0.001)
    # arm_group.set_goal_orientation_tolerance(0.01)
    # arm_group.set_max_allowed_execution_time(1)
    # arm_group.set_planning_time(5)

    # 设置允许的最大速度和加速度
    # arm_group.set_max_acceleration_scaling_factor(0.5)
    # arm_group.set_max_velocity_scaling_factor(0.5)
    waypoints = []
    # 获取终端link的名称
    # end_effector_link=arm_group.get_end_effector_link()
    current_pose = arm_group.get_current_pose().pose
    print('current_pose:',current_pose)
    # waypoints.append(current_pose)
    # arm_group.set_start_state_to_current_state()
    target_pose = Pose()
    target_pose.position.x = current_pose.position.x - 0.01
    target_pose.position.y = current_pose.position.y
    target_pose.position.z = current_pose.position.z
    target_pose.orientation.x = current_pose.orientation.x
    target_pose.orientation.y = current_pose.orientation.y
    target_pose.orientation.z = current_pose.orientation.z
    target_pose.orientation.w = current_pose.orientation.w
    print('target_pose:',target_pose)
    # arm_group.set_pose_target(target_pose)
    # plan_success, plan, planning_time, error_code=arm_group.plan()

    waypoints.append(target_pose)
    # 使用MoveIt规划路径
    (plan, fraction) = arm_group.compute_cartesian_path(
                                        waypoints,   # 路径点
                                        0.01,        # eef_step
                                        0.0)         # jump_threshold
    # 执行路径
    arm_group.execute(plan, wait=True) 
    # 关闭MoveIt
    moveit_commander.roscpp_shutdown()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

Steps to Reproduce

my launch:

<launch>
    <!-- Define parameters -->
    <arg name="robot_ip" default="192.168.0.10" />
    <arg name="kinematics_config" default="$(find ur_calibration)/etc/ur5_calibration.yaml" />

    <!-- Include the ur5_bringup.launch file -->
    <include file="$(find ur_robot_driver)/launch/ur5_bringup.launch">
        <arg name="robot_ip" value="$(arg robot_ip)" />
        <arg name="kinematics_config" value="$(find ur_calibration)/etc/ur5_calibration.yaml"/>
    </include>

    <!-- Include the ur5_moveit_planning_execution.launch file -->
    <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
    </include>

    <!-- Include the moveit_rviz.launch file -->
    <include file="$(find ur5_moveit_config)/launch/moveit_rviz.launch">
        <arg name="rviz_config" value="$(find ur5_moveit_config)/launch/moveit.rviz" />
    </include>

</launch>

first, launch and then run the test.py.and then it tell me that

Expected Behavior

no error

Actual Behavior

[ INFO] [1720681757.421580728]: Loading robot model 'ur5_robot'...
[ INFO] [1720681757.483437146]: Starting planning scene monitor
[ INFO] [1720681757.485312082]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1720681757.619958100]: Constructing new MoveGroup connection for group 'manipulator' in namespace ''
[ INFO] [1720681759.042623512]: Ready to take commands for planning group manipulator.
[ INFO] [1720681761.265207746]: Robot requested program
[ INFO] [1720681761.265314577]: Sent program to robot
[ INFO] [1720681761.666777741]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681767.330293074]: Connection to reverse interface dropped.
[ERROR] [1720681767.330372262]: Sending data through socket failed.
[ INFO] [1720681767.330578258]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681767.394339703]: Connection to reverse interface dropped.
[ INFO] [1720681767.394645176]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681767.762313332]: Connection to reverse interface dropped.
[ INFO] [1720681767.762605620]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681768.653402305]: Received request to compute Cartesian path
[ INFO] [1720681768.653640091]: Attempting to follow 1 waypoints for link 'tool_ros' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1720681768.654493803]: Computed Cartesian path with 3 points (followed 100.000000% of requested trajectory)
[ INFO] [1720681768.655322868]: Execution request received
[ERROR] [1720681769.547295425]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 0.887946 seconds). Stopping trajectory.
[ INFO] [1720681769.547412965]: Cancelling execution for scaled_pos_joint_traj_controller
[ INFO] [1720681769.547523602]: Completed trajectory execution with status TIMED_OUT ...
[ INFO] [1720681769.547772439]: Execution completed: TIMED_OUT
[ INFO] [1720681769.547821691]: Controller 'scaled_pos_joint_traj_controller' successfully finished

Relevant log output

You can start planning now!

[ INFO] [1720681757.421580728]: Loading robot model 'ur5_robot'...
[ INFO] [1720681757.483437146]: Starting planning scene monitor
[ INFO] [1720681757.485312082]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1720681757.619958100]: Constructing new MoveGroup connection for group 'manipulator' in namespace ''
[ INFO] [1720681759.042623512]: Ready to take commands for planning group manipulator.
[ INFO] [1720681761.265207746]: Robot requested program
[ INFO] [1720681761.265314577]: Sent program to robot
[ INFO] [1720681761.666777741]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681767.330293074]: Connection to reverse interface dropped.
[ERROR] [1720681767.330372262]: Sending data through socket failed.
[ INFO] [1720681767.330578258]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681767.394339703]: Connection to reverse interface dropped.
[ INFO] [1720681767.394645176]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681767.762313332]: Connection to reverse interface dropped.
[ INFO] [1720681767.762605620]: Robot connected to reverse interface. Ready to receive control commands.
[ INFO] [1720681768.653402305]: Received request to compute Cartesian path
[ INFO] [1720681768.653640091]: Attempting to follow 1 waypoints for link 'tool_ros' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1720681768.654493803]: Computed Cartesian path with 3 points (followed 100.000000% of requested trajectory)
[ INFO] [1720681768.655322868]: Execution request received
[ERROR] [1720681769.547295425]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 0.887946 seconds). Stopping trajectory.
[ INFO] [1720681769.547412965]: Cancelling execution for scaled_pos_joint_traj_controller
[ INFO] [1720681769.547523602]: Completed trajectory execution with status TIMED_OUT ...
[ INFO] [1720681769.547772439]: Execution completed: TIMED_OUT
[ INFO] [1720681769.547821691]: Controller 'scaled_pos_joint_traj_controller' successfully finished

zp@zp-ros:~/ur_ws$ rosrun ur_motion test.py
[ INFO] [1720681766.835566262]: Loading robot model 'ur5_robot'...
[ INFO] [1720681768.445747971]: Ready to take commands for planning group manipulator.
current_pose: position: 
  x: 0.5781285236584838
  y: 0.002712864269137985
  z: 0.3416969679843662
orientation: 
  x: 0.9765838885197804
  y: 0.20517306711715044
  z: -0.01503785462199369
  w: 0.06294270523018755
target_pose: position: 
  x: 0.5681285236584838
  y: 0.002712864269137985
  z: 0.3416969679843662
orientation: 
  x: 0.9765838885197804
  y: 0.20517306711715044
  z: -0.01503785462199369
  w: 0.06294270523018755
[ INFO] [1720681769.548078583]: ABORTED: TIMED_OUT

Accept Public visibility

fmauch commented 1 month ago

Thank you for reporting this:

We tried reproducing this, but we (almost) couldn't.

As a start: As far as I am aware, the default moveit config in ur5_moveit_config doesn't check for execution times, so I assume, you have altered that? To which value did you set the allowed_execution_duration_scaling value? By default, that's set to 1.2 to allow some additional time for communication and so on. Also, the motion will take a bit longer, since there will have to be a buffer of positions filled up before the robot starts moving.

I was only able to reproduce this though, when setting the speed scaling to a value smaller than 1.0.

For example, when setting allowed_execution_duration_scaling=2.0, setting execution_duration_monitoring=true and setting the speed slider to 45% (rosservice call /ur_hardware_interface/set_speed_slider "speed_slider_fraction: 0.45") I was able to reproduce what you describe. Could you check whether your speed slider is not at 100%?

Otherwise: Setting the allowed duration scaling to a value at least somewhat greater than 1.0 seems a good idea to robustify things.

zp2546265641 commented 1 month ago

Thank you for reporting this:

We tried reproducing this, but we (almost) couldn't.

As a start: As far as I am aware, the default moveit config in ur5_moveit_config doesn't check for execution times, so I assume, you have altered that? To which value did you set the allowed_execution_duration_scaling value? By default, that's set to 1.2 to allow some additional time for communication and so on. Also, the motion will take a bit longer, since there will have to be a buffer of positions filled up before the robot starts moving.

I was only able to reproduce this though, when setting the speed scaling to a value smaller than 1.0.

For example, when setting allowed_execution_duration_scaling=2.0, setting execution_duration_monitoring=true and setting the speed slider to 45% (rosservice call /ur_hardware_interface/set_speed_slider "speed_slider_fraction: 0.45") I was able to reproduce what you describe. Could you check whether your speed slider is not at 100%?

Otherwise: Setting the allowed duration scaling to a value at least somewhat greater than 1.0 seems a good idea to robustify things.

speed_slider_fraction 100 is too fast. For safety reasons, I always set it to less than 20. So if I set it to 100, will this problem not occur? Or do I need to make some changes?

zp2546265641 commented 1 month ago

Thank you for reporting this:

We tried reproducing this, but we (almost) couldn't.

As a start: As far as I am aware, the default moveit config in ur5_moveit_config doesn't check for execution times, so I assume, you have altered that? To which value did you set the allowed_execution_duration_scaling value? By default, that's set to 1.2 to allow some additional time for communication and so on. Also, the motion will take a bit longer, since there will have to be a buffer of positions filled up before the robot starts moving.

I was only able to reproduce this though, when setting the speed scaling to a value smaller than 1.0.

For example, when setting allowed_execution_duration_scaling=2.0, setting execution_duration_monitoring=true and setting the speed slider to 45% (rosservice call /ur_hardware_interface/set_speed_slider "speed_slider_fraction: 0.45") I was able to reproduce what you describe. Could you check whether your speed slider is not at 100%?

Otherwise: Setting the allowed duration scaling to a value at least somewhat greater than 1.0 seems a good idea to robustify things.

I used the default configuration of moveit, only changed the IP and the corresponding port, and did not change anything else.

fmauch commented 1 month ago

speed_slider_fraction 100 is too fast. For safety reasons, I always set it to less than 20. So if I set it to 100, will this problem not occur? Or do I need to make some changes?

Yes. If the robot moves to fast, you can also reduce the planning max velocity and acceleration. Or you change the moveit configuration as mentioned above and combine it with the speed slider. You could probably even change the expected duration scaling based on the speed slider value with a small node.

VinDp commented 4 weeks ago

Since the issue is quite old and no further problems came up, I will close it. Feel free to re-open if it is still relevant.