From 1d11b96b496b16f1cfc04b9a6a376dd8dad5db96 Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Mon, 27 Mar 2023 17:22:23 +0300 Subject: [PATCH] Fix robot limit loading and LIN planner --- .../config/lite6/cartesian_limits.yaml | 4 ++-- .../config/lite6/joint_limits.yaml | 24 +++++++++---------- .../launch/lib/robot_moveit_config_lib.py | 15 +++++++----- .../launch/lite6_gazebo.launch.py | 14 +++++++---- .../launch/lite6_real.launch.py | 13 ++++------ .../launch/lite6_real_no_gui.launch.py | 10 ++++---- src/lite6_controller/src/lite6_controller.cpp | 12 ++++++---- 7 files changed, 49 insertions(+), 43 deletions(-) diff --git a/src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml b/src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml index c1e8d32..8734004 100644 --- a/src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml +++ b/src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml @@ -1,5 +1,5 @@ cartesian_limits: - max_trans_vel: 1.0 - max_trans_acc: 2.25 + max_trans_vel: 2.0 + max_trans_acc: 3.00 max_trans_dec: -5.0 max_rot_vel: 1.57 diff --git a/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml b/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml index 4f18d64..d3b0f1c 100755 --- a/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml +++ b/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml @@ -4,31 +4,31 @@ joint_limits: joint1: has_velocity_limits: true - max_velocity: 2.14 + max_velocity: 3.14 has_acceleration_limits: true - max_acceleration: 1.0 + max_acceleration: 20.0 joint2: has_velocity_limits: true - max_velocity: 2.14 + max_velocity: 3.14 has_acceleration_limits: true - max_acceleration: 1.0 + max_acceleration: 20.0 joint3: has_velocity_limits: true - max_velocity: 2.14 + max_velocity: 3.14 has_acceleration_limits: true - max_acceleration: 1.0 + max_acceleration: 20.0 joint4: has_velocity_limits: true - max_velocity: 2.14 + max_velocity: 3.14 has_acceleration_limits: true - max_acceleration: 1.0 + max_acceleration: 20.0 joint5: has_velocity_limits: true - max_velocity: 2.14 + max_velocity: 3.14 has_acceleration_limits: true - max_acceleration: 1.0 + max_acceleration: 20.0 joint6: has_velocity_limits: true - max_velocity: 2.14 + max_velocity: 3.14 has_acceleration_limits: true - max_acceleration: 1.0 + max_acceleration: 20.0 diff --git a/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py b/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py index 151bb3d..86e0efd 100644 --- a/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py +++ b/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py @@ -65,19 +65,22 @@ def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None, def get_xarm_robot_description_parameters( - xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), - xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), + xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), urdf_arguments={}, srdf_arguments={}, arguments={}): urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware') - moveit_config_package_name = 'xarm_moveit_config' + moveit_config_package_name = 'custom_xarm_moveit_config' xarm_type = arguments.get('xarm_type', None) # xarm_description/launch/lib/robot_description_lib.py - mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py')) + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('custom_xarm_description'), 'launch', 'lib', 'robot_description_lib.py')) get_xacro_file_content = getattr(mod, 'get_xacro_file_content') - + + joint_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml') + cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') + joint_limits['cartesian_limits'] = cartesian_limits['cartesian_limits'] return { 'robot_description': get_xacro_file_content( xacro_file=xacro_urdf_file, @@ -87,6 +90,6 @@ def get_xarm_robot_description_parameters( xacro_file=xacro_srdf_file, arguments=srdf_arguments ), - 'robot_description_planning': load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml'), + 'robot_description_planning': joint_limits, 'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml') } diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py index 0e38ecd..cb5086a 100644 --- a/src/lite6_controller/launch/lite6_gazebo.launch.py +++ b/src/lite6_controller/launch/lite6_gazebo.launch.py @@ -263,10 +263,14 @@ def launch_setup(context, *args, **kwargs): kinematics_yaml = robot_description_parameters['robot_description_kinematics'] joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) + #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') + #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits'] + #input(joint_limits_yaml) + # FIX acceleration limits - for i in range(1,7): - joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True - joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0 + #for i in range(1,7): + # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True + # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0 kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin' #kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin' @@ -300,8 +304,8 @@ def launch_setup(context, *args, **kwargs): prefix=prefix.perform(context)) - cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') - robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits'] + #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') + #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits'] # Planning pipeline # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py diff --git a/src/lite6_controller/launch/lite6_real.launch.py b/src/lite6_controller/launch/lite6_real.launch.py index c776d92..c349d32 100644 --- a/src/lite6_controller/launch/lite6_real.launch.py +++ b/src/lite6_controller/launch/lite6_real.launch.py @@ -154,9 +154,6 @@ def launch_setup(context, *args, **kwargs): ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') kinematics_yaml = robot_description_parameters['robot_description_kinematics'] - cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') - robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits'] - kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin' kinematics_yaml['kinematics_solver_search_resolution'] = 0.005 kinematics_yaml['kinematics_solver_timeout'] = 0.005 @@ -184,11 +181,11 @@ def launch_setup(context, *args, **kwargs): joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits']) # FIX acceleration limits - for i in range(1,7): - joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True - #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5 - #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5 - joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0 + #for i in range(1,7): + # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True + # #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5 + # #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5 + # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0 add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params') add_prefix_to_moveit_params( diff --git a/src/lite6_controller/launch/lite6_real_no_gui.launch.py b/src/lite6_controller/launch/lite6_real_no_gui.launch.py index 1107d19..cdf5c6b 100644 --- a/src/lite6_controller/launch/lite6_real_no_gui.launch.py +++ b/src/lite6_controller/launch/lite6_real_no_gui.launch.py @@ -156,9 +156,9 @@ def launch_setup(context, *args, **kwargs): joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) # FIX acceleration limits - for i in range(1,7): - joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True - joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0 + #for i in range(1,7): + # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True + # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0 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))) @@ -182,8 +182,8 @@ def launch_setup(context, *args, **kwargs): kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml, prefix=prefix.perform(context)) - cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') - robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits'] + #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') + #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits'] # Planning Configuration ompl_planning_pipeline_config = { diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp index 7d0b9b4..39b32c3 100644 --- a/src/lite6_controller/src/lite6_controller.cpp +++ b/src/lite6_controller/src/lite6_controller.cpp @@ -233,13 +233,15 @@ public: moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem(); planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest(); - mpr.planner_id = "PTP"; - //mpr.planner_id = "LIN"; + //mpr.planner_id = "PTP"; + mpr.planner_id = "LIN"; mpr.group_name = move_group.getName(); - mpr.max_velocity_scaling_factor = 1.0; - mpr.max_acceleration_scaling_factor = 0.98; + //mpr.max_velocity_scaling_factor = 1.0; + mpr.max_velocity_scaling_factor = 0.7; + mpr.max_acceleration_scaling_factor = 0.4; + //mpr.max_acceleration_scaling_factor = 0.1; mpr.allowed_planning_time = 10; - mpr.max_cartesian_speed = 2; // m/s + mpr.max_cartesian_speed = 3; // m/s //mpr.goal_constraints.position_constraints.header.frame_id = "world"; // A tolerance of 0.01 m is specified in position