diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py index 91087a1..985c39b 100644 --- a/src/lite6_controller/launch/lite6_gazebo.launch.py +++ b/src/lite6_controller/launch/lite6_gazebo.launch.py @@ -302,7 +302,7 @@ def launch_setup(context, *args, **kwargs): # 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'] = 0.5 + 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' @@ -336,6 +336,12 @@ def launch_setup(context, *args, **kwargs): prefix=prefix.perform(context)) + robot_description_parameters['cartesian_limits'] = {} + robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1 + robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25 + robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5 + robot_description_parameters['cartesian_limits']['max_rot_vel'] = 1.57 + # 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 d8ec192..56b1380 100644 --- a/src/lite6_controller/launch/lite6_real.launch.py +++ b/src/lite6_controller/launch/lite6_real.launch.py @@ -154,6 +154,12 @@ 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'] + robot_description_parameters['cartesian_limits'] = {} + robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1 + robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25 + robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5 + robot_description_parameters['cartesian_limits']['max_rot_vel'] = 1.57 + #kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin' #kinematics_yaml['kinematics_solver_search_resolution'] = 0.005 #kinematics_yaml['kinematics_solver_timeout'] = 0.005 @@ -182,7 +188,7 @@ def launch_setup(context, *args, **kwargs): # 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'] = 0.5 + joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.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 2ddcf70..6d2de17 100644 --- a/src/lite6_controller/launch/lite6_real_no_gui.launch.py +++ b/src/lite6_controller/launch/lite6_real_no_gui.launch.py @@ -158,7 +158,7 @@ def launch_setup(context, *args, **kwargs): # 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'] = 0.5 + 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,6 +182,12 @@ def launch_setup(context, *args, **kwargs): kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml, prefix=prefix.perform(context)) + robot_description_parameters['cartesian_limits'] = {} + robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1 + robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25 + robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5 + robot_description_parameters['cartesian_limits']['max_rot_vel'] = 1.57 + # Planning Configuration ompl_planning_pipeline_config = { 'move_group': {