diff --git a/src/hand_eye_calibration_sim/CMakeLists.txt b/src/hand_eye_calibration_sim/CMakeLists.txt new file mode 100644 index 000000000..96541ba26 --- /dev/null +++ b/src/hand_eye_calibration_sim/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.22) +project(hand_eye_calibration_sim) + +find_package(ament_cmake REQUIRED) +find_package(picknik_accessories REQUIRED) + + +# Install all XML files in directory +set(PICKNIK_ACCESSORIES_SHARE_DIR +"${CMAKE_INSTALL_PREFIX}/../picknik_accessories/share/picknik_accessories/mujoco_assets/" +) +# Destination directory +set(DEST_DIR "${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/description/") + +install(DIRECTORY "${PICKNIK_ACCESSORIES_SHARE_DIR}" + DESTINATION "${DEST_DIR}" + FILES_MATCHING PATTERN "*") + +install( + DIRECTORY + 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() diff --git a/src/hand_eye_calibration_sim/CONTRIBUTING.md b/src/hand_eye_calibration_sim/CONTRIBUTING.md new file mode 100644 index 000000000..be449dc44 --- /dev/null +++ b/src/hand_eye_calibration_sim/CONTRIBUTING.md @@ -0,0 +1,7 @@ +Any contribution that you make to this repository will +be under the 3-Clause BSD License, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). + +# Contributing to this Repository + +Thanks for getting involved! If you want to add to this repository, please reach out to support@picknik.ai. diff --git a/src/hand_eye_calibration_sim/LICENSE b/src/hand_eye_calibration_sim/LICENSE new file mode 100644 index 000000000..574ef0790 --- /dev/null +++ b/src/hand_eye_calibration_sim/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. diff --git a/src/hand_eye_calibration_sim/MOVEIT_PRO_IGNORE b/src/hand_eye_calibration_sim/MOVEIT_PRO_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/src/hand_eye_calibration_sim/README.md b/src/hand_eye_calibration_sim/README.md new file mode 100644 index 000000000..148b26efc --- /dev/null +++ b/src/hand_eye_calibration_sim/README.md @@ -0,0 +1,76 @@ +# hand_eye_calibration_sim + +A MoveIt Pro simulation for validating hand-eye camera calibration. The scene +mirrors a mixed-mode multi-camera setup: a wrist-mounted camera (eye-in-hand) +that observes a world-fixed ChArUco board, and two fixed scene cameras +(eye-to-hand) that observe a ChArUco board rigidly mounted to the robot wrist. +Every camera and board pose is a designed value in the MJCF, so calibration +results can be asserted against exact ground truth. + +The package is hidden from the user-facing configuration list via +`MOVEIT_PRO_IGNORE`; it exists for QA of the calibration Behaviors and +Objectives (moveit_pro issues #19835, #20570, #20572). + +## ChArUco boards + +Generated by `scripts/generate_charuco_boards.py` (run it inside the dev +container to regenerate the PNGs in `description/assets/`). The two boards use +distinct dictionaries so detections stay unambiguous when a camera sees both. +These parameters are the intended defaults for the `DetectCharucoBoard` +Behavior ports. + +| Board | Dictionary | Squares (x by y) | Square size | Marker size | Physical size | +|---|---|---|---|---|---| +| World board | `DICT_4X4_50` | 7 by 5 | 0.035 m | 0.026 m | 0.245 x 0.175 m | +| Flange board | `DICT_5X5_50` | 5 by 4 | 0.030 m | 0.022 m | 0.150 x 0.120 m | + +The textures are drawn with `marginSize=0`, so the chessboard exactly spans the +plate geom's top face: interior corner `(i, j)` sits at +`(i * square_length, j * square_length)` from the plate corner. The in-plane +orientation of the texture on the plate (which plate corner is the OpenCV board +origin) must be pinned empirically from a rendered image when the calibration +QA objectives land; only the plate poses below are designed values. + +## Ground-truth transforms + +All values are exactly as placed in the MJCF (`description/scene.xml` and +`description/ur5e_hand_eye.xml`). Rotations are given exactly as they appear in +the MJCF: quaternions in MuJoCo (w, x, y, z) order where the MJCF uses `quat`, +euler angles (radians, extrinsic x-y-z) where it uses `euler`. Optical frames +follow REP 103 (the camera pose rotated 180 degrees about its x-axis). + +| Frame | Relative to | Position (m) | Rotation | +|---|---|---|---| +| `wrist_camera_optical_frame` | `wrist_3_link` | (0, 0.2, 0.05) | euler (-1.57, 0, 0) | +| `scene_camera_1_optical_frame` | world | (0.917266, 1.074437, 1.861509) | quat (0.233342, -0.306871, -0.734484, 0.558494) | +| `scene_camera_2_optical_frame` | world | (-0.401516, 1.072484, 1.82102) | quat (0.244002, -0.306655, 0.719923, -0.572833) | +| `world_board` (plate center) | world | (0.266, 0.650113, 0.665599) | quat (0.972489, 0.232951, 0, 0) | +| `flange_board` (plate center) | `wrist_3_link` | (0, 0.14, 0.16) | euler (0.6435, 0, 0) | + +The board top faces (the textured faces) are at +0.004 m along each plate's +local z-axis; sites `world_board_top` and `flange_board_top` mark them. + +Expected calibration results: + +- Eye-in-hand: the wrist camera extrinsic must recover + `wrist_3_link -> wrist_camera_optical_frame`, and the fiducial mount must + recover the world board pose. +- Eye-to-hand: each scene camera extrinsic must recover that camera's pose in + the robot base frame, and the fiducial mount must recover + `wrist_3_link -> flange_board`. The scene camera poses above are world-frame + values; convert through the robot's base pose (which depends on the linear + rail position) when asserting in the base frame. + +## Notes + +- The wrist camera pose is fixed by the shared ur5e model in + `picknik_accessories`; `description/ur5e_hand_eye.xml` is a copy of that + model that adds only the rigidly attached flange board (MJCF includes are + textual, so the board cannot be attached from the scene file, and a weld + constraint would be soft and corrupt the ground truth). +- All cameras render at 1280x720 with `fovy` 58 degrees; the offscreen buffer + must match every camera resolution exactly. +- Both board plates have contact disabled. They are calibration targets and + are not part of the URDF, so the motion planner does not know about them. + +For detailed documentation see: [MoveIt Pro Documentation](https://docs.picknik.ai/) diff --git a/src/hand_eye_calibration_sim/config/config.yaml b/src/hand_eye_calibration_sim/config/config.yaml new file mode 100644 index 000000000..4d5860108 --- /dev/null +++ b/src/hand_eye_calibration_sim/config/config.yaml @@ -0,0 +1,39 @@ +# +# This contains information for a unique instance of a robot. +# + +# Name of the package to specialize +based_on_package: "lab_sim" +runtime_launch_file: + package: "hand_eye_calibration_sim" + path: "launch/runtime.launch.xml" +hardware: + # Parameters used to configure the robot description through XACRO. + # A URDF and SRDF are both required. + # [Required] + robot_description: + urdf: + package: "lab_sim" + path: "description/picknik_ur.xacro" + srdf: + package: "lab_sim" + path: "config/moveit/picknik_ur.srdf" + urdf_params: + - mujoco_model_package: "hand_eye_calibration_sim" +# Configuration for loading behaviors and objectives. +# [Required] +objectives: + # Specify source folder for objectives + # [Required] + objective_library_paths: + mujoco_objectives: + package_name: "moveit_pro_objectives" + relative_path: "objectives/mujoco" + sim_objectives: + package_name: "hand_eye_calibration_sim" + relative_path: "objectives" + # Specify the location of the saved waypoints file. + # [Required] + waypoints_file: + package_name: "hand_eye_calibration_sim" + relative_path: "waypoints/ur_waypoints.yaml" diff --git a/src/hand_eye_calibration_sim/description/LICENSE b/src/hand_eye_calibration_sim/description/LICENSE new file mode 100644 index 000000000..f24e07cb6 --- /dev/null +++ b/src/hand_eye_calibration_sim/description/LICENSE @@ -0,0 +1,26 @@ +Copyright 2018 ROS Industrial Consortium + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +2. 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. + +3. 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. diff --git a/src/hand_eye_calibration_sim/description/assets/charuco_flange_board.png b/src/hand_eye_calibration_sim/description/assets/charuco_flange_board.png new file mode 100644 index 000000000..1f43e429a --- /dev/null +++ b/src/hand_eye_calibration_sim/description/assets/charuco_flange_board.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a5d3ef25d0fe029cb1b2c725613a1ce78c379905ece76a1b3e7fa4e675a80ee1 +size 16095 diff --git a/src/hand_eye_calibration_sim/description/assets/charuco_world_board.png b/src/hand_eye_calibration_sim/description/assets/charuco_world_board.png new file mode 100644 index 000000000..5e8694935 --- /dev/null +++ b/src/hand_eye_calibration_sim/description/assets/charuco_world_board.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:14c38a35fbb8ae1afc7e7f7a0d13bd714fe678166e6a0ba6308ae1e3ba26f0e6 +size 25477 diff --git a/src/hand_eye_calibration_sim/description/scene.xml b/src/hand_eye_calibration_sim/description/scene.xml new file mode 100644 index 000000000..004076e91 --- /dev/null +++ b/src/hand_eye_calibration_sim/description/scene.xml @@ -0,0 +1,172 @@ + + + + + + + + + diff --git a/src/hand_eye_calibration_sim/description/ur5e_hand_eye.xml b/src/hand_eye_calibration_sim/description/ur5e_hand_eye.xml new file mode 100644 index 000000000..bfc552a9f --- /dev/null +++ b/src/hand_eye_calibration_sim/description/ur5e_hand_eye.xml @@ -0,0 +1,202 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/hand_eye_calibration_sim/description/ur5e_linear_rail_hand_eye.xml b/src/hand_eye_calibration_sim/description/ur5e_linear_rail_hand_eye.xml new file mode 100644 index 000000000..a693ac104 --- /dev/null +++ b/src/hand_eye_calibration_sim/description/ur5e_linear_rail_hand_eye.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + diff --git a/src/hand_eye_calibration_sim/launch/runtime.launch.xml b/src/hand_eye_calibration_sim/launch/runtime.launch.xml new file mode 100644 index 000000000..b2aa75434 --- /dev/null +++ b/src/hand_eye_calibration_sim/launch/runtime.launch.xml @@ -0,0 +1,6 @@ + + + + diff --git a/src/hand_eye_calibration_sim/objectives/close_gripper.xml b/src/hand_eye_calibration_sim/objectives/close_gripper.xml new file mode 100644 index 000000000..afb243195 --- /dev/null +++ b/src/hand_eye_calibration_sim/objectives/close_gripper.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/hand_eye_calibration_sim/objectives/open_gripper.xml b/src/hand_eye_calibration_sim/objectives/open_gripper.xml new file mode 100644 index 000000000..14921bf6d --- /dev/null +++ b/src/hand_eye_calibration_sim/objectives/open_gripper.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + diff --git a/src/hand_eye_calibration_sim/package.xml b/src/hand_eye_calibration_sim/package.xml new file mode 100644 index 000000000..7961a666b --- /dev/null +++ b/src/hand_eye_calibration_sim/package.xml @@ -0,0 +1,44 @@ + + + hand_eye_calibration_sim + 9.5.0 + + + MuJoCo simulation configuration package for validating hand-eye camera + calibration: a wrist camera (eye-in-hand), two fixed scene cameras + (eye-to-hand), and ChArUco boards with known ground-truth poses + + + MoveIt Pro Maintainer + + BSD-3-Clause + + ament_cmake + + admittance_controller + lab_sim + moveit_ros_perception + moveit_studio_agent + moveit_pro_behavior + picknik_accessories + picknik_mujoco_ros + picknik_ur_base_config + realsense2_camera + realsense2_description + robotiq_controllers + robotiq_description + ur_description + velocity_force_controller + + ament_clang_format + ament_clang_tidy + ament_cmake_copyright + ament_cmake_lint_cmake + ament_flake8 + ament_lint_auto + picknik_ament_copyright + + + ament_cmake + + diff --git a/src/hand_eye_calibration_sim/scripts/generate_charuco_boards.py b/src/hand_eye_calibration_sim/scripts/generate_charuco_boards.py new file mode 100644 index 000000000..bf42009b0 --- /dev/null +++ b/src/hand_eye_calibration_sim/scripts/generate_charuco_boards.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +# Copyright 2026 PickNik Inc. +# +# 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 PickNik Inc. 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. + +"""Generate the ChArUco board textures used by the hand_eye_calibration_sim scene. + +Regenerates description/assets/charuco_world_board.png and +description/assets/charuco_flange_board.png. The board parameters here are the +single source of truth for the physical boards in the scene; the MJCF plate +geoms must stay dimensioned to squares_x * square_length_m by +squares_y * square_length_m. + +The boards are drawn with marginSize=0 so the chessboard exactly fills the +image, which makes the texture span the plate geom's top face with no border +offset: ChArUco corner (i, j) sits exactly at (i * square_length_m, +j * square_length_m) from the plate corner. + +Uses the pre-4.7 OpenCV aruco API (the dev container ships OpenCV 4.6.0). Run +inside the MoveIt Pro dev container: + + docker run --rm --entrypoint bash -v "$(pwd)":/ws moveit-pro-dev:main-jazzy \ + -c 'python3 /ws/src/hand_eye_calibration_sim/scripts/generate_charuco_boards.py \ + --output-dir /ws/src/hand_eye_calibration_sim/description/assets' +""" + +import argparse +from pathlib import Path + +import cv2 + +# Pixels per chessboard square in the generated textures. High enough that the +# markers stay crisp in 1280x720 renders at the working distances in the scene. +PIXELS_PER_SQUARE = 200 + +# Board definitions. These parameters are mirrored in the package README and +# are the intended defaults for the DetectCharucoBoard Behavior ports. +BOARDS = [ + { + "filename": "charuco_world_board.png", + "dictionary": cv2.aruco.DICT_4X4_50, + "squares_x": 7, + "squares_y": 5, + "square_length_m": 0.035, + "marker_length_m": 0.026, + }, + { + "filename": "charuco_flange_board.png", + "dictionary": cv2.aruco.DICT_5X5_50, + "squares_x": 5, + "squares_y": 4, + "square_length_m": 0.030, + "marker_length_m": 0.022, + }, +] + + +def generate_board(board_spec: dict, output_dir: Path) -> Path: + """Draw one ChArUco board PNG and return the written path.""" + dictionary = cv2.aruco.getPredefinedDictionary(board_spec["dictionary"]) + board = cv2.aruco.CharucoBoard_create( + board_spec["squares_x"], + board_spec["squares_y"], + board_spec["square_length_m"], + board_spec["marker_length_m"], + dictionary, + ) + size_px = ( + board_spec["squares_x"] * PIXELS_PER_SQUARE, + board_spec["squares_y"] * PIXELS_PER_SQUARE, + ) + image = board.draw(size_px, marginSize=0, borderBits=1) + output_path = output_dir / board_spec["filename"] + if not cv2.imwrite(str(output_path), image): + raise RuntimeError(f"Failed to write {output_path}") + return output_path + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--output-dir", + type=Path, + default=Path(__file__).resolve().parent.parent / "description" / "assets", + help="Directory the PNGs are written to.", + ) + args = parser.parse_args() + args.output_dir.mkdir(parents=True, exist_ok=True) + for board_spec in BOARDS: + path = generate_board(board_spec, args.output_dir) + print(f"Wrote {path}") + + +if __name__ == "__main__": + main() diff --git a/src/hand_eye_calibration_sim/waypoints/ur_waypoints.yaml b/src/hand_eye_calibration_sim/waypoints/ur_waypoints.yaml new file mode 100644 index 000000000..823949562 --- /dev/null +++ b/src/hand_eye_calibration_sim/waypoints/ur_waypoints.yaml @@ -0,0 +1,43 @@ +- description: '' + favorite: false + joint_group_names: + - gripper + - linear_actuator + - manipulator + joint_state: + effort: [] + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + name: + - robotiq_85_left_knuckle_joint + - linear_rail_joint + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + position: + - 0.0026035957049470554 + - 8.220538339495654e-21 + - -4.101983724358406e-20 + - -1.8849000000000018 + - 1.0996999999999981 + - -1.2566000000000017 + - -1.5707500000000016 + - -2.5032559135860884e-20 + velocity: [] + multi_dof_joint_state: + header: + frame_id: '' + stamp: + nanosec: 0 + sec: 0 + joint_names: [] + transforms: [] + twist: [] + wrench: [] + name: Home