Merge branch 'main' into remove-getstringfromuser

src/picknik_ur_base_config/config/config.yaml

@@ -196,6 +196,7 @@ ros2_control:
     - "joint_trajectory_controller"
     - "admittance_controller_open_door"
     - "joint_trajectory_controller_chained_open_door"
+    - "joint_trajectory_admittance_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []

src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml

@@ -19,6 +19,8 @@ controller_manager:
       type: admittance_controller/AdmittanceController
     joint_trajectory_controller_chained_open_door:
       type: joint_trajectory_controller/JointTrajectoryController
+    joint_trajectory_admittance_controller:
+      type: moveit_pro_controllers/JointTrajectoryAdmittanceController
 
 io_and_status_controller:
   ros__parameters:
@@ -258,3 +260,18 @@ admittance_controller_open_door:
 
     # general settings
     enable_parameter_update_without_reactivation: true
+
+joint_trajectory_admittance_controller:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    base_frame: base_link
+    tip_frame: grasp_link
+    sensor_frame: tool0
+    ee_frame: grasp_link
+    ft_sensor_name: tcp_fts_sensor

src/picknik_ur_base_config/package.xml

@@ -14,6 +14,7 @@
   <exec_depend>admittance_controller</exec_depend>
   <exec_depend>kinematics_interface_kdl</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>
+  <exec_depend>moveit_pro_controllers</exec_depend>
   <exec_depend>moveit_ros_perception</exec_depend>
   <exec_depend>moveit_studio_agent</exec_depend>
   <exec_depend>moveit_studio_behavior</exec_depend>

src/picknik_ur_multi_arm_config/config/cameras.yaml

@@ -23,8 +23,8 @@
       registered_info: "/scene_camera/color/camera_info"
       registered_image: "/scene_camera/depth/image_rect_raw"
 
-- left_wrist_mounted_camera:
-    camera_name: "left_wrist_mounted_camera"
+- first_wrist_mounted_camera:
+    camera_name: "first_wrist_mounted_camera"
     type: "sim"
     use: True
     serial_no: "0"
@@ -35,18 +35,18 @@
     enable_pointcloud: True
 
     # information about the topics the camera publishes the raw image and info
-    rgb_info: "/left_wrist_mounted_camera/color/camera_info"
-    rgb_image: "/left_wrist_mounted_camera/color/image_raw"
+    rgb_info: "/first_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/first_wrist_mounted_camera/color/image_raw"
 
     # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
     registered_rgb_depth_pair:
-      depth_info: "/left_wrist_mounted_camera/depth/camera_info"
-      depth_image: "/left_wrist_mounted_camera/depth/image_rect_raw"
-      registered_info: "/left_wrist_mounted_camera/depth_registered/camera_info"
-      registered_image: "/left_wrist_mounted_camera/depth_registered/image_rect"
+      depth_info: "/first_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/first_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/first_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/first_wrist_mounted_camera/depth_registered/image_rect"
 
-- right_wrist_mounted_camera:
-    camera_name: "right_wrist_mounted_camera"
+- second_wrist_mounted_camera:
+    camera_name: "second_wrist_mounted_camera"
     type: "sim"
     use: True
     serial_no: "0"
@@ -57,12 +57,56 @@
     enable_pointcloud: True
 
     # information about the topics the camera publishes the raw image and info
-    rgb_info: "/right_wrist_mounted_camera/color/camera_info"
-    rgb_image: "/right_wrist_mounted_camera/color/image_raw"
+    rgb_info: "/second_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/second_wrist_mounted_camera/color/image_raw"
 
     # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
     registered_rgb_depth_pair:
-      depth_info: "/right_wrist_mounted_camera/depth/camera_info"
-      depth_image: "/right_wrist_mounted_camera/depth/image_rect_raw"
-      registered_info: "/right_wrist_mounted_camera/depth_registered/camera_info"
-      registered_image: "/right_wrist_mounted_camera/depth_registered/image_rect"
+      depth_info: "/second_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/second_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/second_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/second_wrist_mounted_camera/depth_registered/image_rect"
+
+- third_wrist_mounted_camera:
+    camera_name: "third_wrist_mounted_camera"
+    type: "sim"
+    use: True
+    serial_no: "0"
+    device_type: "D415"
+    framerate: 6
+    image_width: 640
+    image_height: 480
+    enable_pointcloud: True
+
+    # information about the topics the camera publishes the raw image and info
+    rgb_info: "/third_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/third_wrist_mounted_camera/color/image_raw"
+
+    # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
+    registered_rgb_depth_pair:
+      depth_info: "/third_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/third_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/third_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/third_wrist_mounted_camera/depth_registered/image_rect"
+
+- fourth_wrist_mounted_camera:
+    camera_name: "fourth_wrist_mounted_camera"
+    type: "sim"
+    use: True
+    serial_no: "0"
+    device_type: "D415"
+    framerate: 6
+    image_width: 640
+    image_height: 480
+    enable_pointcloud: True
+
+    # information about the topics the camera publishes the raw image and info
+    rgb_info: "/fourth_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/fourth_wrist_mounted_camera/color/image_raw"
+
+    # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
+    registered_rgb_depth_pair:
+      depth_info: "/fourth_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/fourth_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/fourth_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/fourth_wrist_mounted_camera/depth_registered/image_rect"

src/picknik_ur_multi_arm_config/config/config.yaml

@@ -62,15 +62,15 @@ hardware:
   robot_description:
     urdf:
       package: "picknik_ur_multi_arm_config"
-      path: "description/dual_arm_ur.xacro"
+      path: "description/multi_arm_ur.xacro"
     srdf:
       package: "picknik_ur_multi_arm_config"
-      path: "config/moveit/dual_arm_ur.srdf"
+      path: "config/moveit/multi_arm_ur.srdf"
     # Specify any additional parameters required for the URDF.
     # Many of these are specific to the UR descriptions packages, and can be customized as needed.
     # [Optional]
     urdf_params:
-      - name: "dual_arm_ur"
+      - name: "multi_arm_ur"
       - prefix: ""
       - use_fake_hardware: "%>> hardware.simulated"
       - use_pinch_links: "true"
@@ -112,7 +112,7 @@ ros_global_params:
 # [Required]
 moveit_params:
   # Used by the Waypoint Manager to save joint states from this joint group.
-  joint_group_name: "dual_arm_manipulator"
+  joint_group_name: "first_manipulator"
 
   ompl_planning:
     package: "picknik_ur_multi_arm_config"
@@ -128,7 +128,7 @@ moveit_params:
     path: "config/moveit/trac_ik_kinematics_distance.yaml"
   servo:
     package: "picknik_ur_multi_arm_config"
-    path: "config/moveit/right_ur_servo.yaml"
+    path: "config/moveit/multi_ur_servo.yaml"
   sensors_3d:
     package: "picknik_ur_base_config"
     path: "config/moveit/sensors_3d.yaml"
@@ -137,7 +137,7 @@ moveit_params:
     path: "config/moveit/trac_ik_kinematics_speed.yaml"
   joint_limits:
     package: "picknik_ur_multi_arm_config"
-    path: "config/moveit/dual_arm_joint_limits.yaml"
+    path: "config/moveit/multi_arm_joint_limits.yaml"
   pilz_cartesian_limits:
     package: "picknik_ur_base_config"
     path: "config/moveit/pilz_cartesian_limits.yaml"
@@ -160,34 +160,37 @@ ui_params:
   # By default, MoveIt Pro uses a frame called "grasp_link" for tool grasp pose rendering
   # and planning.
   # [Required]
-  servo_endpoint_frame_id: "right_grasp_link"
+  servo_endpoint_frame_id: "first_grasp_link"
 
 # Configuration for launching ros2_control processes.
 # [Required, if using ros2_control]
 ros2_control:
   config:
     package: "picknik_ur_multi_arm_config"
-    path: "config/control/picknik_dual_ur.ros2_control.yaml"
+    path: "config/control/picknik_multi_ur.ros2_control.yaml"
   # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
   # If not specified, it is up to the user to ensure the appropriate controllers are active and available
   # for running the application.
   # [Optional, default=[]]
   controllers_active_at_startup:
     - "joint_state_broadcaster"
-    - "left_servo_controller"
-    - "right_servo_controller"
-    - "left_io_and_status_controller"
-    - "right_io_and_status_controller"
-    - "left_robotiq_activation_controller"
-    - "left_robotiq_gripper_controller"
-    - "right_robotiq_activation_controller"
-    - "right_robotiq_gripper_controller"
+    - "servo_controller"
+    - "first_io_and_status_controller"
+    - "second_io_and_status_controller"
+    - "third_io_and_status_controller"
+    - "fourth_io_and_status_controller"
+    - "first_robotiq_activation_controller"
+    - "second_robotiq_activation_controller"
+    - "third_robotiq_activation_controller"
+    - "fourth_robotiq_activation_controller"
+    - "first_robotiq_gripper_controller"
+    - "second_robotiq_gripper_controller"
+    - "third_robotiq_gripper_controller"
+    - "fourth_robotiq_gripper_controller"
   # Load but do not start these controllers so they can be activated later if needed.
   # [Optional, default=[]]
   controllers_inactive_at_startup:
-    - "left_joint_trajectory_controller"
-    - "right_joint_trajectory_controller"
-    - "dual_arm_joint_trajectory_controller"
+    - "multi_arm_joint_trajectory_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []