diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index d2af4832ab..73ccc7b01a 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -57,6 +57,7 @@ class GlobalConfig(BaseSettings): unitree_aes_128_key: str | None = None xarm7_ip: str | None = None xarm6_ip: str | None = None + lite6_ip: str | None = None can_port: str | None = None device_path: str | None = None # device path for real robot (e.g. /dev/ttyUSB0) simulation: str = "" diff --git a/dimos/hardware/manipulators/xarm/adapter.py b/dimos/hardware/manipulators/xarm/adapter.py index 4aaebea102..4bb2727a96 100644 --- a/dimos/hardware/manipulators/xarm/adapter.py +++ b/dimos/hardware/manipulators/xarm/adapter.py @@ -93,6 +93,10 @@ def __init__( # anywhere, and ManipulationModule already adopts wherever it is as the # "init" preset from the first joint state it receives. self._initial_positions = None if initial_positions is None else list(initial_positions) + self._lite6: bool = False + # Lite 6 gripper is open/close over tool GPIO with no feedback; echo the last command. + self._lite6_gripper: float = XARM_GRIPPER_MIN + self._lite6_gripper_sent: bool = False def connect(self) -> bool: """Connect to XArm via TCP/IP.""" @@ -103,6 +107,8 @@ def connect(self) -> bool: if not self._arm.connected: logger.error("XArm at %s not reachable (connected=False)", self._ip) return False + # Mirrors the SDK's is_lite6, which the XArmAPI wrapper does not expose. + self._lite6 = self._arm.axis == 6 and self._arm.device_type == 9 # Initialize to servo mode for high-frequency control self._arm.set_mode(_XARM_MODE_SERVO_CARTESIAN) # Mode 1 = servo mode @@ -128,7 +134,7 @@ def get_info(self) -> ManipulatorInfo: """Get XArm information.""" return ManipulatorInfo( vendor="UFACTORY", - model=f"xArm{self._arm_dof}", + model="Lite6" if self._lite6 else f"xArm{self._arm_dof}", dof=self._dof, ) @@ -419,6 +425,8 @@ def _read_gripper(self) -> float: """Read the gripper position in SDK units (0-850).""" if not self._arm: return 0.0 + if self._lite6: + return self._lite6_gripper result = self._arm.get_gripper_position() code: int = result[0] @@ -431,6 +439,18 @@ def _write_gripper(self, position: float) -> bool: """Command the gripper in SDK units (0-850).""" if not self._arm: return False + if self._lite6: + opening = position > (XARM_GRIPPER_MIN + XARM_GRIPPER_MAX) / 2 + target = XARM_GRIPPER_MAX if opening else XARM_GRIPPER_MIN + if target == self._lite6_gripper and self._lite6_gripper_sent: + return True # tool GPIO write per tick spams the controller; send transitions only + lite_code: int = ( + self._arm.open_lite6_gripper() if opening else self._arm.close_lite6_gripper() + ) + if lite_code == 0: + self._lite6_gripper = target + self._lite6_gripper_sent = True + return lite_code == 0 if not self._gripper_enabled: self._arm.set_gripper_enable(True) diff --git a/dimos/hardware/manipulators/xarm/test_adapter.py b/dimos/hardware/manipulators/xarm/test_adapter.py index ff1315a4e9..8975cfc0af 100644 --- a/dimos/hardware/manipulators/xarm/test_adapter.py +++ b/dimos/hardware/manipulators/xarm/test_adapter.py @@ -30,6 +30,8 @@ class _FakeXArmSdk: instances: ClassVar[list[_FakeXArmSdk]] = [] + axis = 6 + device_type = 6 # 9 = Lite 6 def __init__(self, ip: str) -> None: self.instances.append(self) @@ -102,6 +104,14 @@ def set_gripper_position(self, position: float, *, wait: bool) -> int: self.actions.append(("set_gripper_position", position, wait)) return 0 + def open_lite6_gripper(self) -> int: + self.actions.append(("open_lite6_gripper",)) + return 0 + + def close_lite6_gripper(self) -> int: + self.actions.append(("close_lite6_gripper",)) + return 0 + @pytest.fixture def xarm_adapter_module(monkeypatch: pytest.MonkeyPatch) -> Iterator[ModuleType]: @@ -198,3 +208,25 @@ def test_gripper_command_reaches_sdk_once_in_native_units( sdk = _FakeXArmSdk.instances[-1] assert sdk.servo_joint_commands == [[0.0] * 7] assert sdk.gripper_commands == [850.0] + + +def test_lite6_detected_and_uses_gpio_gripper( + xarm_adapter_module: ModuleType, monkeypatch: pytest.MonkeyPatch +) -> None: + monkeypatch.setattr(_FakeXArmSdk, "device_type", 9) + adapter = xarm_adapter_module.XArmAdapter(address="192.0.2.10", dof=7, arm_dof=6) + assert adapter.connect() + assert adapter.get_info().model == "Lite6" + + assert adapter.activate() + arm = _FakeXArmSdk.instances[-1] + + assert adapter.write_joint_positions([0.0] * 6 + [850.0]) + assert adapter.write_joint_positions([0.0] * 6 + [850.0]) # repeat: no resend + assert adapter.write_joint_positions([0.0] * 6 + [0.0]) + assert arm.gripper_commands == [] + assert [a for a in arm.actions if "lite6_gripper" in a[0]] == [ + ("open_lite6_gripper",), + ("close_lite6_gripper",), + ] + assert adapter.read_joint_positions()[-1] == 0.0 diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index e80c50e777..d9ca4d4e87 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -30,6 +30,7 @@ "coordinator-dual-xarm": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_dual_xarm", "coordinator-flowbase": "dimos.control.blueprints.mobile:coordinator_flowbase", "coordinator-flowbase-keyboard-teleop": "dimos.control.blueprints.mobile:coordinator_flowbase_keyboard_teleop", + "coordinator-lite6": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_lite6", "coordinator-mobile-manip-mock": "dimos.control.blueprints.mobile:coordinator_mobile_manip_mock", "coordinator-mock": "dimos.robot.manipulators.common.mock:coordinator_mock", "coordinator-mock-twist-base": "dimos.control.blueprints.mobile:coordinator_mock_twist_base", @@ -80,6 +81,7 @@ "habitat-voxel": "dimos.simulation.habitat.blueprints:habitat_voxel", "keyboard-teleop-a1z": "dimos.robot.manipulators.a1z.blueprints.teleop:keyboard_teleop_a1z", "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints.teleop:keyboard_teleop_a750", + "keyboard-teleop-lite6": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_lite6", "keyboard-teleop-openyam": "dimos.robot.manipulators.openyam.blueprints.teleop:keyboard_teleop_openyam", "keyboard-teleop-openyam-planner": "dimos.robot.manipulators.openyam.blueprints.teleop:keyboard_teleop_openyam_planner", "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints.teleop:keyboard_teleop_piper", @@ -87,6 +89,7 @@ "keyboard-teleop-xarm7": "dimos.robot.manipulators.xarm.blueprints.teleop:keyboard_teleop_xarm7", "learning-collect-webxr-piper": "dimos.imitation.collection.blueprint:learning_collect_webxr_piper", "learning-collect-webxr-xarm7": "dimos.imitation.collection.blueprint:learning_collect_webxr_xarm7", + "lite6-planner-coordinator": "dimos.robot.manipulators.xarm.blueprints.basic:lite6_planner_coordinator", "mid360": "dimos.hardware.sensors.lidar.livox.livox_blueprints:mid360", "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", "mid360-fastlio-ray-trace": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_ray_trace", diff --git a/dimos/robot/manipulators/xarm/blueprints/basic.py b/dimos/robot/manipulators/xarm/blueprints/basic.py index 56c45109b7..bd4a958ec0 100644 --- a/dimos/robot/manipulators/xarm/blueprints/basic.py +++ b/dimos/robot/manipulators/xarm/blueprints/basic.py @@ -23,7 +23,9 @@ from dimos.robot.manipulators.xarm.config import ( XARM6_SIM_PATH, XARM7_SIM_PATH, + lite6_hardware, make_dual_xarm6_model_config, + make_lite6_model_config, make_xarm7_model_config, make_xarm_hardware, xarm6_hardware, @@ -94,6 +96,29 @@ def _gripper_task() -> TaskConfig: *mujoco_if_sim(XARM6_SIM_PATH, len(_coordinator_xarm6_hw.joints)), ) +_coordinator_lite6_hw = lite6_hardware("arm", gripper=True) + +coordinator_lite6 = ControlCoordinator.blueprint( + hardware=[_coordinator_lite6_hw], + tasks=[trajectory_task(_coordinator_lite6_hw), _gripper_task()], +) + +_lite6_hw = lite6_hardware("arm", gripper=True, mock_without_address=True) + +lite6_planner_coordinator = autoconnect( + planner( + model=make_lite6_model_config( + add_gripper=True, + gripper_hardware_id="arm", + ), + visualization={"backend": "viser"}, + ), + coordinator( + hardware=[_lite6_hw], + tasks=[trajectory_task(_lite6_hw), _gripper_task()], + ), +) + _xarm7_left = xarm7_hardware( "left_arm", canonical_joint_names=[f"left_arm/joint{i}" for i in range(1, 8)] ) diff --git a/dimos/robot/manipulators/xarm/blueprints/teleop.py b/dimos/robot/manipulators/xarm/blueprints/teleop.py index 97494614a1..f89956bea8 100644 --- a/dimos/robot/manipulators/xarm/blueprints/teleop.py +++ b/dimos/robot/manipulators/xarm/blueprints/teleop.py @@ -33,6 +33,8 @@ from dimos.robot.manipulators.xarm.config import ( XARM6_SIM_PATH, XARM7_SIM_PATH, + lite6_hardware, + make_lite6_model_config, make_xarm6_model_config, make_xarm7_model_config, make_xarm_hardware, @@ -110,6 +112,40 @@ ), ) +_lite6_hw = lite6_hardware("arm", gripper=True, mock_without_address=True) + +keyboard_teleop_lite6 = autoconnect( + KeyboardTeleopModule.blueprint(), + ArmTwistCoordinator.blueprint( + instance_name="ControlCoordinator", + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[_lite6_hw], + tasks=[ + eef_twist_task( + _lite6_hw, + robot_model=make_lite6_model_config(add_gripper=False), + target_frame="link6", + timeout=0.0, + ), + TaskConfig( + name="arm_gripper", + type="gripper", + joint_names=["arm/gripper"], + priority=20, + ), + ], + ), + ManipulationModule.blueprint( + model=make_lite6_model_config( + add_gripper=True, + gripper_hardware_id="arm", + ), + visualization={"backend": "viser"}, + ), +) + _xarm6_control_hw = make_xarm_hardware( "arm", 6, diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index 4c8a6be9ac..627c78f77b 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -309,6 +309,7 @@ def xarm6_hardware( def make_xarm_model_config( dof: int, *, + robot_type: str = "xarm", prefix: str = "", add_gripper: bool = True, gripper_hardware_id: str | None = None, @@ -319,6 +320,7 @@ def make_xarm_model_config( ) -> RobotModelConfig: xacro_args = { "dof": str(dof), + "robot_type": robot_type, "prefix": prefix, "limited": "true", "attach_xyz": "0 0 0", @@ -350,7 +352,9 @@ def make_xarm_model_config( ) ], auto_convert_meshes=True, - collision_exclusion_pairs=collision_exclusions if add_gripper else [], + collision_exclusion_pairs=( + collision_exclusions if add_gripper and robot_type == "xarm" else [] + ), gripper_hardware_id=gripper_hardware_id, tf_extra_links=[f"{prefix}{link}" for link in (tf_extra_links or [])], home_joints=home_joints or [0.0] * dof, @@ -368,3 +372,31 @@ def make_xarm7_model_config( **kwargs: Any, ) -> RobotModelConfig: return make_xarm_model_config(7, **kwargs) + + +def make_lite6_model_config( + **kwargs: Any, +) -> RobotModelConfig: + return make_xarm_model_config(6, robot_type="lite", **kwargs) + + +def lite6_hardware( + hw_id: str = "arm", + *, + gripper: bool = False, + mock_without_address: bool = False, + home_joints: list[float] | None = None, + canonical_joint_names: list[str] | None = None, +) -> HardwareComponent: + """Lite 6 speaks the xArm SDK; the adapter detects the model on connect. No sim scene yet.""" + address = global_config.lite6_ip + adapter_type = "mock" if mock_without_address and not address else "xarm" + return make_xarm_hardware( + hw_id, + 6, + adapter_type=adapter_type, + address=address, + gripper=gripper, + home_joints=home_joints, + canonical_joint_names=canonical_joint_names, + ) diff --git a/dimos/robot/manipulators/xarm/test_model_config.py b/dimos/robot/manipulators/xarm/test_model_config.py index ae072a4f34..3d43f4f797 100644 --- a/dimos/robot/manipulators/xarm/test_model_config.py +++ b/dimos/robot/manipulators/xarm/test_model_config.py @@ -20,6 +20,7 @@ from dimos.robot.manipulators.xarm.config import ( XARM_GRIPPER_COLLISION_EXCLUSIONS, make_dual_xarm6_model_config, + make_lite6_model_config, make_xarm6_model_config, ) @@ -80,3 +81,23 @@ def test_prefixed_xarm_model_asset_uses_coordinator_facing_names() -> None: model = prepare_robot_model(config).description assert [joint.name for joint in model.joints if joint.type != "fixed"] == config.joint_names + + +def test_lite6_model_config_selects_lite_robot_type() -> None: + config = make_lite6_model_config(add_gripper=True, gripper_hardware_id="arm") + + assert dict(config.model._xacro_args)["robot_type"] == "lite" + assert config.joint_names == [f"joint{i}" for i in range(1, 7)] + assert config.planning_groups[0].tip_link == "link_tcp" + assert config.collision_exclusion_pairs == [] + + +@pytest.mark.self_hosted +def test_lite6_model_asset_has_lite_gripper() -> None: + config = make_lite6_model_config(add_gripper=True) + model = prepare_robot_model(config).description + + joint_names = {joint.name for joint in model.joints} + assert {"joint1", "joint6", "gripper_fix", "joint_tcp"} <= joint_names + assert "drive_joint" not in joint_names + assert 'name="uflite_gripper_link"' in model.xml diff --git a/docs/usage/configuration.md b/docs/usage/configuration.md index 5ba546d3d8..8b39d8edf3 100644 --- a/docs/usage/configuration.md +++ b/docs/usage/configuration.md @@ -87,6 +87,7 @@ Config( unitree_aes_128_key=None, xarm7_ip=None, xarm6_ip=None, + lite6_ip=None, can_port=None, device_path=None, simulation='',