search

moveit / moveit2

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

computeTimeStamps uses maximum velocity and acceleration values that are different from those used by computeCartesianPath. #2907

Closed bodkal closed 1 month ago

bodkal commented 4 months ago

Description

Hi all,

I'm using a UR10e robot (https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and trying to control the Cartesian path's velocity and acceleration through code.

When I generate a Cartesian path, it uses the default values from the ur_moveit_config/config/joint_limits.yaml file. To slow it down, I use the computeTimeStamps function with a scale of 1 and 1 (which isn't supposed to change anything), but I end up with a new scale with a maximum acceleration of 1.

Do computeTimeStamps and computeCartesianPath use the same joint_limits.yaml file? Any ideas?

Thanks!

Your environment

Steps to reproduce

my snip code:

    double fraction = this->move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, false);
    // The trajectory needs to be modified so it will include velocities as well.
    // First to create a RobotTrajectory object
    robot_trajectory::RobotTrajectory rt(move_group_interface.getCurrentState()->getRobotModel(), "ur_manipulator");
    // Second get a RobotTrajectory from trajectory
    rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
    this->printRobotTrajectory(trajectory);
    // Thrid create a IterativeParabolicTimeParameterization object
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    // Fourth compute computeTimeStamps
    bool success = iptp.computeTimeStamps(rt,1.0,1.0);
    // Get RobotTrajectory_msg from RobotTrajectory
    rt.getRobotTrajectoryMsg(trajectory);
    this->printRobotTrajectory(trajectory);

Expected behavior:

Scaling the trajectory according to the ur_moveit_config/config/joint_limits.yaml file.

Actual behavior:

Scaling the trajectory with a maximum acceleration of 1.

Backtrace or Console output:

the trajectory from computeCartesianPath:

[hello_moveit_ur-1] [INFO] [1720955358.787508461] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787513947] [UrIntrface]: Joint 1: Pos = -2.066405, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787516961] [UrIntrface]: Joint 2: Pos = -0.904878, Vel = -0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787520392] [UrIntrface]: Joint 3: Pos = -1.741106, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787523321] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787526091] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787528938] [UrIntrface]: Point 1:
[hello_moveit_ur-1] [INFO] [1720955358.787531190] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = -0.000001
[hello_moveit_ur-1] [INFO] [1720955358.787535333] [UrIntrface]: Joint 1: Pos = -2.062843, Vel = 0.067846, Acc = 0.506396
[hello_moveit_ur-1] [INFO] [1720955358.787538609] [UrIntrface]: Joint 2: Pos = -0.929877, Vel = -0.499965, Acc = -4.999757
[hello_moveit_ur-1] [INFO] [1720955358.787541760] [UrIntrface]: Joint 3: Pos = -1.719669, Vel = 0.432119, Acc = 4.493361
[hello_moveit_ur-1] [INFO] [1720955358.787545016] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787548208] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = -0.000001
[hello_moveit_ur-1] [INFO] [1720955358.787551260] [UrIntrface]: Point 2:
[hello_moveit_ur-1] [INFO] [1720955358.787553455] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = -0.000001
[hello_moveit_ur-1] [INFO] [1720955358.787556588] [UrIntrface]: Joint 1: Pos = -2.054714, Vel = 0.079032, Acc = -0.460861
[hello_moveit_ur-1] [INFO] [1720955358.787559765] [UrIntrface]: Joint 2: Pos = -1.004782, Vel = -0.992390, Acc = -4.540463
[hello_moveit_ur-1] [INFO] [1720955358.787563011] [UrIntrface]: Joint 3: Pos = -1.652893, Vel = 0.913358, Acc = 5.001324
[hello_moveit_ur-1] [INFO] [1720955358.787566327] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787569921] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = -0.000001
[hello_moveit_ur-1] [INFO] [1720955358.787573741] [UrIntrface]: Point 3:
[hello_moveit_ur-1] [INFO] [1720955358.787576031] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = -0.000001
[hello_moveit_ur-1] [INFO] [1720955358.787579100] [UrIntrface]: Joint 1: Pos = -2.051780, Vel = -0.050934, Acc = -2.272780
[hello_moveit_ur-1] [INFO] [1720955358.787582048] [UrIntrface]: Joint 2: Pos = -1.124058, Vel = -1.362601, Acc = -2.729476
[hello_moveit_ur-1] [INFO] [1720955358.787585182] [UrIntrface]: Joint 3: Pos = -1.536551, Vel = 1.413535, Acc = 5.002257
[hello_moveit_ur-1] [INFO] [1720955358.787588092] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.787590978] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = -0.000001

the trajectory aftre scaile of 1.0:

[hello_moveit_ur-1] [INFO] [1720955358.789620880] [UrIntrface]: Point 0:
[hello_moveit_ur-1] [INFO] [1720955358.789623938] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789628148] [UrIntrface]: Joint 1: Pos = -2.066405, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789632070] [UrIntrface]: Joint 2: Pos = -0.904878, Vel = -0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789635972] [UrIntrface]: Joint 3: Pos = -1.741106, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789640596] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789644547] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789648407] [UrIntrface]: Point 1:
[hello_moveit_ur-1] [INFO] [1720955358.789651061] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = -0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789655013] [UrIntrface]: Joint 1: Pos = -2.062843, Vel = 0.027366, Acc = 0.071435
[hello_moveit_ur-1] [INFO] [1720955358.789659172] [UrIntrface]: Joint 2: Pos = -0.929877, Vel = -0.230203, Acc = -0.871461
[hello_moveit_ur-1] [INFO] [1720955358.789663338] [UrIntrface]: Joint 3: Pos = -1.719669, Vel = 0.202837, Acc = 0.800026
[hello_moveit_ur-1] [INFO] [1720955358.789667227] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789674011] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = -0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789678018] [UrIntrface]: Point 2:
[hello_moveit_ur-1] [INFO] [1720955358.789680875] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = -0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789684632] [UrIntrface]: Joint 1: Pos = -2.054714, Vel = 0.023265, Acc = -0.094984
[hello_moveit_ur-1] [INFO] [1720955358.789688727] [UrIntrface]: Joint 2: Pos = -1.004782, Vel = -0.399931, Acc = -0.662526
[hello_moveit_ur-1] [INFO] [1720955358.789692781] [UrIntrface]: Joint 3: Pos = -1.652893, Vel = 0.376665, Acc = 0.757510
[hello_moveit_ur-1] [INFO] [1720955358.789696868] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789700483] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = -0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789704390] [UrIntrface]: Point 3:
[hello_moveit_ur-1] [INFO] [1720955358.789707210] [UrIntrface]: Joint 0: Pos = -2.179356, Vel = -0.000000, Acc = -0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789711358] [UrIntrface]: Joint 1: Pos = -2.051780, Vel = -0.037939, Acc = -0.411608
[hello_moveit_ur-1] [INFO] [1720955358.789715236] [UrIntrface]: Joint 2: Pos = -1.124058, Vel = -0.550285, Acc = -0.582638
[hello_moveit_ur-1] [INFO] [1720955358.789719092] [UrIntrface]: Joint 3: Pos = -1.536551, Vel = 0.588224, Acc = 0.994246
[hello_moveit_ur-1] [INFO] [1720955358.789723151] [UrIntrface]: Joint 4: Pos = 1.570796, Vel = 0.000000, Acc = 0.000000
[hello_moveit_ur-1] [INFO] [1720955358.789727116] [UrIntrface]: Joint 5: Pos = -3.750153, Vel = -0.000000, Acc = -0.000000

ur_moveit_config/config/joint_limits.yaml


joint_limits:
  shoulder_pan_joint:
    has_acceleration_limits: true
    max_acceleration: 5.0
  shoulder_lift_joint:
    has_acceleration_limits: true
    max_acceleration: 5.0
  elbow_joint:
    has_acceleration_limits: true
    max_acceleration: 5.0
  wrist_1_joint:
    has_acceleration_limits: true
    max_acceleration: 5.0
  wrist_2_joint:
    has_acceleration_limits: true
    max_acceleration: 5.0
  wrist_3_joint:
    has_acceleration_limits: true
    max_acceleration: 5.0

ur_description/config/ur10e:

# Joints limits
#
# Sources:
#
#  - Universal Robots e-Series, User Manual, UR10e, Version 5.8
#    https://s3-eu-west-1.amazonaws.com/ur-support-site/69139/99405_UR10e_User_Manual_en_Global.pdf
#  - Support > Articles > UR articles > Max. joint torques
#    https://www.universal-robots.com/articles/ur-articles/max-joint-torques
#    retrieved: 2020-06-16, last modified: 2020-06-09
joint_limits:
  shoulder_pan_joint:
    # acceleration limits are not publicly available
    has_acceleration_limits: false
    has_effort_limits: true
    has_position_limits: true
    has_velocity_limits: true
    max_effort: 330.0
    max_position: !degrees  360.0
    max_velocity: !degrees  120.0
    min_position: !degrees -360.0
  shoulder_lift_joint:
    # acceleration limits are not publicly available
    has_acceleration_limits: false
    has_effort_limits: true
    has_position_limits: true
    has_velocity_limits: true
    max_effort: 330.0
    max_position: !degrees  360.0
    max_velocity: !degrees  120.0
    min_position: !degrees -360.0
  elbow_joint:
    # acceleration limits are not publicly available
    has_acceleration_limits: false
    has_effort_limits: true
    has_position_limits: true
    has_velocity_limits: true
    max_effort: 150.0
    # we artificially limit this joint to half its actual joint position limit
    # to avoid (MoveIt/OMPL) planning problems, as due to the physical
    # construction of the robot, it's impossible to rotate the 'elbow_joint'
    # over more than approx +- 1 pi (the shoulder lift joint gets in the way).
    #
    # This leads to planning problems as the search space will be divided into
    # two sections, with no connections from one to the other.
    #
    # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for
    # more information.
    max_position: !degrees  180.0
    max_velocity: !degrees  180.0
    min_position: !degrees -180.0
  wrist_1_joint:
    # acceleration limits are not publicly available
    has_acceleration_limits: false
    has_effort_limits: true
    has_position_limits: true
    has_velocity_limits: true
    max_effort: 56.0
    max_position: !degrees  360.0
    max_velocity: !degrees  180.0
    min_position: !degrees -360.0
  wrist_2_joint:
    # acceleration limits are not publicly available
    has_acceleration_limits: false
    has_effort_limits: true
    has_position_limits: true
    has_velocity_limits: true
    max_effort: 56.0
    max_position: !degrees  360.0
    max_velocity: !degrees  180.0
    min_position: !degrees -360.0
  wrist_3_joint:
    # acceleration limits are not publicly available
    has_acceleration_limits: false
    has_effort_limits: true
    has_position_limits: true
    has_velocity_limits: true
    max_effort: 56.0
    max_position: !degrees  360.0
    max_velocity: !degrees  180.0
    min_position: !degrees -360.0
github-actions[bot] commented 3 months 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.

github-actions[bot] commented 1 month ago

This issue was closed because it has been stalled for 45 days with no activity.