Skip to content

Commit 37c831a

Browse files
jodle001Yadunund
jodle001
and
Yadunund
committed
Fixed the wrong type for time parameterization in ompl (#55)
* Fixed the wrong type for time parameterization in ompl * Updated to use the MoveItConfigsBuilder * Optimized the imports. * Update abb_bringup/launch/abb_moveit.launch.py Removed hard coded config package Co-authored-by: Yadu <[email protected]> * Update abb_bringup/launch/abb_moveit.launch.py added argument names for clarity. Co-authored-by: Yadu <[email protected]> * Update package.xml Added moveit_configs_utils dependency to package.xml * Formatting adn also publishing robot_description and semantic * Fixed formatting to pass format test per requeset by Yadunund. * Used pre-commit to fix formatting locally. * Removed planning pipeline limits to ompl planner, also added pilz_cartesian_limits yaml. --------- Co-authored-by: Yadu <[email protected]>
1 parent 51c2065 commit 37c831a

File tree

4 files changed

+76
-87
lines changed

4 files changed

+76
-87
lines changed

‎abb_bringup/launch/abb_moveit.launch.py

Lines changed: 69 additions & 86 deletions
Original file line numberDiff line numberDiff line change
@@ -1,17 +1,15 @@
1+
import os
2+
3+
import yaml
14
from ament_index_python.packages import get_package_share_directory
25
from launch import LaunchDescription
36
from launch.actions import DeclareLaunchArgument
47
from launch.actions import OpaqueFunction
58
from launch.substitutions import (
6-
Command,
7-
FindExecutable,
89
LaunchConfiguration,
9-
PathJoinSubstitution,
1010
)
1111
from launch_ros.actions import Node
12-
from launch_ros.substitutions import FindPackageShare
13-
import os
14-
import yaml
12+
from moveit_configs_utils import MoveItConfigsBuilder
1513

1614

1715
def load_yaml(package_name, file_path):
@@ -32,94 +30,84 @@ def launch_setup(context, *args, **kwargs):
3230
moveit_config_package = LaunchConfiguration("moveit_config_package")
3331
moveit_config_file = LaunchConfiguration("moveit_config_file")
3432

35-
# Planning context
36-
robot_description_content = Command(
37-
[
38-
PathJoinSubstitution([FindExecutable(name="xacro")]),
39-
" ",
40-
PathJoinSubstitution(
41-
[FindPackageShare(support_package), "urdf", robot_xacro_file]
42-
),
43-
]
44-
)
45-
robot_description = {"robot_description": robot_description_content}
46-
47-
robot_description_semantic_content = Command(
48-
[
49-
PathJoinSubstitution([FindExecutable(name="xacro")]),
50-
" ",
51-
PathJoinSubstitution(
52-
[FindPackageShare(moveit_config_package), "config", moveit_config_file]
53-
),
54-
]
55-
)
56-
robot_description_semantic = {
57-
"robot_description_semantic": robot_description_semantic_content.perform(
58-
context
33+
# MoveIt configuration
34+
moveit_config = (
35+
MoveItConfigsBuilder(
36+
"abb_bringup", package_name=f"{moveit_config_package.perform(context)}"
5937
)
60-
}
61-
62-
kinematics_yaml = load_yaml(
63-
"abb_irb1200_5_90_moveit_config", "config/kinematics.yaml"
64-
)
65-
66-
joint_limits_yaml = {
67-
"robot_description_planning": load_yaml(
68-
moveit_config_package.perform(context), "config/joint_limits.yaml"
38+
.robot_description(
39+
file_path=os.path.join(
40+
get_package_share_directory(f"{support_package.perform(context)}"),
41+
"urdf",
42+
f"{robot_xacro_file.perform(context)}",
43+
)
6944
)
70-
}
71-
72-
# Planning Functionality
73-
ompl_planning_pipeline_config = {
74-
"move_group": {
75-
"planning_plugin": "ompl_interface/OMPLPlanner",
76-
"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""",
77-
"start_state_max_bounds_error": 0.1,
78-
}
79-
}
80-
ompl_planning_yaml = load_yaml(
81-
"abb_irb1200_5_90_moveit_config", "config/ompl_planning.yaml"
45+
.robot_description_semantic(
46+
file_path=os.path.join(
47+
get_package_share_directory(
48+
f"{moveit_config_package.perform(context)}"
49+
),
50+
"config",
51+
f"{moveit_config_file.perform(context)}",
52+
)
53+
)
54+
.planning_pipelines()
55+
.robot_description_kinematics(
56+
file_path=os.path.join(
57+
get_package_share_directory(
58+
f"{moveit_config_package.perform(context)}"
59+
),
60+
"config",
61+
"kinematics.yaml",
62+
)
63+
)
64+
.trajectory_execution(
65+
file_path=os.path.join(
66+
get_package_share_directory(
67+
f"{moveit_config_package.perform(context)}"
68+
),
69+
"config",
70+
"moveit_controllers.yaml",
71+
)
72+
)
73+
.planning_scene_monitor(
74+
publish_planning_scene=True,
75+
publish_geometry_updates=True,
76+
publish_state_updates=True,
77+
publish_transforms_updates=True,
78+
publish_robot_description=True,
79+
publish_robot_description_semantic=True,
80+
)
81+
.joint_limits(
82+
file_path=os.path.join(
83+
get_package_share_directory(
84+
f"{moveit_config_package.perform(context)}"
85+
),
86+
"config",
87+
"joint_limits.yaml",
88+
)
89+
)
90+
.to_moveit_configs()
8291
)
83-
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
8492

85-
# Trajectory Execution Functionality
86-
moveit_simple_controllers_yaml = load_yaml(
87-
"abb_irb1200_5_90_moveit_config", "config/moveit_controllers.yaml"
88-
)
93+
# MoveIt controllers
8994
moveit_controllers = {
90-
"moveit_simple_controller_manager": moveit_simple_controllers_yaml,
95+
"moveit_simple_controller_manager": load_yaml(
96+
f"{moveit_config_package.perform(context)}",
97+
"config/moveit_controllers.yaml",
98+
),
9199
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
92100
}
93101

94-
trajectory_execution = {
95-
# MoveIt does not handle controller switching automatically
96-
"moveit_manage_controllers": False,
97-
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
98-
"trajectory_execution.allowed_goal_duration_margin": 0.5,
99-
"trajectory_execution.allowed_start_tolerance": 0.01,
100-
}
101-
102-
planning_scene_monitor_parameters = {
103-
"publish_planning_scene": True,
104-
"publish_geometry_updates": True,
105-
"publish_state_updates": True,
106-
"publish_transforms_updates": True,
107-
}
108-
109102
# Start the actual move_group node/action server
110103
move_group_node = Node(
111104
package="moveit_ros_move_group",
112105
executable="move_group",
113106
output="screen",
114107
parameters=[
115-
robot_description,
116-
robot_description_semantic,
117-
kinematics_yaml,
118-
ompl_planning_pipeline_config,
119-
trajectory_execution,
108+
moveit_config.trajectory_execution,
120109
moveit_controllers,
121-
planning_scene_monitor_parameters,
122-
joint_limits_yaml,
110+
moveit_config.to_dict(),
123111
],
124112
)
125113

@@ -135,11 +123,7 @@ def launch_setup(context, *args, **kwargs):
135123
output="log",
136124
arguments=["-d", rviz_config],
137125
parameters=[
138-
robot_description,
139-
robot_description_semantic,
140-
ompl_planning_pipeline_config,
141-
kinematics_yaml,
142-
joint_limits_yaml,
126+
moveit_config.to_dict(),
143127
],
144128
)
145129

@@ -158,15 +142,14 @@ def launch_setup(context, *args, **kwargs):
158142
executable="robot_state_publisher",
159143
name="robot_state_publisher",
160144
output="both",
161-
parameters=[robot_description],
145+
parameters=[moveit_config.robot_description],
162146
)
163147

164148
nodes_to_start = [move_group_node, rviz_node, static_tf_node, robot_state_pub_node]
165149
return nodes_to_start
166150

167151

168152
def generate_launch_description():
169-
170153
declared_arguments = []
171154

172155
# TODO(andyz): add other options

‎abb_bringup/package.xml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,7 @@
3333
<exec_depend>rviz2</exec_depend>
3434
<exec_depend>urdf</exec_depend>
3535
<exec_depend>xacro</exec_depend>
36+
<exec_depend>moveit_configs_utils</exec_depend>
3637

3738
<export>
3839
<build_type>ament_cmake</build_type>

‎robot_specific_config/abb_irb1200_5_90_moveit_config/config/ompl_planning.yaml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
planning_plugin: ompl_interface/OMPLPlanner
22
request_adapters: >-
3-
default_planner_request_adapters/AddTimeParameterization
3+
default_planner_request_adapters/AddTimeOptimalParameterization
44
default_planner_request_adapters/ResolveConstraintFrames
55
default_planner_request_adapters/FixWorkspaceBounds
66
default_planner_request_adapters/FixStartStateBounds

‎robot_specific_config/abb_irb1200_5_90_moveit_config/config/pilz_cartesian_limits.yaml

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
cartesian_limits:
2+
max_trans_vel: 1.0
3+
max_trans_acc: 2.25
4+
max_trans_dec: -5.0
5+
max_rot_vel: 1.57

0 commit comments

Comments
 (0)