diff --git a/src/axidraw_controller/CMakeLists.txt b/src/axidraw_controller/CMakeLists.txt index 4af5abc..b84e195 100644 --- a/src/axidraw_controller/CMakeLists.txt +++ b/src/axidraw_controller/CMakeLists.txt @@ -40,6 +40,11 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/src/lite6_controller/CMakeLists.txt b/src/lite6_controller/CMakeLists.txt index f944cbb..c6054ff 100644 --- a/src/lite6_controller/CMakeLists.txt +++ b/src/lite6_controller/CMakeLists.txt @@ -72,4 +72,9 @@ install(TARGETS install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) + ament_package() diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py index c72f93c..9d8c87a 100644 --- a/src/lite6_controller/launch/lite6_gazebo.launch.py +++ b/src/lite6_controller/launch/lite6_gazebo.launch.py @@ -19,6 +19,7 @@ def launch_setup(context, *args, **kwargs): log_level = LaunchConfiguration("log_level", default='info') rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("custom_xarm_description"), "rviz", "display.rviz")) + prefix = LaunchConfiguration('prefix', default='') hw_ns = LaunchConfiguration('hw_ns', default='xarm') limited = LaunchConfiguration('limited', default=False) @@ -189,6 +190,7 @@ def launch_setup(context, *args, **kwargs): robot_description, robot_description_semantic, {"use_sim_time": use_sim_time}, + lite6_config, ], ), Node( @@ -263,6 +265,8 @@ 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) + lite6_config = load_yaml('lite6_controller', 'config', 'config.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'] #input(joint_limits_yaml) diff --git a/src/lite6_controller/launch/lite6_real.launch.py b/src/lite6_controller/launch/lite6_real.launch.py index 04a0302..57fb59d 100644 --- a/src/lite6_controller/launch/lite6_real.launch.py +++ b/src/lite6_controller/launch/lite6_real.launch.py @@ -15,6 +15,7 @@ from launch.events import Shutdown def launch_setup(context, *args, **kwargs): + robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150') report_type = LaunchConfiguration('report_type', default='normal') prefix = LaunchConfiguration('prefix', default='') @@ -164,6 +165,8 @@ def launch_setup(context, *args, **kwargs): joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) + lite6_config = load_yaml('lite6_controller', 'config', 'config.yaml') + 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_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml') @@ -352,6 +355,7 @@ def launch_setup(context, *args, **kwargs): #robot_description_parameters['robot_description_kinematics'], robot_description_parameters, {"use_sim_time": use_sim_time}, + lite6_config, ], ), ] 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 df866fc..af0e551 100644 --- a/src/lite6_controller/launch/lite6_real_no_gui.launch.py +++ b/src/lite6_controller/launch/lite6_real_no_gui.launch.py @@ -15,6 +15,7 @@ from launch.events import Shutdown def launch_setup(context, *args, **kwargs): + robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150') report_type = LaunchConfiguration('report_type', default='normal') prefix = LaunchConfiguration('prefix', default='') @@ -155,6 +156,9 @@ 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) + #lite6_config = load_yaml('lite6_controller', 'config', 'config.yaml') + lite6_config = os.path.join('lite6_controller', 'config', 'config.yaml') + # FIX acceleration limits #for i in range(1,7): # joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True @@ -342,6 +346,7 @@ def launch_setup(context, *args, **kwargs): #robot_description_parameters['robot_description_kinematics'], robot_description_parameters, {"use_sim_time": use_sim_time}, + lite6_config, ], ), ] diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp index dc2a966..6a9d05d 100644 --- a/src/lite6_controller/src/lite6_controller.cpp +++ b/src/lite6_controller/src/lite6_controller.cpp @@ -80,8 +80,7 @@ public: float xlim_upper = 0.305; float ylim_lower = -0.1475; float ylim_upper = 0.1475; - //float zlim_lower = 0.1945; - float zlim_lower = 0.207493; + float zlim_lower = 0.141087; float zlim_upper = zlim_lower + 0.01; @@ -92,6 +91,24 @@ public: : RobotController(options), move_group(std::shared_ptr(std::move(this)), MOVE_GROUP) { + // Declare parameters + this->declare_parameter("x_limit_lower", 0.1); + this->declare_parameter("x_limit_upper", 0.305); + this->declare_parameter("y_limit_lower", -0.1475); + this->declare_parameter("y_limit_upper", 0.1475); + this->declare_parameter("z_offset", 0.141087); + this->declare_parameter("z_lift_amount", 0.01); + this->declare_parameter("max_velocity_scaling_factor", 1.0); + this->declare_parameter("max_acceleration_scaling_factor", 0.6); + this->declare_parameter("blend_radius", 0.0); + + xlim_lower = this->get_parameter("x_limit_lower").as_double(); + xlim_upper = this->get_parameter("x_limit_upper").as_double(); + ylim_lower = this->get_parameter("y_limit_lower").as_double(); + ylim_upper = this->get_parameter("y_limit_upper").as_double(); + zlim_lower = this->get_parameter("z_offset").as_double(); + zlim_upper = zlim_lower + this->get_parameter("z_lift_amount").as_double(); + this->move_group.setMaxAccelerationScalingFactor(1.0); this->move_group.setMaxVelocityScalingFactor(1.0); //this->move_group.setMaxVelocityScalingFactor(0.8); @@ -199,8 +216,9 @@ public: //mpr.planner_id = "LIN"; 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.8; + mpr.max_velocity_scaling_factor = this->get_parameter("max_velocity_scaling_factor").as_double(); + mpr.max_acceleration_scaling_factor = this->get_parameter("max_acceleration_scaling_factor").as_double(); + //mpr.max_acceleration_scaling_factor = 0.1; mpr.allowed_planning_time = 20; //mpr.max_cartesian_speed = 2; // m/s @@ -232,9 +250,7 @@ public: mpr.goal_constraints.push_back(pose_goal); msi.req = mpr; - //msi.blend_radius = 6e-7; //TODO make configurable - //msi.blend_radius = 0.000001; //TODO make configurable - msi.blend_radius = 0.0; //TODO make configurable + msi.blend_radius = this->get_parameter("blend_radius").as_double(); if (coincidentPoints(&pose.pose.position, &previous_point, msi.blend_radius + 1e-5)) { RCLCPP_ERROR(this->get_logger(), "Detected coincident points, setting blend radius to 0.0"); @@ -250,6 +266,7 @@ public: msr.items.push_back(msi); } msr.items.back().blend_radius = 0.0; // Last element blend must be 0 + moveit_msgs::msg::RobotState state_msg; if (move_group_state == NULL) {