Removed unused modules, code cleanup

src/iisy_config/config/cartesian_limits.yaml

@@ -1,5 +0,0 @@
-cartesian_limits:
-  max_trans_vel: 1
-  max_trans_acc: 2.25
-  max_trans_dec: -5
-  max_rot_vel: 1.57

src/iisy_config/config/chomp_planning.yaml

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

src/iisy_config/config/fake_controllers.yaml

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

src/iisy_config/config/gazebo_controllers.yaml

@@ -1,4 +0,0 @@
-# Publish joint_states
-joint_state_controller:
-  type: joint_state_controller/JointStateController
-  publish_rate: 50

src/iisy_config/config/joint_limits.yaml

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

src/iisy_config/config/ros_controllers.yaml

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

src/iisy_config/config/sensors_3d.yaml

@@ -1,2 +0,0 @@
-sensors:
-  []

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

src/iisy_config/config/simple_moveit_controllers.yaml

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

src/iisy_config/config/stomp_planning.yaml

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

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"), " ",
+            "robot_name:=", "iisy", " ",
-            "sim:=", LaunchConfiguration("sim")
+            "sim:=", "true"
         ]
     )
 
@@ -35,11 +32,7 @@ def launch_setup(context, *args, **kwargs):
                 "lbr_control.launch.py"
             ])
         ), launch_arguments=[
-            ("robot_description", robot_description_content),
+            ("robot_description", robot_description_content)
-            ("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
-            ("controller_configurations", LaunchConfiguration("controller_configurations")),
-            ("controller", LaunchConfiguration("controller")),
-            ("sim", LaunchConfiguration("sim"))
         ]
     )
 
@@ -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),
+            ("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"))
         ]
     )
 
@@ -88,65 +73,6 @@ def generate_launch_description():
     # Launch arguments
     launch_args = []
 
-    launch_args.append(DeclareLaunchArgument(
+    return LaunchDescription([
-        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 + [
         OpaqueFunction(function=launch_setup)
     ])

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")),
+        FindPackageShare("iisy_config"),
-        LaunchConfiguration("controller_configurations")
+        "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(

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),
+        "iisy_config",
-        LaunchConfiguration("moveit_controller_configurations").perform(context)
+        "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)

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(
+    return LaunchDescription([
-        launch_args + [
+        gazebo,
-            gazebo,
+        spawn_entity
-            spawn_entity
     ])