diff --git a/lerobot_robot_ax_arm/examples/simulate_ee_keyboard.py b/lerobot_robot_ax_arm/examples/simulate_ee_keyboard.py new file mode 100644 index 000000000..58c017e82 --- /dev/null +++ b/lerobot_robot_ax_arm/examples/simulate_ee_keyboard.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python + +"""Hardware-free 3D simulation of the AX-arm keyboard EE teleop. + +Reuses the real IK (``_build_kinematics`` / ``_joint_velocity`` from ``teleoperate_ee_keyboard``) +and a synthetic calibration, driving a simulated (ideal) servo bus instead of a real one. Lets you +sanity-check the solver/frame behaviour and joint limits in a matplotlib window. + +Controls (focus the plot window): + - w / s : +X / -X - a / d : +Y / -Y - r / f : +Z / -Z + - o / c : open / close gripper + - t : toggle global <-> local frame + - m : toggle dq <-> pos solver + - q / esc : quit + +Run: + python examples/simulate_ee_keyboard.py [--solver pos] [--frame global] [--speed 0.06] +""" + +import argparse +import importlib.util +import os +import tempfile +from pathlib import Path + +os.environ.setdefault("MPLCONFIGDIR", tempfile.gettempdir()) + +import numpy as np + +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation + +from lerobot.motors import MotorCalibration +from lerobot_robot_ax_arm.urdf_mapping import ( + ARM_JOINTS, + REFERENCE_URDF_DEG, + SCALE, + URDF_LIMITS_DEG, + ticks_to_urdf_vector, + urdf_vector_to_ticks, +) + +# Reuse the real teleop IK helpers without duplicating them. +_EE = importlib.util.spec_from_file_location( + "_ee_teleop", str(Path(__file__).with_name("teleoperate_ee_keyboard.py")) +) +ee = importlib.util.module_from_spec(_EE) +_EE.loader.exec_module(ee) + +TICK_REF = 512 # tick chosen to sit at each joint's URDF reference angle +GRIP_RANGE = (350, 600) + + +def _synthetic_calibration() -> dict[str, MotorCalibration]: + """Calibration consistent with urdf_mapping: reference tick + travel limits per joint.""" + calib: dict[str, MotorCalibration] = {} + for i, j in enumerate(ARM_JOINTS): + lo_deg, hi_deg = URDF_LIMITS_DEG[j] + ticks = sorted(int(round(TICK_REF + (d - REFERENCE_URDF_DEG[j]) / SCALE)) for d in (lo_deg, hi_deg)) + calib[j] = MotorCalibration(id=i + 1, drive_mode=0, homing_offset=TICK_REF, + range_min=ticks[0], range_max=ticks[1]) + calib["gripper"] = MotorCalibration(id=4, drive_mode=0, homing_offset=0, + range_min=GRIP_RANGE[0], range_max=GRIP_RANGE[1]) + return calib + + +class _SimBus: + """Ideal servo bus: Present_Position instantly follows the last commanded Goal_Position.""" + + def __init__(self, ticks: dict[str, float]): + self.ticks = ticks + + def read(self, _reg, motor, normalize=False): + return self.ticks[motor] + + def write(self, _reg, motor, value, normalize=False): + self.ticks[motor] = float(value) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--speed", type=float, default=ee.CART_STEP_M) + parser.add_argument("--frame", choices=("local", "global"), default="local") + parser.add_argument("--solver", choices=("dq", "pos"), default="dq") + args = parser.parse_args() + + from importlib.resources import files + urdf_path = str(files("lerobot_robot_ax_arm") / "urdf" / "ax_arm.urdf") + kin = ee._build_kinematics(urdf_path) + link_fks = ee.build_link_fks(urdf_path) + calib = _synthetic_calibration() + + q_start_deg = np.array([REFERENCE_URDF_DEG[j] for j in ARM_JOINTS]) # reference "zero" pose (0, 45, 90) + start_ticks = urdf_vector_to_ticks(q_start_deg, calib) + ticks = {j: float(start_ticks[j]) for j in ARM_JOINTS} + ticks["gripper"] = float(sum(GRIP_RANGE) / 2) + bus = _SimBus(ticks) + + pending = {"x": 0.0, "y": 0.0, "z": 0.0, "g": 0.0} + state = {"frame": args.frame, "solver": args.solver} + keymap = {"w": ("x", 1), "s": ("x", -1), "a": ("y", 1), "d": ("y", -1), "r": ("z", 1), "f": ("z", -1), + "o": ("g", 1), "c": ("g", -1)} + + fig = plt.figure(figsize=(7, 6)) + ax = fig.add_subplot(projection="3d") + (chain_line,) = ax.plot([], [], [], "-o", lw=3, color="tab:blue") + (ee_pt,) = ax.plot([], [], [], "o", ms=10, color="tab:red") + reach = 0.32 + ax.set_xlim(-reach, reach); ax.set_ylim(-reach, reach); ax.set_zlim(-0.05, reach) + ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z") + + def on_key(event): + k = (event.key or "").lower() + if k in ("q", "escape"): + plt.close(fig) + elif k == "t": + state["frame"] = "global" if state["frame"] == "local" else "local" + elif k == "m": + state["solver"] = "pos" if state["solver"] == "dq" else "dq" + elif k in keymap: + axis, direction = keymap[k] + pending[axis] += direction + + fig.canvas.mpl_connect("key_press_event", on_key) + + def update(_frame): + q_rad = np.deg2rad(ticks_to_urdf_vector({j: ticks[j] for j in ARM_JOINTS}, calib)) + cmd = np.array([np.sign(pending["x"]), np.sign(pending["y"]), np.sign(pending["z"])], dtype=float) + pending["x"] = pending["y"] = pending["z"] = 0.0 + + if np.any(cmd): + q_dot = ee._joint_velocity(kin, q_rad, cmd, state["frame"], state["solver"]) + ee_speed = float(np.linalg.norm(np.array(kin["pos_jac"](q_rad)) @ q_dot)) + if ee_speed > 1e-6: + q_step = q_dot * (args.speed / ee_speed) + step_norm = np.linalg.norm(q_step) + if step_norm > ee.MAX_JOINT_STEP_RAD: + q_step *= ee.MAX_JOINT_STEP_RAD / step_norm + q_target = np.clip(q_rad + q_step, kin["lower"], kin["upper"]) + target_ticks = urdf_vector_to_ticks(np.rad2deg(q_target), calib) + for j in ARM_JOINTS: + c = calib[j] + bus.write("Goal_Position", j, int(np.clip(target_ticks[j], c.range_min, c.range_max))) + + g = np.sign(pending["g"]); pending["g"] = 0.0 + if g: + gc = calib["gripper"] + bus.write("Goal_Position", "gripper", + int(np.clip(ticks["gripper"] + g * ee.GRIP_STEP_TICK, gc.range_min, gc.range_max))) + + pts = ee.chain_points(link_fks, np.deg2rad(ticks_to_urdf_vector({j: ticks[j] for j in ARM_JOINTS}, calib))) + chain_line.set_data(pts[:, 0], pts[:, 1]); chain_line.set_3d_properties(pts[:, 2]) + ee_pt.set_data(pts[-1:, 0], pts[-1:, 1]); ee_pt.set_3d_properties(pts[-1:, 2]) + grip_frac = (ticks["gripper"] - GRIP_RANGE[0]) / (GRIP_RANGE[1] - GRIP_RANGE[0]) + ax.set_title(f"frame={state['frame']} solver={state['solver']} gripper={grip_frac:.0%}\n" + f"w/s/a/d/r/f=move o/c=grip t=frame m=solver q=quit") + return chain_line, ee_pt + + anim = FuncAnimation(fig, update, interval=int(1000 / ee.FPS), blit=False, cache_frame_data=False) + fig._anim = anim # keep a reference so it isn't garbage-collected + plt.show() + + +if __name__ == "__main__": + main() diff --git a/lerobot_robot_ax_arm/examples/teleoperate_ee_keyboard.py b/lerobot_robot_ax_arm/examples/teleoperate_ee_keyboard.py index ed35baa9c..28146881e 100644 --- a/lerobot_robot_ax_arm/examples/teleoperate_ee_keyboard.py +++ b/lerobot_robot_ax_arm/examples/teleoperate_ee_keyboard.py @@ -1,15 +1,25 @@ #!/usr/bin/env python -"""Keyboard end-effector teleoperation for the 4-DoF AX arm (position-only IK on the URDF). +"""Keyboard end-effector teleoperation for the 4-DoF AX arm (velocity IK, position output). -The arm has 3 revolute joints (base yaw + shoulder/elbow pitch) giving exactly 3 task-space DoF, -all spent on reaching a 3D *position* (orientation is not controllable). Each frame we: +Adapted from a resolved-rate (twist) servoing controller: instead of commanding joint +velocities, the per-tick joint velocity is integrated into a joint *position* target and sent as +``Goal_Position`` (the AX arm runs in position mode). - 1. read the raw motor ticks and map them to URDF joint degrees (see ``urdf_mapping``), - 2. forward-kinematics to the current end-effector pose, - 3. offset the target position by the pressed keys, - 4. solve position-only IK (``orientation_weight=0.0``) for new URDF joint degrees, - 5. map back to ticks and command the servos. +Each frame we read the raw motor ticks, map them to URDF joint angles, solve for a joint velocity +that produces the requested Cartesian motion, scale it to a fixed end-effector Cartesian step +``CART_STEP_M`` per tick (capped in joint space near singularities), and command ``Goal_Position``. + +Two IK solvers are selectable at runtime (``--solver`` / ``m`` key): + - "dq" : dual-quaternion resolved-rate (matches the full pose velocity via ``scipy.least_squares``; + faithful to the source controller, but rotation/translation coupling on a 3-DoF arm + causes axis leakage), + - "pos" : position-only Jacobian ``dEE_pos/dq`` solved with least-squares (crisp axis-aligned + Cartesian motion). + +The motion frame is also selectable (``--frame`` / ``t`` key): "local" moves along the +end-effector's own axes, "global" along the fixed world axes. Global always uses the position +Jacobian (the dq solver's held-orientation constraint leaks axes on this underactuated arm). The tick<->URDF mapping is established once by ``lerobot-calibrate`` (reference pose + travel limits), so no separate alignment step is needed here. @@ -19,6 +29,8 @@ Controls (letter keys; hold to keep moving via terminal key-repeat): - a / d : +Y / -Y (left / right) - r / f : +Z / -Z (up / down) - o / c : open / close gripper + - t : toggle global <-> local frame + - m : toggle dq <-> pos solver - ESC / q : stop Run: @@ -29,30 +41,175 @@ import argparse import time from importlib.resources import files +import casadi as cs import numpy as np +import scipy as sp +from urdf2casadi import urdfparser as u2c +from urdf2casadi.geometry import dual_quaternion, quaternion -from lerobot.model.kinematics import RobotKinematics from lerobot.utils.keyboard_input import create_key_listener from lerobot.utils.robot_utils import precise_sleep from lerobot_robot_ax_arm import AXArm, AXArmConfig from lerobot_robot_ax_arm.urdf_mapping import ( ARM_JOINTS, - URDF_JOINT_NAMES, + REFERENCE_URDF_DEG, ticks_to_urdf_vector, urdf_vector_to_ticks, ) FPS = 30 -LINEAR_STEP_M = 0.005 # EE position change per pressed frame +HOME_TIME_S = 2.0 # duration of the ramped move to the reference pose at startup +CART_STEP_M = 0.008 # default end-effector Cartesian motion per tick, meters (override with --speed) +MAX_JOINT_STEP_RAD = 0.15 # safety cap on joint motion per tick (keeps motion bounded near singularities) +DLS_LAMBDA = 0.02 # damping factor for the position IK, well-behaved near singularities GRIP_STEP_TICK = 15 # gripper ticks per press +FIT_THRESHOLD = 0.1 # only fit dual-quaternion derivative components above this magnitude + +ROOT_LINK = "base_link" +TIP_LINK = "gripper_link" +CHAIN_LINKS = ("robot_link_1", "robot_link_2", "robot_link_3", "gripper_link") + + +def _skew(x: np.ndarray) -> np.ndarray: + return np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) + + +def _build_kinematics(urdf_path: str) -> dict: + """FK + Jacobians (dual-quaternion and position-only) and joint limits from the URDF.""" + parser = u2c.URDFparser() + parser.from_file(urdf_path) + fk = parser.get_forward_kinematics(ROOT_LINK, TIP_LINK) + q_sym = fk["q"] + fk_dq = fk["dual_quaternion_fk"] + fk_T = fk["T_fk"] + return { + "fk_dq": fk_dq, + "fk_T": fk_T, + "dq_jac": cs.Function("dq_jac", [q_sym], [cs.jacobian(fk_dq(q_sym), q_sym)]), + "pos_jac": cs.Function("pos_jac", [q_sym], [cs.jacobian(fk_T(q_sym)[:3, 3], q_sym)]), + "lower": np.array(fk["lower"], dtype=float).flatten(), + "upper": np.array(fk["upper"], dtype=float).flatten(), + } + + +def build_link_fks(urdf_path: str): + """Casadi T_fk functions base->each link in CHAIN_LINKS, for drawing the arm.""" + fks = [] + for tip in CHAIN_LINKS: + parser = u2c.URDFparser() + parser.from_file(urdf_path) + fk = parser.get_forward_kinematics(ROOT_LINK, tip) + fks.append((fk["T_fk"], fk["q"].shape[0])) + return fks + + +def chain_points(link_fks, q_rad: np.ndarray) -> np.ndarray: + """3D positions of the base and each link origin along the kinematic chain.""" + pts = [np.zeros(3)] + for T, n in link_fks: + pts.append(np.array(T(q_rad[:n]))[:3, 3]) + return np.array(pts) + + +def open_live_view(link_fks, reach: float = 0.42): + """Open a non-blocking 3D plot; returns an update(q_rad, title) callback (False once closed).""" + import os + import tempfile + + os.environ.setdefault("MPLCONFIGDIR", tempfile.gettempdir()) + import matplotlib.pyplot as plt + + fig = plt.figure(figsize=(7, 6)) + ax = fig.add_subplot(projection="3d") + (chain_line,) = ax.plot([], [], [], "-o", lw=3, color="tab:blue") + (ee_pt,) = ax.plot([], [], [], "o", ms=10, color="tab:red") + ax.set_xlim(-reach, reach); ax.set_ylim(-reach, reach); ax.set_zlim(-0.05, reach) + ax.set_xlabel("X"); ax.set_ylabel("Y"); ax.set_zlabel("Z") + plt.ion(); plt.show(block=False) + + def update(q_rad: np.ndarray, title: str) -> bool: + if not plt.fignum_exists(fig.number): + return False + pts = chain_points(link_fks, q_rad) + chain_line.set_data(pts[:, 0], pts[:, 1]); chain_line.set_3d_properties(pts[:, 2]) + ee_pt.set_data(pts[-1:, 0], pts[-1:, 1]); ee_pt.set_3d_properties(pts[-1:, 2]) + ax.set_title(title) + fig.canvas.draw_idle(); fig.canvas.flush_events() + return True + + return update + + +def _world_twist_to_dq_dot(pose: np.ndarray, twist_world: np.ndarray) -> np.ndarray: + """World-frame twist [v, w] -> dual-quaternion derivative x_dot at the current pose.""" + T = dual_quaternion.to_numpy_transformation_matrix(pose) + adj = np.zeros((6, 6)) + adj[:3, :3] = T[:3, :3] + adj[3:, 3:] = T[:3, :3] + adj[:3, 3:] = _skew(T[:3, -1]) @ T[:3, :3] + twist = adj @ twist_world + + v = np.append(twist[:3], 0) + w = np.append(twist[3:], 0) + primal = pose[:4] + dual = pose[4:] + primal_conj = np.append(-primal[:3], primal[3]) + p = 2 * quaternion.numpy_product(dual, primal_conj) + + xi = np.zeros(8) + xi[:4] = np.append(0, twist[3:]) + xi[4:] = v + (quaternion.numpy_product(p, w) - quaternion.numpy_product(w, p)) / 2 + return 0.5 * dual_quaternion.numpy_product(xi, pose) + + +def _fitness(q_dot, jacobian, x_dot): + index = np.where(np.abs(x_dot) > FIT_THRESHOLD) + delta = (np.dot(jacobian, q_dot) - x_dot)[index] + return np.dot(delta.T, delta) + + +def _fitness_jacobian(q_dot, jacobian, x_dot): + return 2 * np.dot(jacobian.T, np.dot(jacobian, q_dot) - x_dot) + + +def _joint_velocity(kin: dict, q_rad: np.ndarray, cmd: np.ndarray, frame: str, solver: str) -> np.ndarray: + """Joint velocity producing the requested unit Cartesian motion ``cmd`` (x, y, z).""" + rot = np.array(kin["fk_T"](q_rad))[:3, :3] + # World-frame ("global") translation on this 3-DoF (no-wrist) arm is only reliable via the position + # Jacobian: the dq resolved-rate tries to hold EE orientation fixed, which an underactuated arm + # cannot do, leaking the motion onto other axes as the arm reorients. So global always uses pos. + if solver == "pos" or frame == "global": + v_world = cmd.astype(float) if frame == "global" else rot @ cmd + J = np.array(kin["pos_jac"](q_rad)) + # Damped least squares: bounded, well-conditioned joint velocity even near singularities. + return J.T @ np.linalg.solve(J @ J.T + DLS_LAMBDA**2 * np.eye(3), v_world) + + # Dual-quaternion resolved-rate (local frame only): move along the end-effector's own axes. + lin = cmd.astype(float) + pose = np.array(kin["fk_dq"](q_rad)).flatten() + x_dot = _world_twist_to_dq_dot(pose, np.concatenate([lin, np.zeros(3)])) + jacobian = np.array(kin["dq_jac"](q_rad)) + sol = sp.optimize.least_squares( + _fitness, np.zeros(len(ARM_JOINTS)), args=(jacobian, x_dot), xtol=1e-4, jac=_fitness_jacobian, + ) + return sol.x if sol.success else np.zeros(len(ARM_JOINTS)) def main(): parser = argparse.ArgumentParser() parser.add_argument("--port", required=True, help="Serial port of the AX arm") parser.add_argument("--id", default="my_ax_arm", help="Robot id used for calibration files") + parser.add_argument("--speed", type=float, default=CART_STEP_M, + help="End-effector Cartesian motion per tick in meters (higher = faster)") + parser.add_argument("--frame", choices=("local", "global"), default="local", + help="Motion frame: 'local' = end-effector axes, 'global' = world axes") + parser.add_argument("--solver", choices=("dq", "pos"), default="dq", + help="IK solver: 'dq' = dual-quaternion resolved-rate, 'pos' = position Jacobian") + parser.add_argument("--view", action="store_true", + help="Show a live 3D plot of the real arm (digital twin) alongside teleop") args = parser.parse_args() + cart_step = args.speed robot = AXArm(AXArmConfig(port=args.port, id=args.id, use_degrees=True)) robot.connect(calibrate=False) @@ -60,13 +217,29 @@ def main(): raise RuntimeError(f"No calibration found for id '{args.id}'. Run lerobot-calibrate first.") urdf_path = str(files("lerobot_robot_ax_arm") / "urdf" / "ax_arm.urdf") - kin = RobotKinematics(urdf_path, target_frame_name="gripper_link", joint_names=URDF_JOINT_NAMES) + kin = _build_kinematics(urdf_path) + q_lower, q_upper = kin["lower"], kin["upper"] + view_update = open_live_view(build_link_fks(urdf_path)) if args.view else None grip_calib = robot.calibration["gripper"] grip_tick = int(robot.bus.read("Present_Position", "gripper", normalize=False)) + # Slowly ramp to the reference ("zero") pose (0, 45, 90) before starting teleop. + home_deg = np.array([REFERENCE_URDF_DEG[j] for j in ARM_JOINTS]) + home_ticks = urdf_vector_to_ticks(home_deg, robot.calibration) + start_ticks = {j: float(robot.bus.read("Present_Position", j, normalize=False)) for j in ARM_JOINTS} + print("Homing to zero pose (0, 45, 90)...") + steps = max(1, int(HOME_TIME_S * FPS)) + for i in range(1, steps + 1): + alpha = i / steps + for j in ARM_JOINTS: + c = robot.calibration[j] + tick = (1 - alpha) * start_ticks[j] + alpha * home_ticks[j] + robot.bus.write("Goal_Position", j, int(np.clip(tick, c.range_min, c.range_max)), normalize=False) + precise_sleep(1.0 / FPS) + pending = {"x": 0.0, "y": 0.0, "z": 0.0, "g": 0.0} - state = {"quit": False} + state = {"quit": False, "frame": args.frame, "solver": args.solver} keymap = {"w": ("x", 1), "s": ("x", -1), "a": ("y", 1), "d": ("y", -1), "r": ("z", 1), "f": ("z", -1), "o": ("g", 1), "c": ("g", -1)} @@ -74,29 +247,47 @@ def main(): k = name.lower() if k in ("esc", "q"): state["quit"] = True + elif k == "t": + state["frame"] = "global" if state["frame"] == "local" else "local" + elif k == "m": + state["solver"] = "pos" if state["solver"] == "dq" else "dq" elif k in keymap: axis, direction = keymap[k] pending[axis] += direction - listener = create_key_listener(on_key, controls_help="w/s a/d r/f = XYZ, o/c = gripper, esc = stop") + listener = create_key_listener( + on_key, controls_help="w/s a/d r/f = XYZ, o/c = gripper, t = frame, m = solver, esc = stop" + ) if listener is None: raise RuntimeError("Needs an interactive terminal with a usable key listener.") - print("Keyboard EE teleop. w/s=X a/d=Y r/f=Z o/c=gripper, ESC=stop.") + print("Keyboard EE teleop (velocity IK -> position). w/s=X a/d=Y r/f=Z o/c=gripper, t=frame, m=solver, ESC=stop.") try: while not state["quit"]: t0 = time.perf_counter() ticks = {j: float(robot.bus.read("Present_Position", j, normalize=False)) for j in ARM_JOINTS} q_deg = ticks_to_urdf_vector(ticks, robot.calibration) + q_rad = np.deg2rad(q_deg) - pose = kin.forward_kinematics(q_deg) - dx, dy, dz = (np.sign(pending[a]) for a in ("x", "y", "z")) + lin = np.array([np.sign(pending["x"]), np.sign(pending["y"]), np.sign(pending["z"])]) + cmd_disp = lin.copy() pending["x"] = pending["y"] = pending["z"] = 0.0 - pose[:3, 3] += LINEAR_STEP_M * np.array([dx, dy, dz]) - q_target = kin.inverse_kinematics(q_deg, pose, orientation_weight=0.0) - target_ticks = urdf_vector_to_ticks(q_target, robot.calibration) + q_target_rad = q_rad + if np.any(lin): + q_dot = _joint_velocity(kin, q_rad, lin.astype(float), state["frame"], state["solver"]) + # Scale for a consistent end-effector Cartesian speed (uniform across axes/poses), + # then cap the joint step so motion stays bounded near singularities. + ee_speed = float(np.linalg.norm(np.array(kin["pos_jac"](q_rad)) @ q_dot)) + if ee_speed > 1e-6: + q_step = q_dot * (cart_step / ee_speed) + step_norm = np.linalg.norm(q_step) + if step_norm > MAX_JOINT_STEP_RAD: + q_step *= MAX_JOINT_STEP_RAD / step_norm + q_target_rad = np.clip(q_rad + q_step, q_lower, q_upper) + + target_ticks = urdf_vector_to_ticks(np.rad2deg(q_target_rad), robot.calibration) for j in ARM_JOINTS: c = robot.calibration[j] tick = int(np.clip(target_ticks[j], c.range_min, c.range_max)) @@ -108,10 +299,15 @@ def main(): grip_tick = int(np.clip(grip_tick + g * GRIP_STEP_TICK, grip_calib.range_min, grip_calib.range_max)) robot.bus.write("Goal_Position", "gripper", grip_tick, normalize=False) - urdf_str = " ".join(f"{j}={v:+6.1f}" for j, v in zip(ARM_JOINTS, q_target)) - print(f"cmd[x={dx:+.0f} y={dy:+.0f} z={dz:+.0f} g={g:+.0f}] -> urdf[{urdf_str}] gripper={grip_tick}", + urdf_str = " ".join(f"{j}={v:+6.1f}" for j, v in zip(ARM_JOINTS, np.rad2deg(q_target_rad))) + print(f"[{state['frame']:>6}|{state['solver']}] cmd[x={cmd_disp[0]:+.0f} y={cmd_disp[1]:+.0f}" + f" z={cmd_disp[2]:+.0f} g={g:+.0f}] -> urdf[{urdf_str}] gripper={grip_tick}", end="\r", flush=True) + if view_update is not None: + # Draw the arm from the measured joint angles (real state, not the command). + view_update(q_rad, f"REAL arm | frame={state['frame']} solver={state['solver']}") + precise_sleep(max(1.0 / FPS - (time.perf_counter() - t0), 0.0)) except KeyboardInterrupt: pass diff --git a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/ax_arm.py b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/ax_arm.py index bd6bf190e..dd9416e82 100644 --- a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/ax_arm.py +++ b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/ax_arm.py @@ -240,7 +240,13 @@ class AXArm(Robot): @check_if_not_connected def send_action(self, action: RobotAction) -> RobotAction: - goal_pos = {key.removesuffix(".pos"): val for key, val in action.items() if key.endswith(".pos")} + goal_pos = { + key.removesuffix(".pos"): val + for key, val in action.items() + if isinstance(key, str) and key.endswith(".pos") + } + if not goal_pos: + return {} if self.config.max_relative_target is not None: present_pos = {motor: self.bus.read("Present_Position", motor) for motor in goal_pos} diff --git a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/urdf_mapping.py b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/urdf_mapping.py index 73a56c7f3..54da6a6d3 100644 --- a/lerobot_robot_ax_arm/lerobot_robot_ax_arm/urdf_mapping.py +++ b/lerobot_robot_ax_arm/lerobot_robot_ax_arm/urdf_mapping.py @@ -33,9 +33,9 @@ SCALE = AX_TRAVEL_DEG / AX_MAX_TICK # URDF degrees per motor tick REFERENCE_URDF_DEG = {"shoulder_pan": 0.0, "shoulder_lift": 45.0, "elbow_flex": 90.0} # URDF joint limits (deg) from ax_arm.urdf, used to guide the lower/upper jog during calibration. URDF_LIMITS_DEG = { - "shoulder_pan": (-45.0, 45.0), + "shoulder_pan": (-90.0, 90.0), "shoulder_lift": (0.0, 90.0), - "elbow_flex": (0.0, 90.0), + "elbow_flex": (0.0, 180.0), }