Merge pull request #53 from 2lian/dev_shami

merge dev_shami into main

Author: shamikarim

launch_stack.bash

@@ -7,7 +7,8 @@ colcon build --cmake-args -Wno-dev
 # colcon test --packages-select easy_robot_control
 # colcon test-result --verbose
 . install/setup.bash
+. "${ROS2_INTERFACE_INSTALL_PATH}"/setup.bash
 export RCUTILS_CONSOLE_OUTPUT_FORMAT="{message}"
 export NUMBA_CACHE_DIR="./numba_cache" # this will compile numba in a permanant file
 
-ros2 launch moonbot_zero_tuto myrobot.launch.py MS_up_to_level:=4
+ros2 launch realman_interface realman_75.py MS_up_to_level:=3

src/easy_robot_control/easy_robot_control/EliaNode.py

@@ -367,7 +367,7 @@ def __init__(self, name: str):
     def wait_for_lower_level(
         self, more_services: Iterable[str] = set(), all_requiered: bool = False
     ):
-        """You cdan overload this or use the launch setting to wait for a service"""
+        """You can overload this or use the launch setting to wait for a service"""
         self.declare_parameter("services_to_wait", [""])
         from_prams = set(
             self.get_parameter("services_to_wait")

src/easy_robot_control/easy_robot_control/gait_key_dev.py

@@ -62,8 +62,8 @@
 # VVV Settings to tweek
 #
 # LEGNUMS_TO_SCAN = [2,4]
-LEGNUMS_TO_SCAN = [1, 2, 3, 4]
-TRANSLATION_SPEED = 50  # mm/s ; full stick will send this speed
+LEGNUMS_TO_SCAN = [75, 16]
+TRANSLATION_SPEED = 30  # mm/s ; full stick will send this speed
 ROTATION_SPEED = np.deg2rad(5)  # rad/s ; full stick will send this angular speed
 ALLOWED_DELTA_XYZ = 50  # mm ; ik2 commands cannot be further than ALOWED_DELTA_XYZ away
 # from the current tip position
@@ -81,7 +81,7 @@
 
 TRICYCLE_FLIPPED: List[int] = []
 
-MAX_JOINT_SPEED = 0.15
+MAX_JOINT_SPEED = 0.3
 #
 # ^^^ Settings to tweek
 
@@ -93,7 +93,9 @@
 # type def V
 ANY: Final[str] = "ANY"
 ALWAYS: Final[str] = "ALWAYS"
-KeyCodeModifier = Tuple[int, Union[int, Literal["ANY"]]]  # keyboard input: key + modifier
+KeyCodeModifier = Tuple[
+    int, Union[int, Literal["ANY"]]
+]  # keyboard input: key + modifier
 JoyBits = int  # 32 bits to represent all buttons pressed or not
 ButtonName = Literal[  # type with all possible buttons
     "NONE",
@@ -126,7 +128,9 @@
     Literal["ALWAYS"],  # functions associated with "ALWAYS" string will always execute
 ]
 NakedCall = Callable[[], Any]
-InputMap = Dict[UserInput, List[NakedCall]]  # User input are linked to a list of function
+InputMap = Dict[
+    UserInput, List[NakedCall]
+]  # User input are linked to a list of function
 
 # Namespace
 operator = str(environ.get("OPERATOR"))
@@ -148,6 +152,51 @@
 }
 ALPHAB_TO_STICKER = {v: k for k, v in STICKER_TO_ALPHAB.items()}
 
+STICKER_TO_ALPHAB_LEG75: Dict[int, int] = {
+    1: 0,
+    2: 1,
+    3: 2,
+    4: 3,
+    5: 4,
+    6: 5,
+    7: 6,
+}
+
+ALPHAB_TO_STICKER_LEG75 = {v: k for k, v in STICKER_TO_ALPHAB_LEG75.items()}
+
+STICKER_TO_ALPHAB_LEG16_ARM: Dict[int, int] = {
+    1: 2,
+    2: 1,
+    3: 0,  # first 1 to 6 is remap for launch without the gripper
+    4: 3,
+    5: 4,
+    6: 5,
+    # 1: 13,
+    # 2: 12,
+    # 3: 0,       # second 1 to 6 is remap for launch with gripper
+    # 4: 14,
+    # 5: 15,
+    # 6: 16,
+}
+
+ALPHAB_TO_STICKER_LEG16_ARM = {v: k for k, v in STICKER_TO_ALPHAB_LEG16_ARM.items()}
+
+STICKER_TO_ALPHAB_LEG16_GRIP: Dict[int, int] = {
+    1: 1,
+    2: 2,
+    3: 3,
+    4: 4,
+    5: 5,  # gripper joints (too many)
+    6: 6,
+    7: 7,
+    8: 8,
+    9: 9,
+    # 10: 10,  # palm finger 1
+    # 11: 11,  # palm finger 2
+}
+
+ALPHAB_TO_STICKER_LEG16_GRIP = {v: k for k, v in STICKER_TO_ALPHAB_LEG16_GRIP.items()}
+
 BUTT_BITS: Dict[ButtonName, int] = {  # button name to bit position
     # butts
     "x": 0,
@@ -177,7 +226,9 @@
 }
 BITS_BUTT: Dict[int, ButtonName] = {v: k for k, v in BUTT_BITS.items()}
 # Bitmask of each button
-BUTT_INTS: Dict[ButtonName, JoyBits] = {butt: 1 << bit for butt, bit in BUTT_BITS.items()}
+BUTT_INTS: Dict[ButtonName, JoyBits] = {
+    butt: 1 << bit for butt, bit in BUTT_BITS.items()
+}
 BUTT_INTS["NONE"] = 0
 INTS_BUTT: Dict[JoyBits, ButtonName] = {v: k for k, v in BUTT_INTS.items()}
 
@@ -359,7 +410,10 @@ def __init__(self, name: str = "keygait_node"):
         self.Alias = "K"
 
         self.leg_aliveCLI: Dict[int, Client] = dict(
-            [(l, self.create_client(Empty, f"leg{l}/leg_alive")) for l in LEGNUMS_TO_SCAN]
+            [
+                (l, self.create_client(Empty, f"leg{l}/leg_alive"))
+                for l in LEGNUMS_TO_SCAN
+            ]
         )
         self.legs: Dict[int, Leg] = {}
 
@@ -377,7 +431,6 @@ def __init__(self, name: str = "keygait_node"):
         )
         self.next_scan_ind = 0
         self.selected_joint: Union[int, str, None] = None
-        self.selected_joint_joy: Union[int, None] = None
         self.selected_legs: List[int] = []
 
         self.main_map: Final[InputMap] = (
@@ -421,12 +474,17 @@ def __init__(self, name: str = "keygait_node"):
 
         self.joint_timer = self.create_timer(0.1, self.joint_control_joy)
 
+        self.launch_case = "HERO"
+        self.joint_mapping = STICKER_TO_ALPHAB
+
         # config
         self.config_index = 0  # current
         self.num_configs = 3  # total configs
         self.prev_config_button = False  # prev config
 
-        self.sendTargetBody: Client = self.create_client(SendTargetBody, "go2_targetbody")
+        self.sendTargetBody: Client = self.create_client(
+            SendTargetBody, "go2_targetbody"
+        )
         self.execute_in_cbk_group(self.makeTBclient, MutuallyExclusiveCallbackGroup())
 
         self.recover_allCLI = [
@@ -488,6 +546,29 @@ def leg_scanTMRCBK(self):
 
         return  # stops scanning if all fails
 
+    def refresh_joint_mapping(self):
+        """joint mapping based on leg number (realguy or MoonbotH)"""
+        if 75 in self.selected_legs:
+            self.joint_mapping = STICKER_TO_ALPHAB_LEG75
+            self.launch_case = "75"
+        elif 16 in self.selected_legs:
+            self.joint_mapping = STICKER_TO_ALPHAB_LEG16_ARM
+            self.launch_case = "16"
+        else:
+            self.joint_mapping = STICKER_TO_ALPHAB
+            self.launch_case = "HERO"
+        # self.pinfo(self.joint_mapping)
+
+    def switch_to_grip_ur16(self):
+        """joint mapping based on leg number (realguy or MoonbotH)"""
+        if 16 in self.selected_legs:
+            self.joint_mapping = STICKER_TO_ALPHAB_LEG16_GRIP
+            self.launch_case = "16"
+            # self.pinfo(self.joint_mapping)
+        else:
+            self.joint_mapping = STICKER_TO_ALPHAB
+            self.launch_case = "HERO"
+
     @error_catcher
     def key_upSUBCBK(self, msg: Key):
         """Executes when keyboard released"""
@@ -712,7 +793,30 @@ def dragon_back_right(self):
         main_leg.move(quat=rot, blocking=False)
 
     def zero_without_grippers(self):
-        angs = {
+        angs_75 = {
+            0: 0.0,
+            1: 1.5708,
+            2: 0.0,
+            3: -1.5708,
+            4: 0.0,
+            5: 1.5708,
+            6: 0.0,
+        }
+        angs_16 = {
+            0: -1.5708,
+            1: 0.0,
+            2: 0.0,
+            3: 0.0,
+            4: 0.0,
+            5: 0.0,
+            # 13: 0.0,
+            # 12: 0.0,
+            # 0: -1.5708,
+            # 14: 0.0,
+            # 15: 0.0,
+            # 16: 0.0,
+        }
+        angs_hero = {
             0: 0.0,
             3: 0.0,
             4: 0.0,
@@ -721,6 +825,12 @@ def zero_without_grippers(self):
             7: 0.0,
             8: 0.0,
         }
+        if self.launch_case == "75":
+            angs = angs_75
+        elif self.launch_case == "16":
+            angs = angs_16
+        else:
+            angs = angs_hero
         for leg in self.get_active_leg():
             # for leg in [self.legs[4]]:
             for num, ang in angs.items():
@@ -913,6 +1023,10 @@ def joy_released(self, button_name: ButtonName):
         self.stop_all_joints()
         # self.pinfo(f"released: {dic_key}")
 
+    def get_joint_index(self, selected_joint: int) -> Optional[int]:
+        # self.pinfo(f"yes: {self.joint_mapping}")
+        return self.joint_mapping.get(selected_joint)
+
     @error_catcher
     def joySUBCBK(self, msg: Joy):
         """Processes incomming joy messages.
@@ -938,7 +1052,6 @@ def joySUBCBK(self, msg: Joy):
         return
 
     def joint_control_joy(self):
-
         bits = self.joy_state.bits
         if not self.any_pressed(bits, ["stickL", "stickR"]):
             self.joint_timer.cancel()
@@ -967,7 +1080,7 @@ def joint_control_joy(self):
             ),
         }
 
-        joint_mapping = {
+        joint_mapping_default = {
             ("L1", "stickL_vert"): 1,
             ("L1", "stickL_horiz"): 2,
             ("L1", "stickR_vert"): 9,
@@ -979,6 +1092,33 @@ def joint_control_joy(self):
             ("L2", "stickL_vert"): 5,
         }
 
+        joint_mapping_75 = {
+            ("L1", "stickL_vert"): 1,
+            ("L1", "stickL_horiz"): 2,
+            ("L1", "stickR_vert"): 3,
+            ("L1", "stickR_horiz"): 4,
+            ("R1", "stickL_vert"): 5,
+            ("R1", "stickL_horiz"): 6,
+            ("R1", "stickR_vert"): 7,
+        }
+
+        joint_mapping_16 = {
+            ("L1", "stickL_vert"): 1,
+            ("L1", "stickL_horiz"): 2,
+            ("L1", "stickR_vert"): 3,
+            ("L1", "stickR_horiz"): 4,
+            ("R1", "stickL_vert"): 5,
+            ("R1", "stickL_horiz"): 6,
+        }
+        joint_mapping = joint_mapping_default
+
+        if self.launch_case == "75":
+            joint_mapping = joint_mapping_75
+        elif self.launch_case == "16":
+            joint_mapping = joint_mapping_16
+        else:
+            joint_mapping = joint_mapping_default
+
         held_buttons = []
         if self.any_pressed(bits, ["L1"]):
             held_buttons.append("L1")
@@ -1003,7 +1143,7 @@ def joint_control_joy(self):
         if selected_joint is None or stick_to_use is None:
             return
 
-        joint_ind = STICKER_TO_ALPHAB.get(selected_joint)
+        joint_ind = self.get_joint_index(selected_joint)
         if joint_ind is None:
             return
 
@@ -1348,7 +1488,10 @@ def enter_dragon_mode(self) -> None:
             (Key.KEY_DOWN, ANY): [self.dragon_base_lookdown],
             # joystick mapping
             ("x", ANY): [self.default_dragon],
-            ("left", BUTT_INTS["left"]): [self.dragon_back_right, self.dragon_front_left],
+            ("left", BUTT_INTS["left"]): [
+                self.dragon_back_right,
+                self.dragon_front_left,
+            ],
             ("right", BUTT_INTS["right"]): [
                 self.dragon_back_left,
                 self.dragon_front_right,
@@ -1476,6 +1619,9 @@ def enter_joint_mode(self) -> None:
         Returns:
             InputMap for joint control
         """
+        self.refresh_joint_mapping()
+        # self.pinfo(self.get_active_leg_keys(1))
+
         self.pinfo(f"Joint Control Mode")
         submap: InputMap = {
             (Key.KEY_W, ANY): [lambda: self.set_joint_speed(MAX_JOINT_SPEED)],
@@ -1485,6 +1631,8 @@ def enter_joint_mode(self) -> None:
             (Key.KEY_O, ANY): [lambda: self.minimal_wheel_speed(1000000)],
             (Key.KEY_L, ANY): [lambda: self.minimal_wheel_speed(-10000000)],
             (Key.KEY_P, ANY): [lambda: self.minimal_wheel_speed(0.0)],
+            # (Key.KEY_G, ANY): [self.switch_to_grip_ur16],
+            (Key.KEY_R, ANY): [self.refresh_joint_mapping],
             # joy mapping
             ("stickL", BUTT_INTS["stickL"] + BUTT_INTS["L1"]): [self.joint_timer_start],
             ("stickR", BUTT_INTS["stickR"] + BUTT_INTS["L1"]): [self.joint_timer_start],
@@ -1505,7 +1653,7 @@ def enter_joint_mode(self) -> None:
             (8, Key.KEY_9),
         ]
         for n, keyb in one2nine_keys:
-            n = STICKER_TO_ALPHAB[n + 1]
+            n = self.get_joint_index(n + 1)
             submap[(keyb, ANY)] = [lambda n=n: self.select_joint(n)]
 
         self.sub_map = submap
@@ -1583,7 +1731,9 @@ def easy_mode(self):
             ("down", BUTT_INTS["down"] + BUTT_INTS["L2"]): [
                 lambda: self.dragon_wheel_speed(-100000)
             ],
-            ("x", BUTT_INTS["x"] + BUTT_INTS["L2"]): [lambda: self.dragon_wheel_speed(0)],
+            ("x", BUTT_INTS["x"] + BUTT_INTS["L2"]): [
+                lambda: self.dragon_wheel_speed(0)
+            ],
             # ("R2", ANY): [self.recover_legs],
             # ("R2", BUTT_INTS["L2"] + BUTT_INTS["R2"]): [self.recover_all],
             # ("L2", BUTT_INTS["L2"] + BUTT_INTS["R2"]): [self.recover_all],

src/easy_robot_control/easy_robot_control/gait_node.py

@@ -70,7 +70,7 @@ def __init__(self, joint_name: str, prefix: str, parent_leg: "Leg"):
         self.speed_set_angle_save: Optional[float] = None
         self.speedTMR = self.node.create_timer(0.05, self.speedTMRCBK)
         self.speedTMR.cancel()
-        self.MAX_DELTA = np.deg2rad(7)
+        self.MAX_DELTA = np.deg2rad(20)
 
     def is_active(self):
         if self.state.time is None:

src/easy_robot_control/easy_robot_control/joint_state_interface.py

@@ -417,7 +417,9 @@ def __init__(self):
 
         self.wait_for_lower_level(["rviz_interface_alive"], all_requiered=False)
 
-        self.pinfo(f"""{bcolors.OKBLUE}Interface connected to motors :){bcolors.ENDC}""")
+        self.pinfo(
+            f"""{bcolors.OKBLUE}Interface connected to motors :){bcolors.ENDC}"""
+        )
 
         # V Params V
         #   \  /   #

src/easy_robot_control/launch/ur16e.launch.py

@@ -18,7 +18,7 @@
 
 # leg number -> end effector (number or link name)
 LEGS_DIC = {
-    1: "palm_straight",
+    16: "palm_straight",
 }
 
 lvl_builder = LevelBuilder(robot_name=ROBOT_NAME, leg_dict=LEGS_DIC)