diff --git a/ada_calibrate_camera/LICENSE b/ada_calibrate_camera/LICENSE new file mode 100644 index 0000000..574ef07 --- /dev/null +++ b/ada_calibrate_camera/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/ada_calibrate_camera/README.md b/ada_calibrate_camera/README.md new file mode 100644 index 0000000..dd567a5 --- /dev/null +++ b/ada_calibrate_camera/README.md @@ -0,0 +1,20 @@ +# ada_camera_calibration + +This file contains a nodes to do the following: +1. Calibrate ADA's eye-in-hand system's extrinsics (run every time the eye-in-hand system changes); +2. Publish the transform between ADA's end-effector and the camera (run every time the robot is used with perception). + +## Calibrating the Camera's Extrinsics +1. Be in the `src` directory of your workspace. +2. `python3 src/ada_ros2/ada_calibrate_camera/calibrate_camera_start.py` +3. `screen -r calibrate` + 1. (See here for relevant [`screen`` commands](https://gist.github.com/jctosta/af918e1618682638aa82)) +4. Follow the instructions on-screen. (Motions are expected to take ~6 mins and collect up to 30 samples) +5. Once it is done, verify it: + 1. Re-build your workspace. + 2. Run `ros2 launch ada_moveit demo.launch.py sim:=mock` + 3. In RVIZ, add an axis for `camera_color_optical_frame` + 4. Verify it looks like the frame is correct. + +## Publishing the Transform +1. `ros2 launch ada_calibrate_camera publish_camera_extrinsics_launch.xml` (this is done by default in the `ada_moveit` launchfile). diff --git a/ada_calibrate_camera/ada_calibrate_camera/__init__.py b/ada_calibrate_camera/ada_calibrate_camera/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py b/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py new file mode 100755 index 0000000..3e295ba --- /dev/null +++ b/ada_calibrate_camera/ada_calibrate_camera/calibrate_camera_node.py @@ -0,0 +1,1469 @@ +#!/usr/bin/env python3 +""" +This module defines a node that calibrates the camera using a charucoboard. +""" + +# Standard imports +import copy +import os +import readline # pylint: disable=unused-import +import sys +import threading +from typing import List, Optional, Tuple, Union + +# Third-party imports +from action_msgs.msg import GoalStatus +from controller_manager_msgs.srv import SwitchController +import cv2 +from cv_bridge import CvBridge +from geometry_msgs.msg import TwistStamped, Vector3 +from moveit_msgs.msg import MoveItErrorCodes +import numpy as np +from pymoveit2 import MoveIt2, MoveIt2State +from pymoveit2.robots import kinova +import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.duration import Duration +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy +from rclpy.time import Time +from rclpy.timer import Rate +from rcl_interfaces.msg import ParameterDescriptor, ParameterType +import ros2_numpy +from scipy.spatial.transform import Rotation as R +from sensor_msgs.msg import CameraInfo, CompressedImage +from std_srvs.srv import SetBool +from tf2_geometry_msgs import PoseStamped, Vector3Stamped +import tf2_py as tf2 +import tf2_ros +import yaml + +# Local imports +from ada_calibrate_camera.charuco_detector import CharucoDetector +from ada_calibrate_camera.camera_calibration import CameraCalibration +from ada_calibrate_camera.helpers import ( + pose_to_matrix, + pose_to_twist, + matrix_to_pose, +) + + +# pylint: disable=invalid-name +# Although names like R_gripper2base are not snake case, they align with OpenCV +# conventions. + +# pylint: disable=too-many-lines +# It is only slightly over. + + +class CalibrateCameraNode(Node): + """ + This node uses a command-line interface to enable users to calibrate ADA's + eye-on-hand camera using a charucoboard of known size. The robot base should + be mounted on the tripod, for ease of changing height, and in a location with + at least 1m empty space on all sides of the robot's base. After detecting the + initial charucoboard, this node automatically moves the robot around it, and + saves the images and joint states. It then computes the camera calibration, + given those images. Users are then encouraged to change the height of the robot + relative to the charucoboard and repeat the process to improve the calibration. + """ + + # pylint: disable=too-many-instance-attributes + + def __init__(self): + """ + Do the initialization steps that don't require rclpy to be spinning. + """ + super().__init__("calibrate_camera") + + # Read the parameters + self.active_controller = None + self.read_params() + + # Create the TF2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create the twist publisher + self.twist_pub = self.create_publisher( + TwistStamped, + "/jaco_arm_cartesian_controller/twist_cmd", + qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE), + ) + + # Create the MoveIt2 object + callback_group = ReentrantCallbackGroup() + self.moveit2 = MoveIt2( + node=self, + joint_names=kinova.joint_names(), + base_link_name=kinova.base_link_name(), + end_effector_name=self.robot_end_effector_frame, + group_name="jaco_arm", + callback_group=callback_group, + ) + + # Create the charuco detector + self.charuco_detector = CharucoDetector( + n_rows=self.charuco_n_rows, + n_cols=self.charuco_n_cols, + sq_length_m=self.charuco_sq_length_m, + marker_length_m=self.charuco_marker_length_m, + predefined_dictionary=self.charuco_predefined_dictionary, + ) + + # Subscribe to the camera info + self.camera_info_sub = self.create_subscription( + CameraInfo, + "/local/camera/aligned_depth_to_color/camera_info", + self.camera_info_callback, + qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT), + callback_group=MutuallyExclusiveCallbackGroup(), + ) + + # Subscribe to the RGB camera feed + self.bridge = CvBridge() + self.latest_img_lock = threading.Lock() + self.latest_annotated_img = None + self.latest_raw_img = None + self.latest_charuco_rvec = None + self.latest_charuco_tvec = None + self.rgb_img_sub = self.create_subscription( + CompressedImage, + "/local/camera/color/image_raw/compressed", + self.rgb_img_callback, + qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT), + callback_group=MutuallyExclusiveCallbackGroup(), + ) + + # Create the service to (de)activate the controller + self.switch_controller_client = self.create_client( + SwitchController, + "/controller_manager/switch_controller", + qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE), + ) + + # Create the service to re-tare the F/T sensor + self.re_tare_ft_sensor_client = self.create_client( + SetBool, + "/wireless_ft/set_bias", + qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.RELIABLE), + ) + + def read_params(self): + """ + Read the parameters from the parameter server. + """ + # General parameters + self.run_node_online = self.declare_parameter( + "run_online", + True, + ParameterDescriptor( + name="run_online", + type=ParameterType.PARAMETER_BOOL, + description="Whether to run the calibration online.", + read_only=True, + ), + ).value + data_dir = self.declare_parameter( + "data_dir", + "data", + ParameterDescriptor( + name="data_dir", + type=ParameterType.PARAMETER_STRING, + description="The directory to save the data, relative to the package's source directory", + read_only=True, + ), + ).value + self.data_dir = os.path.expanduser(data_dir) + self.offline_data_dir = self.declare_parameter( + "offline_data_dir", + "2024_10_09_21_12_30", + ParameterDescriptor( + name="offline_data_dir", + type=ParameterType.PARAMETER_STRING, + description="The directory to load the offline data, relative to the data_dir.", + read_only=True, + ), + ).value + self.offline_data_dir = os.path.join(self.data_dir, self.offline_data_dir) + output_calib_file = self.declare_parameter( + "output_calib_name", + "config/calibs/auto.yaml", + ParameterDescriptor( + name="output_calib_name", + type=ParameterType.PARAMETER_STRING, + description="The name of the output calibration file, relative to the package's source directory", + read_only=True, + ), + ).value + self.output_calib_file = os.path.expanduser(output_calib_file) + ref_calib_file = self.declare_parameter( + "reference_calib_name", + "config/calibs/manual.yaml", + ParameterDescriptor( + name="reference_calib_name", + type=ParameterType.PARAMETER_STRING, + description="The name of the reference calibration file, relative to the package's source directory", + read_only=True, + ), + ).value + self.ref_calib_file = os.path.expanduser(ref_calib_file) + self.ref_translation_error_threshold = self.declare_parameter( + "reference_translation_error_threshold", + 0.03, + ParameterDescriptor( + name="reference_translation_error_threshold", + type=ParameterType.PARAMETER_DOUBLE, + description=( + "If the computed camera calibration's translation differs from the reference by more than " + "this threshold, the calibration is considered invalid." + ), + read_only=True, + ), + ).value + self.ref_rotation_error_threshold = self.declare_parameter( + "reference_rotation_error_threshold", + 0.03, + ParameterDescriptor( + name="reference_rotation_error_threshold", + type=ParameterType.PARAMETER_DOUBLE, + description=( + "If the computed camera calibration's rotation differs from the reference by more than " + "this threshold, the calibration is considered invalid." + ), + read_only=True, + ), + ).value + self.child_frame_id = self.declare_parameter( + "child_frame_id", + "camera_color_optical_frame", + ParameterDescriptor( + name="child_frame_id", + type=ParameterType.PARAMETER_STRING, + description="The child frame ID (camera optical frame ID).", + read_only=True, + ), + ).value + + # Frame names + self.robot_end_effector_frame = self.declare_parameter( + "robot_end_effector_frame", + "forkTip", + ParameterDescriptor( + name="robot_end_effector_frame", + type=ParameterType.PARAMETER_STRING, + description="The robot end effector frame. Used for cartesian motions.", + read_only=True, + ), + ).value + self.extrinsics_base_frame = self.declare_parameter( + "extrinsics_base_frame", + "j2n6s200_end_effector", + ParameterDescriptor( + name="extrinsics_base_frame", + type=ParameterType.PARAMETER_STRING, + description=( + "The extrinsics base frame. The camera should have a static transform " + "from this frame. This frame should be on the robot arm." + ), + read_only=True, + ), + ).value + + # Controller parameters + self.all_controllers = self.declare_parameter( + "all_controllers", + [ + "jaco_arm_servo_controller", + "jaco_arm_cartesian_controller", + "jaco_arm_controller", + ], + ParameterDescriptor( + name="all_controllers", + type=ParameterType.PARAMETER_STRING_ARRAY, + description="The names of all the controllers.", + read_only=True, + ), + ).value + self.default_moveit2_controller = self.declare_parameter( + "default_moveit2_controller", + "jaco_arm_controller", + ParameterDescriptor( + name="default_moveit2_controller", + type=ParameterType.PARAMETER_STRING, + description="The default controller for MoveIt2.", + read_only=True, + ), + ).value + if self.default_moveit2_controller not in self.all_controllers: + self.get_logger().error( + "The default controller for MoveIt2 is not in the list of all controllers." + ) + sys.exit(1) + + # Arm parameters + self.starting_arm_configuration = self.declare_parameter( + "starting_arm_configuration", + [ + -2.3149168248766614, # j2n6s200_joint_1 + 3.1444595465032634, # j2n6s200_joint_2 + 1.7332586075115999, # j2n6s200_joint_3 + -2.3609596843308234, # j2n6s200_joint_4 + 4.43936623280362, # j2n6s200_joint_5 + 3.06866544924739, # j2n6s200_joint_6 + ], + ParameterDescriptor( + name="starting_arm_configuration", + type=ParameterType.PARAMETER_DOUBLE_ARRAY, + description="The starting configuration of the arm.", + read_only=True, + ), + ).value + + self.wait_before_capture_secs = self.declare_parameter( + "wait_before_capture_secs", + 2.0, + ParameterDescriptor( + name="wait_before_capture_secs", + type=ParameterType.PARAMETER_DOUBLE, + description="The time to wait between the end of motion and capturing the image.", + read_only=True, + ), + ).value + + # Charuco board parameters + self.charuco_n_rows = self.declare_parameter( + "charuco_n_rows", + 5, + ParameterDescriptor( + name="charuco_n_rows", + type=ParameterType.PARAMETER_INTEGER, + description="The number of rows in the charucoboard.", + read_only=True, + ), + ).value + self.charuco_n_cols = self.declare_parameter( + "charuco_n_cols", + 5, + ParameterDescriptor( + name="charuco_n_cols", + type=ParameterType.PARAMETER_INTEGER, + description="The number of columns in the charucoboard.", + read_only=True, + ), + ).value + self.charuco_sq_length_m = self.declare_parameter( + "charuco_sq_length_m", + 0.0351, + ParameterDescriptor( + name="charuco_sq_length_m", + type=ParameterType.PARAMETER_DOUBLE, + description="The length of a square in the charucoboard.", + read_only=True, + ), + ).value + self.charuco_marker_length_m = self.declare_parameter( + "charuco_marker_length_m", + 0.0257, + ParameterDescriptor( + name="charuco_marker_length_m", + type=ParameterType.PARAMETER_DOUBLE, + description="The length of a marker in the charucoboard.", + read_only=True, + ), + ).value + self.charuco_predefined_dictionary = self.declare_parameter( + "charuco_predefined_dictionary", + cv2.aruco.DICT_5X5_50, + ParameterDescriptor( + name="charuco_predefined_dictionary", + type=ParameterType.PARAMETER_INTEGER, + description=( + "The predefined dictionary to use. See cv::aruco::PredefinedDictionaryType for the enum: " + "https://docs.opencv.org/4.x/de/d67/group__objdetect__aruco.html#ga4e13135a118f497c6172311d601ce00d" + ), + read_only=True, + ), + ).value + + # Hand-eye calibration method + hand_eye_calibration_method = self.declare_parameter( + "hand_eye_calibration_method", + "andreff", + ParameterDescriptor( + name="hand_eye_calibration_method", + type=ParameterType.PARAMETER_STRING, + description=( + "The hand-eye calibration method to use. Valid values include: " + "'all', 'tsai', 'park', 'horaud', 'andreff', 'daniilidis'. " + "See cv::HandEyeCalibrationMethod for more " + "https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#gad10a5ef12ee3499a0774c7904a801b99" + ), + read_only=True, + ), + ).value + self.hand_eye_calibration_methods = [] + if hand_eye_calibration_method.lower().strip() in ["all", "tsai"]: + self.hand_eye_calibration_methods.append(cv2.CALIB_HAND_EYE_TSAI) + if hand_eye_calibration_method.lower().strip() in ["all", "park"]: + self.hand_eye_calibration_methods.append(cv2.CALIB_HAND_EYE_PARK) + if hand_eye_calibration_method.lower().strip() in ["all", "horaud"]: + self.hand_eye_calibration_methods.append(cv2.CALIB_HAND_EYE_HORAUD) + if hand_eye_calibration_method.lower().strip() in ["all", "andreff"]: + self.hand_eye_calibration_methods.append(cv2.CALIB_HAND_EYE_ANDREFF) + if hand_eye_calibration_method.lower().strip() in ["all", "daniilidis"]: + self.hand_eye_calibration_methods.append(cv2.CALIB_HAND_EYE_DANIILIDIS) + if len(self.hand_eye_calibration_methods) == 0: + self.get_logger().error( + f"Invalid hand-eye calibration method {hand_eye_calibration_method}. " + "Valid values include: 'all', 'tsai', 'park', 'horaud', 'andreff', 'daniilidis'." + ) + sys.exit(1) + + def initialize( + self, + timeout_secs: float = 10.0, + rate: Union[Rate, float] = 10.0, + ) -> bool: + """ + Do the initialization steps that require rclpy to be spinning. + """ + # Configuration for the timeout + start_time = self.get_clock().now() + timeout = Duration(seconds=timeout_secs) + created_rate = False + if isinstance(rate, float): + rate = self.create_rate(rate) + created_rate = True + + def cleanup(retval: bool) -> bool: + if created_rate: + self.destroy_rate(rate) + return retval + + if self.run_node_online: + # Wait for the joint states + while self.moveit2.joint_state is None: + if not rclpy.ok(): + self.get_logger().error( + "Interrupted while waiting for the joint states." + ) + return cleanup(False) + if self.get_clock().now() - start_time > timeout: + self.get_logger().error("Timed out while getting the joint states.") + return cleanup(False) + rate.sleep() + + # Wait for the RGB image + latest_raw_img = None + while latest_raw_img is None: + with self.latest_img_lock: + latest_raw_img = self.latest_raw_img + if not rclpy.ok(): + self.get_logger().error( + "Interrupted while waiting for the RGB image." + ) + return cleanup(False) + if self.get_clock().now() - start_time > timeout: + self.get_logger().error("Timed out while getting the RGB image.") + return cleanup(False) + rate.sleep() + + return cleanup(True) + + def get_remaining_time(self, start_time: Time, timeout_secs: float) -> float: + """ + Get the remaining time before the timeout. + + Parameters + ---------- + start_time : Time + The start time. + timeout_secs : float + The timeout in seconds. + + Returns + ------- + float + The remaining time in seconds. + """ + return timeout_secs - (self.get_clock().now() - start_time).nanoseconds / 1e9 + + def activate_controller( + self, + controller_name: str = "jaco_arm_cartesian_controller", + timeout_secs: float = 10.0, + rate: Union[Rate, float] = 10.0, + ) -> bool: + """ + Activate the specified controller and deactivate all others. + """ + if self.active_controller == controller_name: + return True + + # Configuration for the timeout + start_time = self.get_clock().now() + timeout = Duration(seconds=timeout_secs) + created_rate = False + if isinstance(rate, float): + rate = self.create_rate(rate) + created_rate = True + + def cleanup(retval: bool) -> bool: + if created_rate: + self.destroy_rate(rate) + return retval + + # Activate the controller + request = SwitchController.Request( + activate_controllers=[controller_name], + deactivate_controllers=[ + controller + for controller in self.all_controllers + if controller != controller_name + ], + activate_asap=True, + strictness=SwitchController.Request.BEST_EFFORT, + ) + if not self.switch_controller_client.wait_for_service( + timeout_sec=self.get_remaining_time(start_time, timeout_secs), + ): + self.get_logger().error( + "Failed to connect to the switch_controller service." + ) + return cleanup(False) + future = self.switch_controller_client.call_async(request) + while not future.done(): + if not rclpy.ok(): + self.get_logger().error("Interrupted while activating the controller.") + return cleanup(False) + if self.get_clock().now() - start_time > timeout: + self.get_logger().error("Timeout while activating the controller.") + return cleanup(False) + rate.sleep() + response = future.result() + if not response.ok: + self.get_logger().error("Failed to activate the controller.") + return cleanup(False) + + self.active_controller = controller_name + return cleanup(True) + + def re_tare_ft_sensor( + self, + timeout_secs: float = 10.0, + rate: Union[Rate, float] = 10.0, + ) -> bool: + """ + Re-tare the F/T sensor. + """ + # Configuration for the timeout + start_time = self.get_clock().now() + timeout = Duration(seconds=timeout_secs) + created_rate = False + if isinstance(rate, float): + rate = self.create_rate(rate) + created_rate = True + + def cleanup(retval: bool) -> bool: + if created_rate: + self.destroy_rate(rate) + return retval + + # Re-tare the F/T sensor + request = SetBool.Request(data=True) + if not self.re_tare_ft_sensor_client.wait_for_service( + timeout_sec=self.get_remaining_time(start_time, timeout_secs), + ): + self.get_logger().error("Failed to connect to the re-tare service.") + return cleanup(False) + future = self.re_tare_ft_sensor_client.call_async(request) + while not future.done(): + if not rclpy.ok(): + self.get_logger().error("Interrupted while re-taring the F/T sensor.") + return cleanup(False) + if self.get_clock().now() - start_time > timeout: + self.get_logger().error("Timeout while re-taring the F/T sensor.") + return cleanup(False) + rate.sleep() + response = future.result() + if not response.success: + self.get_logger().error("Failed to re-tare the F/T sensor.") + return cleanup(False) + return cleanup(True) + + def move_to_configuration( + self, + configuration: List[float], + timeout_secs: float = 10.0, + rate: Union[Rate, float] = 10.0, + ) -> bool: + """ + Move the robot to the specified configuration. + """ + # pylint: disable=too-many-return-statements, too-many-branches + # Okay due to extensive error checking + + # Configuration for the timeout + start_time = self.get_clock().now() + timeout = Duration(seconds=timeout_secs) + created_rate = False + if isinstance(rate, float): + rate = self.create_rate(rate) + created_rate = True + + def cleanup(retval: bool) -> bool: + if created_rate: + self.destroy_rate(rate) + return retval + + # Re-tare the F/T sensor + if not self.re_tare_ft_sensor(timeout_secs=timeout_secs, rate=rate): + return cleanup(False) + + # Plan the motion to the configuration + future = self.moveit2.plan_async( + joint_positions=configuration, + ) + while not future.done(): + if not rclpy.ok(): + self.get_logger().error( + "Interrupted while moving to the configuration." + ) + return cleanup(False) + if self.get_clock().now() - start_time > timeout: + self.get_logger().error("Timeout while moving to the configuration.") + return cleanup(False) + rate.sleep() + traj = self.moveit2.get_trajectory(future) + if traj is None: + self.get_logger().error("Failed to plan to the configuration.") + return cleanup(False) + + # Execute the motion + self.moveit2.execute(traj) + while self.moveit2.query_state() != MoveIt2State.EXECUTING: + if not rclpy.ok(): + self.get_logger().error("Interrupted while executing the trajectory.") + return cleanup(False) + if self.get_clock().now() - start_time > timeout: + self.get_logger().error("Timeout while executing the trajectory.") + return cleanup(False) + rate.sleep() + self.active_controller = ( + self.default_moveit2_controller + ) # MoveIt2 automatically activates this controller + future = self.moveit2.get_execution_future() + while not future.done(): + if not rclpy.ok(): + self.get_logger().error("Interrupted while executing the trajectory.") + return cleanup(False) + if self.get_clock().now() - start_time > timeout: + self.get_logger().error("Timeout while executing the trajectory.") + return cleanup(False) + rate.sleep() + result = future.result() + if ( + result.status != GoalStatus.STATUS_SUCCEEDED + or result.result.error_code.val != MoveItErrorCodes.SUCCESS + ): + self.get_logger().error("Failed to execute the trajectory.") + return cleanup(False) + return cleanup(True) + + def transform_stamped_msg( + self, + stamped_msg: Union[PoseStamped, TwistStamped], + target_frame: str, + timeout_secs: float = 10.0, + ) -> Optional[Union[PoseStamped, TwistStamped]]: + """ + Transform the pose stamped to the target frame. + + Parameters + ---------- + stamped_msg : Union[PoseStamped, TwistStamped] + The stamped message to transform. + target_frame : str + The target frame. + timeout_secs : float + The timeout in seconds. + + Returns + ------- + Optional[Union[PoseStamped, TwistStamped]] + The transformed stamped message, or None if the transformation failed. + """ + stamped_msgs = [] + if isinstance(stamped_msg, PoseStamped): + stamped_msgs.append(stamped_msg) + else: + stamped_msgs.append( + Vector3Stamped( + header=stamped_msg.header, vector=stamped_msg.twist.linear + ) + ) + stamped_msgs.append( + Vector3Stamped( + header=stamped_msg.header, vector=stamped_msg.twist.angular + ) + ) + transformed_msgs = [] + for msg in stamped_msgs: + try: + transformed_msg = self.tf_buffer.transform( + msg, + target_frame, + timeout=Duration(seconds=timeout_secs), + ) + transformed_msgs.append(transformed_msg) + except ( + tf2.ConnectivityException, + tf2.ExtrapolationException, + tf2.InvalidArgumentException, + tf2.LookupException, + tf2.TimeoutException, + tf2.TransformException, + tf2_ros.TypeException, + ) as e: + self.get_logger().error(f"Failed to transform the pose: {e}") + return None + if isinstance(stamped_msg, PoseStamped): + return transformed_msgs[0] + transformed_twist = TwistStamped() + transformed_twist.header = transformed_msgs[0].header + transformed_twist.twist.linear = transformed_msgs[0].vector + transformed_twist.twist.angular = transformed_msgs[1].vector + return transformed_twist + + def move_end_effector_to_pose_cartesian( + self, + pose_stamped: PoseStamped, + timeout_secs: float = 10.0, + rate: Union[Rate, float] = 10.0, + linear_threshold: float = 0.01, # meters + angular_threshold: float = 0.01, # radians + only_linear: bool = False, + only_angular: bool = False, + do_not_oscillate: bool = True, + verbose: bool = False, + ): + """ + Move the end-effector to the specified pose via Cartesian motion. + + Parameters + ---------- + pose_stamped : PoseStamped + The target pose. + timeout_secs : float + The timeout in seconds. + rate : Union[Rate, float] + The rate at which to run the loop. + linear_threshold : float + The linear threshold to consider the pose reached. + angular_threshold : float + The angular threshold to consider the pose reached. + only_linear : bool + If true, only move in the linear dimensions. + only_angular : bool + If true, only move in the angular dimensions. + do_not_oscillate : bool + If true, as soon as the sign of any of the velocities changes, zero out + that dimension of the velocity. + verbose : bool + If true, print verbose output. + """ + # pylint: disable=too-many-arguments, too-many-locals, too-many-branches + # pylint: disable=too-many-return-statements, too-many-statements + # This is intended to be a flexible function. + + # Configuration for the timeout + start_time = self.get_clock().now() + timeout = Duration(seconds=timeout_secs) + created_rate = False + if isinstance(rate, float): + rate = self.create_rate(rate) + created_rate = True + + def cleanup(retval: bool) -> bool: + zero_twist = TwistStamped() + zero_twist.header.frame_id = self.moveit2.base_link_name + self.twist_pub.publish(zero_twist) # Stop the motion + if created_rate: + self.destroy_rate(rate) + return retval + + # Re-tare the F/T sensor + if not self.re_tare_ft_sensor(timeout_secs, rate): + return cleanup(False) + + # Activate the controller + if not self.activate_controller( + "jaco_arm_cartesian_controller", timeout_secs, rate + ): + return cleanup(False) + + # Convert pose to base link frame if it isn't already + if pose_stamped.header.frame_id != self.moveit2.base_link_name: + pose_stamped_base = self.transform_stamped_msg( + pose_stamped, + self.moveit2.base_link_name, + self.get_remaining_time(start_time, timeout_secs), + ) + if pose_stamped_base is None: + return cleanup(False) + else: + pose_stamped_base = pose_stamped + + # Move towards the pose until it is reached or timeout + signs = None + while self.get_clock().now() - start_time < timeout: + if not rclpy.ok(): + return cleanup(False) + + # Convert the target pose to the end-effector frame + pose_stamped_base.header.stamp = self.get_clock().now().to_msg() + pose_stamped_ee = self.transform_stamped_msg( + pose_stamped_base, + self.moveit2.end_effector_name, + self.get_remaining_time(start_time, timeout_secs), + ) + if verbose: + print(f"Pose in end-effector frame: {pose_stamped_ee}", flush=True) + if pose_stamped_ee is None: + return cleanup(False) + + # Check if the pose is reached + if only_angular: + position_diff = 0.0 + else: + position_diff = np.linalg.norm( + ros2_numpy.numpify(pose_stamped_ee.pose.position) + ) + if only_linear: + angular_diff = 0.0 + else: + angular_diff = np.linalg.norm( + R.from_quat( + ros2_numpy.numpify(pose_stamped_ee.pose.orientation) + ).as_rotvec() + ) + if position_diff < linear_threshold and angular_diff < angular_threshold: + return cleanup(True) + + # Compute the twist required to move the end effector to the target pose + twist_stamped = pose_to_twist( + pose_stamped_ee, + rate_hz=1.0e9 + / rate._timer.timer_period_ns, # pylint: disable=protected-access + ) + if only_angular: + twist_stamped.twist.linear = Vector3() + if only_linear: + twist_stamped.twist.angular = Vector3() + if do_not_oscillate and signs is not None: + # pylint: disable=unsubscriptable-object + # Okay since signs is a numpy array + if np.sign(twist_stamped.twist.linear.x) != signs[0]: + twist_stamped.twist.linear.x = 0.0 + if np.sign(twist_stamped.twist.linear.y) != signs[1]: + twist_stamped.twist.linear.y = 0.0 + if np.sign(twist_stamped.twist.linear.z) != signs[2]: + twist_stamped.twist.linear.z = 0.0 + if np.sign(twist_stamped.twist.angular.x) != signs[3]: + twist_stamped.twist.angular.x = 0.0 + if np.sign(twist_stamped.twist.angular.y) != signs[4]: + twist_stamped.twist.angular.y = 0.0 + if np.sign(twist_stamped.twist.angular.z) != signs[5]: + twist_stamped.twist.angular.z = 0.0 + if signs is None: + signs = np.sign( + np.concatenate( + [ + ros2_numpy.numpify(twist_stamped.twist.linear), + ros2_numpy.numpify(twist_stamped.twist.angular), + ] + ) + ) + if verbose: + print(f"Twist in EE frame{twist_stamped}", flush=True) + + # Transform to the base frame + twist_stamped_base = self.transform_stamped_msg( + twist_stamped, + self.moveit2.base_link_name, + self.get_remaining_time(start_time, timeout_secs), + ) + if verbose: + print(f"Twist in base frame{twist_stamped_base}", flush=True) + if twist_stamped_base is None: + return cleanup(False) + + # Publish the twist + self.twist_pub.publish(twist_stamped_base) + + # If the commanded twist is 0, succeed + if ( + np.linalg.norm(ros2_numpy.numpify(twist_stamped_base.twist.linear)) + < 1.0e-6 + and np.linalg.norm(ros2_numpy.numpify(twist_stamped_base.twist.angular)) + < 1.0e-6 + ): + return cleanup(True) + + # Sleep + rate.sleep() + + self.get_logger().error("Timeout while moving the end-effector to the pose.") + return cleanup(False) + + def camera_info_callback(self, msg: CameraInfo): + """ + Callback for the camera info. + + Parameters + ---------- + msg : CameraInfo + The camera info message. + """ + if not self.charuco_detector.got_camera_intrinsics(): + self.charuco_detector.set_camera_intrinsics( + np.array(msg.k).reshape((3, 3)), + np.array(msg.d), + ) + self.destroy_subscription(self.camera_info_sub) + + def rgb_img_callback(self, msg: CompressedImage): + """ + Callback for the RGB image. + + Parameters + ---------- + msg : CompressedImage + The compressed image message. + """ + img = self.bridge.compressed_imgmsg_to_cv2(msg, desired_encoding="passthrough") + succ, rvec, tvec, annotated_img = self.charuco_detector.detect(img, viz=True) + if succ: + with self.latest_img_lock: + self.latest_annotated_img = annotated_img + self.latest_raw_img = img + self.latest_charuco_rvec = rvec + self.latest_charuco_tvec = tvec + else: + with self.latest_img_lock: + self.latest_annotated_img = img + self.latest_raw_img = img + self.latest_charuco_rvec = None + self.latest_charuco_tvec = None + + def get_input(self, prompt: str) -> str: + """ + Get input from the user. + + Parameters + ---------- + prompt : str + The prompt to display. + + Returns + ------- + str + The input from the user. + """ + print(prompt, flush=True) + return input() + + def run_online( + self, + rate: Union[Rate, float] = 10.0, + ): + """ + Run the node. + + Parameters + ---------- + rate : Union[Rate, float] + The rate at which to run the loop. + """ + # pylint: disable=too-many-locals, too-many-branches, too-many-statements + # pylint: disable=too-many-nested-blocks + # Fine since this does the main work. + + try: + created_rate = False + if isinstance(rate, float): + rate = self.create_rate(rate) + created_rate = True + # Wait for the user to place the robot on its tripod mount + _ = self.get_input( + "Place the robot on a mount (e.g., wheelchair mount). Ensure " + "there is at least 30cm of empty space below and around the robot's " + "gripper in the home position. Press Enter when done." + ) + print("Moving the robot...", flush=True) + + # Move the robot to the starting configuration + self.move_to_configuration( + configuration=self.starting_arm_configuration, + rate=rate, + ) + + # Wait for the user to place the charucoboard in front of the robot + _ = self.get_input( + "Place the charucoboard on the hospital table. Roll the hospital table " + "under the robot's end-effector. Adjust the height of the tripod and hospital " + "table so the end-effector is around 10cm above the charuboard while " + "the charucoboard is still fully visible in the camera. Press Enter when done." + ) + + # Load the reference calibration + ref_R_cam2gripper, ref_t_cam2gripper = self.load_yaml(self.ref_calib_file) + + # Get the current end-effector pose in base frame + zero_pose_ee_frame = PoseStamped() + zero_pose_ee_frame.header.frame_id = self.moveit2.end_effector_name + zero_pose_ee_frame.pose.orientation.w = 1.0 + zero_pose_extrinsics_frame = copy.deepcopy(zero_pose_ee_frame) + zero_pose_extrinsics_frame.header.frame_id = self.extrinsics_base_frame + init_ee_pose = self.transform_stamped_msg( + copy.deepcopy(zero_pose_ee_frame), + self.moveit2.base_link_name, + ) + if init_ee_pose is None: + return + init_ee_M = pose_to_matrix(init_ee_pose.pose) + print(f"Initial end-effector transform: {init_ee_pose}", flush=True) + + # Capture the poses and images. + camera_calibration = CameraCalibration( + data_dir=self.data_dir, + methods=self.hand_eye_calibration_methods, + ) + lateral_radius = 0.10 # meters + lateral_intervals = 5 + wait_before_capture = Duration(seconds=self.wait_before_capture_secs) + # In practice, modifying pitch resulted in too much variation in z, which hindered reliability. + # This is kept here for legacy purposes. + pitches = [0.0] + prev_target_pose = None + for d_z in [0.0, 0.08, -0.08]: + for lateral_i in range(-1, lateral_intervals): + # Get the target pose + if lateral_i == -1: + d_x = 0.0 + d_y = 0.0 + yaws = [0.0, np.pi] + else: + theta = 2 * np.pi * lateral_i / lateral_intervals + d_x = lateral_radius * np.cos(theta) + d_y = lateral_radius * np.sin(theta) + if (np.pi / 4 <= theta <= 3 * np.pi / 4) or ( + 5 * np.pi / 4 <= theta <= 7 * np.pi / 4 + ): + yaws = [0.0, np.pi] + else: + yaws = [np.pi / 2, 3 * np.pi / 2] + pitches.reverse() + for d_yaw in yaws: + for d_pitch in pitches: + # Rotation is in EE frame, translation is in base frame + target_M = np.eye(4) + target_M[:3, :3] = ( + init_ee_M[:3, :3] + @ R.from_euler("ZYX", [d_yaw, 0.0, d_pitch]).as_matrix() + ) + target_M[:3, 3] = init_ee_M[:3, 3] + [d_x, d_y, d_z] + + target_pose = copy.deepcopy(init_ee_pose) + target_pose.pose = matrix_to_pose(target_M) + target_pose.header.stamp = self.get_clock().now().to_msg() + + # Move to the target pose. First move linearly, then rotate. + print( + f"Moving to the target pose: {target_pose}", flush=True + ) + if ( + prev_target_pose is None + or prev_target_pose.pose.position.x + != target_pose.pose.position.x + or prev_target_pose.pose.position.y + != target_pose.pose.position.y + or prev_target_pose.pose.position.z + != target_pose.pose.position.z + ): + self.move_end_effector_to_pose_cartesian( + target_pose, + rate=rate, + only_linear=True, + ) + if ( + prev_target_pose is None + or prev_target_pose.pose.orientation.x + != target_pose.pose.orientation.x + or prev_target_pose.pose.orientation.y + != target_pose.pose.orientation.y + or prev_target_pose.pose.orientation.z + != target_pose.pose.orientation.z + or prev_target_pose.pose.orientation.w + != target_pose.pose.orientation.w + ): + self.move_end_effector_to_pose_cartesian( + target_pose, + rate=rate, + only_angular=True, + ) + prev_target_pose = target_pose + + # Wait for the joint states to update + print( + f"Waiting {self.wait_before_capture_secs} secs...", + flush=True, + ) + wait_start_time = self.get_clock().now() + while ( + self.get_clock().now() - wait_start_time + < wait_before_capture + ): + if not rclpy.ok(): + return + rate.sleep() + + # Capture the transform from the end effector to the base link + ee_pose_in_base_frame = self.transform_stamped_msg( + copy.deepcopy(zero_pose_extrinsics_frame), + self.moveit2.base_link_name, + ) + if ee_pose_in_base_frame is None: + self.get_logger().error( + "Failed to capture the EE pose in base frame. Skipping this sample." + ) + continue + + # Capture the transform from the camera to the charucoboard + with self.latest_img_lock: + latest_raw_img = self.latest_raw_img + charuco_rvec = self.latest_charuco_rvec + charuco_tvec = self.latest_charuco_tvec + if charuco_rvec is None or charuco_tvec is None: + self.get_logger().error( + "Failed to detect the CharucoBoard. Skipping this sample." + ) + continue + + # Save the transforms + camera_calibration.add_sample( + latest_raw_img, + ee_pose_in_base_frame, + (charuco_rvec, charuco_tvec), + ) + + # Compute the camera extrinsics calibration + ( + R_cam2gripper, + T_cam2gripper, + rotation_error, + translation_error, + method, + ) = camera_calibration.compute_calibration( + save_data=False, + ref_R_cam2gripper=ref_R_cam2gripper, + ref_t_cam2gripper=ref_t_cam2gripper, + ref_translation_error_threshold=self.ref_translation_error_threshold, + ref_rotation_error_threshold=self.ref_rotation_error_threshold, + ) + if R_cam2gripper is not None: + print(f"Rotation error: {rotation_error}", flush=True) + print( + f"Translation error: {translation_error}", + flush=True, + ) + print( + f"R_cam2gripper: {R.from_matrix(R_cam2gripper).as_quat()}", + flush=True, + ) + print( + f"R_cam2gripper: {R.from_matrix(R_cam2gripper).as_euler('ZYX')}", + flush=True, + ) + print(f"T_cam2gripper: {T_cam2gripper}", flush=True) + + # Save the latest camera calibration to the yaml file + if R_cam2gripper is not None: + self.save_yaml( + R_cam2gripper, + T_cam2gripper, + rotation_error, + translation_error, + method, + ) + + if created_rate: + self.destroy_rate(rate) + except KeyboardInterrupt: + zero_twist = TwistStamped() + zero_twist.header.frame_id = self.moveit2.base_link_name + self.twist_pub.publish(zero_twist) + return + + def run_offline( + self, + _: Union[Rate, float] = 10.0, + ): + """ + Run the node offline. + """ + # Load the reference calibration + ref_R_cam2gripper, ref_t_cam2gripper = self.load_yaml(self.ref_calib_file) + + # Load the camera calibration data + camera_calibration = CameraCalibration( + methods=self.hand_eye_calibration_methods, + ) + camera_calibration.add_samples_from_folder(self.offline_data_dir) + + # Compute the camera extrinsics calibration + ( + R_cam2gripper, + T_cam2gripper, + rotation_error, + translation_error, + method, + ) = camera_calibration.compute_calibration( + save_data=False, + ref_R_cam2gripper=ref_R_cam2gripper, + ref_t_cam2gripper=ref_t_cam2gripper, + ref_translation_error_threshold=self.ref_translation_error_threshold, + ref_rotation_error_threshold=self.ref_rotation_error_threshold, + verbose=len(self.hand_eye_calibration_methods) > 1, + ) + + # Print the results + if R_cam2gripper is not None: + print(f"Used method: {method}", flush=True) + print(f"Rotation error: {rotation_error}", flush=True) + print(f"Translation error: {translation_error}", flush=True) + print( + f"R_cam2gripper: {R.from_matrix(R_cam2gripper).as_quat()}", flush=True + ) + print( + f"R_cam2gripper: {R.from_matrix(R_cam2gripper).as_euler('ZYX')}", + flush=True, + ) + print(f"T_cam2gripper: {T_cam2gripper}", flush=True) + + # Save the camera calibration to the yaml file + self.save_yaml( + R_cam2gripper, + T_cam2gripper, + rotation_error, + translation_error, + method, + ) + + def run( + self, + rate: Union[Rate, float] = 10.0, + ): + """ + Run the node. + """ + if self.run_node_online: + self.run_online(rate) + else: + self.run_offline(rate) + + def save_yaml( + self, + R_cam2gripper: np.ndarray, + t_cam2gripper: np.ndarray, + rotation_error: float, + translation_error: float, + method: int, + ) -> None: + """ + Save the camera calibration to a yaml file. + + Parameters + ---------- + R_cam2gripper : np.ndarray + The rotation matrix from the camera to the gripper. + t_cam2gripper : np.ndarray + The translation vector from the camera to the gripper. + rotation_error : float + The rotation error. + translation_error : float + The translation error. + method : int + The method used for the calibration. + """ + # pylint: disable=too-many-arguments, too-many-locals + # Okay since this is a utility function. + + # Generate the yaml data + x, y, z = t_cam2gripper.flatten() + yaw, pitch, roll = R.from_matrix(R_cam2gripper).as_euler("ZYX") + yaml_data = { + "publish_camera_extrinsics": { + "ros__parameters": { + "x": float(x), + "y": float(y), + "z": float(z), + "roll": float(roll), + "pitch": float(pitch), + "yaw": float(yaw), + "child_frame_id": self.child_frame_id, + "frame_id": self.extrinsics_base_frame, + } + } + } + + # Create the comment on top of the yaml file + method_str = "" + if method == cv2.CALIB_HAND_EYE_TSAI: + method_str = "tsai" + elif method == cv2.CALIB_HAND_EYE_PARK: + method_str = "park" + elif method == cv2.CALIB_HAND_EYE_HORAUD: + method_str = "horaud" + elif method == cv2.CALIB_HAND_EYE_ANDREFF: + method_str = "andreff" + elif method == cv2.CALIB_HAND_EYE_DANIILIDIS: + method_str = "daniilidis" + comment = ( + "# This file was auto-generated by the ada_calibrate_camera node.\n" + f"# It used the following hand-eye calibration method: '{method_str}'.\n" + f"# The rotation error is {rotation_error} and the translation error is {translation_error}.\n" + "# See the README for more details.\n" + ) + + # Save the camera calibration to the yaml file + with open(self.output_calib_file, "w", encoding="utf-8") as f: + f.write(comment) + yaml.dump(yaml_data, f) + print(f"Saved the camera calibration to {self.output_calib_file}", flush=True) + + def load_yaml(self, yaml_file: str) -> Tuple[np.ndarray, np.ndarray]: + """ + Load the camera calibration from a yaml file. + + Parameters + ---------- + yaml_file : str + The yaml file to load. + + Returns + ------- + Tuple[np.ndarray, np.ndarray] + The rotation matrix and translation vector. + """ + # Load the yaml file + with open(yaml_file, "r", encoding="utf-8") as f: + yaml_data = yaml.safe_load(f) + x = yaml_data["publish_camera_extrinsics"]["ros__parameters"]["x"] + y = yaml_data["publish_camera_extrinsics"]["ros__parameters"]["y"] + z = yaml_data["publish_camera_extrinsics"]["ros__parameters"]["z"] + roll = yaml_data["publish_camera_extrinsics"]["ros__parameters"]["roll"] + pitch = yaml_data["publish_camera_extrinsics"]["ros__parameters"]["pitch"] + yaw = yaml_data["publish_camera_extrinsics"]["ros__parameters"]["yaw"] + + # Create the rotation matrix and translation vector + R_cam2gripper = R.from_euler("ZYX", [yaw, pitch, roll]).as_matrix() + t_cam2gripper = np.array([x, y, z]).reshape((3, 1)) + return R_cam2gripper, t_cam2gripper + + def show_img(self, rate_hz: float = 10.0): + """ + Show the latest RGB image. + """ + if not self.run_node_online: + return + # Configuration for the timeout + rate = self.create_rate(rate_hz) + try: + cv2.namedWindow("RGB Image", cv2.WINDOW_NORMAL) + while rclpy.ok(): + with self.latest_img_lock: + img = self.latest_annotated_img + if img is not None: + cv2.imshow("RGB Image", img) + cv2.waitKey(int(1000 // rate_hz)) + rate.sleep() + self.destroy_rate(rate) + except (KeyboardInterrupt, rclpy.exceptions.ROSInterruptException): + pass + + +def spin(node: Node, executor: rclpy.executors.Executor): + """ + Spin the node in the background. + + Parameters + ---------- + node : Node + The node to spin. + executor : rclpy.executors.Executor + The executor to spin. + """ + try: + rclpy.spin(node, executor) + except rclpy.executors.ExternalShutdownException: + pass + + +def main(): + """ + Initialize and execute the node. + """ + # Initialize the node + rclpy.init() + node = CalibrateCameraNode() + print("Node created.", flush=True) + + # Spin in the background, as the node initializes + executor = MultiThreadedExecutor(num_threads=4) + spin_thread = threading.Thread( + target=spin, + args=(node,), + kwargs={"executor": executor}, + daemon=True, + ) + spin_thread.start() + + # Run the node + print("Initializing node", flush=True) + initialized = node.initialize() + show_img_thread = None + if initialized: + print("Node initialized", flush=True) + # Show the image stream in the background + show_img_thread = threading.Thread( + target=node.show_img, + daemon=True, + ) + show_img_thread.start() + + # Run the node in the main thread + try: + node.run() + except KeyboardInterrupt: + zero_twist = TwistStamped() + zero_twist.header.frame_id = node.moveit2.base_link_name + node.twist_pub.publish(zero_twist) + + # Cleanly terminate the node + print("Terminating node", flush=True) + node.destroy_node() + try: + rclpy.shutdown() + except rclpy._rclpy_pybind11.RCLError: # pylint: disable=protected-access + pass + if show_img_thread: + show_img_thread.join() + spin_thread.join() + print("Terminated", flush=True) + + +if __name__ == "__main__": + main() diff --git a/ada_calibrate_camera/ada_calibrate_camera/camera_calibration.py b/ada_calibrate_camera/ada_calibrate_camera/camera_calibration.py new file mode 100644 index 0000000..4b312f2 --- /dev/null +++ b/ada_calibrate_camera/ada_calibrate_camera/camera_calibration.py @@ -0,0 +1,514 @@ +""" +This module defines the CameraCalibration class, which allows users to add samples +consisting of an RGB image, a gripper2base transform, and a target2cam transform. +It saves the data in the specified data directory, and computes the cam2gripper +transform. It also computes the transformation error. +""" + +# Standard imports +from datetime import datetime +import os +from typing import List, Optional, Tuple, Union + +# Third-party imports +import cv2 +from geometry_msgs.msg import PoseStamped +import numpy as np +from scipy.spatial.transform import Rotation as R + +# Local imports +from ada_calibrate_camera.helpers import pose_to_rot_trans + +# pylint: disable=invalid-name +# Although names like R_gripper2base are not snake case, they align with OpenCV +# conventions. + + +class CameraCalibration: + """ + This class performs camera calibration. It allows users to add samples, which + consist of an RGB image, a gripper2base transform, and a target2cam transform. + It saves the data in the specified data directory, and computes the cam2gripper + transform. It also computes the transformation error. + """ + + # pylint: disable=too-many-instance-attributes + # One over is okay. + + # pylint: disable=dangerous-default-value + def __init__( + self, + data_dir: Optional[str] = None, + methods: List[int] = [cv2.CALIB_HAND_EYE_ANDREFF], + ): + """ + Initialize the CameraCalibration. + + Parameters + ---------- + data_dir : Optional[str] + The directory to save the data. + methods : List[int] + The hand-eye calibration methods. + """ + # Saving data in a folder corresponding to the datetime: YYYY_MM_DD_HH_MM_SS + self.data_dir = None + if data_dir is not None: + folder_name = datetime.now().strftime("%Y_%m_%d_%H_%M_%S") + self.data_dir = os.path.join(data_dir, folder_name) + print(f"Saving data to: {self.data_dir}", flush=True) + os.makedirs(self.data_dir, exist_ok=True) + self.sample_i = 0 + + # Hand-eye calibration method + self.methods = methods + + # Tracking samples + self.rgb_images = [] + self.Rs_gripper2base = [] + self.ts_gripper2base = [] + self.Rs_target2cam = [] + self.ts_target2cam = [] + + def add_sample( + self, + rgb_img: np.ndarray, + gripper2base: Union[PoseStamped, Tuple[np.ndarray, np.ndarray]], + target2cam: Union[PoseStamped, Tuple[np.ndarray, np.ndarray]], + save_data: bool = True, + ): + """ + Add a sample to the camera calibration. + + Parameters + ---------- + rgb_img : np.ndarray + The RGB image. + gripper2base : Union[PoseStamped, Tuple[np.ndarray, np.ndarray]] + The gripper2base transform. + target2cam : Union[PoseStamped, Tuple[np.ndarray, np.ndarray]] + The target2cam transform. + save_data : bool + Whether to save the data. + """ + if isinstance(gripper2base, PoseStamped): + R_gripper2base, t_gripper2base = pose_to_rot_trans( + gripper2base, rot_vec=False + ) + else: + R_gripper2base, t_gripper2base = gripper2base + if np.prod(R_gripper2base.shape) == 3: + R_gripper2base = R_gripper2base.reshape((3,)) + if np.prod(t_gripper2base.shape) == 3: + t_gripper2base = t_gripper2base.reshape((3,)) + if isinstance(target2cam, PoseStamped): + R_target2cam, t_target2cam = pose_to_rot_trans(target2cam, rot_vec=False) + else: + R_target2cam, t_target2cam = target2cam + if np.prod(R_target2cam.shape) == 3: + R_target2cam = R_target2cam.reshape((3,)) + if np.prod(t_target2cam.shape) == 3: + t_target2cam = t_target2cam.reshape((3,)) + self.rgb_images.append(rgb_img) + self.Rs_gripper2base.append(R_gripper2base) + self.ts_gripper2base.append(t_gripper2base) + self.Rs_target2cam.append(R_target2cam) + self.ts_target2cam.append(t_target2cam) + + # Save the data + if save_data and self.data_dir is not None: + self.save_sample( + self.data_dir, + self.sample_i, + rgb_img, + R_gripper2base, + t_gripper2base, + R_target2cam, + t_target2cam, + ) + self.sample_i += 1 + + def add_samples_from_folder( + self, + data_dir: str, + exclude_samples: Optional[List[int]] = None, + ): + """ + Add samples from a folder. + + Parameters + ---------- + data_dir : str + The directory to load the data from. + exclude_samples : Optional[List[int]] + The samples to exclude. + """ + if not os.path.exists(data_dir): + print(f"Data directory {data_dir} does not exist.", flush=True) + return + if exclude_samples is None: + exclude_samples = [] + + # Load the samples + sample_i = -1 + while True: + sample_i += 1 + if sample_i in exclude_samples: + continue + if not os.path.exists(os.path.join(data_dir, f"{sample_i}_rgb_img.png")): + break + ( + rgb_img, + R_gripper2base, + t_gripper2base, + R_target2cam, + t_target2cam, + ) = self.load_sample(data_dir, sample_i) + self.rgb_images.append(rgb_img) + self.Rs_gripper2base.append(R_gripper2base) + self.ts_gripper2base.append(t_gripper2base) + self.Rs_target2cam.append(R_target2cam) + self.ts_target2cam.append(t_target2cam) + + @staticmethod + def save_sample( + data_dir: str, + sample_i: int, + rgb_img: np.ndarray, + R_gripper2base: np.ndarray, + t_gripper2base: np.ndarray, + R_target2cam: np.ndarray, + t_target2cam: np.ndarray, + ) -> None: + """ + Save the sample to the data directory. + + Parameters + ---------- + data_dir : str + The directory to save the data. + sample_i : int + The sample index. + rgb_img : np.ndarray + The RGB image. + R_gripper2base : np.ndarray + The gripper2base rotation vector. + t_gripper2base : np.ndarray + The gripper2base translation vector. + R_target2cam : np.ndarray + The target2cam rotation vector. + t_target2cam : np.ndarray + The target2cam translation vector. + """ + # pylint: disable=too-many-arguments + # Necessay to get all the data for this sample. + cv2.imwrite(os.path.join(data_dir, f"{sample_i}_rgb_img.png"), rgb_img) + np.savez_compressed( + os.path.join(data_dir, f"{sample_i}_sample.npz"), + R_gripper2base=R_gripper2base, + t_gripper2base=t_gripper2base, + R_target2cam=R_target2cam, + t_target2cam=t_target2cam, + ) + + @staticmethod + def load_sample( + data_dir: str, + sample_i: int, + ) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + """ + Load the sample from the data directory. + + Parameters + ---------- + data_dir : str + The directory to save the data. + sample_i : int + The sample index. + + Returns + ------- + np.ndarray + The RGB image. + np.ndarray + The gripper2base rotation vector. + np.ndarray + The gripper2base translation vector. + np.ndarray + The target2cam rotation vector. + np.ndarray + The target2cam translation vector. + """ + rgb_img = cv2.imread(os.path.join(data_dir, f"{sample_i}_rgb_img.png")) + data = np.load(os.path.join(data_dir, f"{sample_i}_sample.npz")) + return ( + rgb_img, + data["R_gripper2base"], + data["t_gripper2base"], + data["R_target2cam"], + data["t_target2cam"], + ) + + @staticmethod + def translation_error( + t1: np.ndarray, + t2: np.ndarray, + ) -> float: + """ + Compute the translation error between two translation vectors. + + Parameters + ---------- + t1 : np.ndarray + The first translation vector. + t2 : np.ndarray + The second translation vector. + + Returns + ------- + float + The translation error. + """ + return np.linalg.norm(t1 - t2) + + @staticmethod + def rotation_error( + R1: np.ndarray, + R2: np.ndarray, + ) -> float: + """ + Compute the rotation error between two rotation matrices. + + Parameters + ---------- + R1 : np.ndarray + The first rotation matrix. + R2 : np.ndarray + The second rotation matrix. + + Returns + ------- + float + The rotation error. + """ + return (R.from_matrix(R1).inv() * R.from_matrix(R2)).magnitude() + + def compute_calibration( + self, + save_data: bool = False, + ref_R_cam2gripper: Optional[np.ndarray] = None, + ref_t_cam2gripper: Optional[np.ndarray] = None, + ref_translation_error_threshold: Optional[float] = None, + ref_rotation_error_threshold: Optional[float] = None, + verbose: bool = False, + ) -> Tuple[ + Optional[np.ndarray], + Optional[np.ndarray], + Optional[float], + Optional[float], + Optional[int], + ]: + """ + Compute the camera calibration and return the rotation and translation + errors. For the rotation and translation errors, we compute the pose of the + target in the base frame across all samples. In theory, this should be the + same across all samples. We then take the pairwise translation and rotation + distance between the samples, and average them. + + References: + - Eq 27A and 28A here: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC9581431/pdf/pone.0273261.pdf + - The derivation of AX=XB for eye-in-hand calibration from: + https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#gaebfc1c9f7434196a374c382abf43439b + + Parameters + ---------- + save_data : bool + Whether to save the data. + ref_R_cam2gripper : Optional[np.ndarray] + The reference rotation matrix from the camera to the gripper. + ref_t_cam2gripper : Optional[np.ndarray] + The reference translation vector from the camera to the gripper. + ref_translation_error_threshold : Optional[float] + The reference translation error threshold. If the translation error is + greater than this threshold, the calibration is considered invalid. + ref_rotation_error_threshold : Optional[float] + The reference rotation error threshold. If the rotation error is greater + than this threshold, the calibration is considered invalid. + verbose : bool + Whether to print the calibration results. + + Returns + ------- + np.ndarray + The rotation matrix from the camera to the gripper. + np.ndarray + The translation vector from the camera to the gripper. + float + The rotation error. + float + The translation error. + int + The calibration method that got the least error. + """ + # pylint: disable=too-many-locals, too-many-branches, too-many-statements, too-many-arguments + # The comaprison to the reference data requires many arguments. + + if len(self.Rs_gripper2base) < 3: + print("Need at least 3 samples to calibrate the camera.", flush=True) + return None, None, None, None, None + + # Check if we have reference data + check_reference = False + if ( + ref_R_cam2gripper is not None + and ref_t_cam2gripper is not None + and ( + ref_translation_error_threshold is not None + or ref_rotation_error_threshold is not None + ) + ): + check_reference = True + + # Compute the camera extrinsics calibration + best_R_cam2gripper = None + best_t_cam2gripper = None + best_translation_error = np.inf + best_rotation_error = np.inf + best_method = None + for method in self.methods: + R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye( + self.Rs_gripper2base, + self.ts_gripper2base, + self.Rs_target2cam, + self.ts_target2cam, + method=method, + ) + + # Check if the calibration is valid + if check_reference: + translation_error = CameraCalibration.translation_error( + ref_t_cam2gripper, t_cam2gripper + ) + rotation_error = CameraCalibration.rotation_error( + ref_R_cam2gripper, R_cam2gripper + ) + if ( + ref_translation_error_threshold is not None + and translation_error > ref_translation_error_threshold + ): + print( + ( + f"WARNING: Translation error {translation_error} is greater than the threshold " + f"{ref_translation_error_threshold}. Rejecting." + ), + flush=True, + ) + continue + if ( + ref_rotation_error_threshold is not None + and rotation_error > ref_rotation_error_threshold + ): + print( + ( + f"WARNING: Rotation error {rotation_error} is greater than the threshold " + f"{ref_rotation_error_threshold}. Rejecting." + ), + flush=True, + ) + continue + + # Convert to a homogenous transform + T_cam2gripper = np.eye(4) + T_cam2gripper[:3, :3] = R_cam2gripper + T_cam2gripper[:3, 3] = t_cam2gripper.reshape((3,)) + + # For each sample, get the target's pose is base frame + Rs_target2base = [] + ts_target2base = [] + # pylint: disable=consider-using-enumerate + for i in range(len(self.Rs_target2cam)): + # Get the homogenous transform from the gripper to the base + T_gripper2base = np.eye(4) + if self.Rs_gripper2base[i].shape == (3,): + T_gripper2base[:3, :3] = R.from_rotvec( + self.Rs_gripper2base[i] + ).as_matrix() + else: + T_gripper2base[:3, :3] = self.Rs_gripper2base[i] + T_gripper2base[:3, 3] = self.ts_gripper2base[i] + + # Get the homogenous transform from the target to the camera + T_target2cam = np.eye(4) + if self.Rs_target2cam[i].shape == (3,): + T_target2cam[:3, :3] = R.from_rotvec( + self.Rs_target2cam[i] + ).as_matrix() + else: + T_target2cam[:3, :3] = self.Rs_target2cam[i] + T_target2cam[:3, 3] = self.ts_target2cam[i] + + # Compute the homogenous transform from the target to the base + T_target2base = T_gripper2base @ T_cam2gripper @ T_target2cam + + # Extract the rotation and translation + Rs_target2base.append(R.from_matrix(T_target2base[:3, :3])) + ts_target2base.append(T_target2base[:3, 3]) + + # Compute the translation and rotation errors + translation_errors = [] + rotation_errors = [] + for i in range( + len(Rs_target2base) + ): # pylint: disable=consider-using-enumerate + for j in range(i + 1, len(Rs_target2base)): + translation_errors.append( + CameraCalibration.translation_error( + ts_target2base[i], ts_target2base[j] + ) + ) + rotation_errors.append( + CameraCalibration.rotation_error( + Rs_target2base[i].as_matrix(), Rs_target2base[j].as_matrix() + ) + ) + + # Average the errors + translation_error = np.percentile(translation_errors, 50) + rotation_error = np.percentile(rotation_errors, 50) + + # Print the calibration results + if verbose: + print(f"Method: {method}", flush=True) + print(f"Translation error: {translation_error}", flush=True) + print(f"Rotation error: {rotation_error}", flush=True) + print( + f"R_cam2gripper: {R.from_matrix(R_cam2gripper).as_euler('ZYX')}", + flush=True, + ) + print(f"t_cam2gripper: {t_cam2gripper}", flush=True) + + # Save the best calibration + if translation_error < best_translation_error: + best_translation_error = translation_error + best_rotation_error = rotation_error + best_R_cam2gripper = R_cam2gripper + best_t_cam2gripper = t_cam2gripper + best_method = method + + # Save the calibration + if save_data and self.data_dir is not None and best_R_cam2gripper is not None: + np.savez_compressed( + os.path.join(self.data_dir, f"{self.sample_i}_calib.npz"), + R_cam2gripper=best_R_cam2gripper, + t_cam2gripper=best_t_cam2gripper, + translation_error=best_translation_error, + rotation_error=best_rotation_error, + ) + + return ( + best_R_cam2gripper, + best_t_cam2gripper, + best_rotation_error, + best_translation_error, + best_method, + ) diff --git a/ada_calibrate_camera/ada_calibrate_camera/charuco_detector.py b/ada_calibrate_camera/ada_calibrate_camera/charuco_detector.py new file mode 100644 index 0000000..5f05cbe --- /dev/null +++ b/ada_calibrate_camera/ada_calibrate_camera/charuco_detector.py @@ -0,0 +1,176 @@ +""" +This module defines the CharucoDetector class, which is used to detect a charucoboard in an image. +""" + +# Standard imports +import copy +from typing import Optional, Tuple + +# Third-party imports +import cv2 +import numpy as np + +# Local imports + + +class CharucoDetector: + """ + This class returns the pose of a charucoboard in an image. + """ + + def __init__( + self, + n_rows: int, + n_cols: int, + sq_length_m: float, + marker_length_m: float, + predefined_dictionary: int, + ): + """ + Initialize the CharucoDetector. + + Parameters + ---------- + n_rows : int + The number of rows in the charucoboard. + n_cols : int + The number of columns in the charucoboard. + sq_length_m : float + The length of a square in the charucoboard. + marker_length_m : float + The length of a marker in the charucoboard. + predefined_dictionary : int + The predefined dictionary to use, e.g., cv2.aruco.DICT_4X4_50. + """ + # pylint: disable=too-many-arguments + # This is meant to be a flexible class, hence the many arguments. + self.camera_matrix = None + self.dist_coeffs = None + self.board = cv2.aruco.CharucoBoard_create( + squaresX=n_cols, + squaresY=n_rows, + squareLength=sq_length_m, + markerLength=marker_length_m, + dictionary=cv2.aruco.getPredefinedDictionary(predefined_dictionary), + ) + self.detector_params = cv2.aruco.DetectorParameters_create() + + def set_camera_intrinsics(self, camera_matrix: np.ndarray, dist_coeffs: np.ndarray): + """ + Set the camera intrinsics. + + Parameters + ---------- + camera_matrix : np.ndarray + The camera matrix. + dist_coeffs : np.ndarray + The distortion coefficients. + """ + self.camera_matrix = camera_matrix + self.dist_coeffs = dist_coeffs + + def got_camera_intrinsics(self) -> bool: + """ + Check if the camera intrinsics have been set. + + Returns + ------- + bool + Whether the camera intrinsics have been set. + """ + return self.camera_matrix is not None + + def detect( + self, img: np.ndarray, viz: bool = False, verbose: bool = False + ) -> Tuple[bool, Optional[np.ndarray], Optional[np.ndarray], Optional[np.ndarray]]: + """ + Detect the charucoboard in the image. + + Parameters + ---------- + img : np.ndarray + The image. + viz : bool + Whether to visualize the detection. + verbose : bool + Whether to print verbose output. + + Returns + ------- + bool + Whether the charucoboard was detected. + np.ndarray + The rotation vector for the charucoboard in the camera's frame. + np.ndarray + The translation vector for the charucoboard in the camera's frame. + np.ndarray + if viz is True, the image with the charucoboard drawn on it. + + """ + # pylint: disable=too-many-return-statements + # This is meant to be a flexible function, hence the many return statements. + if self.camera_matrix is None: + print("Camera intrinsics not set.", flush=True) + return False, None, None, None + + marker_corners, marker_ids, _ = cv2.aruco.detectMarkers( + img, + self.board.dictionary, + parameters=self.detector_params, + cameraMatrix=self.camera_matrix, + distCoeff=self.dist_coeffs, + ) + if marker_ids is None or len(marker_ids) == 0: + if verbose: + print("Failed to detect the markers.", flush=True) + return False, None, None, None + _, charuco_corners, charuco_ids = cv2.aruco.interpolateCornersCharuco( + marker_corners, + marker_ids, + img, + self.board, + cameraMatrix=self.camera_matrix, + distCoeffs=self.dist_coeffs, + ) + if charuco_corners is None or len(charuco_corners) == 0: + if verbose: + print("Failed to interpolate the charucoboard corners.", flush=True) + return False, None, None, None + if viz: + retval_img = copy.deepcopy(img) + retval_img = cv2.aruco.drawDetectedMarkers( + retval_img, marker_corners, marker_ids, (0, 255, 0) + ) + retval_img = cv2.aruco.drawDetectedCornersCharuco( + retval_img, charuco_corners, charuco_ids, (255, 0, 0) + ) + if len(charuco_ids) >= 4: + rvec = np.zeros((3, 1)) + tvec = np.zeros((3, 1)) + succ, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard( + charuco_corners, + charuco_ids, + self.board, + self.camera_matrix, + self.dist_coeffs, + rvec, + tvec, + ) + if not succ: + if verbose: + print("Failed to get charucoboard pose.", flush=True) + return False, None, None, retval_img + if viz: + cv2.drawFrameAxes( + retval_img, + self.camera_matrix, + self.dist_coeffs, + rvec, + tvec, + 0.1, + ) + return True, rvec, tvec, retval_img + return True, rvec, tvec, None + if verbose: + print("Failed to detect the charucoboard.", flush=True) + return False, None, None, None diff --git a/ada_calibrate_camera/ada_calibrate_camera/helpers.py b/ada_calibrate_camera/ada_calibrate_camera/helpers.py new file mode 100644 index 0000000..1730854 --- /dev/null +++ b/ada_calibrate_camera/ada_calibrate_camera/helpers.py @@ -0,0 +1,172 @@ +""" +This module contains helper functions for calibrating the camera. +""" + +# Standard imports +from typing import Tuple, Optional + +# Third-party imports +import numpy as np +from geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TwistStamped, + Vector3, +) +from scipy.spatial.transform import Rotation as R +import ros2_numpy + + +def pose_to_rot_trans( + pose: PoseStamped, rot_vec: bool = True +) -> Tuple[np.ndarray, np.ndarray]: + """ + Disaggregates a into its rotation and translation components. + + Parameters + ---------- + pose : PoseStamped + The pose. + rot_vec : bool + Whether to return the rotation as a rotation vector or a matrix + + Returns + ------- + np.ndarray + The rotation vector. + np.ndarray + The translation vector. + """ + if rot_vec: + rot = R.from_quat(ros2_numpy.numpify(pose.pose.orientation)).as_rotvec() + else: + rot = R.from_quat(ros2_numpy.numpify(pose.pose.orientation)).as_matrix() + trans = ros2_numpy.numpify(pose.pose.position) + return rot, trans + + +def pose_to_matrix(pose: Pose) -> np.ndarray: + """ + Convert a Pose message to a homogenous transformation matrix. + + Parameters + ---------- + pose : Pose + The pose. + + Returns + ------- + np.ndarray + The homogenous transformation matrix. + """ + M = np.eye(4) + M[:3, :3] = R.from_quat(ros2_numpy.numpify(pose.orientation)).as_matrix() + M[:3, 3] = ros2_numpy.numpify(pose.position) + return M + + +def transform_to_matrix(transform: Transform) -> np.ndarray: + """ + Convert a Transform message to a homogenous transformation matrix. + + Parameters + ---------- + transform : Transform + The transform. + + Returns + ------- + np.ndarray + The homogenous transformation matrix. + """ + M = np.eye(4) + M[:3, :3] = R.from_quat(ros2_numpy.numpify(transform.rotation)).as_matrix() + M[:3, 3] = ros2_numpy.numpify(transform.translation) + return M + + +def matrix_to_pose(M: np.ndarray) -> Pose: + """ + Convert a homogenous transformation matrix to a Pose message. + + Parameters + ---------- + M : np.ndarray + The homogenous transformation matrix. + + Returns + ------- + Pose + The pose. + """ + pose = Pose() + pose.position = ros2_numpy.msgify( + Point, + M[:3, 3], + ) + pose.orientation = ros2_numpy.msgify( + Quaternion, + R.from_matrix(M[:3, :3]).as_quat(), + ) + return pose + + +def pose_to_twist( + pose_stamped: PoseStamped, + max_linear_speed: float = 0.1, # m/s + max_angular_speed: float = 0.5, # rad/s + round_decimals: Optional[int] = 6, + rate_hz: float = 10.0, +) -> TwistStamped: + """ + Convert a PoseStamped message to a TwistStamped message. Essentially, it + returns the lienar and angular velocities to execute for 1/rate_hz sec to + move the pose's frame_id to the pose. Based on + ada_feeding/ada_feeding/behaviors/ros/msgs.py's PoseStampedToTwistStamped behavior. + """ + # pylint: disable=too-many-locals, too-many-arguments + # Okay to provide considerable flexibility. + + # For the linear velocity, normalize the pose's position and multiply + # it by the linear_speed + linear_displacement = ros2_numpy.numpify(pose_stamped.pose.position) + linear_distance = np.linalg.norm(linear_displacement) + linear_speed = min(linear_distance * rate_hz, max_linear_speed) + linear_velocity = linear_displacement / linear_distance * linear_speed + + # Round it + if round_decimals is not None: + linear_velocity = np.round(linear_velocity, round_decimals) + + # Convert to a msg + linear_msg = ros2_numpy.msgify(Vector3, linear_velocity) + + # For the angular velocity, convert the pose's orientation to a + # rotation vector, normalize it, and multiply it by the angular_speed + angular_displacement = R.from_quat( + ros2_numpy.numpify(pose_stamped.pose.orientation) + ).as_rotvec() + angular_distance = np.linalg.norm(angular_displacement) + angular_speed = min(angular_distance * rate_hz, max_angular_speed) + if angular_distance == 0.0: + angular_velocity = np.zeros(3) + else: + angular_velocity = angular_displacement / angular_distance * angular_speed + + # Round it + if round_decimals is not None: + angular_velocity = np.round(angular_velocity, round_decimals) + + # Convert to a msg + angular_msg = ros2_numpy.msgify(Vector3, angular_velocity) + + # Create the twist stamped message + twist_stamped = TwistStamped() + twist_stamped.header = pose_stamped.header + twist_stamped.twist.linear = linear_msg + twist_stamped.twist.angular = angular_msg + + return twist_stamped diff --git a/ada_calibrate_camera/ada_calibrate_camera/publish_camera_extrinsics_node.py b/ada_calibrate_camera/ada_calibrate_camera/publish_camera_extrinsics_node.py new file mode 100755 index 0000000..7287255 --- /dev/null +++ b/ada_calibrate_camera/ada_calibrate_camera/publish_camera_extrinsics_node.py @@ -0,0 +1,100 @@ +# !/usr/bin/env python3 +""" +This module contains a rclpy ROS2 node that takes in parameters for the translation +and rotation (quaternion) as well as the frame_id's and publishes the requested transform. +""" + +# Standard imports + +# Third-party imports +from geometry_msgs.msg import TransformStamped +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation as R +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +# Local imports + + +class PublishCameraExtrinsics(Node): + """ + This class defines a ROS2 node that takes in parameters for the translation + and rotation (quaternion) as well as the frame_id's and publishes the requested transform. + """ + + def __init__(self): + """ + Initializes the node and sets up the timer to publish the transform + """ + super().__init__("publish_camera_extrinsics") + self.declare_parameters( + namespace="", + parameters=[ + ("frame_id", "j2n6s200_end_effector"), + ("child_frame_id", "camera_color_optical_frame"), + ("x", 0.0), + ("y", 0.0), + ("z", 0.0), + ("roll", 0.0), + ("pitch", 0.0), + ("yaw", 0.0), + ("rate_hz", 10.0), + ], + ) + + self.tf_broadcaster = StaticTransformBroadcaster(self) + + timer_interval = ( + 1.0 / self.get_parameter("rate_hz").get_parameter_value().double_value + ) + self.timer = self.create_timer(timer_interval, self.publish_transform) + + def publish_transform(self): + """ + Publishes the transform from the frame_id to the child_frame_id + """ + transform = TransformStamped() + transform.header.stamp = self.get_clock().now().to_msg() + transform.header.frame_id = ( + self.get_parameter("frame_id").get_parameter_value().string_value + ) + transform.child_frame_id = ( + self.get_parameter("child_frame_id").get_parameter_value().string_value + ) + transform.transform.translation.x = ( + self.get_parameter("x").get_parameter_value().double_value + ) + transform.transform.translation.y = ( + self.get_parameter("y").get_parameter_value().double_value + ) + transform.transform.translation.z = ( + self.get_parameter("z").get_parameter_value().double_value + ) + quat = R.from_euler( + "ZYX", + [ + self.get_parameter("yaw").get_parameter_value().double_value, + self.get_parameter("pitch").get_parameter_value().double_value, + self.get_parameter("roll").get_parameter_value().double_value, + ], + ).as_quat() + transform.transform.rotation.x = quat[0] + transform.transform.rotation.y = quat[1] + transform.transform.rotation.z = quat[2] + transform.transform.rotation.w = quat[3] + + self.tf_broadcaster.sendTransform(transform) + + +def main(): + """ + The main function to create and spin the node + """ + rclpy.init() + node = PublishCameraExtrinsics() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ada_calibrate_camera/calibrate_camera_start.py b/ada_calibrate_camera/calibrate_camera_start.py new file mode 100644 index 0000000..acc474d --- /dev/null +++ b/ada_calibrate_camera/calibrate_camera_start.py @@ -0,0 +1,270 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module starts all the screen sessions to run the ada_feeding demo. +""" + +import asyncio +import argparse +import getpass +import os +import sys + +# pylint: disable=duplicate-code +# This is intentionally similar to start_nano.py + +parser = argparse.ArgumentParser() +parser.add_argument( + "--sim", + default="real", + help=( + "`real` or `mock` (default `real`). `real` executes all commands for running " + "the calibrate camera code on the real robot. `mock` uses a simulated robot." + ), +) +parser.add_argument( + "-t", + "--termination_wait_secs", + default=5, + help="How long (secs) to wait for the code within screens to terminate (default, 5)", +) +parser.add_argument( + "-l", + "--launch_wait_secs", + default=0.1, + help=( + "How long (secs) to wait between running code in screens. Not that " + "too low of a value can result in commands getting rearranged. (default, 0.1)" + ), +) +parser.add_argument( + "-c", + "--close", + action="store_true", + help="If set, only terminate the code in the screens.", +) +parser.add_argument( + "--real_domain_id", + default=42, + type=int, + help=( + "If sim is set to real, export this ROS_DOMAIN_ID before running code in " + "every screen session. (default: 42)" + ), +) + + +async def get_existing_screens(): + """ + Get a list of active screen sessions. + + Adapted from + https://serverfault.com/questions/886405/create-screen-session-in-background-only-if-it-doesnt-already-exist + """ + proc = await asyncio.create_subprocess_shell( + "screen -ls", stdout=asyncio.subprocess.PIPE + ) + stdout, _ = await proc.communicate() + existing_screens = [ + line.split(".")[1].split("\t")[0].rstrip() + for line in stdout.decode("utf-8").splitlines() + if line.startswith("\t") + ] + return existing_screens + + +async def execute_command(screen_name: str, command: str, indent: int = 8) -> None: + """ + Execute a command in a screen. + """ + global sudo_password # pylint: disable=global-statement + indentation = " " * indent + printable_command = command.replace("\003", "SIGINT") + print(f"# {indentation}`{printable_command}`") + if command != "\003": + command += "\n" + await asyncio.create_subprocess_shell( + f"screen -S {screen_name} -X stuff '{command}'" + ) + await asyncio.sleep(args.launch_wait_secs) + if command.startswith("sudo"): + if sudo_password is None: + sudo_password = getpass.getpass( + prompt=f"# {indentation}Enter sudo password: " + ) + await asyncio.create_subprocess_shell( + f"screen -S {screen_name} -X stuff '{sudo_password}\n'" + ) + await asyncio.sleep(args.launch_wait_secs) + elif command.startswith("ssh"): + ssh_password = getpass.getpass(prompt=f"# {indentation}Enter ssh password: ") + await asyncio.create_subprocess_shell( + f"screen -S {screen_name} -X stuff '{ssh_password}\n'" + ) + await asyncio.sleep(args.launch_wait_secs) + + +async def main(args: argparse.Namespace, pwd: str) -> None: + """ + Start the ada_feeding demo. + + Args: + args: The command-line arguments. + pwd: The absolute path to the current directory. + """ + + # pylint: disable=too-many-branches, too-many-statements + # This is meant to be a flexible function, hence the many branches and statements. + # pylint: disable=redefined-outer-name + # That is okay in this case. + + print( + "################################################################################" + ) + if not args.close: + print("# Starting the calibrate_camera code") + print("# Prerequisites / Notes:") + print("# 1. Be in the top-level of your colcon workspace") + print( + "# 2. Your workspace should be built (e.g., `colcon build --symlink-install`)" + ) + else: + print("# Terminating the calibrate_camera code") + print( + "################################################################################" + ) + + # Determine which screen sessions to start and what commands to run + screen_sessions = { + "ft": [ + "ros2 run forque_sensor_hardware forque_sensor_hardware --ros-args -p host:=ft-sensor-2", + ], + "camera": [ + "ssh nano -t './start_nano.sh'", + ], + "receiver": [ + "ros2 launch nano_bridge receiver.launch.xml", + ], + "moveit": [ + f"ros2 launch ada_moveit demo.launch.py use_rviz:=true sim:={args.sim} use_octomap:=false" + ], + "calibrate": [ + "ros2 run ada_calibrate_camera calibrate_camera --ros-args " + f"--params-file {os.path.join(pwd, 'src/ada_ros2/ada_calibrate_camera/config/calibrate_camera.yaml')}" + ], + } + close_commands = {} + initial_close_commands = ["\003"] + initial_start_commands = [ + f"cd {pwd}", + "source install/setup.bash", + f"export ROS_DOMAIN_ID={args.real_domain_id}", + ] + for screen_name, commands in screen_sessions.items(): + screen_sessions[screen_name] = initial_start_commands + commands + if screen_name not in close_commands: + close_commands[screen_name] = [] + close_commands[screen_name] = ( + initial_close_commands + close_commands[screen_name] + ) + + # Determine which screens are already running + print("# Checking for existing screen sessions") + terminated_screen = False + existing_screens = await get_existing_screens() + for screen_name in screen_sessions: + if screen_name in existing_screens: + print(f"# Found session `{screen_name}`: ") + for command in close_commands[screen_name]: + await execute_command(screen_name, command) + terminated_screen = True + elif not args.close: + print(f"# Creating session `{screen_name}`") + await asyncio.create_subprocess_shell(f"screen -dmS {screen_name}") + await asyncio.sleep(args.launch_wait_secs) + + print( + "################################################################################" + ) + + if not args.close: + # Sleep for a bit to allow the screens to terminate + if terminated_screen: + print(f"# Waiting {args.termination_wait_secs} secs for code to terminate") + await asyncio.sleep(args.termination_wait_secs) + print( + "################################################################################" + ) + + # Start the screen sessions + print("# Starting robot feeding code") + for screen_name, commands in screen_sessions.items(): + print(f"# `{screen_name}`") + for command in commands: + await execute_command(screen_name, command) + print( + "################################################################################" + ) + + print("# Done! Next steps:") + print( + "# 1. Check individual screens to verify code is working as expected." + ) + print("# 2. `screen -r calibrate` to actually run calibration.") + + +def check_pwd_is_colcon_workspace() -> str: + """ + Check that the script is being run from the top-level colcon workspace. + Return the absolute path to the current directory. + """ + # The below are a smattering of directories and files that should exist in the + # top-level colcon workspace. + dirs_to_check = ["src", "build", "install", "log"] + files_to_check = [ + "src/feeding_web_interface/feedingwebapp/server.js", + "src/feeding_web_interface/feedingwebapp/.env", + "src/feeding_web_interface/feedingwebapp/start_robot_browser.js", + "src/ada_feeding/ada_feeding_perception/config/republisher.yaml", + ] + for dir_to_check in dirs_to_check: + if not os.path.isdir(dir_to_check): + print( + f"ERROR: This script must be run from the top-level colcon workspace. Could not find `{dir_to_check}`", + file=sys.stderr, + ) + sys.exit(1) + for file_to_check in files_to_check: + if not os.path.isfile(file_to_check): + print( + f"ERROR: This script must be run from the top-level colcon workspace. Could not find `{file_to_check}`", + file=sys.stderr, + ) + sys.exit(1) + + # Return the absolute path to the current directory + return os.path.abspath(".") + + +if __name__ == "__main__": + # Get the arguments + args = parser.parse_args() + + # Ensure the script is not being run as sudo. Sudo has a different screen + # server and may have different versions of libraries installed. + if os.geteuid() == 0: + print( + "ERROR: This script should not be run as sudo. Run as a regular user.", + file=sys.stderr, + ) + sys.exit(1) + + # Check that the script is being run from the top-level colcon workspace + pwd = check_pwd_is_colcon_workspace() + + # Run the main function + sudo_password = None # pylint: disable=invalid-name + asyncio.run(main(args, pwd)) + + # Return success + sys.exit(0) diff --git a/ada_calibrate_camera/config/calibrate_camera.yaml b/ada_calibrate_camera/config/calibrate_camera.yaml new file mode 100644 index 0000000..d0b91c1 --- /dev/null +++ b/ada_calibrate_camera/config/calibrate_camera.yaml @@ -0,0 +1,32 @@ +calibrate_camera: + ros__parameters: + # Frame names + robot_end_effector_frame: "forkTip" + extrinsics_base_frame: "j2n6s200_end_effector" + + # Whether to run camera calibration online or not + run_online: true + # Where to save/load calibration data. + # NOTE: Although it is not good practice to hardcode paths, the only way to avoid this is to + # save the files to the built package's share directory, which hinders pushing it to Github. + data_dir: "~/Workspaces/ada_ws/src/ada_ros2/ada_calibrate_camera/data" + # Where to save the calibration results + output_calib_name: ~/Workspace/ada_ws/src/ada_ros2/ada_calibrate_camera/config/calibs/auto.yaml + # A calibration file to serve as reference. If the translation or rotation error from the reference + # calibration is higher than thresholds, reject the calibration. + reference_calib_name: ~/Workspace/ada_ws/src/ada_ros2/ada_calibrate_camera/config/calibs/manual.yaml + reference_translation_error_threshold: 0.03 # meters + reference_rotation_error_threshold: 0.15 # radians + # If run_online is false, this parameter specifies the folder to load data from + offline_data_dir: "2024_10_16_17_16_32" + + # Charuco board parameters + charuco_n_rows: 9 + charuco_n_cols: 14 + charuco_sq_length_m: 0.02 # 0.0351 + charuco_marker_length_m: 0.015 # 0.0257 + charuco_predefined_dictionary: 5 # https://docs.opencv.org/4.x/de/d67/group__objdetect__aruco.html#ga4e13135a118f497c6172311d601ce00d + + # Hand-eye calibration method. Options: "all", "tsai", "park", "horaud", "andreff", "daniilidis" + # More details: https://docs.opencv.org/4.5.4/d9/d0c/group__calib3d.html#gad10a5ef12ee3499a0774c7904a801b99 + hand_eye_calibration_method: "park" diff --git a/ada_calibrate_camera/config/calibs/auto.yaml b/ada_calibrate_camera/config/calibs/auto.yaml new file mode 100644 index 0000000..23407ac --- /dev/null +++ b/ada_calibrate_camera/config/calibs/auto.yaml @@ -0,0 +1,14 @@ +# This file was auto-generated by the ada_calibrate_camera node. +# It used the following hand-eye calibration method: 'park'. +# The rotation error is 0.016407379903910026 and the translation error is 0.009963572232739004. +# See the README for more details. +publish_camera_extrinsics: + ros__parameters: + child_frame_id: camera_color_optical_frame + frame_id: j2n6s200_end_effector + pitch: -0.005679467602140109 + roll: -0.24264555089529039 + x: 0.04298988343257808 + y: 0.13152553261843783 + yaw: 3.057148318374235 + z: -0.14201903964568033 diff --git a/ada_calibrate_camera/config/calibs/manual.yaml b/ada_calibrate_camera/config/calibs/manual.yaml new file mode 100644 index 0000000..1b4dc97 --- /dev/null +++ b/ada_calibrate_camera/config/calibs/manual.yaml @@ -0,0 +1,14 @@ + + + + +publish_camera_extrinsics: + ros__parameters: + child_frame_id: camera_color_optical_frame + frame_id: j2n6s200_end_effector + pitch: 0.00 + roll: -0.2385 + x: 0.032 + y: 0.127 + yaw: -3.08 + z: -0.159 diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/0_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/0_rgb_img.png new file mode 100644 index 0000000..46883c2 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/0_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/0_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/0_sample.npz new file mode 100644 index 0000000..a3ae395 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/0_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/10_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/10_rgb_img.png new file mode 100644 index 0000000..6877033 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/10_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/10_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/10_sample.npz new file mode 100644 index 0000000..f704645 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/10_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/11_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/11_rgb_img.png new file mode 100644 index 0000000..aeec90b Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/11_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/11_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/11_sample.npz new file mode 100644 index 0000000..6eda287 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/11_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/12_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/12_rgb_img.png new file mode 100644 index 0000000..fadc1bf Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/12_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/12_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/12_sample.npz new file mode 100644 index 0000000..db58d27 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/12_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/13_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/13_rgb_img.png new file mode 100644 index 0000000..90db7a8 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/13_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/13_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/13_sample.npz new file mode 100644 index 0000000..d0cd193 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/13_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/14_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/14_rgb_img.png new file mode 100644 index 0000000..f5bab60 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/14_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/14_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/14_sample.npz new file mode 100644 index 0000000..c25695f Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/14_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/15_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/15_rgb_img.png new file mode 100644 index 0000000..83c343f Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/15_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/15_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/15_sample.npz new file mode 100644 index 0000000..c94c875 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/15_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/16_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/16_rgb_img.png new file mode 100644 index 0000000..41d0f31 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/16_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/16_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/16_sample.npz new file mode 100644 index 0000000..ef18a6f Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/16_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/17_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/17_rgb_img.png new file mode 100644 index 0000000..3ba424c Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/17_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/17_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/17_sample.npz new file mode 100644 index 0000000..f1199ef Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/17_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/18_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/18_rgb_img.png new file mode 100644 index 0000000..fa4881f Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/18_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/18_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/18_sample.npz new file mode 100644 index 0000000..06f7ea7 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/18_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/19_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/19_rgb_img.png new file mode 100644 index 0000000..c7d5a31 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/19_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/19_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/19_sample.npz new file mode 100644 index 0000000..ce1d9cb Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/19_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/1_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/1_rgb_img.png new file mode 100644 index 0000000..8c6622e Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/1_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/1_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/1_sample.npz new file mode 100644 index 0000000..75fc408 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/1_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/20_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/20_rgb_img.png new file mode 100644 index 0000000..fbac38e Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/20_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/20_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/20_sample.npz new file mode 100644 index 0000000..81b425f Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/20_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/21_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/21_rgb_img.png new file mode 100644 index 0000000..8191323 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/21_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/21_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/21_sample.npz new file mode 100644 index 0000000..907dab1 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/21_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/22_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/22_rgb_img.png new file mode 100644 index 0000000..2ec8946 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/22_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/22_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/22_sample.npz new file mode 100644 index 0000000..5eeec4f Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/22_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/23_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/23_rgb_img.png new file mode 100644 index 0000000..f003c23 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/23_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/23_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/23_sample.npz new file mode 100644 index 0000000..4aa715e Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/23_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/2_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/2_rgb_img.png new file mode 100644 index 0000000..bbbc4ba Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/2_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/2_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/2_sample.npz new file mode 100644 index 0000000..d5c11a5 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/2_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/3_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/3_rgb_img.png new file mode 100644 index 0000000..e639fd5 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/3_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/3_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/3_sample.npz new file mode 100644 index 0000000..a3460d2 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/3_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/4_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/4_rgb_img.png new file mode 100644 index 0000000..0b7ee3e Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/4_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/4_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/4_sample.npz new file mode 100644 index 0000000..4349485 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/4_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/5_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/5_rgb_img.png new file mode 100644 index 0000000..94b1948 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/5_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/5_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/5_sample.npz new file mode 100644 index 0000000..c1fa5b1 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/5_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/6_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/6_rgb_img.png new file mode 100644 index 0000000..eac8e59 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/6_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/6_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/6_sample.npz new file mode 100644 index 0000000..a0b3170 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/6_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/7_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/7_rgb_img.png new file mode 100644 index 0000000..78c0b77 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/7_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/7_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/7_sample.npz new file mode 100644 index 0000000..ebe3c1b Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/7_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/8_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/8_rgb_img.png new file mode 100644 index 0000000..35a27c8 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/8_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/8_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/8_sample.npz new file mode 100644 index 0000000..5228114 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/8_sample.npz differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/9_rgb_img.png b/ada_calibrate_camera/data/2024_10_16_17_16_32/9_rgb_img.png new file mode 100644 index 0000000..65319aa Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/9_rgb_img.png differ diff --git a/ada_calibrate_camera/data/2024_10_16_17_16_32/9_sample.npz b/ada_calibrate_camera/data/2024_10_16_17_16_32/9_sample.npz new file mode 100644 index 0000000..c8438d6 Binary files /dev/null and b/ada_calibrate_camera/data/2024_10_16_17_16_32/9_sample.npz differ diff --git a/ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml b/ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml new file mode 100644 index 0000000..f9264b3 --- /dev/null +++ b/ada_calibrate_camera/launch/publish_camera_extrinsics_launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/ada_calibrate_camera/package.xml b/ada_calibrate_camera/package.xml new file mode 100644 index 0000000..e39be0e --- /dev/null +++ b/ada_calibrate_camera/package.xml @@ -0,0 +1,22 @@ + + + + ada_calibrate_camera + 0.0.0 + This package contains a script to calibrate ADA's camera. + Amal Nanavati + BSD-3-Clause + + geometry_msgs + moveit_msgs + python3-numpy + python3-opencv + python3-scipy + ros2_numpy + sensor_msgs + std_srvs + + + ament_python + + diff --git a/ada_calibrate_camera/resource/ada_calibrate_camera b/ada_calibrate_camera/resource/ada_calibrate_camera new file mode 100644 index 0000000..e69de29 diff --git a/ada_calibrate_camera/setup.cfg b/ada_calibrate_camera/setup.cfg new file mode 100644 index 0000000..bf9b3ad --- /dev/null +++ b/ada_calibrate_camera/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/ada_calibrate_camera +[install] +install_scripts=$base/lib/ada_calibrate_camera diff --git a/ada_calibrate_camera/setup.py b/ada_calibrate_camera/setup.py new file mode 100644 index 0000000..f5f8347 --- /dev/null +++ b/ada_calibrate_camera/setup.py @@ -0,0 +1,52 @@ +from glob import glob +import os +from setuptools import find_packages, setup + +package_name = "ada_calibrate_camera" + +data_files = [ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + # Include all launch files. + ( + os.path.join("share", package_name, "launch"), + glob(os.path.join("launch", "*launch.[pxy][yma]*")), + ), + # Include all config files. + ( + os.path.join("share", package_name, "config"), + glob(os.path.join("config", "*.yaml")), + ), + ( + os.path.join("share", package_name, "config/calibs"), + glob(os.path.join("config/calibs", "*.yaml")), + ), +] +for path in os.listdir("data"): + if os.path.isdir(os.path.join("data", path)): + data_files.append( + ( + os.path.join("share", package_name, "data", path), + glob(os.path.join("data", path, "*.[pn][np][gz]")), + ) + ) + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=data_files, + install_requires=["setuptools"], + zip_safe=True, + maintainer="Amal Nanavati", + maintainer_email="amaln@cs.washington.edu", + description=("This package contains a script to calibrate ADA's camera."), + license="BSD-3-Clause", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "calibrate_camera = ada_calibrate_camera.calibrate_camera_node:main", + "publish_camera_extrinsics = ada_calibrate_camera.publish_camera_extrinsics_node:main", + ], + }, +) diff --git a/ada_description/urdf/forque/forque.xacro b/ada_description/urdf/forque/forque.xacro index e647603..54161fb 100644 --- a/ada_description/urdf/forque/forque.xacro +++ b/ada_description/urdf/forque/forque.xacro @@ -81,7 +81,8 @@ - + + diff --git a/ada_moveit/CMakeLists.txt b/ada_moveit/CMakeLists.txt index 3037056..3d535ba 100644 --- a/ada_moveit/CMakeLists.txt +++ b/ada_moveit/CMakeLists.txt @@ -16,7 +16,6 @@ install(PROGRAMS install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} PATTERN "setup_assistant.launch" EXCLUDE) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY calib DESTINATION share/${PROJECT_NAME}) install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/ada_moveit/calib/autocalib/calib_camera_pose.launch.py b/ada_moveit/calib/autocalib/calib_camera_pose.launch.py deleted file mode 100644 index ca0c13e..0000000 --- a/ada_moveit/calib/autocalib/calib_camera_pose.launch.py +++ /dev/null @@ -1,48 +0,0 @@ -""" Static transform publisher acquired via MoveIt 2 hand-eye calibration """ -""" EYE-IN-HAND: j2n6s200_end_effector -> camera_color_optical_frame """ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - # Log Level - log_level_da = DeclareLaunchArgument( - "log_level", - default_value="info", - description="Logging level (debug, info, warn, error, fatal)", - ) - log_level = LaunchConfiguration("log_level") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - output="log", - arguments=[ - "--frame-id", - "j2n6s200_link_6", - "--child-frame-id", - "camera_color_optical_frame", - "--x", - "0.1269866", - "--y", - "0.03250718", - "--z", - "-0.00325277", - "--qx", - "-0.6970163", - "--qy", - "0.7051804", - "--qz", - "-0.0876462", - "--qw", - "-0.0959528", - "--ros-args", - "--log-level", - log_level, - ], - ), - ] - return LaunchDescription([log_level_da] + nodes) diff --git a/ada_moveit/calib/calib_target.png b/ada_moveit/calib/calib_target.png deleted file mode 100644 index dfcaeaf..0000000 Binary files a/ada_moveit/calib/calib_target.png and /dev/null differ diff --git a/ada_moveit/calib/default/README.md b/ada_moveit/calib/default/README.md deleted file mode 100644 index 3051f38..0000000 --- a/ada_moveit/calib/default/README.md +++ /dev/null @@ -1,40 +0,0 @@ -# Default Camera Calibration Methodology - -This camera calibration was done in ROS1, using [MoveIt's Hand-Eye Calibration](https://ros-planning.github.io/moveit_tutorials/doc/hand_eye_calibration/hand_eye_calibration_tutorial.html). For the sake of reproduceability, this readme documents the methodology followed to do the camera calibration. - -## Calibrations - -To account for the potential error in measuring the size of the Aruco tags and the space in between tags (e.g., are the tags 0.059m or 0.06m?), we ran calibration twice. -- **Calib1**: All the joint states used in this calibration, the samples measured from HandEyeCalibration, and the ROS1 launchfile with the final transform, can be found in the `calib1` folder. This calibration had a reprojection error of 0.0293236m and 0.0154105rad. -- **Calib2**: We did not save the joint states from this calibration, but all the samples and the launchfile with the final transform can be found in the `calib2` folder. This calibration had a reprojection error of 0.0153083m and 0.00549327rad. - -For both, we used the solver `crigroup/Daniilidis1999`, which achieved the lowest reprojection error. - -## Testing the Calibrations - -We used two visual tests to test the calibration: - -**Test 1**: Turning the robot to point at itself so part of its chassis is visible from the depth camera. In RVIZ, the depth cloud that the camera sees should align with the robot model. (See the below image, which includes the joint states required to re-create this configuration). - -![Screenshot from 2023-08-16 17-26-50](https://github.com/personalrobotics/ada_ros2/assets/8277986/9064820f-0350-4d13-a1c9-4ce79c49155c) -(Note: my index finger was touching the actual bottom of the fork in the real-world.) - -**Test 2**: Touching the fork to the plate on the table. In RVIZ, the tip of the fork should touch the bottom of the plate as detected by the depth cloud.(See the below image, which includes the joint states required to re-create this configuration). Note that errors in this test could either be due to camera miscalibration or because the actual fork does not align wiht the URDF (e.g., it got bent). - -![Screenshot from 2023-08-16 17-29-47](https://github.com/personalrobotics/ada_ros2/assets/8277986/94f89a37-11f1-48ac-b886-b49b0694597d) - -**Results**: The results of the above test revealed that **Calib1** detected obstacles as being slightly closer to it than they actually were, and **Calib2** detected obstacles slightly farther from it than they actually were. Visually, the distance for each looked to be 0.5-1.0cm. Therefore, **_we decided to average the transforms_**. - -## Transform Calculations - -The script `transform.py` contains the following calculations: -1. Averaging the transforms for **Calib1** and **Calib2** together, and printing the resulting transform from `j2n6s200_end_effector` to `camera_color_optical_frame`. This is the value in `calib_camera_pose.launch.py`. -2. Combining that transform with the fixed transforms between---(a) `camera_link` and `camera_color_optical_frame` and (b) `j2n6s200_end_effector` and `cameraMount`---in order to compute the transform from `cameraMount` to `camera_link`. This is the transform in `ada_description/urdf/ada.xacro`. - -It is a standalone Python script, and can be executed with `python3 /path/to/transform.py`. It requires the following dependency: `python3 -m pip install transformations`. - -## Usage - -There are two ways to use this transform: -1. Run `ros2 launch ada_moveit demo.launch.py` to publish the static transform from `j2n6s200_end_effector` to `camera_color_optical_frame`, and launch the RealSense with the parameter `publish_tf:=false`. The upside of this is that the published transform will likely be more accurate, since it is directly computed from camera calibration. The downside is that no other camera frames (e.g., the depth frame) are published, so we can only use messages in `camera_color_optical_frame`. -2. Run `ros2 launch ada_moveit demo.launch.py calib:=none` to not publish a transform directly to `camera_color_optical_frame`, and launch the RealSense with the parameter `publish_tf:=true` (the default value). This uses the computed transform from `cameraMount` to `camera_link` from the URDF, as well as the RealSense's URDF. The benefit of this is that all camera frames are published, but the downside is that there are more potential sources of error (e.g., if the RealSense URDF is inaccurate, that introduces error with this approach). diff --git a/ada_moveit/calib/default/calib1/camera_calib_pose.launch.py b/ada_moveit/calib/default/calib1/camera_calib_pose.launch.py deleted file mode 100644 index 7b7b8d8..0000000 --- a/ada_moveit/calib/default/calib1/camera_calib_pose.launch.py +++ /dev/null @@ -1,54 +0,0 @@ -""" Static transform publisher acquired via MoveIt 2 hand-eye calibration """ -""" EYE-IN-HAND: j2n6s200_end_effector -> camera_color_optical_frame """ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - # Log Level - log_level_da = DeclareLaunchArgument( - "log_level", - default_value="info", - description="Logging level (debug, info, warn, error, fatal)", - ) - log_level = LaunchConfiguration("log_level") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - output="log", - arguments=[ - "--frame-id", - "j2n6s200_end_effector", - "--child-frame-id", - "camera_color_optical_frame", - "--x", - "0.032072", - "--y", - "0.13256", - "--z", - "-0.155098", - "--qx", - "-0.00145183", - "--qy", - "-0.13495", - "--qz", - "0.990841", - "--qw", - "-0.00447916", - # "--roll", - # "0.270738", - # "--pitch", - # "-0.00166814", - # "--yaw", - # "-3.13232", - "--ros-args", - "--log-level", - log_level, - ], - ), - ] - return LaunchDescription([log_level_da] + nodes) diff --git a/ada_moveit/calib/default/calib1/joint_states_0.yaml b/ada_moveit/calib/default/calib1/joint_states_0.yaml deleted file mode 100644 index 58f7abc..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_0.yaml +++ /dev/null @@ -1,15 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - -2.051182916861679 - - 3.411026258969764 - - 2.634167255291545 - - -1.732889093910352 - - 2.841286885905297 - - -0.9686478145817924 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_1.yaml b/ada_moveit/calib/default/calib1/joint_states_1.yaml deleted file mode 100644 index 2493b96..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_1.yaml +++ /dev/null @@ -1,15 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_10.yaml b/ada_moveit/calib/default/calib1/joint_states_10.yaml deleted file mode 100644 index b13b919..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_10.yaml +++ /dev/null @@ -1,127 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 - - - - -11.97299275714305 - - 3.530480751768614 - - 4.702883625821979 - - -5.414792324991882 - - 4.636997553085637 - - -1.115476074698213 - - - - -11.10763158411932 - - 3.105288441609833 - - 4.22940847141617 - - -5.586650238034384 - - 3.555895031735131 - - -3.532077849474335 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_11.yaml b/ada_moveit/calib/default/calib1/joint_states_11.yaml deleted file mode 100644 index d516aca..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_11.yaml +++ /dev/null @@ -1,134 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 - - - - -11.97299275714305 - - 3.530480751768614 - - 4.702883625821979 - - -5.414792324991882 - - 4.636997553085637 - - -1.115476074698213 - - - - -11.10763158411932 - - 3.105288441609833 - - 4.22940847141617 - - -5.586650238034384 - - 3.555895031735131 - - -3.532077849474335 - - - - -12.04470476108005 - - 2.617623964916082 - - 3.504455808964142 - - -5.050303642667176 - - 3.334481150286488 - - -4.140334787700726 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_12.yaml b/ada_moveit/calib/default/calib1/joint_states_12.yaml deleted file mode 100644 index 285e1d8..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_12.yaml +++ /dev/null @@ -1,141 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 - - - - -11.97299275714305 - - 3.530480751768614 - - 4.702883625821979 - - -5.414792324991882 - - 4.636997553085637 - - -1.115476074698213 - - - - -11.10763158411932 - - 3.105288441609833 - - 4.22940847141617 - - -5.586650238034384 - - 3.555895031735131 - - -3.532077849474335 - - - - -12.04470476108005 - - 2.617623964916082 - - 3.504455808964142 - - -5.050303642667176 - - 3.334481150286488 - - -4.140334787700726 - - - - -12.03383373751035 - - 2.630790633345434 - - 3.536251821850815 - - -5.133372830230873 - - 3.420009633958819 - - -3.234422193390576 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_13.yaml b/ada_moveit/calib/default/calib1/joint_states_13.yaml deleted file mode 100644 index a122dd2..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_13.yaml +++ /dev/null @@ -1,148 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 - - - - -11.97299275714305 - - 3.530480751768614 - - 4.702883625821979 - - -5.414792324991882 - - 4.636997553085637 - - -1.115476074698213 - - - - -11.10763158411932 - - 3.105288441609833 - - 4.22940847141617 - - -5.586650238034384 - - 3.555895031735131 - - -3.532077849474335 - - - - -12.04470476108005 - - 2.617623964916082 - - 3.504455808964142 - - -5.050303642667176 - - 3.334481150286488 - - -4.140334787700726 - - - - -12.03383373751035 - - 2.630790633345434 - - 3.536251821850815 - - -5.133372830230873 - - 3.420009633958819 - - -3.234422193390576 - - - - -15.11307539839477 - - 3.66912198864094 - - 2.779617662438986 - - -7.863898180609501 - - 2.988645714289889 - - -1.709515194812999 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_14.yaml b/ada_moveit/calib/default/calib1/joint_states_14.yaml deleted file mode 100644 index e50b5b2..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_14.yaml +++ /dev/null @@ -1,155 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 - - - - -11.97299275714305 - - 3.530480751768614 - - 4.702883625821979 - - -5.414792324991882 - - 4.636997553085637 - - -1.115476074698213 - - - - -11.10763158411932 - - 3.105288441609833 - - 4.22940847141617 - - -5.586650238034384 - - 3.555895031735131 - - -3.532077849474335 - - - - -12.04470476108005 - - 2.617623964916082 - - 3.504455808964142 - - -5.050303642667176 - - 3.334481150286488 - - -4.140334787700726 - - - - -12.03383373751035 - - 2.630790633345434 - - 3.536251821850815 - - -5.133372830230873 - - 3.420009633958819 - - -3.234422193390576 - - - - -15.11307539839477 - - 3.66912198864094 - - 2.779617662438986 - - -7.863898180609501 - - 2.988645714289889 - - -1.709515194812999 - - - - -11.98412956418955 - - 2.597135201385667 - - 3.475059570535738 - - -4.846107497140059 - - 3.115444939059068 - - -0.1253736429600387 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_2.yaml b/ada_moveit/calib/default/calib1/joint_states_2.yaml deleted file mode 100644 index 43e8f94..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_2.yaml +++ /dev/null @@ -1,29 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_3.yaml b/ada_moveit/calib/default/calib1/joint_states_3.yaml deleted file mode 100644 index d41d3d4..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_3.yaml +++ /dev/null @@ -1,36 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - -5.140541793569257 - - 3.318208437077777 - - 4.269709555560121 - - 1.66645227946058 - - 2.533739450584657 - - -1.73153261280912 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_4.yaml b/ada_moveit/calib/default/calib1/joint_states_4.yaml deleted file mode 100644 index 7264099..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_4.yaml +++ /dev/null @@ -1,43 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - -5.140541793569257 - - 3.318208437077777 - - 4.269709555560121 - - 1.66645227946058 - - 2.533739450584657 - - -1.73153261280912 - - - - -5.118719984755202 - - 3.316665934174403 - - 4.274146115620084 - - 1.656535200193344 - - 2.539548071238229 - - -0.5678840153120358 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_5.yaml b/ada_moveit/calib/default/calib1/joint_states_5.yaml deleted file mode 100644 index 42a8733..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_5.yaml +++ /dev/null @@ -1,50 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - -5.140541793569257 - - 3.318208437077777 - - 4.269709555560121 - - 1.66645227946058 - - 2.533739450584657 - - -1.73153261280912 - - - - -5.118719984755202 - - 3.316665934174403 - - 4.274146115620084 - - 1.656535200193344 - - 2.539548071238229 - - -0.5678840153120358 - - - - -3.79399628057421 - - 2.392527730628187 - - 2.182486479140564 - - -0.8199031437449511 - - 3.202835238649473 - - -2.104490213979304 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_6.yaml b/ada_moveit/calib/default/calib1/joint_states_6.yaml deleted file mode 100644 index 17b721e..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_6.yaml +++ /dev/null @@ -1,92 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - -10.35542546265197 - - 2.014241277274808 - - 1.323689066097496 - - -6.283936492921805 - - 2.142022143221791 - - -2.913601563196982 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_7.yaml b/ada_moveit/calib/default/calib1/joint_states_7.yaml deleted file mode 100644 index 7cfb0af..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_7.yaml +++ /dev/null @@ -1,113 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_8.yaml b/ada_moveit/calib/default/calib1/joint_states_8.yaml deleted file mode 100644 index 7cfb0af..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_8.yaml +++ /dev/null @@ -1,113 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/joint_states_9.yaml b/ada_moveit/calib/default/calib1/joint_states_9.yaml deleted file mode 100644 index 04df1c2..0000000 --- a/ada_moveit/calib/default/calib1/joint_states_9.yaml +++ /dev/null @@ -1,120 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 2.21068096612707 - - 2.014246337280879 - - 1.323581740705566 - - -0.0004836300539583505 - - 2.142056897474017 - - -9.196876352589195 - - - - 1.026714113988362 - - 3.23086687333532 - - 4.12626184555242 - - 1.505805076184821 - - 2.879567696046314 - - -6.462540151848716 - - - - 2.449361985135459 - - 2.729725736261333 - - 1.496848199648612 - - -0.2929157619756353 - - 2.640078141330977 - - -9.325450574224913 - - - - 4.2117828723737 - - 3.027484989311495 - - 2.140141818334141 - - -1.642000199595909 - - 2.904123639193499 - - -8.168041297108694 - - - - 5.416634233780808 - - 3.988167621967997 - - 4.288325051579776 - - -3.836516310538415 - - 2.916259131648772 - - -9.37860833484733 - - - - 8.504239285752515 - - 2.26587551235118 - - 1.945154084908191 - - -6.820855793214187 - - 2.794614188853331 - - -8.058038895650752 - - - - 7.384514270693135 - - 3.517399304494145 - - 4.068328504463273 - - -4.803637001972129 - - 2.874472802564883 - - -7.13039384158112 - - - - 7.327974295486317 - - 3.616663041488559 - - 4.642509231277665 - - -4.06101679569046 - - 1.952262328173721 - - -6.346034311008802 - - - - 5.579346984798033 - - 2.116936097859626 - - 1.478817134330176 - - -1.785231394606987 - - 2.502963162079375 - - -6.864617821642264 - - - - 6.092488059427808 - - 3.584720554741931 - - 4.624657529858646 - - -3.652113976655258 - - 2.514845920547199 - - -8.736448823312012 - - - - 6.114609340706442 - - 3.708313333558655 - - 4.665592446342036 - - -3.764647979045428 - - 2.666327588615343 - - -8.799959888988221 - - - - 6.099153419004059 - - 3.4684123204549 - - 4.560300111588009 - - -3.587692908834568 - - 2.416385925569771 - - -8.682209286654803 - - - - 8.637265247466409 - - 3.657335902920685 - - 4.884685915532892 - - -5.622759173727414 - - 3.14210078472578 - - -7.253900600294634 - - - - 6.751913376942328 - - 3.43912527163119 - - 4.44215775825824 - - -5.225238172558993 - - 4.094043987949078 - - -9.559389035964088 - - - - -9.980646525612277 - - 2.776735323191193 - - 1.563472500639124 - - -6.554502183569163 - - 2.546058434839037 - - -1.808664282722485 - - - - -11.97299275714305 - - 3.530480751768614 - - 4.702883625821979 - - -5.414792324991882 - - 4.636997553085637 - - -1.115476074698213 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib1/samples.yaml b/ada_moveit/calib/default/calib1/samples.yaml deleted file mode 100644 index 307cab0..0000000 --- a/ada_moveit/calib/default/calib1/samples.yaml +++ /dev/null @@ -1,510 +0,0 @@ -- effector_wrt_world: - - 0.4363234558470004 - - 0.7756860441869826 - - 0.4559967135092987 - - 0.2811330590727208 - - 0.8791152486554319 - - -0.475513379166336 - - -0.03230179275691891 - - -0.3215493877079887 - - 0.191776488285788 - - 0.4149676940285393 - - -0.8893950705133284 - - 0.6479512864913848 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.8722612738054889 - - 0.4257194241121007 - - -0.2406724790101452 - - 0.02983309104485141 - - 0.4776015014471469 - - 0.8473782107437636 - - -0.232049507157694 - - -0.04705661151563468 - - 0.1051526320862126 - - -0.3173533360315581 - - -0.9424594336495253 - - 0.6497258336299866 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.5816842226272165 - - -0.05832540767225693 - - 0.811320905663373 - - 0.1063108980863489 - - -0.06244871386435559 - - -0.99768425276093 - - -0.02694976678104621 - - -0.1913048943853194 - - 0.81101394765026 - - -0.03498969294992416 - - -0.5839795356894035 - - 0.5587159131143602 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.06735061113950092 - - 0.5587552183289897 - - -0.8265933106245531 - - 0.0529743666421859 - - 0.9652742297993704 - - 0.1730978704825437 - - 0.1956598796883025 - - -0.1836332485233312 - - 0.2524075206176872 - - -0.8110670337429156 - - -0.527693765654949 - - 0.6388566581132725 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.8879288166412053 - - -0.0580969214653734 - - 0.4562972323983513 - - 0.1640239079514964 - - -0.06765059543118732 - - -0.9976984017337052 - - 0.004614771480351974 - - -0.1963836873148184 - - 0.4549789154630736 - - -0.03496636804496984 - - -0.889815452546082 - - 0.6086437775434579 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.08019357247369718 - - 0.863281850710769 - - -0.4983105830376228 - - -0.09867498101521009 - - 0.9587565758031523 - - 0.06995522405423282 - - 0.2754851992063031 - - -0.1678269677905053 - - 0.2726808010992345 - - -0.4998506905676052 - - -0.8220671918103581 - - 0.6231994842548911 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.4171002820587149 - - 0.7859914512675225 - - 0.4563384634686346 - - 0.1609018598842307 - - 0.8822034274561309 - - -0.4708456909394041 - - 0.004631188664966812 - - -0.1988108672313921 - - 0.2185050738339862 - - 0.4006516864536691 - - -0.8897942227563829 - - 0.6070198761893144 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.8761638487752237 - - 0.4119084756394136 - - -0.2503364092492389 - - 0.09256354221992662 - - 0.4620413535087486 - - 0.8656597161078751 - - -0.1927460597673448 - - -0.2115048349497813 - - 0.1373124092978907 - - -0.2845429029240544 - - -0.9487784982009033 - - 0.611292467255474 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.4169720944840322 - - 0.494263200225792 - - 0.7627831679606979 - - 0.04812854488166803 - - 0.8822499817163973 - - -0.4218946435992372 - - -0.2089016023483057 - - -0.1750033956729665 - - 0.2185617582813061 - - 0.7600715746591447 - - -0.6119819925554613 - - 0.7267644069919448 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.8811488589614778 - - 0.4101951141564162 - - -0.235194933349108 - - 0.09271833529928131 - - 0.4647343129031439 - - 0.6596075418468903 - - -0.5907113585746813 - - -0.03077061303837572 - - -0.0871705613227847 - - -0.629807795331963 - - -0.7718441773944813 - - 0.8661436755759176 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.5222118533832614 - - 0.6848403821587241 - - 0.5082208487957885 - - 0.03292078666441266 - - 0.7574033891257301 - - -0.6463338154187794 - - 0.092696845615482 - - -0.2689680101165137 - - 0.3919628634537677 - - 0.3365208017506426 - - -0.8562236061112931 - - 0.4980775273094994 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.7517757509834102 - - 0.5158883925448161 - - -0.4107217874314137 - - -0.06186793730188411 - - 0.6078278166382149 - - 0.7836423353095803 - - -0.1282576922892656 - - -0.291341229801881 - - 0.2556923259586612 - - -0.3460691502403431 - - -0.902694620399267 - - 0.5629393091690182 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.5819451729764731 - - -0.05830563685596669 - - 0.8111351726809798 - - 0.1062251752046167 - - -0.06252540550795554 - - -0.9976819686113569 - - -0.02685634326994736 - - -0.1912272331653753 - - 0.8108208120882495 - - -0.03508763626389874 - - -0.5842417893868632 - - 0.5586500355991523 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.06970269986940114 - - 0.5603606881141371 - - -0.825310506928857 - - 0.05243580995913062 - - 0.9656710555017683 - - 0.1696335147695002 - - 0.196733025272948 - - -0.1805547509221383 - - 0.250241775483265 - - -0.8106912913576885 - - -0.5293001831851059 - - 0.6399153891274197 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.5460949689173447 - - 0.8023872345158299 - - 0.2407384697326189 - - 0.1959283945466365 - - 0.736250069208905 - - -0.3226078946568003 - - -0.5948613131604626 - - -0.1016851180040693 - - -0.3996449931039387 - - 0.5020944853223802 - - -0.7669322051497169 - - 0.5868929205612636 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.7340749695114748 - - 0.5667462945327993 - - 0.3740756297462704 - - -0.1032267532166812 - - 0.4823863154640705 - - 0.8229260841531387 - - -0.3001601283871318 - - -0.1864658009624718 - - -0.477951233694114 - - -0.0398910723561389 - - -0.8774800969575047 - - 0.6032507826213945 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.7057123713501821 - - 0.6661621474231921 - - 0.2412427040634055 - - 0.1832418816864177 - - 0.4347795042834394 - - 0.6760384305362679 - - -0.594927578023667 - - -0.1653323684631107 - - -0.5594075719708747 - - -0.3149603685840187 - - -0.7667223321666521 - - 0.5609187256812503 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.4544633403344969 - - -0.674570878742756 - - 0.5817363679917538 - - 0.1451665570897299 - - -0.497245062558451 - - 0.7339694596998554 - - 0.4626404435294698 - - -0.2236907310375313 - - -0.7390604982362765 - - -0.07901241535434961 - - -0.6689892511592532 - - 0.5523747676013406 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.3147770020712479 - - -0.8586374658294081 - - 0.404545598469554 - - 0.1622314958800135 - - -0.9482439670968491 - - -0.3032564093150263 - - 0.09417499176420024 - - -0.2732669813090622 - - 0.04181886932315695 - - -0.4132520447419583 - - -0.9096559402791387 - - 0.5844255517844386 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.9509099424541561 - - 0.3073760412777087 - - -0.03592005832787475 - - -0.1165079585600769 - - 0.2495805132215038 - - -0.6930819196403826 - - 0.6762743674631573 - - 0.09153814690218647 - - 0.1829749949089338 - - -0.6520409664400291 - - -0.7357735584553371 - - 0.6544988115676889 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.3141639219842051 - - -0.9257033436605691 - - 0.2106522006985563 - - 0.4629585563159698 - - -0.9484326709869433 - - -0.3158815092305898 - - 0.02635034593278546 - - -0.2191887409779263 - - 0.04214853174281287 - - -0.2080677573816938 - - -0.9772058685917098 - - 0.5632251855075951 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.9513254515868023 - - 0.3047154454305542 - - -0.04613439583678964 - - -0.07625805140382272 - - 0.2837444713847237 - - -0.8075761448020002 - - 0.5170201594767293 - - -0.06803132022239844 - - 0.1202869906588837 - - -0.5049448164530436 - - -0.8547290636309448 - - 0.4799324200306603 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.9426406397853727 - - -0.258966149235505 - - 0.2106303818900713 - - 0.46019511994832 - - -0.2703388554844798 - - -0.9623975428781133 - - 0.02660587674765418 - - -0.2168879258056171 - - 0.1958201405381088 - - -0.08202135704988028 - - -0.9772036479400448 - - 0.5626149750472588 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.2789107031718536 - - 0.9276836151085924 - - -0.2482175052554417 - - 0.03008854732434851 - - 0.9159016767207128 - - -0.1792730226038406 - - 0.3591452379562762 - - -0.1255207546994526 - - 0.2886744502659863 - - -0.3275122801140991 - - -0.8996681433384732 - - 0.4641567340653292 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.6951543188046263 - - 0.6871733940647389 - - 0.2110762884286702 - - 0.4466461714537513 - - 0.6986356484646783 - - -0.7149761947043456 - - 0.02678192861674777 - - -0.2551167640993338 - - 0.1693183502802176 - - 0.128847846298032 - - -0.9771026193613159 - - 0.5585379648151977 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.678638790846581 - - 0.6890559040593635 - - -0.2542663026026531 - - 0.1071352668191894 - - 0.6857614508927415 - - 0.7184241507071153 - - 0.1166103432386769 - - -0.04817803879826291 - - 0.2630220979837486 - - -0.09522972625022778 - - -0.9600784734648173 - - 0.4875801108728516 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.7845835684286906 - - 0.5830607751964874 - - 0.2108761640844546 - - 0.4475070979410203 - - 0.6008027459712366 - - 0.7989664443020061 - - 0.02624273828759351 - - -0.2610683702113406 - - -0.1531818676773603 - - 0.1472845996928417 - - -0.9771604587314182 - - 0.5586518029440959 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.6038973848433624 - - -0.7718426108566533 - - 0.1989143852146792 - - 0.06457129770463622 - - -0.7759279457727472 - - 0.6263710119621431 - - 0.07480092474298683 - - 0.07864448412909492 - - -0.1823287458088652 - - -0.10917114746812 - - -0.9771580675675056 - - 0.5237555465842125 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.6748933142228936 - - 0.07759157197780431 - - 0.7338246128164767 - - 0.1812921759658302 - - 0.5580088842254992 - - -0.7043806705101503 - - -0.4387185386293643 - - -0.1528203342718945 - - 0.4828510117444636 - - 0.7055688619615273 - - -0.5186785916997765 - - 0.4637448260091031 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.5448240203527717 - - 0.6532159556447967 - - -0.5258095683207927 - - 0.03318176903054774 - - 0.8123056609801209 - - 0.2554764031238416 - - -0.5243007920903359 - - -0.03142293854737085 - - -0.2081497057079165 - - -0.7127697543652928 - - -0.6698006996680048 - - 0.6001607712244561 - - 0 - - 0 - - 0 - - 1 \ No newline at end of file diff --git a/ada_moveit/calib/default/calib2/camera_calib_pose.launch.py b/ada_moveit/calib/default/calib2/camera_calib_pose.launch.py deleted file mode 100644 index 3ee8858..0000000 --- a/ada_moveit/calib/default/calib2/camera_calib_pose.launch.py +++ /dev/null @@ -1,54 +0,0 @@ -""" Static transform publisher acquired via MoveIt 2 hand-eye calibration """ -""" EYE-IN-HAND: j2n6s200_end_effector -> camera_color_optical_frame """ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - # Log Level - log_level_da = DeclareLaunchArgument( - "log_level", - default_value="info", - description="Logging level (debug, info, warn, error, fatal)", - ) - log_level = LaunchConfiguration("log_level") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - output="log", - arguments=[ - "--frame-id", - "j2n6s200_end_effector", - "--child-frame-id", - "camera_color_optical_frame", - "--x", - "0.0300509", - "--y", - "0.134319", - "--z", - "-0.138905", - "--qx", - "0.0017638", - "--qy", - "-0.134985", - "--qz", - "0.990838", - "--qw", - "0.00400733", - # "--roll", - # "0.27081", - # "--pitch", - # "0.00241343", - # "--yaw", - # "3.13318", - "--ros-args", - "--log-level", - log_level, - ], - ), - ] - return LaunchDescription([log_level_da] + nodes) diff --git a/ada_moveit/calib/default/calib2/samples.yaml b/ada_moveit/calib/default/calib2/samples.yaml deleted file mode 100644 index ffc54a4..0000000 --- a/ada_moveit/calib/default/calib2/samples.yaml +++ /dev/null @@ -1,340 +0,0 @@ -- effector_wrt_world: - - -0.1700148433049358 - - 0.9640833860630571 - - 0.2040543500472082 - - -0.08941021225137585 - - 0.952536329693938 - - 0.2138388849433204 - - -0.2166736529885414 - - -0.2613246038023842 - - -0.2525262237257738 - - 0.1575314444908948 - - -0.954680234595549 - - 0.5075944811492825 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.9506521677376056 - - -0.1626523500046196 - - 0.2642057323635623 - - 0.08763378005656307 - - -0.1543931204070705 - - 0.9866466074094704 - - 0.05187712847055254 - - -0.1402059097793148 - - -0.2691156263518702 - - 0.0085255571874873 - - -0.9630701399835235 - - 0.4331286364917661 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.1696902428603508 - - 0.9641555307723639 - - 0.2039836610100344 - - -0.0585816936004146 - - 0.9525841510304749 - - 0.2135173310613678 - - -0.2167804985278426 - - -0.2544462699455194 - - -0.252564163498199 - - 0.1575260671047543 - - -0.9546710855048266 - - 0.5126362754442555 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.9521192601102524 - - -0.1589633485523944 - - 0.2611504706947998 - - 0.08697116321273857 - - -0.1556780259270202 - - 0.9872444233040096 - - 0.03335867051606748 - - -0.1099718920273098 - - -0.2631221518051484 - - -0.008893957057658698 - - -0.9647215301615721 - - 0.4429175855651303 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.1698405825488687 - - 0.9641491838823409 - - 0.2038885179172109 - - -0.04489997103946988 - - 0.952568180338316 - - 0.213633668966286 - - -0.2167360544324656 - - -0.331219338815394 - - -0.2525233421416924 - - 0.1574071367601205 - - -0.9547015004547603 - - 0.5330132024833099 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.9510352441349581 - - -0.1563611141437872 - - 0.2666142651788816 - - 0.006855521353269109 - - -0.1529931580751347 - - 0.9876595193421188 - - 0.03349279676425391 - - -0.1108757915893666 - - -0.2685610880141767 - - -0.008937328270143772 - - -0.9632211927527519 - - 0.4418842074406809 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.1698046185175581 - - 0.9641043222157442 - - 0.2041304666506855 - - 0.01123475270776628 - - 0.952449746071282 - - 0.2137313368220217 - - -0.2171598417503144 - - -0.3721426205456645 - - -0.2529938195665193 - - 0.1575492670411123 - - -0.9545534849949222 - - 0.554302781799672 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.955371357110343 - - -0.1583062058919822 - - 0.2494087311808232 - - -0.04654327950607925 - - -0.1534190098389102 - - 0.9873895977208909 - - 0.03904343392461372 - - -0.06441397588014919 - - -0.2524444046383107 - - -0.0009630621281346251 - - -0.9676109213305457 - - 0.4523183334269255 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.1695393857364802 - - 0.9641178639199425 - - 0.2042869089162219 - - 0.08082759380315063 - - 0.9523936776479708 - - 0.2135760828875932 - - -0.2175581292311677 - - -0.2750348133487039 - - -0.2533824766243135 - - 0.1576768888862267 - - -0.9544293160055859 - - 0.5471542265312349 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.9504671572154637 - - -0.1654257680935193 - - 0.2631472939352125 - - 0.03443179407876891 - - -0.1541415074867993 - - 0.986030893593603 - - 0.06311475698042285 - - 0.01853148873127145 - - -0.2699121685371983 - - 0.0194265830672818 - - -0.962688957631632 - - 0.4766485508872659 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.1703540757081763 - - 0.965820297266928 - - -0.1953730848321649 - - 0.1459181804098987 - - 0.95227850891275 - - 0.1104017295545665 - - -0.2845647546240929 - - -0.2635982336857834 - - -0.2532688894288496 - - -0.2345263556587694 - - -0.9385372971538715 - - 0.5460962372497242 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.9510640861551445 - - -0.1661626978508723 - - 0.2605130742761316 - - 0.03376765183300331 - - -0.04117365105258523 - - 0.9037327242910927 - - 0.426112536197188 - - -0.034219646743897 - - -0.3062381989316185 - - 0.3945340554227475 - - -0.8663492624956752 - - 0.4557288664364947 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.170021566825439 - - 0.9658888674634445 - - -0.1953237377391889 - - 0.2074324966855933 - - 0.9524241125716497 - - 0.1101880910148647 - - -0.2841599802762615 - - -0.2565666586289644 - - -0.2529446117361037 - - -0.2343443626560804 - - -0.938670199316558 - - 0.5310591722837323 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.9505659501861523 - - -0.1672561248193642 - - 0.2616290562172094 - - 0.03320261174214091 - - -0.04100630955544984 - - 0.9027778659432875 - - 0.428147880223091 - - 0.02705857064850991 - - -0.3078032763362683 - - 0.3962543545165061 - - -0.8650084563767959 - - 0.473275043429732 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.1702781925750898 - - 0.9658037137990207 - - -0.195521158817607 - - 0.07973802306785105 - - 0.9524576066748862 - - 0.1104413431525516 - - -0.283949321551942 - - -0.2063094176220714 - - -0.252645689891018 - - -0.2345759922385059 - - -0.9386928460603139 - - 0.5792906621500082 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.9546178448966263 - - -0.162440009670286 - - 0.2496357615872364 - - 0.09058233847252305 - - -0.04239542741413552 - - 0.9037501998385392 - - 0.4259556362183429 - - -0.1057779391418496 - - -0.2948006070877312 - - 0.3960414366579815 - - -0.8696227817338897 - - 0.4723234218894953 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - -0.8779153628719032 - - 0.432975156838551 - - -0.2044434620968016 - - 0.08365359633803568 - - 0.4731657983727087 - - 0.8498980059319361 - - -0.2319213374470656 - - -0.2175005760476392 - - 0.0733399133065433 - - -0.3003429590876915 - - -0.9510075520429044 - - 0.5814112487771359 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.4671498181675737 - - -0.8837204737012908 - - -0.02844594430089309 - - 0.1580157118480719 - - -0.7665258746528941 - - 0.3887440949841531 - - 0.5111908763882309 - - -0.007792719280609419 - - -0.440691650560365 - - 0.2606072772892543 - - -0.8589963423381171 - - 0.4957191427763006 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.9200845377696003 - - -0.3514303301643098 - - -0.1730351594269357 - - 0.08079361805189289 - - -0.3794940023299461 - - -0.9091872646445445 - - -0.1713558286250263 - - -0.2202026720320895 - - -0.09710172785743648 - - 0.2233276535693123 - - -0.9698948466706714 - - 0.5826163856729982 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.3756147758365375 - - 0.9261032051991809 - - 0.03530429850700598 - - -0.09488972715227603 - - 0.922754483869177 - - -0.3772583335226263 - - 0.07874206173996115 - - -0.02256557295597251 - - 0.08624211658230821 - - 0.003000517877833349 - - -0.9962696895017278 - - 0.4950831032459649 - - 0 - - 0 - - 0 - - 1 diff --git a/ada_moveit/calib/default/calib_camera_pose.launch.py b/ada_moveit/calib/default/calib_camera_pose.launch.py deleted file mode 100644 index f8e35f8..0000000 --- a/ada_moveit/calib/default/calib_camera_pose.launch.py +++ /dev/null @@ -1,46 +0,0 @@ -""" Static transform publisher acquired via MoveIt 2 hand-eye calibration """ -""" EYE-IN-HAND: j2n6s200_end_effector -> camera_color_optical_frame """ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - # Log Level - log_level_da = DeclareLaunchArgument( - "log_level", - default_value="info", - description="Logging level (debug, info, warn, error, fatal)", - ) - log_level = LaunchConfiguration("log_level") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - output="log", - arguments=[ - "--frame-id", - "j2n6s200_end_effector", - "--child-frame-id", - "camera_color_optical_frame", - "--x", - "0.032072", - "--y", - "0.13256", - "--z", - "-0.165098", - "--roll", - "-0.2707118", - "--pitch", - "-0.024086", - "--yaw", - "-3.1331081", - "--ros-args", - "--log-level", - log_level, - ], - ), - ] - return LaunchDescription([log_level_da] + nodes) diff --git a/ada_moveit/calib/default/transform.py b/ada_moveit/calib/default/transform.py deleted file mode 100644 index 9fdf36a..0000000 --- a/ada_moveit/calib/default/transform.py +++ /dev/null @@ -1,65 +0,0 @@ -import transformations - -################################################################################ -# j2n6s200_end_effector -> camera_color_optical_frame : outputted by calibration -################################################################################ - -# The below calibration had reprojection error 0.0293236m and 0.0154105rad. In practice, it detects -# objects in the depth camera as slightly closer (looks like ~1cm in RVIZ) than they are. -ee_to_camera_optical_1 = transformations.quaternion_matrix( - [-0.00145183, -0.13495, 0.990841, -0.00447916] -) -ee_to_camera_optical_1[:3, 3] = [0.032072, 0.13256, -0.155098] - -# The below calibration had reprojection error 0.0153083m and 0.00546327rad. In practice, it detects -# objects in the depth camera as farther (looks like ~1cm in RVIZ) than they are. -ee_to_camera_optical_2 = transformations.quaternion_matrix( - [0.0017638, -0.134985, 0.990838, 0.00400733] -) -ee_to_camera_optical_2[:3, 3] = [0.0300509, 0.134319, -0.138905] - -# Therefore, we average the two transforms together to get the ee_to_camera_optical transform -ee_to_camera_optical_quat = transformations.quaternion_slerp( - transformations.quaternion_from_matrix(ee_to_camera_optical_1), - transformations.quaternion_from_matrix(ee_to_camera_optical_2), - 0.5, -) -ee_to_camera_optical_trans = ( - ee_to_camera_optical_1[:3, 3] + ee_to_camera_optical_2[:3, 3] -) / 2.0 -print( - "j2n6s200_end_effector -> camera_color_optical_frame translation: %s, quaternion: %s" - % (ee_to_camera_optical_trans, ee_to_camera_optical_quat) -) -ee_to_camera_optical = transformations.quaternion_matrix(ee_to_camera_optical_quat) -ee_to_camera_optical[:3, 3] = ee_to_camera_optical_trans - -################################################################################ -# j2n6s200_end_effector -> cameraMount : hardcoded into ADA URDF -# ROS1: rosrun tf tf_echo /j2n6s200_end_effector /cameraMount -################################################################################ -ee_to_camera_mount = transformations.quaternion_matrix([0.090, -0.090, 0.701, 0.701]) -ee_to_camera_mount[:3, 3] = [-0.000, 0.122, -0.180] - -################################################################################ -# camera_link -> camera_color_optical_frame : outputted by RealSense's URDF -# ROS1: rosrun tf tf_echo /camera_link /camera_color_optical_frame -################################################################################ -camera_link_to_camera_optical = transformations.quaternion_matrix( - [0.502, -0.501, 0.498, -0.499] -) -camera_link_to_camera_optical[:3, 3] = [-0.000, 0.015, -0.000] - -################################################################################ -# Compute cameraMount -> camera_link -################################################################################ -camera_mount_to_camera_link = transformations.concatenate_matrices( - transformations.inverse_matrix(ee_to_camera_mount), - ee_to_camera_optical, - transformations.inverse_matrix(camera_link_to_camera_optical), -) -print("camera_mount_to_camera_link Translation: ", camera_mount_to_camera_link[:3, 3]) -print( - "camera_mount_to_camera_link Euler: ", - transformations.euler_from_matrix(camera_mount_to_camera_link, "sxyz"), -) diff --git a/ada_moveit/calib/manual/calib_camera_pose.launch.py b/ada_moveit/calib/manual/calib_camera_pose.launch.py deleted file mode 100644 index 10313ac..0000000 --- a/ada_moveit/calib/manual/calib_camera_pose.launch.py +++ /dev/null @@ -1,52 +0,0 @@ -""" Static transform publisher acquired via MoveIt 2 hand-eye calibration """ -""" EYE-IN-HAND: j2n6s200_end_effector -> camera_color_optical_frame """ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -# NOTE: This was hand-tweaked, assimng 0 pitch and -3.14 yaw -# (i.e., assuming the only unknown orientation was roll) -# by having the camera observe the robot itself (point at -# joint 1) and comparing the depthcloud to the robot's mesh -# in RVIZ. The transform was then adjusted until the depthcloud -# visually matched the mesh. -def generate_launch_description() -> LaunchDescription: - # Log Level - log_level_da = DeclareLaunchArgument( - "log_level", - default_value="info", - description="Logging level (debug, info, warn, error, fatal)", - ) - log_level = LaunchConfiguration("log_level") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - output="log", - arguments=[ - "--frame-id", - "j2n6s200_end_effector", - "--child-frame-id", - "camera_color_optical_frame", - "--x", - "0.032", - "--y", - "0.127", - "--z", - "-0.159", - "--roll", - "-0.2385", - "--pitch", - "0.00", - "--yaw", - "-3.08", - "--ros-args", - "--log-level", - log_level, - ], - ), - ] - return LaunchDescription([log_level_da] + nodes) diff --git a/ada_moveit/calib/none/calib_camera_pose.launch.py b/ada_moveit/calib/none/calib_camera_pose.launch.py deleted file mode 100644 index cc6bb70..0000000 --- a/ada_moveit/calib/none/calib_camera_pose.launch.py +++ /dev/null @@ -1,6 +0,0 @@ -""" Empty Transform for no calib """ -from launch import LaunchDescription - - -def generate_launch_description() -> LaunchDescription: - return LaunchDescription() diff --git a/ada_moveit/calib/ros2/README.md b/ada_moveit/calib/ros2/README.md deleted file mode 100644 index 951bf79..0000000 --- a/ada_moveit/calib/ros2/README.md +++ /dev/null @@ -1 +0,0 @@ -**Note**: This calibration is calling `ros2` because it was done using ROS2's MoveIt2 Hand-Eye Calibration Pipeline ([Github](https://github.com/AndrejOrsula/moveit2_calibration/tree/ros2_port), [Documentation](https://moveit.picknik.ai/main/doc/examples/hand_eye_calibration/hand_eye_calibration_tutorial.html)). Most likely, `default` will have better calibration parameters. However, these joint states and samples are included in case someone wants to expand upon them (e.g., add new joint states or samples) to achieve a better calibration. \ No newline at end of file diff --git a/ada_moveit/calib/ros2/calib_camera_pose.launch.py b/ada_moveit/calib/ros2/calib_camera_pose.launch.py deleted file mode 100644 index 022f003..0000000 --- a/ada_moveit/calib/ros2/calib_camera_pose.launch.py +++ /dev/null @@ -1,54 +0,0 @@ -""" Static transform publisher acquired via MoveIt 2 hand-eye calibration """ -""" EYE-IN-HAND: j2n6s200_end_effector -> camera_color_optical_frame """ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description() -> LaunchDescription: - # Log Level - log_level_da = DeclareLaunchArgument( - "log_level", - default_value="info", - description="Logging level (debug, info, warn, error, fatal)", - ) - log_level = LaunchConfiguration("log_level") - - nodes = [ - Node( - package="tf2_ros", - executable="static_transform_publisher", - output="log", - arguments=[ - "--frame-id", - "j2n6s200_end_effector", - "--child-frame-id", - "camera_color_optical_frame", - "--x", - "0.0580205", - "--y", - "0.142803", - "--z", - "-0.144791", - "--qx", - "-0.00638382", - "--qy", - "-0.154877", - "--qz", - "0.984639", - "--qw", - "0.0803667", - # "--roll", - # "0.30908", - # "--pitch", - # "-0.0374741", - # "--yaw", - # "2.98455", - "--ros-args", - "--log-level", - log_level, - ], - ), - ] - return LaunchDescription([log_level_da] + nodes) diff --git a/ada_moveit/calib/ros2/calib_joint_samples.yaml b/ada_moveit/calib/ros2/calib_joint_samples.yaml deleted file mode 100644 index 6287cf9..0000000 --- a/ada_moveit/calib/ros2/calib_joint_samples.yaml +++ /dev/null @@ -1,204 +0,0 @@ -- effector_wrt_world: - - 0.971070333256707 - - 0.23462982443424499 - - 0.044398799022760393 - - 0.2054650013817817 - - 0.23472980057158657 - - -0.97205595939976885 - - 0.003022005794108054 - - -0.33611963783076526 - - 0.043867169869177686 - - 0.007487141066642289 - - -0.99900931633609824 - - 0.496360245390642 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.13364700349100089 - - -0.98637010018237381 - - -0.095981789544113516 - - 0.097832427825866972 - - -0.93976528434507289 - - -0.15688399603137904 - - 0.30369165633756012 - - 0.092568844525776461 - - -0.31461037617615351 - - 0.049612873888139036 - - -0.9479234536326544 - - 0.447779978650445 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.91685580046285409 - - 0.23316053908639073 - - 0.32405494004962004 - - 0.10697343251624081 - - 0.22210202667616086 - - -0.97241562407141435 - - 0.071263902560443071 - - -0.35924567316413475 - - 0.33173201670016889 - - 0.0066345365132805464 - - -0.94335033366257592 - - 0.48495790286237805 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.12295853395348877 - - -0.9154423357631184 - - -0.38320559601938908 - - 0.069898509004699891 - - -0.92986752986667287 - - -0.24118580250769117 - - 0.27780530154835748 - - 0.096023811904950948 - - -0.34673848333818003 - - 0.32217190839874243 - - -0.88089595617816974 - - 0.44516058408227027 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.91433621114196884 - - 0.23514168479510608 - - -0.32969331365113835 - - 0.32834258463717503 - - 0.21743589982727615 - - -0.97190166320212801 - - -0.090159783336252575 - - -0.30612726213716718 - - -0.34162980323861042 - - 0.010749192372278359 - - -0.93977312815513714 - - 0.46839154861631815 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.12619969217146654 - - -0.95547452721240955 - - 0.2667246999136933 - - 0.13024812595617913 - - -0.95696266673509689 - - -0.046425965307719919 - - 0.28647353144836707 - - 0.088848308713615806 - - -0.26133521035459273 - - -0.29139845159758326 - - -0.9202123940892406 - - 0.46002265377821988 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.9165428858768041 - - 0.3450877398145864 - - -0.20214744662814832 - - 0.27426982953982976 - - 0.21477671471478388 - - -0.85108426338165311 - - -0.47908928128318018 - - -0.22557969646606052 - - -0.33737234795540683 - - 0.39568930798515412 - - -0.85417203792988117 - - 0.47688351348160624 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - 0.12534956177101753 - - -0.95297137416211164 - - 0.27592217633128124 - - 0.096095143389167789 - - -0.97596640239275623 - - -0.16841609021391979 - - -0.13829534322455489 - - 0.11090902557746164 - - 0.17826123741395508 - - -0.25195551310624986 - - -0.95117892672778037 - - 0.44458065889469528 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.29783766198177797 - - -0.85933229024560442 - - 0.41574119599394121 - - 0.13510562537213855 - - -0.94434633380428235 - - -0.32893557475632029 - - -0.0033748318684589385 - - -0.24204648326092282 - - 0.13965217125286369 - - -0.39159852221500252 - - -0.90947670034112882 - - 0.47380002322273823 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.96863599303135151 - - -0.18471911676591179 - - -0.16620216877462995 - - 0.055596786270783023 - - -0.24820591395747416 - - 0.68762582368284941 - - 0.68232290807286988 - - -0.06995151899606683 - - -0.011753181726838036 - - 0.70217488883162482 - - -0.71190749975933842 - - 0.35563110572445622 - - 0 - - 0 - - 0 - - 1 -- effector_wrt_world: - - 0.14082619653464501 - - -0.85850695023090795 - - -0.4930859953140132 - - 0.39473489842393195 - - -0.96080181730307013 - - 0.0016224018578153698 - - -0.27723137571229151 - - -0.18018992729719149 - - 0.23880504650593856 - - 0.51279936058602615 - - -0.82462656126628575 - - 0.41124044742984528 - - 0 - - 0 - - 0 - - 1 - object_wrt_sensor: - - -0.97371343564677715 - - -0.024988007663019757 - - -0.22640173301894775 - - 0.027235945739572075 - - 0.021022126491526016 - - 0.9798632282885017 - - -0.19856012702909398 - - -0.12924790401084801 - - 0.2268043549818308 - - -0.19810010934138456 - - -0.95358068942287599 - - 0.409599294295521 - - 0 - - 0 - - 0 - - 1 \ No newline at end of file diff --git a/ada_moveit/calib/ros2/calib_joint_states.yaml b/ada_moveit/calib/ros2/calib_joint_states.yaml deleted file mode 100644 index 555db31..0000000 --- a/ada_moveit/calib/ros2/calib_joint_states.yaml +++ /dev/null @@ -1,50 +0,0 @@ -joint_names: - - j2n6s200_joint_1 - - j2n6s200_joint_2 - - j2n6s200_joint_3 - - j2n6s200_joint_4 - - j2n6s200_joint_5 - - j2n6s200_joint_6 -joint_values: - - - - 1.1521612518721085 - - 2.9177763345217897 - - 4.2321627126155308 - - 0.78461199824365746 - - 4.0029249965164526 - - -2.5061538953813987 - - - - 1.6143224990189342 - - 2.9524136739754696 - - 4.42458009611539 - - 0.43096710866650856 - - 4.105438322672982 - - -2.1450715958279862 - - - - 3.715218377633013 - - 3.6207826853788028 - - 2.3309623704443894 - - -1.0459479960131324 - - 2.9080904176371716 - - -1.9885894433390559 - - - - 0.1400199553686603 - - 3.046240567604487 - - 4.6977410617570357 - - -3.2768878948685436 - - 2.0027900840616311 - - -1.79165973284622 - - - - 1.151330878244222 - - 3.2811984874089459 - - 4.7248264753075775 - - 2.1753195131729974 - - 1.8329006465388602 - - -2.2475923788355674 - - - - 3.3111454675507783 - - 3.6910254233424058 - - 2.151684224814653 - - -0.85639910332339664 - - 2.8346750558669571 - - -4.0567003441956597 \ No newline at end of file diff --git a/ada_moveit/launch/demo.launch.py b/ada_moveit/launch/demo.launch.py index 4360371..c0793b2 100644 --- a/ada_moveit/launch/demo.launch.py +++ b/ada_moveit/launch/demo.launch.py @@ -6,7 +6,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.launch_description_sources import PythonLaunchDescriptionSource, AnyLaunchDescriptionSource from launch.substitutions import ( LaunchConfiguration, Command, @@ -35,8 +35,8 @@ def generate_launch_description(): # Calibration Launch Argument calib_da = DeclareLaunchArgument( "calib", - default_value="manual", - description="Which calibration folder to use with calib_camera_pose.launch.py", + default_value="auto", + description="Which calibration folder to use. Files are located in the `ada_calibrate_camera` package.", ) calib = LaunchConfiguration("calib") @@ -48,6 +48,15 @@ def generate_launch_description(): ) sim = LaunchConfiguration("sim") + # Use Octomap Launch Argument + use_octomap_da = DeclareLaunchArgument( + "use_octomap", + default_value="true", + description="Whether to use octomap for collision checking", + ) + use_octomap = LaunchConfiguration("use_octomap") + + # Controllers File ctrl_da = DeclareLaunchArgument( "controllers_file", default_value=[sim, "_controllers.yaml"], @@ -55,6 +64,7 @@ def generate_launch_description(): ) controllers_file = LaunchConfiguration("controllers_file") + # Servo File servo_da = DeclareLaunchArgument( "servo_file", default_value=[sim, "_servo.yaml"], @@ -74,6 +84,7 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(calib_da) ld.add_action(sim_da) + ld.add_action(use_octomap_da) ld.add_action(ctrl_da) ld.add_action(servo_da) ld.add_action(log_level_da) @@ -81,25 +92,18 @@ def generate_launch_description(): # Launch argument for whether to use moveit servo or not ld.add_action(DeclareBooleanLaunchArg("use_servo", default_value=False)) - # Camera Calibration File + # Camera Extrinsics Calibration + ada_calibrate_camera_package_path = get_package_share_directory("ada_calibrate_camera") ld.add_action( IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - PathJoinSubstitution( - [ - str(moveit_config.package_path), - "calib", - calib, - "calib_camera_pose.launch.py", - ] - ) - ] + AnyLaunchDescriptionSource( + os.path.join(ada_calibrate_camera_package_path, "launch/publish_camera_extrinsics_launch.xml") ), launch_arguments={ "log_level": log_level, + "calibration_file_name": calib, }.items(), - ) + ), ) # Launch the IMU joint state publisher @@ -166,6 +170,7 @@ def generate_launch_description(): ), launch_arguments={ "sim": sim, + "use_octomap": use_octomap, "log_level": log_level, }.items(), ) diff --git a/ada_moveit/launch/move_group.launch.py b/ada_moveit/launch/move_group.launch.py index e417266..8fe1795 100644 --- a/ada_moveit/launch/move_group.launch.py +++ b/ada_moveit/launch/move_group.launch.py @@ -16,6 +16,7 @@ def get_move_group_launch(context): Adapted from https://robotics.stackexchange.com/questions/104340/getting-the-value-of-launchargument-inside-python-launch-file """ sim = LaunchConfiguration("sim").perform(context) + use_octomap = LaunchConfiguration("use_octomap").perform(context) log_level = LaunchConfiguration("log_level").perform(context) # Get MoveIt Configs @@ -24,7 +25,7 @@ def get_move_group_launch(context): ).to_moveit_configs() # If sim is mock, set moveit_config.sensors_3d to an empty dictionary - if sim == "mock": + if sim == "mock" or use_octomap == "false": moveit_config.sensors_3d = {} entities = generate_move_group_launch(moveit_config).entities @@ -47,6 +48,12 @@ def generate_launch_description(): default_value="real", description="Which sim to use: 'mock', 'isaac', or 'real'", ) + # Use Octomap + octomap_da = DeclareLaunchArgument( + "use_octomap", + default_value="true", + description="Whether to use octomap for collision checking", + ) # Log Level log_level_da = DeclareLaunchArgument( "log_level", @@ -56,6 +63,7 @@ def generate_launch_description(): ld = LaunchDescription() ld.add_action(sim_da) + ld.add_action(octomap_da) ld.add_action(log_level_da) ld.add_action(OpaqueFunction(function=get_move_group_launch)) return ld