From 71e384c8d7f827a9908c42bc2a3dafc67f33fe4e Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Sat, 19 Mar 2022 18:26:59 +0100 Subject: [PATCH] Removed unused modules, code cleanup --- src/iisy_config/config/cartesian_limits.yaml | 5 -- src/iisy_config/config/chomp_planning.yaml | 18 ---- src/iisy_config/config/fake_controllers.yaml | 13 --- .../config/gazebo_controllers.yaml | 4 - src/iisy_config/config/joint_limits.yaml | 40 --------- src/iisy_config/config/ros_controllers.yaml | 40 --------- src/iisy_config/config/sensors_3d.yaml | 2 - src/iisy_config/config/sensors_3d.yml | 9 ++ .../config/simple_moveit_controllers.yaml | 12 --- src/iisy_config/config/stomp_planning.yaml | 39 -------- src/iisy_config/launch/lbr_bringup.launch.py | 88 ++----------------- src/iisy_config/launch/lbr_control.launch.py | 37 +------- .../launch/lbr_move_group.launch.py | 33 ++----- .../launch/lbr_simulation.launch.py | 19 +--- 14 files changed, 30 insertions(+), 329 deletions(-) delete mode 100644 src/iisy_config/config/cartesian_limits.yaml delete mode 100644 src/iisy_config/config/chomp_planning.yaml delete mode 100644 src/iisy_config/config/fake_controllers.yaml delete mode 100644 src/iisy_config/config/gazebo_controllers.yaml delete mode 100644 src/iisy_config/config/joint_limits.yaml delete mode 100644 src/iisy_config/config/ros_controllers.yaml delete mode 100644 src/iisy_config/config/sensors_3d.yaml create mode 100644 src/iisy_config/config/sensors_3d.yml delete mode 100644 src/iisy_config/config/simple_moveit_controllers.yaml delete mode 100644 src/iisy_config/config/stomp_planning.yaml diff --git a/src/iisy_config/config/cartesian_limits.yaml b/src/iisy_config/config/cartesian_limits.yaml deleted file mode 100644 index 7df72f6..0000000 --- a/src/iisy_config/config/cartesian_limits.yaml +++ /dev/null @@ -1,5 +0,0 @@ -cartesian_limits: - max_trans_vel: 1 - max_trans_acc: 2.25 - max_trans_dec: -5 - max_rot_vel: 1.57 diff --git a/src/iisy_config/config/chomp_planning.yaml b/src/iisy_config/config/chomp_planning.yaml deleted file mode 100644 index eb9c912..0000000 --- a/src/iisy_config/config/chomp_planning.yaml +++ /dev/null @@ -1,18 +0,0 @@ -planning_time_limit: 10.0 -max_iterations: 200 -max_iterations_after_collision_free: 5 -smoothness_cost_weight: 0.1 -obstacle_cost_weight: 1.0 -learning_rate: 0.01 -smoothness_cost_velocity: 0.0 -smoothness_cost_acceleration: 1.0 -smoothness_cost_jerk: 0.0 -ridge_factor: 0.0 -use_pseudo_inverse: false -pseudo_inverse_ridge_factor: 1e-4 -joint_update_limit: 0.1 -collision_clearance: 0.2 -collision_threshold: 0.07 -use_stochastic_descent: true -enable_failure_recovery: false -max_recovery_attempts: 5 diff --git a/src/iisy_config/config/fake_controllers.yaml b/src/iisy_config/config/fake_controllers.yaml deleted file mode 100644 index a97a8f3..0000000 --- a/src/iisy_config/config/fake_controllers.yaml +++ /dev/null @@ -1,13 +0,0 @@ -controller_list: - - name: fake_iisy_controller - type: $(arg fake_execution_type) - joints: - - base_rot - - base_link1_joint - - link1_link2_joint - - link2_rot - - link2_link3_joint - - link3_rot -initial: # Define initial robot poses per group - - group: iisy - pose: home \ No newline at end of file diff --git a/src/iisy_config/config/gazebo_controllers.yaml b/src/iisy_config/config/gazebo_controllers.yaml deleted file mode 100644 index e4d2eb0..0000000 --- a/src/iisy_config/config/gazebo_controllers.yaml +++ /dev/null @@ -1,4 +0,0 @@ -# Publish joint_states -joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 diff --git a/src/iisy_config/config/joint_limits.yaml b/src/iisy_config/config/joint_limits.yaml deleted file mode 100644 index 64ec7c1..0000000 --- a/src/iisy_config/config/joint_limits.yaml +++ /dev/null @@ -1,40 +0,0 @@ -# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed - -# For beginners, we downscale velocity and acceleration limits. -# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 - -# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] -# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] -joint_limits: - base_link1_joint: - has_velocity_limits: true - max_velocity: 1.7453292519943 - has_acceleration_limits: false - max_acceleration: 0 - base_rot: - has_velocity_limits: true - max_velocity: 1.7453292519943 - has_acceleration_limits: false - max_acceleration: 0 - link1_link2_joint: - has_velocity_limits: true - max_velocity: 1.7453292519943 - has_acceleration_limits: false - max_acceleration: 0 - link2_link3_joint: - has_velocity_limits: true - max_velocity: 1.7453292519943 - has_acceleration_limits: false - max_acceleration: 0 - link2_rot: - has_velocity_limits: true - max_velocity: 1.7453292519943 - has_acceleration_limits: false - max_acceleration: 0 - link3_rot: - has_velocity_limits: true - max_velocity: 1.7453292519943 - has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file diff --git a/src/iisy_config/config/ros_controllers.yaml b/src/iisy_config/config/ros_controllers.yaml deleted file mode 100644 index 1a50f50..0000000 --- a/src/iisy_config/config/ros_controllers.yaml +++ /dev/null @@ -1,40 +0,0 @@ -iisy_controller: - type: effort_controllers/JointTrajectoryController - joints: - - base_rot - - base_link1_joint - - link1_link2_joint - - link2_rot - - link2_link3_joint - - link3_rot - gains: - base_rot: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - base_link1_joint: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - link1_link2_joint: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - link2_rot: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - link2_link3_joint: - p: 100 - d: 1 - i: 1 - i_clamp: 1 - link3_rot: - p: 100 - d: 1 - i: 1 - i_clamp: 1 \ No newline at end of file diff --git a/src/iisy_config/config/sensors_3d.yaml b/src/iisy_config/config/sensors_3d.yaml deleted file mode 100644 index 51010a3..0000000 --- a/src/iisy_config/config/sensors_3d.yaml +++ /dev/null @@ -1,2 +0,0 @@ -sensors: - [] \ No newline at end of file diff --git a/src/iisy_config/config/sensors_3d.yml b/src/iisy_config/config/sensors_3d.yml new file mode 100644 index 0000000..a8343dd --- /dev/null +++ b/src/iisy_config/config/sensors_3d.yml @@ -0,0 +1,9 @@ +sensors: + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater + point_cloud_topic: /camera/depth_registered/points + max_range: 5.0 + point_subsample: 1 + padding_offset: 0.1 + padding_scale: 1.0 + max_update_rate: 1.0 + filtered_cloud_topic: filtered_cloud \ No newline at end of file diff --git a/src/iisy_config/config/simple_moveit_controllers.yaml b/src/iisy_config/config/simple_moveit_controllers.yaml deleted file mode 100644 index d2307c1..0000000 --- a/src/iisy_config/config/simple_moveit_controllers.yaml +++ /dev/null @@ -1,12 +0,0 @@ -controller_list: - - name: iisy_controller - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: True - joints: - - base_rot - - base_link1_joint - - link1_link2_joint - - link2_rot - - link2_link3_joint - - link3_rot \ No newline at end of file diff --git a/src/iisy_config/config/stomp_planning.yaml b/src/iisy_config/config/stomp_planning.yaml deleted file mode 100644 index 2a3d04a..0000000 --- a/src/iisy_config/config/stomp_planning.yaml +++ /dev/null @@ -1,39 +0,0 @@ -stomp/iisy: - group_name: iisy - optimization: - num_timesteps: 60 - num_iterations: 40 - num_iterations_after_valid: 0 - num_rollouts: 30 - max_rollouts: 30 - initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] - control_cost_weight: 0.0 - task: - noise_generator: - - class: stomp_moveit/NormalDistributionSampling - stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] - cost_functions: - - class: stomp_moveit/CollisionCheck - collision_penalty: 1.0 - cost_weight: 1.0 - kernel_window_percentage: 0.2 - longest_valid_joint_move: 0.05 - noisy_filters: - - class: stomp_moveit/JointLimits - lock_start: True - lock_goal: True - - class: stomp_moveit/MultiTrajectoryVisualization - line_width: 0.02 - rgb: [255, 255, 0] - marker_array_topic: stomp_trajectories - marker_namespace: noisy - update_filters: - - class: stomp_moveit/PolynomialSmoother - poly_order: 6 - - class: stomp_moveit/TrajectoryVisualization - line_width: 0.05 - rgb: [0, 191, 255] - error_rgb: [255, 0, 0] - publish_intermediate: True - marker_topic: stomp_trajectory - marker_namespace: optimized \ No newline at end of file diff --git a/src/iisy_config/launch/lbr_bringup.launch.py b/src/iisy_config/launch/lbr_bringup.launch.py index 8f86f37..2a0f944 100644 --- a/src/iisy_config/launch/lbr_bringup.launch.py +++ b/src/iisy_config/launch/lbr_bringup.launch.py @@ -11,18 +11,15 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource def launch_setup(context, *args, **kwargs): - # Evaluate frequently used variables - model = LaunchConfiguration("model").perform(context) - # Load robot description robot_description_content = Command( [ FindExecutable(name="xacro"), " ", PathJoinSubstitution( - [FindPackageShare("iisy_config"), "urdf/{}.urdf".format( model)] + [FindPackageShare("iisy_config"), "urdf/iisy.urdf"] ), " ", - "robot_name:=", LaunchConfiguration("robot_name"), " ", - "sim:=", LaunchConfiguration("sim") + "robot_name:=", "iisy", " ", + "sim:=", "true" ] ) @@ -35,11 +32,7 @@ def launch_setup(context, *args, **kwargs): "lbr_control.launch.py" ]) ), launch_arguments=[ - ("robot_description", robot_description_content), - ("controller_configurations_package", LaunchConfiguration("controller_configurations_package")), - ("controller_configurations", LaunchConfiguration("controller_configurations")), - ("controller", LaunchConfiguration("controller")), - ("sim", LaunchConfiguration("sim")) + ("robot_description", robot_description_content) ] ) @@ -51,11 +44,7 @@ def launch_setup(context, *args, **kwargs): "launch", "lbr_simulation.launch.py" ]) - ), - launch_arguments=[ - ("robot_name", LaunchConfiguration("robot_name")) - ], - condition=IfCondition(LaunchConfiguration("sim")) + ), ) # Move group @@ -68,11 +57,7 @@ def launch_setup(context, *args, **kwargs): ]) ), launch_arguments=[ - ("robot_description", robot_description_content), - ("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")), - ("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")), - ("model", LaunchConfiguration("model")), - ("sim", LaunchConfiguration("sim")) + ("robot_description", robot_description_content) ] ) @@ -88,65 +73,6 @@ def generate_launch_description(): # Launch arguments launch_args = [] - launch_args.append(DeclareLaunchArgument( - name="model", - default_value="iisy", - description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14." - )) - - launch_args.append(DeclareLaunchArgument( - name="robot_name", - default_value="iisy", - description="Set robot name." - )) - - launch_args.append(DeclareLaunchArgument( - name="sim", - default_value="true", - description="Launch robot in simulation or on real setup." - )) - - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations_package", - default_value="iisy_config", - description="Package that contains controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations", - default_value="config/iisy_controllers.yml", - description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="controller", - default_value="position_trajectory_controller", - description="Robot controller." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations_package", - default_value="iisy_config", - description="Package that contains MoveIt! controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations", - default_value="config/iisy_moveit_controllers.yml", - description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file." - ) - ) - - return LaunchDescription( - launch_args + [ + return LaunchDescription([ OpaqueFunction(function=launch_setup) ]) diff --git a/src/iisy_config/launch/lbr_control.launch.py b/src/iisy_config/launch/lbr_control.launch.py index 2a7ec59..52f5d37 100644 --- a/src/iisy_config/launch/lbr_control.launch.py +++ b/src/iisy_config/launch/lbr_control.launch.py @@ -11,22 +11,6 @@ def generate_launch_description(): # Launch arguments launch_args = [] - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations_package", - default_value="iisy_config", - description="Package that contains controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations", - default_value="config/iisy_controllers.yml", - description="Relative path to controller configurations YAML file." - ) - ) - launch_args.append( DeclareLaunchArgument( name="robot_description", @@ -34,29 +18,14 @@ def generate_launch_description(): ) ) - launch_args.append( - DeclareLaunchArgument( - name="controller", - default_value="position_trajectory_controller", - description="Robot controller." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="sim", - default_value="true", - description="Launch robot in simulation or on real setup." - ) - ) # Configure robot_description robot_description = {"robot_description": LaunchConfiguration("robot_description")} # Load controllers from YAML configuration file controller_configurations = PathJoinSubstitution([ - FindPackageShare(LaunchConfiguration("controller_configurations_package")), - LaunchConfiguration("controller_configurations") + FindPackageShare("iisy_config"), + "config/iisy_controllers.yml" ]) robot_state_publisher = Node( @@ -75,7 +44,7 @@ def generate_launch_description(): controller = Node( package="controller_manager", executable="spawner", - arguments=[LaunchConfiguration("controller"), "--controller-manager", "/controller_manager"], + arguments=["position_trajectory_controller", "--controller-manager", "/controller_manager"], ) return LaunchDescription( diff --git a/src/iisy_config/launch/lbr_move_group.launch.py b/src/iisy_config/launch/lbr_move_group.launch.py index 3335218..3043ee9 100644 --- a/src/iisy_config/launch/lbr_move_group.launch.py +++ b/src/iisy_config/launch/lbr_move_group.launch.py @@ -82,8 +82,8 @@ def launch_setup(context, *args, **kwargs): # Controllers controllers_yaml = load_yaml( - LaunchConfiguration("moveit_controller_configurations_package").perform(context), - LaunchConfiguration("moveit_controller_configurations").perform(context) + "iisy_config", + "config/iisy_moveit_controllers.yml" ) moveit_controllers = {"moveit_simple_controller_manager": controllers_yaml, @@ -96,7 +96,9 @@ def launch_setup(context, *args, **kwargs): "publish_transforms_updates": True} # Time configuration - use_sim_time = {"use_sim_time": LaunchConfiguration("sim")} + use_sim_time = {"use_sim_time": True} + + perception_pipeline = load_yaml("iisy_config","config/sensors_3d.yml") # Prepare move group node move_group_node = Node( @@ -114,7 +116,8 @@ def launch_setup(context, *args, **kwargs): trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, - use_sim_time + use_sim_time, + perception_pipeline ] ) @@ -153,28 +156,6 @@ def generate_launch_description(): ) ) - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations_package", - default_value="iisy_config", - description="Package that contains MoveIt! controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations", - default_value="config/iisy_moveit_controllers.yml", - description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name." - ) - ) - - launch_args.append(DeclareLaunchArgument( - name="sim", - default_value="true", - description="Launch robot in simulation or on real setup." - )) - return LaunchDescription( launch_args + [ OpaqueFunction(function=launch_setup) diff --git a/src/iisy_config/launch/lbr_simulation.launch.py b/src/iisy_config/launch/lbr_simulation.launch.py index e8fa297..36b07b1 100644 --- a/src/iisy_config/launch/lbr_simulation.launch.py +++ b/src/iisy_config/launch/lbr_simulation.launch.py @@ -7,16 +7,6 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): - - # Launch arguments - launch_args = [] - - launch_args.append(DeclareLaunchArgument( - name="robot_name", - default_value="iisy", - description="Set robot name." - )) - # Launch Gazebo gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -37,13 +27,12 @@ def generate_launch_description(): executable="spawn_entity.py", arguments=[ "-topic", "robot_description", - "-entity", LaunchConfiguration("robot_name") + "-entity", "iisy" ], output="screen" ) - return LaunchDescription( - launch_args + [ - gazebo, - spawn_entity + return LaunchDescription([ + gazebo, + spawn_entity ])