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

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']
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

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')
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(

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)
# 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 = {

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

@@ -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