moveit / moveit2

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

joint_limits.yaml - changes don't have any effect on the velocity of the robot in rviz #2198

Closed RobinHeitz closed 1 year ago

RobinHeitz commented 1 year ago

Description

In my moveit config package, there is an auto-generated file 'joint_limits.yaml'. If i change the values (e.g. factor 10 in velocity), there is no change in the trajectory/ movement of the robot arm in RViz - I expect it to be factor 10 slower. It's probably not an error, but I cannot find proper documentation to setup a MoveGroup-Node correctly to use the joint_limits.yaml (and further down the road: Change velocity and acceleration scaling factors with parameters from a launch file).

Your environment

Steps to reproduce

Launching MoveGroup-Node

 run_move_group_node_2 = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        # name="move_group_test",
        output="screen",
        arguments = ['--ros-args', '--log-level', 'WARN'],
        parameters=[
            robot_description,
            robot_description_semantic,
            kinematics_yaml,
            joint_limits_yaml,
            ompl_planning_pipeline_config,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
        ],
    )

Using MoveGroupInterface:

       std::vector<geometry_msgs::msg::Pose> waypoints;
        geometry_msgs::msg::PoseStamped target_pose;
        target_pose.pose = target;
        target_pose.header.frame_id = end_effector_link;
        waypoints.push_back(target);

        moveit_msgs::msg::RobotTrajectory trajectory;
        const double jump_threshold = 0.0;
        const double eef_step = 0.01;
        double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); // Fraction refers to planned path coverage. Should be over 100% = 1

        // Execute the plan
        if (fraction >= 1) // fraction = part of planned trajectory (should be 100%)
        {
            RCLCPP_ERROR(LOGGER, "Execute");
            move_group_interface.execute(trajectory);

            return true;

The terminal window outputs _move_group-4] [WARN] [1684833457.407691711] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or jointlimits.yaml. indicating that joint_limits.yaml is not loaded at all, since there are joint limits defined:

default_velocity_scaling_factor: 1.0
default_acceleration_scaling_factor: 1.0

joint_limits:
  melfa_1_joint1:
    has_velocity_limits: true
    max_velocity: 6.2831853071795862
    # max_velocity: 0.62831853071795862
    has_acceleration_limits: true
    max_acceleration: 6.2831853071795862
  melfa_1_joint2:
    has_velocity_limits: true
    max_velocity: 6.9987703004972612
    # max_velocity: 0.69987703004972612
    has_acceleration_limits: true
    max_acceleration: 6.9987703004972612
  ...

Expected behaviour

Based on documentation, I expect that 1) the file joint_limits.yaml - file is loaded automatically and overrides (maybe existing) joint limits from the urdf. 2), if changes are made to this file, the trajectory/ movement itself in rviz (and of course the real robot too) should represent these changes.

By default MoveIt sets the joint velocity and acceleration limits of a joint trajectory to the default allowed in the robot’s URDF or joint_limits.yaml. The joint_limits.yaml is generated from the Setup Assistant and is initially an exact copy of the values within the URDF. The user can then modify those values to be less than the original URDF values if special constraints are needed. Specific joint properties can be changed with the keys max_position, min_position, max_velocity, max_acceleration, max_jerk. Joint limits can be turned on or off with the keys has_velocity_limits, has_acceleration_limits, has_jerk_limits. https://moveit.picknik.ai/humble/doc/examples/time_parameterization/time_parameterization_tutorial.html?highlight=joint_limits

Actual behaviour

Unfortunately, there is no change in the speed whatsoever.

andrey-pr commented 1 year ago

Here is my code of it:

joint_limits = {"robot_description_planning": load_yaml(
        "robol_moveit_model", "config/joint_limits.yaml"
    )}
main_node = Node(
        package="my_node",
        executable="my_node",
        output="screen",
        parameters=[
            ...
            joint_limits
            ...
        ],
    )
...
run_move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            robot_description,
            robot_description_semantic,
            kinematics_yaml,
            joint_limits,
            ompl_planning_pipeline_config,
            trajectory_execution,
            moveit_controllers,
            planning_scene_monitor_parameters,
        ],
    )
...
rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[robot_description, robot_description_semantic,
                    ompl_planning_pipeline_config, kinematics_yaml, joint_limits],
    )

joint_limits.yaml:

joint_limits:
  joint1:
    has_velocity_limits: true
    max_velocity: 1.047132101
    has_acceleration_limits: true
    max_acceleration: 1.0
henningkayser commented 1 year ago

Thanks for reporting this issue. First-off, I've just tried customizing velocity and acceleration limits on using moveit_resource's Panda robot and they certainly work, speed is changing and no warning for me. However, there are several things that could go wrong when specifying the launch config, which is why we have added the MoveItConfigsBuilder, see the Panda example. From what you've posted I can't identify any obvious issues, unfortunately.

Things to check:

RobinHeitz commented 1 year ago

Thanks for your answer @henningkayser :) I don't know whats wrong with my launch file(s).

First, I'm not able to use "ros2_control_hardware_type": LaunchConfiguration( "ros2_control_hardware_type" ) for the mapping; it yields the error

[ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught exception when trying to load file of format [py]: replace() argument 2 must be str, not LaunchConfiguration

Declared arguments look like this:

    declared_arguments.append(
        DeclareLaunchArgument(
            "prefix_1",
            default_value="melfa_1",
            description="Prefix of the joint names, useful for \
        multi-robot setup. If changed than also joint names in the controllers' configuration \
        have to be updated.",
        )
    )

And they are passed to the LaunchDescription:


return LaunchDescription(
        declared_arguments +
        [
            rviz_node,
            static_tf,
            robot_state_publisher,
            run_move_group_node_2,
            ros2_control_node
        ]
        + controller_nodes,
    )

So, I need to hard-code the xacro parameters :(

Anyways, this is my builder:

moveit_config = (
        MoveItConfigsBuilder("melfa")
        .robot_description(
            file_path=get_package_share_directory(MELFA_DESCRIPTION_PKG) + "/urdf/rv7fm_dual_cell.urdf.xacro",
            mappings={
                "prefix_1": "melfa_1,
                "prefix_2": "melfa_2",
                "use_fake_hardware": "true",
                "fake_sensor_commands": "false",
                "slowdown": "3.0",
                "robot_ip_1": "",
                "robot_ip_2": "",
                "attached_object":"true"

            },
        )
        .robot_description_semantic(file_path="config/" + SEMANTIC_FILE_NAME)
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .joint_limits(file_path="config/joint_limits.yaml")
        .to_moveit_configs()
    )

I used this config builder in any launch file and pass all parameters with 'to_dict()'.

I still get the error

[move_group-4] [WARN] [1685093015.010618537] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.


Some additional words to my setup: I've 2 robot arms in a single urdf file. One robot uses the/ your hybrid planning architecture and should plan around the other arm that is moving in its way.

Because the HP robot's movement is not fluent, I want to change the velo/ acc parameters. I'm using one moveit config package that contains both arms; e.g. kinematics.yaml contains the 2 planning groups for both arms.

(just in case this has something to do with it. Since they've different prefixes it should not collide parameter-wise).

henningkayser commented 1 year ago

That's really odd, the joint names should exactly match the ones used in the description including prefix (looks like this already is the case for you). Different groups don't change the joint names, though. Are other limits being picked up, say if you use RViz motion planning panel? Maybe @JafarAbdi could have a look at your launch config.

RobinHeitz commented 1 year ago

Well, the error regarding that no acceleration_limits are set is gone now. Don't really know whats changed. There is a noticeable difference in movement based on joint_limits.yaml's max_velocity and max_acceleration values. However, default_velocity_scaling_factor and default_acceleration_scaling_factor have no effect at all.

Seems like MoveitConfigBuilder did it's job! Thanks for that!

But still the error with the mapping:

 mappings={
                "prefix_1": "melfa_1",
                  ...

works fine, but as soon as a LaunchConfiguration appears:

[DEBUG] [launch]: processing event: '<launch.events.include_launch_description.IncludeLaunchDescription object at 0x7faf88db28c0>'
[DEBUG] [launch]: processing event: '<launch.events.include_launch_description.IncludeLaunchDescription object at 0x7faf88db28c0>' ✓ '<launch.event_handlers.on_include_launch_description.OnIncludeLaunchDescription object at 0x7faf89708100>'
[DEBUG] [launch]: An exception was raised in an async action/event
[DEBUG] [launch]: Traceback (most recent call last):
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/any_launch_file_utilities.py", line 54, in get_launch_description_from_any_launch_file
    return loader(launch_file_path)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/python_launch_file_utilities.py", line 68, in get_launch_description_from_python_launch_file
    return getattr(launch_file_module, 'generate_launch_description')()
  File "/home/ros7/dev/mitsubishi-ros/colcon_melfa/install/mitsubishi_mover/share/mitsubishi_mover/launch/move2.launch.py", line 170, in generate_launch_description
    MoveItConfigsBuilder("melfa")
  File "/opt/ros/humble/lib/python3.10/site-packages/moveit_configs_utils/moveit_configs_builder.py", line 213, in robot_description
    se[DEBUG] [launch]: processing event: '<launch.events.include_launch_description.IncludeLaunchDescription object at 0x7faf88db28c0>'
[DEBUG] [launch]: processing event: '<launch.events.include_launch_description.IncludeLaunchDescription object at 0x7faf88db28c0>' ✓ '<launch.event_handlers.on_include_launch_description.OnIncludeLaunchDescription object at 0x7faf89708100>'
[DEBUG] [launch]: An exception was raised in an async action/event
[DEBUG] [launch]: Traceback (most recent call last):
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/any_launch_file_utilities.py", line 54, in get_launch_description_from_any_launch_file
    return loader(launch_file_path)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/python_launch_file_utilities.py", line 68, in get_launch_description_from_python_launch_file
    return getattr(launch_file_module, 'generate_launch_description')()
  File "/home/ros7/dev/mitsubishi-ros/colcon_melfa/install/mitsubishi_mover/share/mitsubishi_mover/launch/move2.launch.py", line 170, in generate_launch_description
    MoveItConfigsBuilder("melfa")
  File "/opt/ros/humble/lib/python3.10/site-packages/moveit_configs_utils/moveit_configs_builder.py", line 213, in robot_description
    self.__robot_description: load_xacro(
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_param_builder/utils.py", line 82, in load_xacro
    file = xacro.process_file(file_path, mappings=mappings)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 1098, in process_file
    process_doc(doc, **kwargs)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 1050, in process_doc
    eval_all(doc.documentElement, macros, symbols)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 977, in eval_all
    elif handle_macro_call(node, macros, symbols):
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 790, in handle_macro_call
    scoped_symbols._setitem(name, eval_text(value, symbols), unevaluated=False)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 706, in eval_text
    results.append(handle_extension(lex.next()[1][2:-1]))
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 696, in handle_extension
    return eval_extension("$(%s)" % eval_text(s, symbols))
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 289, in eval_extension
    return resolve_args(s, context=substitution_args_context)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/substitution_args.py", line 322, in resolve_args
    resolved = _resolve_args(arg_str, context, commands)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/substitution_args.py", line 338, in _resolve_args
    resolved = commands[command](resolved, a, args, context)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/substitution_args.py", line 182, in _arg
    return resolved.replace('$(%s)' % a, _eval_arg(name=args[0], args=context['arg']))
TypeError: replace() argument 2 must be str, not LaunchConfiguration

The above exception was the direct cause of the following exception:

Traceback (most recent call last):
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 336, in run_async
    raise completed_tasks_exceptions[0]
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 230, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 250, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/actions/include_launch_description.py", line 148, in execute
    launch_description = self.__launch_description_source.get_launch_description(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_source.py", line 84, in get_launch_description
    self._get_launch_description(self.__expanded_location)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/any_launch_description_source.py", line 53, in _get_launch_description
    return get_launch_description_from_any_launch_file(location)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/any_launch_file_utilities.py", line 57, in get_launch_description_from_any_launch_file
    raise InvalidLaunchFileError(extension, likely_errors=exceptions)
launch.invalid_launch_file_error.InvalidLaunchFileError: Caught exception when trying to load file of format [py]: replace() argument 2 must be str, not LaunchConfiguration

[ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught exception when trying to load file of format [py]: replace() argument 2 must be str, not LaunchConfiguration
[DEBUG] [launch.launch_context]: emitting event: 'launch.events.Shutdown'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7faf89708580>'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7faf89708580>' ✓ '<launch.event_handlers.on_shutdown.OnShutdown object at 0x7faf8970a650>'
lf.__robot_description: load_xacro(
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_param_builder/utils.py", line 82, in load_xacro
    file = xacro.process_file(file_path, mappings=mappings)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 1098, in process_file
    process_doc(doc, **kwargs)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 1050, in process_doc
    eval_all(doc.documentElement, macros, symbols)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 977, in eval_all
    elif handle_macro_call(node, macros, symbols):
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 790, in handle_macro_call
    scoped_symbols._setitem(name, eval_text(value, symbols), unevaluated=False)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 706, in eval_text
    results.append(handle_extension(lex.next()[1][2:-1]))
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 696, in handle_extension
    return eval_extension("$(%s)" % eval_text(s, symbols))
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/__init__.py", line 289, in eval_extension
    return resolve_args(s, context=substitution_args_context)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/substitution_args.py", line 322, in resolve_args
    resolved = _resolve_args(arg_str, context, commands)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/substitution_args.py", line 338, in _resolve_args
    resolved = commands[command](resolved, a, args, context)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/xacro/substitution_args.py", line 182, in _arg
    return resolved.replace('$(%s)' % a, _eval_arg(name=args[0], args=context['arg']))
TypeError: replace() argument 2 must be str, not LaunchConfiguration

The above exception was the direct cause of the following exception:

Traceback (most recent call last):
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 336, in run_async
    raise completed_tasks_exceptions[0]
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 230, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_service.py", line 250, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/actions/include_launch_description.py", line 148, in execute
    launch_description = self.__launch_description_source.get_launch_description(context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_source.py", line 84, in get_launch_description
    self._get_launch_description(self.__expanded_location)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/any_launch_description_source.py", line 53, in _get_launch_description
    return get_launch_description_from_any_launch_file(location)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch/launch_description_sources/any_launch_file_utilities.py", line 57, in get_launch_description_from_any_launch_file
    raise InvalidLaunchFileError(extension, likely_errors=exceptions)
launch.invalid_launch_file_error.InvalidLaunchFileError: Caught exception when trying to load file of format [py]: replace() argument 2 must be str, not LaunchConfiguration

[ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught exception when trying to load file of format [py]: replace() argument 2 must be str, not LaunchConfiguration
[DEBUG] [launch.launch_context]: emitting event: 'launch.events.Shutdown'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7faf89708580>'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7faf89708580>' ✓ '<launch.event_handlers.on_shutdownce() argument 2 must be str, not LaunchConfiguration
[DEBUG] [launch.launch_context]: emitting event: 'launch.events.Shutdown'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7faf89708580>'
[DEBUG] [launch]: processing event: '<launch.events.shutdown.Shutdown object at 0x7faf89708580>' ✓ '<launch.event_handlers.on_shutdown.OnShutdown object at 0x7faf8970a650>'
.OnShutdown object at 0x7faf8970a650>'

Any idea? Should be the same like your panda example... Thanks!

github-actions[bot] commented 1 year 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.

RobinHeitz commented 1 year ago

Well I'm not working on that project anymore; as far as I know its still there.