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

Fix robot limit loading and LIN planner

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-27 17:22:23 +03:00
parent ef994440f4
commit 1d11b96b49
7 changed files with 49 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml
View File

@@ -1,5 +1,5 @@
cartesian_limits: cartesian_limits:
max_trans_vel: 1.0 max_trans_vel: 2.0
max_trans_acc: 2.25 max_trans_acc: 3.00
max_trans_dec: -5.0 max_trans_dec: -5.0
max_rot_vel: 1.57 max_rot_vel: 1.57

24
src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml
View File

@@ -4,31 +4,31 @@
joint_limits: joint_limits:
joint1: joint1:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 2.14 max_velocity: 3.14
has_acceleration_limits: true has_acceleration_limits: true
max_acceleration: 1.0 max_acceleration: 20.0
joint2: joint2:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 2.14 max_velocity: 3.14
has_acceleration_limits: true has_acceleration_limits: true
max_acceleration: 1.0 max_acceleration: 20.0
joint3: joint3:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 2.14 max_velocity: 3.14
has_acceleration_limits: true has_acceleration_limits: true
max_acceleration: 1.0 max_acceleration: 20.0
joint4: joint4:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 2.14 max_velocity: 3.14
has_acceleration_limits: true has_acceleration_limits: true
max_acceleration: 1.0 max_acceleration: 20.0
joint5: joint5:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 2.14 max_velocity: 3.14
has_acceleration_limits: true has_acceleration_limits: true
max_acceleration: 1.0 max_acceleration: 20.0
joint6: joint6:
has_velocity_limits: true has_velocity_limits: true
max_velocity: 2.14 max_velocity: 3.14
has_acceleration_limits: true has_acceleration_limits: true
max_acceleration: 1.0 max_acceleration: 20.0

15
src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
View File

@@ -65,19 +65,22 @@ def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None,
def get_xarm_robot_description_parameters( def get_xarm_robot_description_parameters(
xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
urdf_arguments={}, urdf_arguments={},
srdf_arguments={}, srdf_arguments={},
arguments={}): arguments={}):
urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware') 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_type = arguments.get('xarm_type', None)
# xarm_description/launch/lib/robot_description_lib.py # 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') 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 { return {
'robot_description': get_xacro_file_content( 'robot_description': get_xacro_file_content(
xacro_file=xacro_urdf_file, xacro_file=xacro_urdf_file,
@@ -87,6 +90,6 @@ def get_xarm_robot_description_parameters(
xacro_file=xacro_srdf_file, xacro_file=xacro_srdf_file,
arguments=srdf_arguments 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') 'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
} }

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

@@ -263,10 +263,14 @@ 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)
#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 # FIX acceleration limits
for i in range(1,7): #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)]['has_acceleration_limits'] = True
joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0 # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin' kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
#kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin' #kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin'
@@ -300,8 +304,8 @@ def launch_setup(context, *args, **kwargs):
prefix=prefix.perform(context)) prefix=prefix.perform(context))
cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') #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'] #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
# 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

13
src/lite6_controller/launch/lite6_real.launch.py
View File

@@ -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') ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
kinematics_yaml = robot_description_parameters['robot_description_kinematics'] 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'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
kinematics_yaml['kinematics_solver_search_resolution'] = 0.005 kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
kinematics_yaml['kinematics_solver_timeout'] = 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']) joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
# FIX acceleration limits # FIX acceleration limits
for i in range(1,7): #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)]['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'] = 1.5
#joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.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 # 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 = getattr(mod, 'add_prefix_to_moveit_params')
add_prefix_to_moveit_params( add_prefix_to_moveit_params(

10
src/lite6_controller/launch/lite6_real_no_gui.launch.py
View File

@@ -156,9 +156,9 @@ def launch_setup(context, *args, **kwargs):
joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
# FIX acceleration limits # FIX acceleration limits
for i in range(1,7): #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)]['has_acceleration_limits'] = True
joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0 # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
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)))
@@ -182,8 +182,8 @@ def launch_setup(context, *args, **kwargs):
kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml, kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
prefix=prefix.perform(context)) prefix=prefix.perform(context))
cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml') #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'] #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
# Planning Configuration # Planning Configuration
ompl_planning_pipeline_config = { ompl_planning_pipeline_config = {

12
src/lite6_controller/src/lite6_controller.cpp
View File

@@ -233,13 +233,15 @@ public:
moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem(); moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest(); planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
mpr.planner_id = "PTP"; //mpr.planner_id = "PTP";
//mpr.planner_id = "LIN"; mpr.planner_id = "LIN";
mpr.group_name = move_group.getName(); mpr.group_name = move_group.getName();
mpr.max_velocity_scaling_factor = 1.0; //mpr.max_velocity_scaling_factor = 1.0;
mpr.max_acceleration_scaling_factor = 0.98; 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.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"; //mpr.goal_constraints.position_constraints.header.frame_id = "world";
// A tolerance of 0.01 m is specified in position // A tolerance of 0.01 m is specified in position