From 1572e9b2aabcf4411b1bedfe811735f375329ee6 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Wed, 21 May 2025 16:18:51 +0200 Subject: [PATCH] (WIP) Add glove to hand joints translation --- .../robots/hope_jr/joints_translation.py | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 lerobot/common/robots/hope_jr/joints_translation.py diff --git a/lerobot/common/robots/hope_jr/joints_translation.py b/lerobot/common/robots/hope_jr/joints_translation.py new file mode 100644 index 00000000..5e0eac76 --- /dev/null +++ b/lerobot/common/robots/hope_jr/joints_translation.py @@ -0,0 +1,49 @@ +INDEX_SPLAY = 0.1 +MIDDLE_SPLAY = 0.1 +RING_SPLAY = 0.1 +PINKY_SPLAY = -0.1 + + +def get_ulnar_flexion(flexion: float, abduction: float, splay: float): + return (100 - abduction) * splay + flexion * (1 - splay) + + +def get_radial_flexion(flexion: float, abduction: float, splay: float): + return abduction * splay + flexion * (1 - splay) + + +def homonculus_glove_to_hope_jr_hand(glove_action: dict[str, float]) -> dict[str, float]: + return { + "thumb_cmc.pos": glove_action["thumb_cmc.pos"], + "thumb_mcp.pos": glove_action["thumb_mcp.pos"], + "thumb_pip.pos": glove_action["thumb_pip.pos"], + "thumb_dip.pos": glove_action["thumb_dip.pos"], + "index_radial_flexor.pos": get_radial_flexion( + glove_action["index_mcp_flexion.pos"], glove_action["index_mcp_abduction.pos"], INDEX_SPLAY + ), + "index_ulnar_flexor.pos": get_ulnar_flexion( + glove_action["index_mcp_flexion.pos"], glove_action["index_mcp_abduction.pos"], INDEX_SPLAY + ), + "index_pip_dip.pos": glove_action["index_pip_dip.pos"], + "middle_radial_flexor.pos": get_radial_flexion( + glove_action["middle_mcp_flexion.pos"], glove_action["middle_mcp_abduction.pos"], MIDDLE_SPLAY + ), + "middle_ulnar_flexor.pos": get_ulnar_flexion( + glove_action["middle_mcp_flexion.pos"], glove_action["middle_mcp_abduction.pos"], MIDDLE_SPLAY + ), + "middle_pip_dip.pos": glove_action["middle_pip_dip.pos"], + "ring_radial_flexor.pos": get_radial_flexion( + glove_action["ring_mcp_flexion.pos"], glove_action["ring_mcp_abduction.pos"], RING_SPLAY + ), + "ring_ulnar_flexor.pos": get_ulnar_flexion( + glove_action["ring_mcp_flexion.pos"], glove_action["ring_mcp_abduction.pos"], RING_SPLAY + ), + "ring_pip_dip.pos": glove_action["ring_pip_dip.pos"], + "pinky_radial_flexor.pos": get_radial_flexion( + glove_action["pinky_mcp_flexion.pos"], glove_action["pinky_mcp_abduction.pos"], PINKY_SPLAY + ), + "pinky_ulnar_flexor.pos": get_ulnar_flexion( + glove_action["pinky_mcp_flexion.pos"], glove_action["pinky_mcp_abduction.pos"], PINKY_SPLAY + ), + "pinky_pip_dip.pos": glove_action["pinky_pip_dip.pos"], + }