superbot2 0.1.0__py3-none-any.whl

superbot/__init__.py

@@ -0,0 +1,5 @@
+"""
+SuperBot - A modular robotics framework
+"""
+
+__version__ = "0.1.0"

superbot/bots/base_bot.py

@@ -0,0 +1,103 @@
+"""
+Base robot class with shared control API
+All specific robot implementations should inherit from this class
+"""
+
+import threading
+import time
+from abc import ABC, abstractmethod
+from typing import Optional
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from loguru import logger
+
+
+class BaseRobot(ABC):
+    """
+    Abstract base class for all robots with shared control API.
+    Each specific robot implementation should inherit from this class.
+    """
+
+    def __init__(self):
+        self._state_cache = {}
+        self._running = False
+        self.connected = False
+        self._state_thread = None
+
+    @abstractmethod
+    def connect(self):
+        """Connect to the robot hardware."""
+        pass
+
+    @abstractmethod
+    def disconnect(self):
+        """Disconnect from the robot hardware."""
+        pass
+
+    @abstractmethod
+    def _state_update_loop(self):
+        """Continuously update the robot state in a background thread."""
+        pass
+
+    def start_state_monitoring(self):
+        """Start the background thread for state monitoring."""
+        if self._state_thread is None or not self._state_thread.is_alive():
+            self._running = True
+            self._state_thread = threading.Thread(
+                target=self._state_update_loop, daemon=True
+            )
+            self._state_thread.start()
+
+    def stop_state_monitoring(self):
+        """Stop the background thread for state monitoring."""
+        self._running = False
+        if self._state_thread:
+            self._state_thread.join(
+                timeout=1.0
+            )  # Wait up to 1 second for thread to finish
+
+    @abstractmethod
+    def set_gripper(
+        self,
+        left_value: float,
+        right_value: float,
+        speed: int = 1000,
+        force: int = 0,
+    ):
+        """Control the grippers of the robot."""
+        pass
+
+    @abstractmethod
+    def move_to_joint(self, action_joint):
+        """Move the robot to specific joint positions."""
+        pass
+
+    @abstractmethod
+    def move_to_pose(
+        self,
+        left_target: Optional[np.ndarray] = None,
+        right_target: Optional[np.ndarray] = None,
+        duration: float = 1,
+    ):
+        """Move the robot to specific end-effector poses."""
+        pass
+
+    @abstractmethod
+    def go_home(self):
+        """Move the robot to its home position."""
+        pass
+
+    @abstractmethod
+    def safe_stop(self):
+        """Safely stop the robot motion."""
+        pass
+
+    @abstractmethod
+    def get_state_pos(self):
+        """Get the current joint positions."""
+        pass
+
+    @abstractmethod
+    def get_current_state(self):
+        """Get the complete current state of the robot."""
+        pass

superbot/bots/dual_piper.py

@@ -0,0 +1,360 @@
+"""
+Dual Piper robot implementation.
+This module implements the Dual Piper robot using the Piper SDK.
+"""
+
+import time
+from typing import Optional
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+from loguru import logger
+
+from .base_bot import BaseRobot
+
+
+class DualPiperRobot(BaseRobot):
+    """
+    Dual Piper Robot implementation using Piper SDK.
+    Implements the shared control API from BaseRobot.
+    """
+
+    def __init__(
+        self,
+        can_interfaces=("can_left", "can_right"),
+    ):
+        super().__init__()
+
+        # Attempt to import the Piper SDK
+        try:
+            from piper_sdk import C_PiperInterface_V2
+
+            self.piper_sdk_available = True
+        except ImportError:
+            logger.warning(
+                "Piper SDK not found. Please install piper_sdk for full functionality."
+            )
+            logger.warning("Using mock implementation for demonstration.")
+            # Use mock implementation
+            from .dual_piper_mock import C_PiperInterface_V2
+
+            self.piper_sdk_available = False
+
+        self.left_arm = None
+        self.right_arm = None
+        self.can_interfaces = can_interfaces
+
+        for can_if in can_interfaces:
+            arm = C_PiperInterface_V2(can_if)
+            arm.ConnectPort()
+            while not arm.EnablePiper():
+                time.sleep(0.01)
+
+            if "left" in can_if:
+                self.left_arm = arm
+                logger.info("Left arm connected on CAN interface: {}".format(can_if))
+            elif "right" in can_if:
+                self.right_arm = arm
+                logger.info("Right arm connected on CAN interface: {}".format(can_if))
+
+        if self.left_arm is None or self.right_arm is None:
+            raise RuntimeError("左右臂 CAN 接口未正确配置")
+
+        self._state_cache = {
+            "left_arm": {"qpos": [0.0] * 6},
+            "right_arm": {"qpos": [0.0] * 6},
+            "left_joint": {"qpos": [0.0] * 6},
+            "right_joint": {"qpos": [0.0] * 6},
+            "left_ctrl": {"qpos": [0.0] * 6},
+            "right_ctrl": {"qpos": [0.0] * 6},
+            "left_vr_pose": {"qpos": [0.0] * 7},
+            "right_vr_pose": {"qpos": [0.0] * 7},
+            "left_gripper": [0.0],
+            "right_gripper": [0.0],
+        }
+
+        self.connected = True
+        self.set_gripper(0.0, 0.0)
+        time.sleep(0.5)
+
+    def connect(self):
+        """Connect to the Dual Piper robot hardware."""
+        if not self.connected:
+            for can_if in self.can_interfaces:
+                arm = C_PiperInterface_V2(can_if)
+                arm.ConnectPort()
+                while not arm.EnablePiper():
+                    time.sleep(0.01)
+
+                if "left" in can_if:
+                    self.left_arm = arm
+                    logger.info(
+                        "Left arm connected on CAN interface: {}".format(can_if)
+                    )
+                elif "right" in can_if:
+                    self.right_arm = arm
+                    logger.info(
+                        "Right arm connected on CAN interface: {}".format(can_if)
+                    )
+
+            self.connected = True
+            self.start_state_monitoring()
+        return True
+
+    def disconnect(self):
+        """Disconnect from the Dual Piper robot hardware."""
+        self._running = False
+        self.safe_stop()
+        time.sleep(0.5)
+        if self.left_arm:
+            self.left_arm.DisConnectPort()
+        if self.right_arm:
+            self.right_arm.DisConnectPort()
+        self.connected = False
+
+    def _state_update_loop(self):
+        """Continuously update the robot state in a background thread."""
+        while self._running:
+            try:
+                left_pose = self.left_arm.GetArmEndPoseMsgs().end_pose
+                right_pose = self.right_arm.GetArmEndPoseMsgs().end_pose
+                left_joint = self.left_arm.GetArmJointMsgs().joint_state
+
+                right_joint = self.right_arm.GetArmJointMsgs().joint_state
+                left_ctrl = self.left_arm.GetArmJointCtrl().joint_ctrl
+                right_ctrl = self.right_arm.GetArmJointCtrl().joint_ctrl
+                # print(left_ctrl)
+                if left_pose:
+                    self._state_cache["left_arm"]["qpos"] = [
+                        left_pose.X_axis / 1000000,
+                        left_pose.Y_axis / 1000000,
+                        left_pose.Z_axis / 1000000,
+                        left_pose.RX_axis / 57295.7795,
+                        left_pose.RY_axis / 57295.7795,
+                        left_pose.RZ_axis / 57295.7795,
+                    ]
+                if left_ctrl:
+                    self._state_cache["left_ctrl"]["qpos"] = [
+                        left_ctrl.joint_1 / 57295.7795,
+                        left_ctrl.joint_2 / 57295.7795,
+                        left_ctrl.joint_3 / 57295.7795,
+                        left_ctrl.joint_4 / 57295.7795,
+                        left_ctrl.joint_5 / 57295.7795,
+                        left_ctrl.joint_6 / 57295.7795,
+                    ]
+                if left_joint:
+                    self._state_cache["left_joint"]["qpos"] = [
+                        left_joint.joint_1 / 57295.7795,
+                        left_joint.joint_2 / 57295.7795,
+                        left_joint.joint_3 / 57295.7795,
+                        left_joint.joint_4 / 57295.7795,
+                        left_joint.joint_5 / 57295.7795,
+                        left_joint.joint_6 / 57295.7795,
+                    ]
+                if right_pose:
+                    self._state_cache["right_arm"]["qpos"] = [
+                        right_pose.X_axis / 1000000,
+                        right_pose.Y_axis / 1000000,
+                        right_pose.Z_axis / 1000000,
+                        right_pose.RX_axis / 57295.7795,
+                        right_pose.RY_axis / 57295.7795,
+                        right_pose.RZ_axis / 57295.7795,
+                    ]
+                if right_ctrl:
+                    self._state_cache["right_ctrl"]["qpos"] = [
+                        right_ctrl.joint_1 / 57295.7795,
+                        right_ctrl.joint_2 / 57295.7795,
+                        right_ctrl.joint_3 / 57295.7795,
+                        right_ctrl.joint_4 / 57295.7795,
+                        right_ctrl.joint_5 / 57295.7795,
+                        right_ctrl.joint_6 / 57295.7795,
+                    ]
+                if right_joint:
+                    self._state_cache["right_joint"]["qpos"] = [
+                        right_joint.joint_1 / 57295.7795,
+                        right_joint.joint_2 / 57295.7795,
+                        right_joint.joint_3 / 57295.7795,
+                        right_joint.joint_4 / 57295.7795,
+                        right_joint.joint_5 / 57295.7795,
+                        right_joint.joint_6 / 57295.7795,
+                    ]
+                left_grip = self.left_arm.GetArmGripperMsgs().gripper_state
+                right_grip = self.right_arm.GetArmGripperMsgs().gripper_state
+
+                if left_grip:
+                    self._state_cache["left_gripper"] = [left_grip.grippers_angle / 1e6]
+
+                if right_grip:
+                    self._state_cache["right_gripper"] = [
+                        right_grip.grippers_angle / 1e6
+                    ]
+
+            except Exception as e:
+                logger.error(f"Error updating robot state: {e}")
+                pass
+
+            time.sleep(0.02)
+
+    def set_gripper(
+        self,
+        left_value: float,
+        right_value: float,
+        speed: int = 1000,
+        force: int = 0,
+    ):
+        """Control the grippers of the robot."""
+        max_open_um = 50000
+
+        left_pos = int((1.0 - left_value) * max_open_um)
+        right_pos = int((1.0 - right_value) * max_open_um)
+        self._state_cache["left_vr_pose"]["qpos"][6] = left_pos / 1000000.0
+        self._state_cache["right_vr_pose"]["qpos"][6] = right_pos / 1000000.0
+        self.left_arm.GripperCtrl(abs(left_pos), speed, 0x01, force)
+        self.right_arm.GripperCtrl(abs(right_pos), speed, 0x01, force)
+
+    def move_to_joint(self, action_joint):
+        """Move the robot to specific joint positions."""
+        factor = 57295.7795
+        if action_joint is not None:
+            joint_0 = round(action_joint[0] * factor)
+            joint_1 = round(action_joint[1] * factor)
+            joint_2 = round(action_joint[2] * factor)
+            joint_3 = round(action_joint[3] * factor)
+            joint_4 = round(action_joint[4] * factor)
+            joint_5 = round(action_joint[5] * factor)
+            joint_6 = round(action_joint[6] * 1000 * 1000)
+            self.left_arm.MotionCtrl_2(0x01, 0x01, 100, 0x00)
+            self.left_arm.JointCtrl(
+                joint_0, joint_1, joint_2, joint_3, joint_4, joint_5
+            )
+            self.left_arm.GripperCtrl(abs(joint_6), 1000, 0x01, 0)
+            joint_0 = round(action_joint[7] * factor)
+            joint_1 = round(action_joint[8] * factor)
+            joint_2 = round(action_joint[9] * factor)
+            joint_3 = round(action_joint[10] * factor)
+            joint_4 = round(action_joint[11] * factor)
+            joint_5 = round(action_joint[12] * factor)
+            joint_6 = round(action_joint[13] * 1000 * 1000)
+            self.right_arm.MotionCtrl_2(0x01, 0x01, 100, 0x00)
+            self.right_arm.JointCtrl(
+                joint_0, joint_1, joint_2, joint_3, joint_4, joint_5
+            )
+            self.right_arm.GripperCtrl(abs(joint_6), 1000, 0x01, 0)
+
+    def move_to_pose(
+        self,
+        left_target: Optional[np.ndarray] = None,
+        right_target: Optional[np.ndarray] = None,
+        duration: float = 1,
+    ):
+        """Move the robot to specific end-effector poses."""
+        factor = 1000.0
+
+        if left_target is not None:
+            pos = left_target[:3, 3] * 1000.0
+            rot = np.dot(
+                left_target[:3, :3], np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
+            )
+            rot = R.from_matrix(rot).as_euler("xyz", degrees=True)
+            # print(f"pos:{pos}, rot: {rot}")
+            self._state_cache["left_vr_pose"]["qpos"] = [
+                pos[0] / 1000.0,
+                pos[1] / 1000.0,
+                pos[2] / 1000.0,
+                rot[0],
+                rot[1],
+                rot[2],
+                self._state_cache["left_vr_pose"]["qpos"][6],
+            ]
+            # rot = [0, 85, 0]
+
+            self.left_arm.MotionCtrl_2(0x01, 0x00, int(duration * 100), 0x00)
+            self.left_arm.EndPoseCtrl(
+                int(pos[0] * factor),
+                int(pos[1] * factor),
+                int(pos[2] * factor),
+                int(rot[0] * factor),
+                int(rot[1] * factor),
+                int(rot[2] * factor),
+            )
+
+        if right_target is not None:
+            pos = right_target[:3, 3] * 1000.0
+            rot = np.dot(
+                right_target[:3, :3], np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
+            )
+            rot = R.from_matrix(rot).as_euler("xyz", degrees=True)
+            self._state_cache["right_vr_pose"]["qpos"] = [
+                pos[0] / 1000.0,
+                pos[1] / 1000.0,
+                pos[2] / 1000.0,
+                rot[0],
+                rot[1],
+                rot[2],
+                self._state_cache["right_vr_pose"]["qpos"][6],
+            ]
+            # print(f"pos_R:{pos}, rot_R: {rot}")
+            self.right_arm.MotionCtrl_2(0x01, 0x00, int(duration * 100), 0x00)
+            self.right_arm.EndPoseCtrl(
+                int(pos[0] * factor),
+                int(pos[1] * factor),
+                int(pos[2] * factor),
+                int(rot[0] * factor),
+                int(rot[1] * factor),
+                int(rot[2] * factor),
+            )
+
+    def go_home(self):
+        """Move the robot to its home position."""
+        self.left_arm.MotionCtrl_2(0x01, 0x01, 30, 0x00)
+        self.left_arm.JointCtrl(0, 0, 0, 0, 0, 0)
+        self.right_arm.MotionCtrl_2(0x01, 0x01, 30, 0x00)
+        self.right_arm.JointCtrl(0, 0, 0, 0, 0, 0)
+        self._state_cache["left_vr_pose"]["qpos"] = [
+            0.057,
+            0,
+            0.215,
+            0,
+            1.4835,
+            0,
+            0.05,
+        ]
+        self._state_cache["right_vr_pose"]["qpos"] = [
+            0.057,
+            0,
+            0.215,
+            0,
+            1.4835,
+            0,
+            0.05,
+        ]
+
+    def safe_stop(self):
+        """Safely stop the robot motion."""
+        self.left_arm.EmergencyStop()
+        self.right_arm.EmergencyStop()
+
+    def get_state_pos(self):
+        """Get the current joint positions."""
+        # print(f"self.jstate_cache{self._state_cache}")
+        return (
+            self._state_cache["left_joint"]["qpos"]
+            + self._state_cache["left_gripper"]
+            + self._state_cache["right_joint"]["qpos"]
+            + self._state_cache["right_gripper"]
+        )
+
+    def get_current_state(self):
+        """Get the complete current state of the robot."""
+        return {
+            "left_arm": self._state_cache["left_arm"],
+            "right_arm": self._state_cache["right_arm"],
+            "left_gripper": self._state_cache["left_gripper"],
+            "right_gripper": self._state_cache["right_gripper"],
+            "left_joint": self._state_cache["left_joint"],
+            "right_joint": self._state_cache["right_joint"],
+            "left_ctrl": self._state_cache["left_ctrl"],
+            "right_ctrl": self._state_cache["right_ctrl"],
+            "left_vr_pose": self._state_cache["left_vr_pose"],
+            "right_vr_pose": self._state_cache["right_vr_pose"],
+            "body": {"qpos": []},
+        }

superbot/sensors/base_sensor.py

@@ -0,0 +1,43 @@
+"""
+Sensors module for SuperBot
+Contains classes and utilities for managing robot sensors.
+"""
+
+from abc import ABC, abstractmethod
+import time
+from typing import Dict, Any, Optional
+import numpy as np
+
+
+class BaseSensor(ABC):
+    """Abstract base class for all sensors."""
+
+    def __init__(self, name: str, sensor_type: str):
+        self.name = name
+        self.sensor_type = sensor_type
+        self.last_reading_time = 0
+        self.is_connected = False
+
+    @abstractmethod
+    def connect(self):
+        """Connect to the sensor."""
+        pass
+
+    @abstractmethod
+    def disconnect(self):
+        """Disconnect from the sensor."""
+        pass
+
+    @abstractmethod
+    def read(self) -> Dict[str, Any]:
+        """Read data from the sensor."""
+        pass
+
+    def get_status(self) -> Dict[str, Any]:
+        """Get sensor status information."""
+        return {
+            "name": self.name,
+            "type": self.sensor_type,
+            "connected": self.is_connected,
+            "last_reading_time": self.last_reading_time,
+        }

superbot/sensors/cams/cam_rs.py

@@ -0,0 +1,140 @@
+import numpy as np
+import cv2
+from typing import Any, cast
+
+from loguru import logger
+
+try:
+    import pyrealsense2 as rs
+except ImportError:
+    print("Warning: pyrealsense2 not available. Camera functionality will be limited.")
+    rs = None
+
+# Help static analyzers: treat rs as dynamic Any when available
+if rs is not None:
+    rs = cast(Any, rs)
+
+
+class CameraWrapper:
+    def __init__(
+        self,
+        devices=None,
+        width=640,
+        height=480,
+        fps=30,
+        num_realsense=0,
+        cv_format="MJPEG",
+    ):
+        self.width = width
+        self.height = height
+        self.fps = fps
+        self.num_realsense = max(0, int(num_realsense))
+        self.cv_format = cv_format
+        self.cameras = []  # list of dicts: {type: 'rs'|'cv', handle: pipeline|cap}
+        self.device_ids = devices if devices is not None else []
+        self._open_cameras()
+        print(f"successfully opened {len(self.cameras)} cameras!")
+
+    def _open_cameras(self):
+        if not self.device_ids:
+            print("No devices provided for CameraWrapper")
+            return
+
+        for idx, dev in enumerate(self.device_ids):
+            # Decide camera type
+            use_realsense = idx < self.num_realsense
+
+            if use_realsense:
+                if rs is None:
+                    print(
+                        f"pyrealsense2 not available, skipping RealSense device at index {idx} (id: {dev})"
+                    )
+                    continue
+                try:
+                    serial = str(dev)
+                    pipeline = rs.pipeline()  # type: ignore[attr-defined]
+                    config = rs.config()  # type: ignore[attr-defined]
+                    config.enable_device(serial)
+                    config.enable_stream(rs.stream.color, self.width, self.height, rs.format.bgr8, self.fps)  # type: ignore[attr-defined]
+                    pipeline.start(config)
+                    self.cameras.append({"type": "rs", "handle": pipeline})
+                    print(f"RealSense camera {serial} opened successfully")
+                except Exception as e:
+                    print(f"Failed to open RealSense camera {dev}: {e}")
+            else:
+                try:
+                    device_index = int(dev)
+                    print(f"Ready to read deive: {device_index}")
+                    cap = cv2.VideoCapture(device_index)
+
+                    if self.cv_format == "MJPEG":
+                        cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG"))  # type: ignore[attr-defined]
+                    elif self.cv_format == "YUYV":
+                        cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))  # type: ignore[attr-defined]
+
+                    cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.width)
+                    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height)
+                    cap.set(cv2.CAP_PROP_FPS, self.fps)
+
+                    if not cap.isOpened():
+                        raise ValueError(f"Cannot open OpenCV camera {device_index}")
+
+                    self.cameras.append({"type": "cv", "handle": cap})
+                    print(f"OpenCV camera {device_index} opened successfully")
+                except Exception as e:
+                    print(f"Failed to open OpenCV camera {dev}: {e}")
+
+    def get_images(self):
+        images = []
+        if len(self.cameras) == 0:
+            # Return dummy images if no cameras available - use 640x480 which is expected by the model
+            for _ in range(max(1, len(self.device_ids))):
+                dummy_img = np.zeros((self.height, self.width, 3), dtype=np.uint8)
+                dummy_img[:, :, :] = 128  # Gray color instead of black
+                images.append(dummy_img)
+            return images
+
+        for cam in self.cameras:
+            if cam["type"] == "rs":
+                try:
+                    pipeline = cam["handle"]
+                    frames = pipeline.wait_for_frames()
+                    color_frame = frames.get_color_frame()
+                    if not color_frame:
+                        dummy_img = np.zeros(
+                            (self.height, self.width, 3), dtype=np.uint8
+                        )
+                        dummy_img[:, :, :] = 128
+                        images.append(dummy_img)
+                    else:
+                        img = np.asanyarray(color_frame.get_data())
+                        images.append(img)
+                except Exception as e:
+                    print(f"Error reading from RealSense: {e}")
+                    dummy_img = np.zeros((self.height, self.width, 3), dtype=np.uint8)
+                    dummy_img[:, :, :] = 128
+                    images.append(dummy_img)
+            elif cam["type"] == "cv":
+                cap = cam["handle"]
+                ret, frame = cap.read()
+                if not ret or frame is None:
+                    dummy_img = np.zeros((self.height, self.width, 3), dtype=np.uint8)
+                    dummy_img[:, :, :] = 128
+                    images.append(dummy_img)
+                else:
+                    images.append(frame)
+        return images
+
+    def release(self):
+        for cam in self.cameras:
+            if cam["type"] == "rs":
+                try:
+                    cam["handle"].stop()
+                except Exception:
+                    pass
+            elif cam["type"] == "cv":
+                try:
+                    cam["handle"].release()
+                except Exception:
+                    pass
+        self.cameras = []

superbot/stabilization/__init__.py

@@ -0,0 +1,203 @@
+"""
+Utils module for SuperBot
+Contains utility functions and classes for common robotic operations.
+"""
+
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+import json
+import yaml
+from typing import Union, Dict, Any, Tuple
+import time
+import logging
+from pathlib import Path
+
+
+def skew_symmetric(v):
+    """
+    Create skew-symmetric matrix from vector
+    For a vector v = [v1, v2, v3], returns:
+    [[0, -v3, v2],
+     [v3, 0, -v1],
+     [-v2, v1, 0]]
+    """
+    return np.array([
+        [0, -v[2], v[1]],
+        [v[2], 0, -v[0]],
+        [-v[1], v[0], 0]
+    ])
+
+
+def rotation_matrix_to_euler(R_mat: np.ndarray, sequence: str = 'xyz') -> np.ndarray:
+    """Convert rotation matrix to Euler angles."""
+    r = R.from_matrix(R_mat)
+    return r.as_euler(sequence, degrees=True)
+
+
+def euler_to_rotation_matrix(euler_angles: np.ndarray, sequence: str = 'xyz') -> np.ndarray:
+    """Convert Euler angles to rotation matrix."""
+    r = R.from_euler(sequence, euler_angles, degrees=True)
+    return r.as_matrix()
+
+
+def quaternion_to_rotation_matrix(quat: np.ndarray) -> np.ndarray:
+    """Convert quaternion [x, y, z, w] to rotation matrix."""
+    r = R.from_quat(quat)
+    return r.as_matrix()
+
+
+def rotation_matrix_to_quaternion(R_mat: np.ndarray) -> np.ndarray:
+    """Convert rotation matrix to quaternion [x, y, z, w]."""
+    r = R.from_matrix(R_mat)
+    return r.as_quat()
+
+
+def transformation_matrix(rotation: np.ndarray, translation: np.ndarray) -> np.ndarray:
+    """Create a 4x4 transformation matrix from rotation matrix and translation vector."""
+    T = np.eye(4)
+    T[:3, :3] = rotation
+    T[:3, 3] = translation
+    return T
+
+
+def get_inverse_transform(T: np.ndarray) -> np.ndarray:
+    """Compute inverse of a transformation matrix."""
+    R_inv = T[:3, :3].T
+    t_inv = -R_inv @ T[:3, 3]
+    T_inv = np.eye(4)
+    T_inv[:3, :3] = R_inv
+    T_inv[:3, 3] = t_inv
+    return T_inv
+
+
+def interpolate_positions(start_pos: np.ndarray, end_pos: np.ndarray, t: float) -> np.ndarray:
+    """
+    Linearly interpolate between two positions.
+    t should be between 0 and 1, where 0 is start_pos and 1 is end_pos.
+    """
+    if not 0 <= t <= 1:
+        raise ValueError("Interpolation parameter t must be between 0 and 1")
+    return start_pos + t * (end_pos - start_pos)
+
+
+def normalize_quaternion(quat: np.ndarray) -> np.ndarray:
+    """Normalize a quaternion."""
+    return quat / np.linalg.norm(quat)
+
+
+def compute_jacobian(robot_model, joint_angles: np.ndarray, link_idx: int) -> np.ndarray:
+    """
+    Compute the Jacobian matrix for a given robot configuration.
+    This is a simplified version - a full implementation would depend on the specific kinematic model.
+    """
+    # This is a placeholder implementation
+    # Real implementation would require forward kinematics and geometric Jacobian calculation
+    num_joints = len(joint_angles)
+    jacobian = np.zeros((6, num_joints))  # [linear_velocities, angular_velocities]
+    
+    # Placeholder computation
+    return jacobian
+
+
+def load_config(config_path: Union[str, Path]) -> Dict[str, Any]:
+    """Load configuration from YAML or JSON file."""
+    config_path = Path(config_path)
+    
+    if config_path.suffix.lower() in ['.yaml', '.yml']:
+        with open(config_path, 'r') as f:
+            return yaml.safe_load(f)
+    elif config_path.suffix.lower() == '.json':
+        with open(config_path, 'r') as f:
+            return json.load(f)
+    else:
+        raise ValueError(f"Unsupported config file format: {config_path.suffix}")
+
+
+def save_config(config: Dict[str, Any], config_path: Union[str, Path]):
+    """Save configuration to YAML or JSON file."""
+    config_path = Path(config_path)
+    
+    if config_path.suffix.lower() in ['.yaml', '.yml']:
+        with open(config_path, 'w') as f:
+            yaml.dump(config, f, default_flow_style=False)
+    elif config_path.suffix.lower() == '.json':
+        with open(config_path, 'w') as f:
+            json.dump(config, f, indent=2)
+    else:
+        raise ValueError(f"Unsupported config file format: {config_path.suffix}")
+
+
+class Timer:
+    """Simple timer utility for measuring execution time."""
+    
+    def __init__(self):
+        self.start_time = None
+        self.elapsed_time = 0
+        
+    def start(self):
+        """Start the timer."""
+        self.start_time = time.time()
+        
+    def stop(self) -> float:
+        """Stop the timer and return elapsed time in seconds."""
+        if self.start_time is None:
+            raise RuntimeError("Timer not started")
+        self.elapsed_time = time.time() - self.start_time
+        self.start_time = None
+        return self.elapsed_time
+        
+    def reset(self):
+        """Reset the timer."""
+        self.start_time = None
+        self.elapsed_time = 0
+
+
+def setup_logging(level: str = "INFO", log_file: str = None):
+    """Setup logging configuration."""
+    level_enum = getattr(logging, level.upper())
+    
+    handlers = []
+    formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
+    
+    console_handler = logging.StreamHandler()
+    console_handler.setLevel(level_enum)
+    console_handler.setFormatter(formatter)
+    handlers.append(console_handler)
+    
+    if log_file:
+        file_handler = logging.FileHandler(log_file)
+        file_handler.setLevel(level_enum)
+        file_handler.setFormatter(formatter)
+        handlers.append(file_handler)
+    
+    logging.basicConfig(level=level_enum, handlers=handlers)
+
+
+def validate_pose(pose: np.ndarray) -> bool:
+    """Validate that the input is a valid 4x4 pose matrix."""
+    if pose.shape != (4, 4):
+        return False
+    
+    # Check if upper-left 3x3 is a valid rotation matrix
+    R = pose[:3, :3]
+    if not np.allclose(R @ R.T, np.eye(3)):
+        return False
+    
+    if not np.isclose(np.linalg.det(R), 1.0):
+        return False
+    
+    return True
+
+
+def smooth_trajectory(waypoints: np.ndarray, smoothing_factor: float = 0.1) -> np.ndarray:
+    """Apply smoothing to a trajectory defined by waypoints."""
+    if len(waypoints) < 3:
+        return waypoints
+    
+    smoothed = np.copy(waypoints)
+    
+    for i in range(1, len(waypoints) - 1):
+        smoothed[i] = (1 - smoothing_factor) * waypoints[i] + \
+                     smoothing_factor * 0.5 * (waypoints[i-1] + waypoints[i+1])
+    
+    return smoothed

superbot/stabilization/realtime_chunking.py

@@ -0,0 +1,149 @@
+import math
+import threading
+import queue
+import time
+import numpy as np
+
+"""
+def worker(obs, horizon):
+    
+    obs: 任意观测
+    return: np.array with shape (horizon, 14)
+    
+
+
+example usage:
+rtc = RealtimeChunking(
+    worker=vla_worker,
+    horizon=32,
+    overlap=8,
+)
+
+while True:
+    obs = get_obs()
+    action = rtc.step(obs)
+    send_action(action)
+
+"""
+
+
+class ActionChunk:
+    def __init__(self, actions):
+        self.actions = np.array(actions)
+
+    def __len__(self):
+        return len(self.actions)
+
+
+class RealtimeChunkingSoft:
+    """
+    A simple realtime chunking implementation which can wraps any VLA model.
+    for REAL RTC as pi does, we need model supports action prefix.
+    """
+
+    def __init__(
+        self,
+        worker,
+        horizon,
+        overlap,
+        blend_mode="cosine",  # "linear" | "cosine" | "minjerk"
+        blend_grippers=False,
+        gripper_indices=[6, 13],
+    ):
+        assert overlap > 0
+        assert overlap < horizon
+
+        self.worker = worker
+        self.horizon = horizon
+        self.overlap = overlap
+        self.blend_mode = blend_mode
+        self.blend_grippers = blend_grippers
+        self.gripper_indices = gripper_indices
+
+        self.current_chunk = None
+        self.step_idx = 0
+
+        self.infer_thread = None
+        self.infer_queue = queue.Queue(maxsize=1)
+
+    def _infer_async(self, obs):
+        def _run():
+            actions = self.worker(obs, self.horizon)
+            self.infer_queue.put(actions)
+
+        self.infer_thread = threading.Thread(target=_run, daemon=True)
+        self.infer_thread.start()
+
+    def _should_start_inference(self):
+        return (
+            self.infer_thread is None and self.step_idx >= self.horizon - self.overlap
+        )
+
+    def _alpha(self, i):
+        s = (i + 1) / self.overlap
+
+        if self.blend_mode == "linear":
+            return s
+
+        if self.blend_mode == "cosine":
+            return 0.5 * (1.0 - math.cos(math.pi * s))
+
+        if self.blend_mode == "minjerk":
+            return 10 * s**3 - 15 * s**4 + 6 * s**5
+
+        raise ValueError(f"Unknown blend_mode: {self.blend_mode}")
+
+    def _blend(self, old_tail, new_head):
+        blended = np.zeros_like(old_tail)
+
+        for i in range(self.overlap):
+            a = self._alpha(i)
+            blended[i] = (1.0 - a) * old_tail[i] + a * new_head[i]
+
+        if not self.blend_grippers:
+            blended[:, self.gripper_indices] = new_head[:, self.gripper_indices]
+
+        return blended
+
+    def _try_commit_next_chunk(self):
+        if self.step_idx < self.horizon:
+            return
+        if self.infer_queue.empty():
+            return
+
+        new_actions = self.infer_queue.get()
+
+        old_tail = self.current_chunk.actions[
+            self.horizon - self.overlap : self.horizon
+        ]
+        new_head = new_actions[: self.overlap]
+
+        blended = self._blend(old_tail, new_head)
+        merged = np.concatenate((blended, new_actions[self.overlap :]), axis=0)
+
+        self.current_chunk = ActionChunk(merged)
+        self.step_idx = 0
+        self.infer_thread = None
+
+    def reset(self):
+        self.current_chunk = None
+        self.step_idx = 0
+        self.infer_thread = None
+        while not self.infer_queue.empty():
+            self.infer_queue.get()
+
+    def step(self, obs):
+        if self.current_chunk is None:
+            actions = self.worker(obs, self.horizon)
+            self.current_chunk = ActionChunk(actions)
+            self.step_idx = 0
+
+        action = self.current_chunk.actions[self.step_idx]
+        self.step_idx += 1
+
+        if self._should_start_inference():
+            self._infer_async(obs)
+
+        self._try_commit_next_chunk()
+
+        return action

superbot2-0.1.0.dist-info/METADATA

@@ -0,0 +1,82 @@
+Metadata-Version: 2.4
+Name: superbot2
+Version: 0.1.0
+Summary: A modular robotics framework for controlling various robots
+Author-email: SuperBot Team <team@superbot.com>
+License: MIT
+Project-URL: Homepage, https://github.com/superbot/superbot
+Project-URL: Repository, https://github.com/superbot/superbot.git
+Project-URL: Documentation, https://superbot.readthedocs.io/
+Classifier: Development Status :: 3 - Alpha
+Classifier: Intended Audience :: Developers
+Classifier: License :: OSI Approved :: MIT License
+Classifier: Programming Language :: Python :: 3
+Classifier: Programming Language :: Python :: 3.8
+Classifier: Programming Language :: Python :: 3.9
+Classifier: Programming Language :: Python :: 3.10
+Classifier: Programming Language :: Python :: 3.11
+Classifier: Programming Language :: Python :: 3.12
+Classifier: Programming Language :: Python :: 3.13
+Description-Content-Type: text/markdown
+Requires-Dist: numpy>=1.21.0
+Requires-Dist: scipy>=1.7.0
+Requires-Dist: PyYAML>=6.0
+Provides-Extra: dev
+Requires-Dist: pytest>=6.0; extra == "dev"
+Requires-Dist: black>=22.0; extra == "dev"
+Requires-Dist: flake8>=4.0; extra == "dev"
+Requires-Dist: mypy>=0.950; extra == "dev"
+Provides-Extra: docs
+Requires-Dist: sphinx>=4.0; extra == "docs"
+Requires-Dist: sphinx-rtd-theme>=1.0; extra == "docs"
+
+# SuperBot
+
+SuperBot is a modern, modular robotics framework designed for controlling dual-arm piper robots and other robotic systems. It provides a clean, intuitive API for robot control, sensor integration, and utility functions.
+
+## Features
+
+- Modular architecture with separate modules for bots, sensors, and utilities
+- Dual-arm piper robot control interface
+- Sensor management system
+- Utility functions for robotics operations
+- Clean, well-documented API
+
+## Installation
+
+```bash
+pip install superbot
+```
+
+## Quick Start
+
+```python
+from superbot import RobotWrapper
+
+# Initialize the robot wrapper with CAN interfaces
+robot = RobotWrapper(can_interfaces=("can_left", "can_right"))
+
+# Move to a specific pose
+import numpy as np
+target_pose = np.eye(4)  # 4x4 identity matrix as example
+robot.move_to_pose(left_target=target_pose, right_target=target_pose)
+
+# Close the grippers halfway
+robot.set_gripper(0.5, 0.5)
+
+# Get current robot state
+state = robot.get_current_state()
+print(state)
+```
+
+## Architecture
+
+SuperBot follows a modular design:
+
+- **bots**: Contains robot-specific implementations (dual_piper, etc.)
+- **sensors**: Sensor management and interfaces
+- **utils**: Common utility functions for robotics operations
+
+## Contributing
+
+Contributions are welcome! Please see the contributing guidelines for details.

superbot2-0.1.0.dist-info/RECORD

@@ -0,0 +1,15 @@
+superbot/__init__.py,sha256=b9R5pWwWJujbsFqglG4sIxKvYFFBz-MzJmeIJDpOB98,71
+superbot/bots/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+superbot/bots/base_bot.py,sha256=e5FuYSpiikwrDYV9R3bQQNav1KTAYLYXwxww3Sz3D6U,2726
+superbot/bots/dual_piper.py,sha256=weeFy5yM1My4TkZ9jeWN5s0sIrYm9tcN8lnUqLi10v8,13810
+superbot/sensors/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+superbot/sensors/base_sensor.py,sha256=9w9K7UyNBucFaLm8BOS7ApgiCtCcA7e49hDmuvHP_HY,1057
+superbot/sensors/cams/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+superbot/sensors/cams/cam_rs.py,sha256=peulTrdGQeulDDArD7OrG8Jd1AIj3lWrxK1lszYc2QE,5544
+superbot/stabilization/__init__.py,sha256=odhzh4dok1cAGDf4rIJ9fVVE5djOwp6rMearJ-YMRg8,6346
+superbot/stabilization/realtime_chunking.py,sha256=uAOlx7uj_BNPwl_fxe-prN1CPC7ZHzaIjjULfZUVRrs,3732
+superbot/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+superbot2-0.1.0.dist-info/METADATA,sha256=OzzqwTrLuDCMCuFTzdK2tOcuDXTcLaO1_PC9pVNKCNM,2631
+superbot2-0.1.0.dist-info/WHEEL,sha256=_zCd3N1l69ArxyTb8rzEoP9TpbYXkqRFSNOD5OuxnTs,91
+superbot2-0.1.0.dist-info/top_level.txt,sha256=ydMxsX5c2ooKqU-gqzhLcqehv-2ZIrDRUrlqh-kpW14,9
+superbot2-0.1.0.dist-info/RECORD,,

superbot2-0.1.0.dist-info/WHEEL

@@ -0,0 +1,5 @@
+Wheel-Version: 1.0
+Generator: setuptools (80.9.0)
+Root-Is-Purelib: true
+Tag: py3-none-any
+

superbot2-0.1.0.dist-info/top_level.txt

@@ -0,0 +1 @@
+superbot