Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed the wrong type for time parameterization in ompl #55

Merged
merged 10 commits into from
Jan 4, 2024
Merged

Fixed the wrong type for time parameterization in ompl #55

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
2e23a07
Fixed the wrong type for time parameterization in ompl
jodle001 Nov 10, 2023
66196fa
Updated to use the MoveItConfigsBuilder
jodle001 Nov 14, 2023
014959d
Optimized the imports.
jodle001 Nov 14, 2023
e5fe1b2
Update abb_bringup/launch/abb_moveit.launch.py
jodle001 Jan 2, 2024
396f044
Update abb_bringup/launch/abb_moveit.launch.py
jodle001 Jan 2, 2024
7eb9379
Update package.xml
jodle001 Jan 2, 2024
eadbc1d
Formatting adn also publishing robot_description and semantic
jodle001 Jan 2, 2024
e447429
Fixed formatting to pass format test per requeset by Yadunund.
jodle001 Jan 3, 2024
6f303b5
Used pre-commit to fix formatting locally.
jodle001 Jan 3, 2024
cee3f4a
Removed planning pipeline limits to ompl planner, also added pilz_car…
jodle001 Jan 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
158 changes: 72 additions & 86 deletions abb_bringup/launch/abb_moveit.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,17 +1,15 @@
import os

import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os
import yaml
from moveit_configs_utils import MoveItConfigsBuilder


def load_yaml(package_name, file_path):
Expand All @@ -32,94 +30,87 @@ def launch_setup(context, *args, **kwargs):
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")

# Planning context
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(support_package), "urdf", robot_xacro_file]
),
]
)
robot_description = {"robot_description": robot_description_content}

robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", moveit_config_file]
),
]
)
robot_description_semantic = {
"robot_description_semantic": robot_description_semantic_content.perform(
context
# MoveIt configuration
moveit_config = (
MoveItConfigsBuilder(
"abb_bringup", package_name=f"{moveit_config_package.perform(context)}"
)
}

kinematics_yaml = load_yaml(
"abb_irb1200_5_90_moveit_config", "config/kinematics.yaml"
)

joint_limits_yaml = {
"robot_description_planning": load_yaml(
moveit_config_package.perform(context), "config/joint_limits.yaml"
.robot_description(
file_path=os.path.join(
get_package_share_directory(f"{support_package.perform(context)}"),
"urdf",
f"{robot_xacro_file.perform(context)}",
)
)
}

# Planning Functionality
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml(
"abb_irb1200_5_90_moveit_config", "config/ompl_planning.yaml"
.robot_description_semantic(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
f"{moveit_config_file.perform(context)}",
)
)
.planning_pipelines(
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry one last thing came to mind. I don't think we should restrict the planning_pipelines to just ompl. If we remove this entry, the builder will load default configs for ompl, stomp, chompand pilz which would be nicer for users to play around with in the rviz palnning panel.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just a quick question, I removed the lines here:

Screenshot from 2024-01-03 12-57-20

But I was getting this error without adding a pilz_cartesian_limits.yaml file to the moveit_config package:

[ERROR] [launch]: Caught exception in launch (see debug for traceback): "File /opt/robotic_platform/install/abb_irb1200_5_90_moveit_config/share/abb_irb1200_5_90_moveit_config/config/pilz_cartesian_limits.yaml doesn't exist"

So I added this to the abb_irb1200_5_90_moveit_config package in the config directory:

cartesian_limits:
  max_trans_vel: 1.0
  max_trans_acc: 2.25
  max_trans_dec: -5.0
  max_rot_vel: 1.57

But even with this, If you select the pilz planner it doesn't seem to successfully plan a path. The other planners I see available are ompl, and CHOMP and they can both successfully plan a path.

Not sure how you would like me to proceed.

Copy link
Collaborator

@Yadunund Yadunund Jan 4, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the problem is because the pilz_indsutrial_motion_planner is loaded based on the deafult config file in moveit_config_utils but there isn't a default pilz_cartesian_limits.yaml file. Hence you had to manually include on in the config folder here. I think if you use moveit_setup_assistant, it does generate this file in the local config.
Perhaps you can open an issue ticket in upstream moveit to include a default pilz_cartesian_limits.yaml file in moveit_config_utils?

But even with this, If you select the pilz planner it doesn't seem to successfully plan a path. The other planners I see available are ompl, and CHOMP and they can both successfully plan a path.

I think this is because we have not set any acceleration limits in the joint_limits.yaml file which are needed by the planner. You should be seeing a message printout indicating this

[move_group-1] [INFO] [1704356296.402945412] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'pilz_industrial_motion_planner'
[move_group-1] [ERROR] [1704356296.402995752] [moveit.ros_planning.planning_pipeline]: Exception caught: 'acceleration limit not set for group manipulator'

If you edit the joint_limits.yaml file by setting has_acceleration_limts: true for each joint a specify some max_acceleration like 5.0, you should be apply to plan PTP motions with pilz_industrial_motion_planner.

I think it's best to leave the acceleration limits as false for now until someone can share accurate values for this specific robot.

pipelines=["ompl"],
default_planning_pipeline="ompl",
)
.robot_description_kinematics(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
"kinematics.yaml",
)
)
.trajectory_execution(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
"moveit_controllers.yaml",
)
)
.planning_scene_monitor(
publish_planning_scene=True,
publish_geometry_updates=True,
publish_state_updates=True,
publish_transforms_updates=True,
publish_robot_description=True,
publish_robot_description_semantic=True,
)
.joint_limits(
file_path=os.path.join(
get_package_share_directory(
f"{moveit_config_package.perform(context)}"
),
"config",
"joint_limits.yaml",
)
)
.to_moveit_configs()
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)

# Trajectory Execution Functionality
moveit_simple_controllers_yaml = load_yaml(
"abb_irb1200_5_90_moveit_config", "config/moveit_controllers.yaml"
)
# MoveIt controllers
moveit_controllers = {
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
"moveit_simple_controller_manager": load_yaml(
f"{moveit_config_package.perform(context)}",
"config/moveit_controllers.yaml",
),
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}

trajectory_execution = {
# MoveIt does not handle controller switching automatically
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This behavior was lost in the changes as by default, moveit_manage_controllers is set to True by the builder. I'll open a PR to update.

"moveit_manage_controllers": False,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}

planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}

# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
kinematics_yaml,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_config.trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
joint_limits_yaml,
moveit_config.to_dict(),
],
)

Expand All @@ -135,11 +126,7 @@ def launch_setup(context, *args, **kwargs):
output="log",
arguments=["-d", rviz_config],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
kinematics_yaml,
joint_limits_yaml,
moveit_config.to_dict(),
],
)

Expand All @@ -158,15 +145,14 @@ def launch_setup(context, *args, **kwargs):
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
parameters=[moveit_config.robot_description],
)

nodes_to_start = [move_group_node, rviz_node, static_tf_node, robot_state_pub_node]
return nodes_to_start


def generate_launch_description():

declared_arguments = []

# TODO(andyz): add other options
Expand Down
1 change: 1 addition & 0 deletions abb_bringup/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
<exec_depend>rviz2</exec_depend>
<exec_depend>urdf</exec_depend>
<exec_depend>xacro</exec_depend>
<exec_depend>moveit_configs_utils</exec_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
2 changes: 1 addition & 1 deletion robot_specific_config/abb_irb1200_5_90_moveit_config/config/ompl_planning.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
planning_plugin: ompl_interface/OMPLPlanner
request_adapters: >-
default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/AddTimeOptimalParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
Expand Down
Loading