update with some customer's request

Author: i2rt-software

.gitignore

@@ -89,4 +89,3 @@ i2rt.egg-info/
 
 # Ignore temporary scripts or generated files
 scripts/temp_*/
-

README.md

@@ -44,14 +44,14 @@ sh scripts/reset_all_can.sh
 
 ## Gripper type
 
-Currently YAM supports three different grippers: 
+Currently YAM supports three different grippers:
 ![YAM supported Grippers](./assets/photos/yam_three_grippers.png)
 
 | Gripper Name        | Description |
 |---------------------|-------------|
-| `yam_compact_small` | Zero-linkage crank gripper, optimized for minimizing gripper width. |
-| `yam_lw_gripper`    | Linear gripper with smaller DM3507 motor. Lightweight, but requires calibration or starting with the gripper in the closed configuration. |
-| `yam_teaching_handle` | Used for the leader arm setup. Includes a trigger to control the gripper and two customizable buttons that can be mapped to different functions. |
+| `crank_4310`        | Zero-linkage crank gripper, optimized for minimizing gripper width. |
+| `linear_3507`       | Linear gripper with smaller DM3507 motor. Lightweight, but requires calibration or starting with the gripper in the closed configuration. |
+| `yam_teaching_handle`| Used for the leader arm setup. Includes a trigger to control the gripper and two customizable buttons that can be mapped to different functions. |
 
 The linear gripper requires an additional calibration step because its motor must rotate more than 2π radians to complete the full stroke.
 
@@ -84,7 +84,7 @@ robot.command_joint_pos(target_pos)
 ### Running the arm and visualizing it
 To launch the follower robot run.
 ```bash
-python scripts/minimum_gello.py --mode follower
+python scripts/minimum_gello.py --gripper $YOUR_FOLLOWER_ARM_GRIPPER --mode follower
 ```
 
 To launch the robot mujoco visualizer run
@@ -177,6 +177,7 @@ python i2rt/flow_base/flow_base_controller.py
 ### Getting started
 ```python
 from i2rt.flow_base.flow_base_controller import Vehicle
+import time
 
 # Get a robot instance
 vehicle = Vehicle()

devices/FlowBase.desktop

@@ -0,0 +1,8 @@
+[Desktop Entry]
+Type=Application
+Name=FlowBase
+Comment=Run my Python program
+Exec=lxterminal -e "bash -c '/usr/bin/python3 /home/i2rt/i2rt/i2rt/flow_base/flow_base_controller.py --channel can0 --game_pad shanwan; echo; echo \"Process finished. Press Enter to close...\"; read'"
+Icon=/home/i2rt/Documents/base_icon.png
+Terminal=true
+Categories=Utility;

devices/install_devices.sh

@@ -0,0 +1,28 @@
+#!/bin/sh
+
+USER_ID="$(id -u)"
+USER=$(logname)
+
+if [ "$USER_ID" -ne 0 ]; then
+    echo "Please run this as root."
+    exit 1
+fi
+
+INSTALL_DIR=$(dirname "$0")
+RULES_DIR="$INSTALL_DIR/rules/*.rules"
+
+for udev_rule in $RULES_DIR; do
+    rule=$(basename "$udev_rule")
+    echo "Installing $rule"
+    cp $udev_rule /etc/udev/rules.d/$rule
+done
+
+echo "Adding $LOGNAME to group plugdev, video"
+adduser $USER plugdev
+adduser $USER video
+
+echo "Reloading udev rules..."
+udevadm control --reload-rules
+udevadm trigger
+
+echo "Finished, please re-insert devices."

devices/pi_setup.md

@@ -0,0 +1,14 @@
+## Create image from SD card
+sudo dd if=/dev/sdb of=pi_system.img bs=4M status=progress
+sync
+
+## Flash image into new SD card
+sudo wipefs -a /dev/sdc
+sudo dd if=pi_system.img of=/dev/sdc bs=4M status=progress
+sync
+
+## Eject SD card
+eject /dev/sdc
+
+## Reduce image size
+sudo ./pishrink.sh pi_system.img pi_system_shrunk.img

devices/rules/flow_base.rules

@@ -0,0 +1,12 @@
+# Flow Base CAN
+# Auto load gs_usb driver
+ACTION=="add|change", SUBSYSTEM=="usb", ATTR{idVendor}=="1d50", ATTR{idProduct}=="606f", RUN+="/sbin/modprobe gs_usb"
+
+# Bind device to gs_usb driver
+ACTION=="add|change", SUBSYSTEMS=="usb", ATTRS{idVendor}=="1d50", ATTRS{idProduct}=="606f", \
+RUN+="/bin/sh -c 'echo 1d50 606f > /sys/bus/usb/drivers/gs_usb/new_id'"
+
+# Configure CAN interface when detected
+SUBSYSTEM=="net", ENV{INTERFACE}=="can0|FlowBaseCAN*", ATTRS{bInterfaceNumber}=="00", \
+RUN+="/sbin/ip link set $name type can bitrate 1000000", \
+RUN+="/sbin/ip link set $name up"

devices/setup_flow_desktop.sh

@@ -0,0 +1,10 @@
+#!/bin/sh
+
+USER_ID="$(id -u)"
+USER=$(logname)
+
+INSTALL_DIR=$(dirname "$0")
+cp $INSTALL_DIR/FlowBase.desktop ~/Desktop/
+
+gio set ~/Desktop/FlowBase.desktop metadata::trusted true
+chmod +x ~/Desktop/FlowBase.desktop

devices/setup_led.sh

@@ -0,0 +1,6 @@
+cd ~/
+git clone https://github.com/goodtft/LCD-show.git
+chmod -R 755 LCD-show
+cd LCD-show/
+
+sudo ./LCD24-show

doc/set_persist_id_socket_can.md

@@ -56,11 +56,11 @@ $ ip link show
 2: enp5s0: <BROADCAST,MULTICAST,UP,LOWER_UP> mtu 1500 qdisc mq state UP mode DEFAULT group default qlen 1000
     link/ether d8:43:ae:b7:43:0b brd ff:ff:ff:ff:ff:ff
 5: tailscale0: <POINTOPOINT,MULTICAST,NOARP,UP,LOWER_UP> mtu 1280 qdisc fq_codel state UNKNOWN mode DEFAULT group default qlen 500
-    link/none 
+    link/none
 6: can_right: <NOARP,UP,LOWER_UP,ECHO> mtu 16 qdisc pfifo_fast state UP mode DEFAULT group default qlen 10
-    link/can 
+    link/can
 7: can_left: <NOARP,UP,LOWER_UP,ECHO> mtu 16 qdisc pfifo_fast state UP mode DEFAULT group default qlen 10
-    link/can 
+    link/can
 ```
 
 You should see that the CAN device is named `can_right`/`can_left`.

i2rt/flow_base/flow_base_controller.py

@@ -457,52 +457,64 @@ def joint_state_spec(self) -> Array:
     def num_dofs(self) -> int:
         return self.num_dofs
 
+    def running(self) -> bool:
+        return self.caster_module_controller.motor_interface.running
+
 
 if __name__ == "__main__":
     import os
-
-    os.environ["SDL_VIDEODRIVER"] = "dummy"  # Force headless mode
     import time
+    import argparse
+    from i2rt.utils.gamepad_utils import Gamepad
 
-    import pygame
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--channel", type=str, default="can0")
 
-    vehicle = Vehicle(max_vel=(0.2, 0.2, 0.3), max_accel=(1, 1, 1))
     # Initialize pygame and joystick
     pygame.init()
     pygame.joystick.init()
+    CALIBRATION_RETRY_DELAY = 1
+    DEADZONE = 0.05
+    args = parser.parse_args()
 
-    if pygame.joystick.get_count() == 0:
-        print("No joystick/gamepad connected!")
-        exit()
-    else:
-        print(f"Detected {pygame.joystick.get_count()} joystick(s).")
+    max_vel = np.array([0.4, 0.4, 1.2])
+    max_accel = np.array([0.5, 0.5, 1])
 
-    # Initialize the joystick
-    joy = pygame.joystick.Joystick(0)
-    joy.init()
+    vehicle = Vehicle(max_vel=max_vel, max_accel=max_accel, channel=args.channel)
 
     print(f"Joystick Name: {joy.get_name()}")
     print(f"Number of Axes: {joy.get_numaxes()}")
     print(f"Number of Buttons: {joy.get_numbuttons()}")
 
+    # Check all x, y, th are 0 at the beginning, if not ask user to check joystick
+    while True:
+        # Pump events to update joystick state
+        pygame.event.pump()
+        four_axis = [joy.get_axis(1), joy.get_axis(0), joy.get_axis(2), joy.get_axis(3)]
+        if all(np.abs(axis) < DEADZONE for axis in four_axis):
+            logging.info("Joystick is at rest, please check joystick")
+            break
+        else:
+            logging.warning(f"four_axis: {four_axis}")
+            logging.warning("Joystick's rest position is not at the center, please check joystick")
+            time.sleep(CALIBRATION_RETRY_DELAY)
+
     # Main loop to read joystick inputs
+    gamepad = Gamepad()
 
+    count = 0
     try:
         while True:
-            # for _ in range(100):
-            pygame.event.pump()
-
-            # Read inputs
-            start = joy.get_button(7)  # Example button
-            x = joy.get_axis(1)  # Left stick Y-axis
-            y = joy.get_axis(0)  # Left stick X-axis
-            th = joy.get_axis(2)  # Right stick X-axis
-
-            user_cmd = np.array([-x, y, -th])
-            # if < 0.05 force to zero
-            user_cmd[np.abs(user_cmd) < 0.05] = 0
-            print(f"user_cmd: {user_cmd}")
-            vehicle.set_target_velocity(user_cmd, frame="local")
+            user_cmd = gamepad.get_user_cmd()
+            if not vehicle.running():
+                print("Motor interface is not running, exiting...")
+                print(f"Please check the E stop or the motor connection. ")
+                break
+            if count % 20 == 0:
+                # print up 1 float point
+                print(f"user_cmd: {user_cmd[0]:.1f}, {user_cmd[1]:.1f}, {user_cmd[2]:.1f}")
+            count += 1
+            vehicle.set_target_velocity(user_cmd*max_vel, frame="local")
 
             time.sleep(0.02)
     except KeyboardInterrupt: