"""Grabbable puck with ArUco marker configuration."""
import numpy as np
import cv2
from cv2 import aruco
from .exoskeleton import BLENDER_STL_DIR, SO100_MODEL_DIR, BOARD_IMG_DIR
# Puck ArUco board
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_250)
n_aruco_row, n_aruco_col = 3, 3
x = 4.5 / ((n_aruco_row + (n_aruco_row - 1) / 10) * 100)
board_ids_puck = np.arange(172, 181, dtype=np.int32).reshape(-1, 1) # IDs 172-180
puck_board = aruco.GridBoard(size=(n_aruco_row, n_aruco_col), markerLength=x, markerSeparation=x/10, dictionary=aruco_dict, ids=board_ids_puck)
class PuckConfig:
"""Configuration for grabbable puck with ArUco marker."""
name = "Grabbable_Puck"
# ArUco configuration
aruco_board = puck_board
aruco_offset_pos = np.array([1.64, 0, 50.62]) / 1000 # meters
aruco_offset_rot = np.array([0, np.pi/2, 0]) # radians
board_length = 0.045
# Grasp offset
grasp_offset = np.array([9.4, 6.9, 57.8]) / 1000 # meters
@classmethod
def get_xml_addition(cls) -> str:
"""Get XML snippet to add puck to scene."""
return f"""
"""
# Convenience instance
PUCK_CONFIG = PuckConfig()