nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Comment ompl config

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-01-19 14:53:53 +02:00
parent d7aa2c2403
commit 3e0da443ea
1 changed files with 40 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
src/lite6_controller/launch/lite6_gazebo.launch.py
View File

@@ -299,21 +299,21 @@ def launch_setup(context, *args, **kwargs):
kinematics_yaml = robot_description_parameters['robot_description_kinematics'] kinematics_yaml = robot_description_parameters['robot_description_kinematics']
joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
if add_gripper.perform(context) in ('True', 'true'): #if add_gripper.perform(context) in ('True', 'true'):
gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context))) # gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml') # gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')
gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml') # gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml')
if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml: # if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml:
for name in gripper_controllers_yaml['controller_names']: # for name in gripper_controllers_yaml['controller_names']:
if name in gripper_controllers_yaml: # if name in gripper_controllers_yaml:
if name not in controllers_yaml['controller_names']: # if name not in controllers_yaml['controller_names']:
controllers_yaml['controller_names'].append(name) # controllers_yaml['controller_names'].append(name)
controllers_yaml[name] = gripper_controllers_yaml[name] # controllers_yaml[name] = gripper_controllers_yaml[name]
if gripper_ompl_planning_yaml: # if gripper_ompl_planning_yaml:
ompl_planning_yaml.update(gripper_ompl_planning_yaml) # ompl_planning_yaml.update(gripper_ompl_planning_yaml)
if joint_limits_yaml and gripper_joint_limits_yaml: # if joint_limits_yaml and gripper_joint_limits_yaml:
joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits']) # joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params') add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
add_prefix_to_moveit_params( add_prefix_to_moveit_params(
@@ -325,22 +325,22 @@ def launch_setup(context, *args, **kwargs):
# Planning pipeline # Planning pipeline
# https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py
planning_pipeline = { #planning_pipeline = {
"planning_pipelines": ["ompl", "pilz_industrial_motion_planner"], # "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"],
"default_planning_pipeline": "pilz_industrial_motion_planner", # "default_planning_pipeline": "pilz_industrial_motion_planner",
"ompl": { # "ompl": {
"planning_plugin": "ompl_interface/OMPLPlanner", # "planning_plugin": "ompl_interface/OMPLPlanner",
# TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved # # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved
"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", # "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",
# TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled # # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled
"start_state_max_bounds_error": 0.31416, # "start_state_max_bounds_error": 0.31416,
}, # },
"pilz_industrial_motion_planner": { # "pilz_industrial_motion_planner": {
"planning_plugin": "pilz_industrial_motion_planner::CommandPlanner", # "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner",
"default_planner_config": "PTP", # "default_planner_config": "PTP",
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService", # "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
}, # },
} #}
# Kinematics # Kinematics
#kinematics = load_yaml('panda_moveit2_config', 'config/kinematics.yaml') #kinematics = load_yaml('panda_moveit2_config', 'config/kinematics.yaml')
@@ -349,14 +349,14 @@ def launch_setup(context, *args, **kwargs):
# Planning Configuration # Planning Configuration
ompl_planning_pipeline_config = { #ompl_planning_pipeline_config = {
'move_group': { # 'move_group': {
'planning_plugin': 'ompl_interface/OMPLPlanner', # 'planning_plugin': 'ompl_interface/OMPLPlanner',
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", # 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization 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, # 'start_state_max_bounds_error': 0.1,
} # }
} #}
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) #ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
pilz_planning_pipeline_config = { pilz_planning_pipeline_config = {
'move_group': { 'move_group': {
@@ -431,7 +431,8 @@ def launch_setup(context, *args, **kwargs):
arguments=['-d', rviz_config_file], arguments=['-d', rviz_config_file],
parameters=[ parameters=[
robot_description_parameters, robot_description_parameters,
ompl_planning_pipeline_config, #ompl_planning_pipeline_config,
pilz_planning_pipeline_config,
{'use_sim_time': use_sim_time}, {'use_sim_time': use_sim_time},
], ],
remappings=[ remappings=[