Skill: yuyoujiang/soarm-control (version 1.0.1)

--- name: soarm-control description: Control the robotic arm through the OpenClaw SOARM API. Use this skill when reading current joint state, moving by joint angles, moving by XYZ coordinates, or handling SOARM robot-control requests. --- # 🦐 SOARM Control Skill Use the existing SOARM API to control the robotic arm directly. ## ⚙️ Configuration Notes **Default Local Setup (when on same machine):** - API Base URL: `http://127.0.0.1:8000` ## 🔍 Key APIs & Examples ### Read Current State ```bash curl -sS http://127.0.0.1:8000/joints ``` Returns current joint values and XYZ end-effector position. --- ### Move To a Position By Joint Angles ```bash curl -sS -X POST http://127.0.0.1:8000/move/joints \ -H 'Content-Type: application/json' \ -d '{"angles":[0,0,0,0,0,0]}' ``` **Parameter Notes:** - Joints order: `shoulder_pan`, `shoulder_lift`, `elbow_flex`, `wrist_flex`, `wrist_roll`, `gripper` - First 5 joints use **degrees (deg)** - Gripper uses **0-100** range **Fixed Positions:** | Name | shoulder_pan | shoulder_lift | elbow_flex | wrist_flex | wrist_roll | gripper | |---|---:|---:|---:|---:|---:|---:| | `initial` | 0 | -104 | 95 | 65 | -95 | 10 | | `top_down` | 0 | -50 | 30 | 90 | -95 | 70 | **Examples:** Return to `initial`: ```bash curl -sS -X POST http://127.0.0.1:8000/move/joints \ -H 'Content-Type: application/json' \ -d '{"angles":[0,-104,95,65,-95,10]}' ``` Return to `top_down`: ```bash curl -sS -X POST http://127.0.0.1:8000/move/joints \ -H 'Content-Type: application/json' \ -d '{"angles":[0,-50,30,90,-95,70]}' ``` --- ### Move By XYZ Coordinates ```bash curl -sS -X POST http://127.0.0.1:8000/move/xyz \ -H 'Content-Type: application/json' \ -d '{"x":0.2,"y":0.0,"z":0.2}' ``` **Parameter Notes:** - `x`: forward/backward (positive = forward) - `y`: left/right (positive = left) - `z`: up/down (positive = up) - Values in **meters** --- ### Trigger a Pick Task ```bash curl -sS -X POST http://127.0.0.1:8000/pick ``` Returns: - `ok`: true if the task was accepted - `message`: `抓取任务已启动` - Returns HTTP `409` if another pick task is already running --- ## 🐙 Quick Commands I Can Run ### Return to initial position ```bash curl -sS -X POST http://localhost:8000/move/joints \ -H 'Content-Type: application/json' \ -d '{"angles":[0,-104,95,65,-95,10]}' ``` ### Return to top-down position ```bash curl -sS -X POST http://localhost:8000/move/joints \ -H 'Content-Type: application/json' \ -d '{"angles":[0,-50,30,90,-95,70]}' ``` ### Read current position ```bash curl -sS http://localhost:8000/joints ``` --- ## 🛠️ Setup Notes When pairing your SOARM device with OpenClaw: 1. **Organize the skill directory** ```bash ├── references │ └── so101_new_calib.urdf # download from TheRobotStudio ├── scripts │ ├── best.pt # yolo11n model │ ├── control_soarm_joints.py │ ├── move_soarm_to_xyz_pinocchio.py │ ├── read_soarm_joints.py │ ├── soarm_api.py │ └── start_server.sh └── SKILL.md ``` 2. **Perpare lerobot env** 3. **Launch the server** ```bash ~/.openclaw/workspace/skills/soarm-control bash scripts/start_server.sh ``` ---

path: scripts/start_server.sh
content: #!/usr/bin/env bash
set -euo pipefail

SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
LEROBOT_ROOT="${LEROBOT_ROOT:-${HOME}/lerobot}"
CONDA_BIN="${CONDA_BIN:-}"

if [[ -z "${CONDA_BIN}" ]]; then
  for candidate in "${HOME}/miniconda3/bin/conda" "${HOME}/anaconda3/bin/conda"; do
    if [[ -x "${candidate}" ]]; then
      CONDA_BIN="${candidate}"
      break
    fi
  done
fi

export SOARM_API_HOST="${SOARM_API_HOST:-127.0.0.1}"
export SOARM_API_PORT="${SOARM_API_PORT:-8000}"
export SOARM_PORT="${SOARM_PORT:-/dev/ttyACM0}"
export SOARM_ID="${SOARM_ID:-openclaw_soarm}"
export SOARM_SKIP_CALIBRATION="${SOARM_SKIP_CALIBRATION:-1}"
if [[ -d "${LEROBOT_ROOT}" ]]; then
  export PYTHONPATH="${LEROBOT_ROOT}:${PYTHONPATH:-}"
fi

if [[ -z "${CONDA_BIN}" ]]; then
  echo "Error: conda executable not found in ~/miniconda3/bin/conda or ~/anaconda3/bin/conda" >&2
  exit 1
fi

API_SCRIPT="${SCRIPT_DIR}/soarm_api.py"
RUNNING_PIDS="$(pgrep -f "${API_SCRIPT}" || true)"
PORT_PID="$(ss -ltnp "( sport = :${SOARM_API_PORT} )" 2>/dev/null | sed -n 's/.*pid=\([0-9]\+\).*/\1/p' | head -n1)"

if [[ -n "${RUNNING_PIDS}" ]]; then
  echo "SOARM API is already running on port ${SOARM_API_PORT}."
  echo "PIDs: ${RUNNING_PIDS}"
  exit 0
fi

if [[ -n "${PORT_PID}" ]]; then
  echo "Error: port ${SOARM_API_PORT} is already in use by PID ${PORT_PID}, not by ${API_SCRIPT}." >&2
  echo "Stop that process or set a different SOARM_API_PORT before starting the server." >&2
  exit 1
fi

exec "${CONDA_BIN}" run -n lerobot python "${API_SCRIPT}"


path: scripts/control_soarm_joints.py
content: #!/usr/bin/env python3

from __future__ import annotations

import argparse
import sys
import time
from pathlib import Path

from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig


JOINT_NAMES = [
    "shoulder_pan",
    "shoulder_lift",
    "elbow_flex",
    "wrist_flex",
    "wrist_roll",
    "gripper",
]


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(
        description=(
            "Control a SOARM 101 arm by sending target joint values. "
            "The first 5 values are degrees. The 6th value is gripper opening in [0, 100]."
        )
    )
    parser.add_argument("--port", required=True, help="Serial port, e.g. /dev/ttyACM0.")
    parser.add_argument(
        "--angles",
        nargs=6,
        type=float,
        metavar=(
            "SHOULDER_PAN",
            "SHOULDER_LIFT",
            "ELBOW_FLEX",
            "WRIST_FLEX",
            "WRIST_ROLL",
            "GRIPPER",
        ),
        help="Six target values. First 5 joints use degrees, gripper uses 0-100.",
    )
    parser.add_argument("--id", default="openclaw_soarm", help="Robot calibration id used by lerobot.")
    parser.add_argument(
        "--sleep",
        type=float,
        default=2.0,
        help="Seconds to wait after sending the command before reading back the position.",
    )
    parser.add_argument(
        "--skip-calibration",
        action="store_true",
        help="Skip automatic calibration on connect. Use this only if calibration already exists.",
    )
    return parser.parse_args()


def prompt_for_angles() -> list[float]:
    prompts = [
        "shoulder_pan (deg)",
        "shoulder_lift (deg)",
        "elbow_flex (deg)",
        "wrist_flex (deg)",
        "wrist_roll (deg)",
        "gripper (0-100)",
    ]
    values: list[float] = []
    for prompt in prompts:
        raw = input(f"Input target for {prompt}: ").strip()
        values.append(float(raw))
    return values


def build_robot(args: argparse.Namespace) -> SO101Follower:
    config = SO101FollowerConfig(
        port=args.port,
        id=args.id,
        disable_torque_on_disconnect=True,
        use_degrees=True,
    )
    return SO101Follower(config)


def print_joint_dict(title: str, obs: dict[str, float]) -> None:
    print(title)
    for joint in JOINT_NAMES:
        key = f"{joint}.pos"
        if key in obs:
            print(f"  {joint:14s}: {obs[key]:8.3f}")


def main() -> int:
    args = parse_args()
    values = args.angles if args.angles is not None else prompt_for_angles()

    if len(values) != 6:
        raise ValueError("Exactly 6 target values are required.")
    if not 0.0 <= values[5] <= 100.0:
        raise ValueError("Gripper target must be in [0, 100].")

    action = {f"{joint}.pos": value for joint, value in zip(JOINT_NAMES, values, strict=True)}
    robot = build_robot(args)

    try:
        robot.connect(calibrate=not args.skip_calibration)

        current = robot.get_observation()
        print_joint_dict("Current joint positions:", current)

        print("\nSending action:")
        for joint, value in zip(JOINT_NAMES, values, strict=True):
            unit = "deg" if joint != "gripper" else "%"
            print(f"  {joint:14s}: {value:8.3f} {unit}")

        sent = robot.send_action(action)
        print_joint_dict("\nCommand sent:", sent)

        if args.sleep > 0:
            time.sleep(args.sleep)

        final_obs = robot.get_observation()
        print_joint_dict("\nJoint positions after motion:", final_obs)
        return 0
    finally:
        if robot.is_connected:
            robot.disconnect()


if __name__ == "__main__":
    raise SystemExit(main())


path: scripts/move_soarm_to_xyz_pinocchio.py
content: #!/usr/bin/env python3

from __future__ import annotations

import argparse
import sys
import time
from pathlib import Path

import numpy as np
import pinocchio as pin

from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig


SCRIPT_DIR = Path(__file__).resolve().parent
SKILL_DIR = SCRIPT_DIR.parent
URDF_PATH = SKILL_DIR / "references" / "so101_new_calib.urdf"
EE_FRAME = "gripper_frame_link"
ARM_JOINTS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"]
ALL_JOINTS = ARM_JOINTS + ["gripper"]


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(
        description="Move the SOARM 101 end effector to target x y z using Pinocchio inverse kinematics."
    )
    parser.add_argument("x", type=float, help="Target x in meters, relative to base_link.")
    parser.add_argument("y", type=float, help="Target y in meters, relative to base_link.")
    parser.add_argument("z", type=float, help="Target z in meters, relative to base_link.")
    parser.add_argument("--port", help="Serial port, e.g. /dev/ttyACM0. Required unless --dry-run is used.")
    parser.add_argument("--id", default="openclaw_soarm", help="Robot calibration id used by lerobot.")
    parser.add_argument("--sleep", type=float, default=1.0, help="Seconds to wait after sending the command.")
    parser.add_argument("--skip-calibration", action="store_true", help="Skip calibration on connect.")
    parser.add_argument("--dry-run", action="store_true", help="Solve IK only and do not connect to the robot.")
    parser.add_argument("--max-iters", type=int, default=300, help="Maximum IK iterations.")
    parser.add_argument("--tol", type=float, default=1e-3, help="Position tolerance in meters.")
    parser.add_argument("--step-size", type=float, default=0.6, help="IK integration step size.")
    parser.add_argument("--damping", type=float, default=1e-4, help="Damping for least-squares IK.")
    return parser.parse_args()


def build_robot(args: argparse.Namespace) -> SO101Follower:
    config = SO101FollowerConfig(
        port=args.port,
        id=args.id,
        disable_torque_on_disconnect=True,
        use_degrees=True,
    )
    return SO101Follower(config)


def build_model() -> tuple[pin.Model, pin.Data, int]:
    model = pin.buildModelFromUrdf(str(URDF_PATH))
    data = model.createData()
    frame_id = model.getFrameId(EE_FRAME)
    return model, data, frame_id


def fk_position(model: pin.Model, data: pin.Data, frame_id: int, q: np.ndarray) -> np.ndarray:
    pin.forwardKinematics(model, data, q)
    pin.updateFramePlacements(model, data)
    return data.oMf[frame_id].translation.copy()


def gripper_pct_to_rad(model: pin.Model, gripper_pct: float) -> float:
    lower = model.lowerPositionLimit[5]
    upper = model.upperPositionLimit[5]
    return lower + (gripper_pct / 100.0) * (upper - lower)


def observation_to_q(model: pin.Model, observation: dict[str, float]) -> np.ndarray:
    q = np.zeros(model.nq)
    for i, joint in enumerate(ARM_JOINTS):
        q[i] = np.deg2rad(observation[f"{joint}.pos"])
    q[5] = gripper_pct_to_rad(model, observation["gripper.pos"])
    return np.clip(q, model.lowerPositionLimit, model.upperPositionLimit)


def q_to_action(q: np.ndarray, gripper_pct: float) -> dict[str, float]:
    action = {f"{joint}.pos": float(np.rad2deg(q[i])) for i, joint in enumerate(ARM_JOINTS)}
    action["gripper.pos"] = float(gripper_pct)
    return action


def solve_ik(
    model: pin.Model,
    data: pin.Data,
    frame_id: int,
    q0: np.ndarray,
    target_xyz: np.ndarray,
    max_iters: int,
    tol: float,
    step_size: float,
    damping: float,
) -> tuple[np.ndarray, float, int]:
    q = q0.copy()
    fixed_gripper = q0[5]

    for iteration in range(max_iters):
        current_xyz = fk_position(model, data, frame_id, q)
        error = target_xyz - current_xyz
        error_norm = float(np.linalg.norm(error))
        if error_norm < tol:
            return q, error_norm, iteration

        jacobian = pin.computeFrameJacobian(
            model,
            data,
            q,
            frame_id,
            pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
        )[:3, :5]

        lhs = jacobian @ jacobian.T + damping * np.eye(3)
        dq_arm = step_size * jacobian.T @ np.linalg.solve(lhs, error)

        dq = np.zeros(model.nv)
        dq[:5] = dq_arm
        q = pin.integrate(model, q, dq)
        q = np.clip(q, model.lowerPositionLimit, model.upperPositionLimit)
        q[5] = fixed_gripper

    final_error = float(np.linalg.norm(target_xyz - fk_position(model, data, frame_id, q)))
    return q, final_error, max_iters


def print_xyz(label: str, xyz: np.ndarray) -> None:
    print(f"{label}: x={xyz[0]:.4f}  y={xyz[1]:.4f}  z={xyz[2]:.4f} m")


def main() -> int:
    args = parse_args()
    if not args.dry_run and not args.port:
        raise ValueError("--port is required unless --dry-run is used.")

    target_xyz = np.array([args.x, args.y, args.z], dtype=float)
    model, data, frame_id = build_model()

    robot = None
    current_gripper_pct = 50.0
    q0 = pin.neutral(model)

    try:
        if not args.dry_run:
            robot = build_robot(args)
            robot.connect(calibrate=not args.skip_calibration)
            observation = robot.get_observation()
            q0 = observation_to_q(model, observation)
            current_gripper_pct = float(observation["gripper.pos"])

        current_xyz = fk_position(model, data, frame_id, q0)
        print_xyz("Current end-effector position", current_xyz)
        print_xyz("Target end-effector position ", target_xyz)

        q_sol, final_error, iterations = solve_ik(
            model=model,
            data=data,
            frame_id=frame_id,
            q0=q0,
            target_xyz=target_xyz,
            max_iters=args.max_iters,
            tol=args.tol,
            step_size=args.step_size,
            damping=args.damping,
        )

        solved_xyz = fk_position(model, data, frame_id, q_sol)
        print_xyz("Solved end-effector position", solved_xyz)
        print(f"IK iterations: {iterations}")
        print(f"Final position error: {final_error:.6f} m")

        if final_error >= args.tol:
            raise RuntimeError("IK did not converge within tolerance. Try a closer target or tune the IK parameters.")

        print("\nSolved arm joints:")
        for i, joint in enumerate(ARM_JOINTS):
            print(f"{joint:14s}: {np.rad2deg(q_sol[i]):8.3f} deg")

        if args.dry_run:
            return 0

        action = q_to_action(q_sol, current_gripper_pct)
        robot.send_action(action)

        if args.sleep > 0:
            time.sleep(args.sleep)

        final_observation = robot.get_observation()
        final_q = observation_to_q(model, final_observation)
        final_xyz_after_move = fk_position(model, data, frame_id, final_q)
        print_xyz("\nMeasured end-effector position", final_xyz_after_move)
        return 0
    finally:
        if robot is not None and robot.is_connected:
            robot.disconnect()


if __name__ == "__main__":
    raise SystemExit(main())


path: scripts/read_soarm_joints.py
content: #!/usr/bin/env python3

from __future__ import annotations

import argparse
import sys
from pathlib import Path

from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig


JOINT_NAMES = [
    "shoulder_pan",
    "shoulder_lift",
    "elbow_flex",
    "wrist_flex",
    "wrist_roll",
    "gripper",
]


def parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description="Read current joint values from a SOARM 101 arm.")
    parser.add_argument("--port", required=True, help="Serial port, e.g. /dev/ttyACM0.")
    parser.add_argument("--id", default="openclaw_soarm", help="Robot calibration id used by lerobot.")
    parser.add_argument(
        "--skip-calibration",
        action="store_true",
        help="Skip automatic calibration on connect. Use this only if calibration already exists.",
    )
    return parser.parse_args()


def build_robot(args: argparse.Namespace) -> SO101Follower:
    config = SO101FollowerConfig(
        port=args.port,
        id=args.id,
        disable_torque_on_disconnect=True,
        use_degrees=True,
    )
    return SO101Follower(config)


def main() -> int:
    args = parse_args()
    robot = build_robot(args)

    try:
        robot.connect(calibrate=not args.skip_calibration)
        obs = robot.get_observation()

        print("Current joint values:")
        for joint in JOINT_NAMES:
            key = f"{joint}.pos"
            value = obs.get(key)
            if value is None:
                continue
            unit = "deg" if joint != "gripper" else "%"
            print(f"{joint:14s}: {value:8.3f} {unit}")
        return 0
    finally:
        if robot.is_connected:
            robot.disconnect()


if __name__ == "__main__":
    raise SystemExit(main())


path: scripts/soarm_api.py
content: #!/usr/bin/env python3

from __future__ import annotations

import atexit
import math
import os
import threading
import time
from pathlib import Path

import cv2
import numpy as np
import pinocchio as pin
from flask import Flask, jsonify, request
from ultralytics import YOLO

from lerobot.robots.so_follower import SO101Follower, SO101FollowerConfig


SCRIPT_DIR = Path(__file__).resolve().parent
SKILL_DIR = SCRIPT_DIR.parent
URDF_PATH = SKILL_DIR / "references" / "so101_new_calib.urdf"
EE_FRAME = "gripper_frame_link"
ARM_JOINTS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"]
JOINTS = ARM_JOINTS + ["gripper"]
DEFAULT_SPEED = 0.2
STEP_DELAY_S = 0.05

IMG_W, IMG_H = 640, 480
IMG_CX, IMG_CY = IMG_W // 2, IMG_H // 2
SERVO_TARGET_X = 422
SERVO_TARGET_Y = 217
MAX_SERVO_STEPS = 200
LOCK_MAX_JUMP = 150
KP_PAN = 0.015
KP_LIFT = 0.01
DEAD_ZONE = 4.0
MAX_STEP = 1.0
PICK_SPEED_DEG = 20
PICK_HZ = 30

OVERHEAD_JOINTS = {
    "shoulder_pan": 0.0,
    "shoulder_lift": -50.0,
    "elbow_flex": 20.0,
    "wrist_flex": 90.0,
    "wrist_roll": -97.0,
}

HOME_JOINTS = {
    "shoulder_pan": 0.0,
    "shoulder_lift": -104.0,
    "elbow_flex": 95.0,
    "wrist_flex": 65.0,
    "wrist_roll": -95.0,
}


class DetectionManager:
    def __init__(self, weights_path: Path, source: int, conf: float):
        self.weights_path = weights_path
        self.source = source
        self.conf = conf
        self.lock = threading.Lock()
        self.state = {
            "timestamp": 0.0,
            "count": 0,
            "objects": [],
        }
        self.ready = False
        self.error: str | None = None
        self._thread: threading.Thread | None = None
        self._stop = threading.Event()

    def start(self) -> None:
        self._thread = threading.Thread(target=self._run, daemon=True)
        self._thread.start()

    def stop(self) -> None:
        self._stop.set()

    def get_state(self) -> dict:
        with self.lock:
            return dict(self.state)

    def get_detection(self, last_pos: tuple[int, int] | None = None) -> tuple[int, int] | None:
        with self.lock:
            objs = list(self.state.get("objects", []))
        if not objs:
            return None
        if last_pos is None:
            best = max(objs, key=lambda o: o["confidence"])
        else:
            lx, ly = last_pos
            best = min(
                objs,
                key=lambda o: (o["center"]["x"] - lx) ** 2 + (o["center"]["y"] - ly) ** 2,
            )
            dist = math.sqrt((best["center"]["x"] - lx) ** 2 + (best["center"]["y"] - ly) ** 2)
            if dist > LOCK_MAX_JUMP:
                return None
        return best["center"]["x"], best["center"]["y"]

    def _run(self) -> None:
        try:
            model = YOLO(str(self.weights_path))
            cap = cv2.VideoCapture(self.source)
            cap.set(cv2.CAP_PROP_FRAME_WIDTH, IMG_W)
            cap.set(cv2.CAP_PROP_FRAME_HEIGHT, IMG_H)
            if not cap.isOpened():
                raise RuntimeError(f"Could not open camera source {self.source}")
            self.ready = True

            while not self._stop.is_set():
                ret, frame = cap.read()
                if not ret:
                    self.error = "Failed to read frame"
                    time.sleep(1.0)
                    continue

                objects_list = []
                results = model.predict(frame, conf=self.conf, verbose=False, stream=True)
                for result in results:
                    boxes = result.boxes
                    for box in boxes:
                        x1, y1, x2, y2 = box.xyxy[0].cpu().numpy()
                        cx = int((x1 + x2) / 2)
                        cy = int((y1 + y2) / 2)
                        cls_id = int(box.cls[0])
                        label = model.names[cls_id]
                        confidence = float(box.conf[0])
                        objects_list.append(
                            {
                                "label": label,
                                "confidence": float(f"{confidence:.2f}"),
                                "center": {"x": cx, "y": cy},
                                "bbox": {
                                    "x1": int(x1),
                                    "y1": int(y1),
                                    "x2": int(x2),
                                    "y2": int(y2),
                                },
                            }
                        )

                with self.lock:
                    self.state = {
                        "timestamp": time.time(),
                        "count": len(objects_list),
                        "objects": objects_list,
                    }
        except Exception as exc:
            self.error = str(exc)


class PickTaskManager:
    def __init__(self) -> None:
        self.lock = threading.Lock()
        self.running = False
        self.last_result: dict | None = None

    def status(self) -> dict:
        with self.lock:
            return {"running": self.running, "last_result": self.last_result}

    def start(self, worker: callable) -> bool:
        with self.lock:
            if self.running:
                return False
            self.running = True
            self.last_result = None

        def runner() -> None:
            result = worker()
            with self.lock:
                self.running = False
                self.last_result = result

        threading.Thread(target=runner, daemon=True).start()
        return True


class SoArmController:
    def __init__(self, port: str, robot_id: str, skip_calibration: bool):
        self.lock = threading.Lock()
        self.model = pin.buildModelFromUrdf(str(URDF_PATH))
        self.data = self.model.createData()
        self.frame_id = self.model.getFrameId(EE_FRAME)
        self.robot = SO101Follower(
            SO101FollowerConfig(
                port=port,
                id=robot_id,
                disable_torque_on_disconnect=True,
                use_degrees=True,
            )
        )
        self.robot.connect(calibrate=not skip_calibration)

    def close(self) -> None:
        if self.robot.is_connected:
            self.robot.disconnect()

    def _fk_position(self, q: np.ndarray) -> np.ndarray:
        pin.forwardKinematics(self.model, self.data, q)
        pin.updateFramePlacements(self.model, self.data)
        return self.data.oMf[self.frame_id].translation.copy()

    def _gripper_pct_to_rad(self, gripper_pct: float) -> float:
        lower = self.model.lowerPositionLimit[5]
        upper = self.model.upperPositionLimit[5]
        return lower + (gripper_pct / 100.0) * (upper - lower)

    def _observation_to_q(self, observation: dict[str, float]) -> np.ndarray:
        q = np.zeros(self.model.nq)
        for i, joint in enumerate(ARM_JOINTS):
            q[i] = np.deg2rad(observation[f"{joint}.pos"])
        q[5] = self._gripper_pct_to_rad(observation["gripper.pos"])
        return np.clip(q, self.model.lowerPositionLimit, self.model.upperPositionLimit)

    def _action_from_q(self, q: np.ndarray, gripper_pct: float) -> dict[str, float]:
        action = {f"{joint}.pos": float(np.rad2deg(q[i])) for i, joint in enumerate(ARM_JOINTS)}
        action["gripper.pos"] = float(gripper_pct)
        return action

    def _solve_ik(
        self,
        q0: np.ndarray,
        target_xyz: np.ndarray,
        max_iters: int,
        tol: float,
        step_size: float,
        damping: float,
    ) -> tuple[np.ndarray, float, int, np.ndarray]:
        q = q0.copy()
        fixed_gripper = q0[5]

        for iteration in range(max_iters):
            current_xyz = self._fk_position(q)
            error = target_xyz - current_xyz
            error_norm = float(np.linalg.norm(error))
            if error_norm < tol:
                return q, error_norm, iteration, current_xyz

            jacobian = pin.computeFrameJacobian(
                self.model,
                self.data,
                q,
                self.frame_id,
                pin.ReferenceFrame.LOCAL_WORLD_ALIGNED,
            )[:3, :5]

            dq_arm = step_size * jacobian.T @ np.linalg.solve(
                jacobian @ jacobian.T + damping * np.eye(3),
                error,
            )
            dq = np.zeros(self.model.nv)
            dq[:5] = dq_arm
            q = pin.integrate(self.model, q, dq)
            q = np.clip(q, self.model.lowerPositionLimit, self.model.upperPositionLimit)
            q[5] = fixed_gripper

        final_xyz = self._fk_position(q)
        final_error = float(np.linalg.norm(target_xyz - final_xyz))
        return q, final_error, max_iters, final_xyz

    def _read_state(self) -> tuple[dict[str, float], np.ndarray]:
        observation = self.robot.get_observation()
        q = self._observation_to_q(observation)
        xyz = self._fk_position(q)
        return observation, xyz

    def _send_smooth_action(
        self, start: dict[str, float], target: dict[str, float], speed: float
    ) -> tuple[dict[str, float], int]:
        if speed <= 0:
            raise ValueError("speed must be > 0.")

        arm_delta = max(abs(target[f"{joint}.pos"] - start[f"{joint}.pos"]) for joint in ARM_JOINTS)
        gripper_delta = abs(target["gripper.pos"] - start["gripper.pos"])
        arm_step_deg = 8.0 * speed
        gripper_step_pct = 10.0 * speed
        steps = max(
            1,
            int(np.ceil(arm_delta / arm_step_deg)) if arm_step_deg > 0 else 1,
            int(np.ceil(gripper_delta / gripper_step_pct)) if gripper_step_pct > 0 else 1,
        )

        sent = target
        for i in range(1, steps + 1):
            alpha = i / steps
            action = {
                f"{joint}.pos": float(
                    start[f"{joint}.pos"] + alpha * (target[f"{joint}.pos"] - start[f"{joint}.pos"])
                )
                for joint in JOINTS
            }
            sent = self.robot.send_action(action)
            if i < steps:
                time.sleep(STEP_DELAY_S)
        return sent, steps

    def _send_pick_action(self, joints: dict[str, float], gripper: float) -> None:
        action = {f"{joint}.pos": float(joints[joint]) for joint in ARM_JOINTS}
        action["gripper.pos"] = float(gripper)
        self.robot.send_action(action)

    def _move_pick_pose(
        self,
        target: dict[str, float],
        gripper: float | None = None,
        speed: float = PICK_SPEED_DEG,
    ) -> tuple[dict[str, float], float]:
        start, cur_gripper = self._read_pick_joints()
        g_target = cur_gripper if gripper is None else gripper
        max_delta = max(abs(target[joint] - start[joint]) for joint in ARM_JOINTS)
        duration = max(1.0, max_delta / speed)
        steps = max(1, int(duration * PICK_HZ))
        for i in range(1, steps + 1):
            alpha = i / steps
            joints = {
                joint: start[joint] + (target[joint] - start[joint]) * alpha
                for joint in ARM_JOINTS
            }
            g = cur_gripper + (g_target - cur_gripper) * alpha
            self._send_pick_action(joints, g)
            time.sleep(1.0 / PICK_HZ)
        time.sleep(0.3)
        return target, g_target

    def _move_pick_gripper(
        self,
        joints: dict[str, float],
        g_start: float,
        g_target: float,
        duration: float = 0.6,
    ) -> float:
        steps = max(1, int(duration * PICK_HZ))
        for i in range(1, steps + 1):
            alpha = i / steps
            g = g_start + (g_target - g_start) * alpha
            self._send_pick_action(joints, g)
            time.sleep(1.0 / PICK_HZ)
        time.sleep(0.3)
        return g_target

    def _read_pick_joints(self) -> tuple[dict[str, float], float]:
        observation = self.robot.get_observation()
        joints = {joint: float(observation[f"{joint}.pos"]) for joint in ARM_JOINTS}
        return joints, float(observation.get("gripper.pos", 0.0))

    def status(self) -> dict:
        with self.lock:
            observation, xyz = self._read_state()
            return {
                "connected": self.robot.is_connected,
                "joints": observation,
                "xyz": xyz.tolist(),
            }

    def move_joints(self, angles: list[float], sleep_s: float, speed: float) -> dict:
        if len(angles) != 6:
            raise ValueError("angles must contain 6 values.")
        if not 0.0 <= angles[5] <= 100.0:
            raise ValueError("gripper must be in [0, 100].")

        target = {f"{joint}.pos": float(value) for joint, value in zip(JOINTS, angles, strict=True)}
        with self.lock:
            before, before_xyz = self._read_state()
            sent, steps = self._send_smooth_action(before, target, speed)
            if sleep_s > 0:
                time.sleep(sleep_s)
            after, after_xyz = self._read_state()
            return {
                "before": before,
                "before_xyz": before_xyz.tolist(),
                "sent": sent,
                "speed": speed,
                "steps": steps,
                "after": after,
                "after_xyz": after_xyz.tolist(),
            }

    def move_xyz(
        self,
        x: float,
        y: float,
        z: float,
        sleep_s: float,
        speed: float,
        max_iters: int,
        tol: float,
        step_size: float,
        damping: float,
    ) -> dict:
        target_xyz = np.array([x, y, z], dtype=float)
        with self.lock:
            before, before_xyz = self._read_state()
            q0 = self._observation_to_q(before)
            gripper_pct = float(before["gripper.pos"])

            q_sol, final_error, iterations, solved_xyz = self._solve_ik(
                q0=q0,
                target_xyz=target_xyz,
                max_iters=max_iters,
                tol=tol,
                step_size=step_size,
                damping=damping,
            )
            if final_error >= tol:
                raise RuntimeError("IK did not converge within tolerance.")

            action = self._action_from_q(q_sol, gripper_pct)
            _, steps = self._send_smooth_action(before, action, speed)
            if sleep_s > 0:
                time.sleep(sleep_s)

            after, after_xyz = self._read_state()
            return {
                "target_xyz": target_xyz.tolist(),
                "before_xyz": before_xyz.tolist(),
                "solved_xyz": solved_xyz.tolist(),
                "measured_xyz": after_xyz.tolist(),
                "final_error": final_error,
                "iterations": iterations,
                "speed": speed,
                "steps": steps,
                "solved_joints_deg": {
                    joint: float(np.rad2deg(q_sol[i])) for i, joint in enumerate(ARM_JOINTS)
                },
                "before_joints": before,
                "after_joints": after,
            }

    def run_pick_task(self, detection_manager: DetectionManager) -> dict:
        with self.lock:
            try:
                self._move_pick_pose(OVERHEAD_JOINTS)

                cur_joints, gripper = self._read_pick_joints()
                t0 = time.monotonic()
                while time.monotonic() - t0 < 1.0:
                    self._send_pick_action(cur_joints, gripper)
                    time.sleep(1.0 / PICK_HZ)

                cur_joints, gripper = self._read_pick_joints()
                initial_det = detection_manager.get_detection()
                if initial_det is None:
                    wrist_adjust = -10.0
                else:
                    cx, cy = initial_det
                    dist_from_center = math.sqrt((cx - IMG_CX) ** 2 + (cy - IMG_CY) ** 2)
                    if dist_from_center > 250:
                        wrist_adjust = -28.0
                    elif dist_from_center > 200:
                        wrist_adjust = -24.0
                    elif dist_from_center > 150:
                        wrist_adjust = -20.0
                    elif dist_from_center > 100:
                        wrist_adjust = -15.0
                    elif dist_from_center > 70:
                        wrist_adjust = -12.0
                    elif dist_from_center > 40:
                        wrist_adjust = -9.0
                    else:
                        wrist_adjust = -7.0
                pre_joints = dict(cur_joints)
                pre_joints["wrist_flex"] += wrist_adjust
                self._move_pick_pose(pre_joints)

                cur_joints, gripper = self._read_pick_joints()
                step = 0
                last_pos: tuple[int, int] | None = None

                while step < MAX_SERVO_STEPS:
                    det = detection_manager.get_detection(last_pos)
                    if det is None:
                        time.sleep(1.0 / PICK_HZ)
                        continue
                    cx, cy = det
                    last_pos = (cx, cy)
                    dx = cx - SERVO_TARGET_X
                    dy = cy - SERVO_TARGET_Y
                    err = math.sqrt(dx**2 + dy**2)
                    step += 1
                    if err < DEAD_ZONE:
                        break

                    delta_pan = float(np.clip(dx * KP_PAN, -MAX_STEP, MAX_STEP))
                    delta_lift = float(np.clip(dy * KP_LIFT, -MAX_STEP, MAX_STEP))
                    cur_joints["shoulder_pan"] += delta_pan
                    cur_joints["shoulder_lift"] += delta_lift
                    self._send_pick_action(cur_joints, gripper)
                    time.sleep(1.0 / PICK_HZ)

                gripper = self._move_pick_gripper(cur_joints, gripper, 50, duration=0.5)

                pick_joints = dict(cur_joints)
                pick_joints["shoulder_lift"] = 20.0
                pick_joints["elbow_flex"] = -5.0
                self._move_pick_pose(pick_joints, gripper=gripper, speed=15)

                gripper = self._move_pick_gripper(pick_joints, 50, 1, duration=0.8)

                lift_joints = dict(pick_joints)
                lift_joints["shoulder_lift"] -= 20.0
                lift_joints["elbow_flex"] -= 35.0
                lift_joints["wrist_flex"] += 45.0
                self._move_pick_pose(lift_joints, gripper=gripper, speed=15)

                place_joints = dict(lift_joints)
                place_joints["shoulder_pan"] = 60.0
                self._move_pick_pose(place_joints, gripper=gripper, speed=PICK_SPEED_DEG)

                self._move_pick_gripper(place_joints, 5, 50, duration=0.6)
                self._move_pick_pose(HOME_JOINTS, gripper=1, speed=PICK_SPEED_DEG)

                return {"ok": True, "message": "抓取任务完成"}
            except Exception as exc:
                return {"ok": False, "error": str(exc)}


def create_app() -> Flask:
    port = os.getenv("SOARM_PORT", "/dev/ttyACM0")
    robot_id = os.getenv("SOARM_ID", "openclaw_soarm")
    skip_calibration = os.getenv("SOARM_SKIP_CALIBRATION", "1") != "0"
    controller = SoArmController(port=port, robot_id=robot_id, skip_calibration=skip_calibration)
    atexit.register(controller.close)

    weights_path = Path(os.getenv("DETECTION_WEIGHTS", str(SCRIPT_DIR / "best.pt")))
    camera_source = int(os.getenv("DETECTION_SOURCE", "0"))
    confidence = float(os.getenv("DETECTION_CONF", "0.25"))
    detection_manager = DetectionManager(weights_path=weights_path, source=camera_source, conf=confidence)
    detection_manager.start()
    atexit.register(detection_manager.stop)

    pick_manager = PickTaskManager()

    app = Flask(__name__)
    app.config["controller"] = controller

    @app.get("/healthz")
    def healthz():
        return jsonify(
            {
                "ok": True,
                "connected": controller.robot.is_connected,
                "detection_ready": detection_manager.ready,
                "detection_error": detection_manager.error,
            }
        )

    @app.get("/joints")
    def joints():
        return jsonify(controller.status())

    @app.get("/coordinates")
    def coordinates():
        return jsonify(detection_manager.get_state())

    @app.get("/status")
    def pick_status():
        return jsonify(pick_manager.status())

    @app.post("/pick")
    def pick():
        started = pick_manager.start(lambda: controller.run_pick_task(detection_manager))
        if not started:
            return jsonify({"ok": False, "message": "任务正在运行中"}), 409
        return jsonify({"ok": True, "message": "抓取任务已启动"})

    @app.post("/move/joints")
    def move_joints():
        payload = request.get_json(force=True, silent=False) or {}
        angles = payload.get("angles")
        sleep_s = float(payload.get("sleep", 2.0))
        speed = float(payload.get("speed", DEFAULT_SPEED))
        return jsonify(controller.move_joints(angles, sleep_s, speed))

    @app.post("/move/xyz")
    def move_xyz():
        payload = request.get_json(force=True, silent=False) or {}
        return jsonify(
            controller.move_xyz(
                x=float(payload["x"]),
                y=float(payload["y"]),
                z=float(payload["z"]),
                sleep_s=float(payload.get("sleep", 2.0)),
                speed=float(payload.get("speed", DEFAULT_SPEED)),
                max_iters=int(payload.get("max_iters", 300)),
                tol=float(payload.get("tol", 1e-3)),
                step_size=float(payload.get("step_size", 0.6)),
                damping=float(payload.get("damping", 1e-4)),
            )
        )

    @app.post("/disconnect")
    def disconnect():
        controller.close()
        return jsonify({"ok": True, "connected": False})

    @app.errorhandler(Exception)
    def handle_error(exc: Exception):
        return jsonify({"ok": False, "error": str(exc)}), 400

    return app


app = create_app()


if __name__ == "__main__":
    host = os.getenv("SOARM_API_HOST", "127.0.0.1")
    port = int(os.getenv("SOARM_API_PORT", "8000"))
    app.run(host=host, port=port, threaded=True, use_reloader=False)


Security scan verdicts — ClawScan: suspicious | static analysis: clean | VirusTotal: clean | SkillSpector: suspicious (CRITICAL)