Bin picking config

src/bin_picking_config/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.22)
+project(bin_picking_config)
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    calibrate
+    config
+    description
+    launch
+    objectives
+    waypoints
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/bin_picking_config/LICENSE

@@ -0,0 +1,25 @@
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+   * Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+
+   * Redistributions in binary form must reproduce the above copyright
+     notice, this list of conditions and the following disclaimer in the
+     documentation and/or other materials provided with the distribution.
+
+   * Neither the name of the copyright holder nor the names of its
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

src/bin_picking_config/README.md

@@ -0,0 +1,5 @@
+# bin_picking_config
+
+A MoveIt Pro base configuration for BMW's UR10e.
+
+For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/)

src/bin_picking_config/calibrate/calibrate.yaml

@@ -0,0 +1,57 @@
+robot_calibration:
+  ros__parameters:
+    verbose: true
+    base_link: world
+    calibration_steps:
+    - single_calibration_step
+    single_calibration_step:
+      models:
+      - manipulator
+      - camera
+      manipulator:
+        type: chain3d
+        frame: checkerboard # this can be 'checkerboard' but you need to remove it from the free_frames
+      camera:
+        type: camera3d
+        frame: zivid_camera_link
+        topic: /zivid/points/xyzrgba # is this used? See line 97-99 in params.cpp
+      free_frames:
+      - scene_camera_mount_link
+      - checkerboard
+      scene_camera_mount_link:
+        x: true
+        y: true
+        z: true
+        roll: true
+        pitch: true
+        yaw: true
+      checkerboard:
+        x: true
+        y: true
+        z: true
+        roll: true
+        pitch: true
+        yaw: true
+      free_frames_initial_values:
+      - scene_camera_mount_link_initial_values
+      - checkerboard_initial_values
+      scene_camera_mount_link_initial_values:
+        x: 0.0
+        y: 0.0
+        z: 0.0
+        roll: 0.0
+        pitch: 0.0
+        yaw: 0.0
+      checkerboard_initial_values:
+        x: 0.0
+        y: 0.0
+        z: 0.0
+        roll: 0.0
+        pitch: 0.0
+        yaw: 0.0
+      error_blocks:
+      - hand_eye
+      hand_eye:
+        type: chain3d_to_chain3d
+        model_a: camera
+        model_b: manipulator

src/bin_picking_config/calibrate/calibration_poses.yaml

@@ -0,0 +1,112 @@
+- features:
+  - checkerboard_finder
+  joints:
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
+    - shoulder_pan_joint
+  positions:
+    - -3.407844682733053
+    - -1.2732188701629639
+    - -0.19711966932330327
+    - -1.5134838263141077
+    - -3.39201528230776
+    - -1.5481632391559046
+- features:
+  - checkerboard_finder
+  joints:
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
+    - shoulder_pan_joint
+  positions:
+    - -3.407741685906881
+    - -1.2731482982635498
+    - -0.19705362737689214
+    - -2.0302661100970667
+    - -3.3919721285449427
+    - -1.5481842199908655
+- features:
+  - checkerboard_finder
+  joints:
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
+    - shoulder_pan_joint
+  positions:
+    - -3.407799860040182
+    - -1.273197889328003
+    - -0.19711931169543462
+    - -2.030310932789938
+    - -3.392024342213766
+    - -1.1758831183062952
+- features:
+  - checkerboard_finder
+  joints:
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
+    - shoulder_pan_joint
+  positions:
+    - -3.407795568505758
+    - -1.2732024192810059
+    - -0.19703610361132817
+    - -1.47742206255068
+    - -3.383038345967428
+    - -1.1758917013751429
+- features:
+  - checkerboard_finder
+  joints:
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
+    - shoulder_pan_joint
+  positions:
+    - -3.4077869854369105
+    - -1.2732467651367188
+    - 0.2530054289051513
+    - -1.4434297720538538
+    - -3.3830588499652308
+    - -1.175887409840719
+- features:
+  - checkerboard_finder
+  joints:
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
+    - shoulder_pan_joint
+  positions:
+    - -3.4078036747374476
+    - -1.0751317739486694
+    - 0.0354965168186645
+    - -1.776360813771383
+    - -4.6230226198779505
+    - -1.1758831183062952
+- features:
+  - checkerboard_finder
+  joints:
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint
+    - shoulder_pan_joint
+  positions:
+    - -3.407823701898092
+    - -1.2104471921920776
+    - -0.5297407668880005
+    - -1.7763741652118128
+    - -4.623052302991049
+    - -1.3884571234332483

src/bin_picking_config/calibrate/capture.yaml

@@ -0,0 +1,29 @@
+robot_calibration:
+  ros__parameters:
+    chains:
+    - manipulator
+    manipulator:
+      topic: /servo_controller/follow_joint_trajectory
+      joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+      #planning_group: arm TODO: revert when moveit is setup
+    features:
+    - checkerboard_finder
+    checkerboard_finder:
+      type: robot_calibration::CheckerboardFinder
+      topic: /zivid/points/xyzrgba
+      camera_info_topic: /zivid/color/camera_info
+      camera_sensor_name: camera
+      camera_link: zivid_camera_link
+      chain_sensor_name: manipulator
+      frame_id: checkerboard # Name of checkerboard frame (if not specified it defaults to: checkerboard)
+      debug: true
+      # Number of inner corners per a chessboard row and column
+      points_x: 6
+      points_y: 7
+      size: 0.03

src/bin_picking_config/calibrate/scene_camera_calibration.yaml

@@ -0,0 +1,7 @@
+camera_pose:
+  x: 0.0
+  y: 0.2
+  z: 1.151
+  roll: 3.02
+  pitch: 0.0
+  yaw: 0.0