job control（&、Ctrl-Z、jobs、fg、bg）工作管理在Linux中
核心：一个终端窗口，可以同时跑好多个程序，前台、后台来回切，这一套能力就叫 job control（作业控制）
基础概念
- 前台程序 (foreground)：你现在终端正在交互的程序。程序跑起来，终端被占住，你敲键盘直接给这个程序；程序不退出，你不能输新命令。
- 后台程序 (background)：程序悄悄跑，不霸占你的命令行，你还可以继续敲别的命令。
- Job（作业）：你这个终端里启动的每一个任务，就是一个 job。注意：只属于当前终端，关掉终端，这些 job 全部被杀掉！
逐个命令通俗讲解
1.&放在命令末尾 -->启动直接丢后台
python main.py &
效果：程序一启动就扔后台运行，终端立刻还给你，可以继续敲别的指令。
缺点：后台程序输出还是会乱打印到你的屏幕上。
2.ctrl + z 暂停当前前台程序（冻结！ 不是退出）
你正在跑一个程序，终端被占住，不想终止它，想先回去敲命令： 按下 Ctrl+Z
程序被暂停（冻住，不再跑），变成后台暂停状态，终端释放出来给你输入命令。
⚠️重点：Ctrl‑Z 不是退出！进程还活着，只是暂停休眠，啥活不干。
3.jobs查看本终端所有任务
jobs
列出当前终端全部 job，会看到编号 [1] [2]，状态：
Running：后台正在跑Stopped：被 Ctrl+Z 冻结暂停了
job 编号
[1]，后面 fg、bg 就用这个数字操作。
4.bg %n把暂停的任务，放到后台继续跑(1是任务编号)
bg %1
%1 代表 jobs 看到的 1 号任务。
把被 Ctrl+Z 冻住的 Stopped 任务，解冻，放到后台继续运行。
场景：程序按了 Ctrl+Z 暂停，不想终止，让它悄悄后台干活。
5.fg %n将后台任务切回前台
fg %1
把 1 号 job 拉回前台，终端再次被这个程序接管，你可以交互。不加编号fg默认操作最近的那个任务。
6.完整实操流程例子
# 1.运行一个会占住终端的程序
sleep 600
# 2.按 Ctrl+Z → 程序暂停，回到命令提示符
#3.看任务列表
jobs
#输出 [1]+ Stopped sleep 600
#4.让1号任务后台继续跑
bg %1
jobs
#输出 [1]+ Running sleep 600 &
#5.想切回来交互
fg %1
#现在sleep回到前台
#6.此时直接Ctrl+C，直接杀死这个前台程序
输出示例：
程序管理（ps -l/ps aux、top、pstree、signal/kill/killall、nice/renice）
- job control（&、ctrl‑z、fg/bg）：只管当前终端里面的任务，终端一关全部没了
- ps /top/kill /nice：系统级进程管理，看整个操作系统所有进程，不受终端限制
1.ps --查看进程快照（一瞬间的进程列表，不会实时刷新）
ps -l
ps -l
只看你当前终端启动的进程。
- F：标志位
- S：进程状态
S睡眠；R正在运行；T被暂停；Z僵尸进程
- UID：用户 ID
- PID：进程 ID（最重要，kill 靠 PID 杀进程）
- PPID：父进程 ID，谁把这个程序启动起来的
- PRI/NI：优先级（和 nice 相关）
ps aux(最常用)
ps aux
输出示例：
查看系统全部进程，所有人、所有终端的进程全部打印。
- USER：哪个用户启动
- PID：进程号
- % CPU：CPU 占用百分比
- % MEM：内存占用百分比
- VSZ：虚拟内存
- RSS：实际占用物理内存
- TTY：属于哪个终端，
?代表和终端无关的后台守护进程 - STAT：进程状态
- START：启动时间
- TIME：已经占用 CPU 总时间
- COMMAND：启动这条进程的完整命令
常用组合过滤：
ps aux | grep python筛选 python 进程
2.top--实时动态进程监视器
top
输出示例：
ps 是拍一张照片；top 是直播视频，实时刷新 CPU 内存进程状态。
交互快捷键（top 界面里面按）
P：按 CPU 占用排序M：按内存占用排序k：输入 PID，直接杀死进程q：退出 top
看到的 PID 就可以拿给 kill 命令使用。
3.pstree -- 进程树，看父子进程关系
pstree
# 带上PID
pstree -p
把进程画成树状，直观看到：哪个父进程创建哪些子进程。 比如 bash 终端启动 python，bash 就是父，python 是子进程。
关闭终端，bash 父进程退出，子进程会收到信号，大部分被回收。
4.signal/kill/killall信号、杀死进程
Linux 杀死程序不是直接硬删，发送信号 Signal 给进程，程序收到信号自己决定怎么处理。
kill
kill不是直接杀，默认发送15号信号SIGTERM（温柔终止）
kill 1234 # 给PID=1234进程发送15号信号，请求程序正常退出
kill -9 1234 # -9 SIGKILL，强制暴力杀死，程序不能拒绝，直接干掉
优先用普通 kill PID，让程序做资源释放；
-9 尽量少用，强制杀，不会执行关闭、清理逻辑，可能文件损坏、资源泄漏。
killall
按程序名字杀，不用找PID
killall python3
killall -9 python3
5.nice/renice修改进程优先级
Linux进程nice值：范围-20 ~ 19
- 数值越小，优先级越高
-20：最高优先级19：最低优先级- 默认启动 nice=0
nice：启动程序的时候直接指定优先级
# 启动程序，设置nice=10，降低优先级，少抢CPU
nice -n 10 python3 main.py
普通用户只能把 nice 调大（降低优先级）；普通用户不能设置负数（调高优先级），root 才可以设置‑20。
renice：程序已经在运行，动态改优先级
renice 5 -p 1234 # 修改PID 1234 nice值改为5
free/uname/uptime/netstat/dmesg/vmstat信息排查命令
这一组命令：看整机状态、内存、内核、开机时长、网络、内核日志、系统整体性能，排错时高频使用。
free--查看内存使用
free
# 更友好，单位MB
free -h
输出示例：
输出字段：
total：总物理内存used：已经被占用内存free：完全空闲内存available：真正可以给新程序分配的内存（重点看这个！）
2.unmae查看内核/系统版本信息
uname -a # 打印全部信息（最常用）
uname -r # 只打印内核版本
输出示例：
输出内容：内核版本、CPU 架构 (x86_64 /aarch64)、操作系统。
排错场景：确认你的 Ubuntu 内核，确认是 ARM 还是 x86 架构，安装包版本匹配。
3.uptime开机多久、系统负载
4.netstat网络状态
# 需要安装 apt install net-tools
netstat -tulnp
t TCP；-u UDP；-l监听端口；-n数字不解析域名；-p哪个进程占用端口 作用：查看哪些端口正在监听，哪个 PID 程序占用端口。
5.dmesg内核环形缓冲区日志
dmesg
# 只看最新，实时滚动
dmesg -w
内核打印出来的底层日志：硬件插拔、USB 摄像头、驱动报错、磁盘、内核崩溃信息
6.vmstat虚拟内存、整机性能采样统计
vmstat
# 每2秒输出一次，持续输出
vmstat 2
7.快速使用场景总结（开发排错直接照抄）
| 命令 | 使用场景 |
|---|---|
free -h | 排查内存够不够，OOM 问题 |
uname -a | 确认内核、CPU 架构 aarch64/x86_64 |
uptime | 看开机时长，看系统负载高不高 |
ss -tulnp | 查看端口占用，代替 netstat |
dmesg -w | USB 设备、驱动、硬件报错排错神器 |
vmstat 2 | 持续观察整机 CPU、内存、IO 瓶颈 |
Flask App‑Factory / Blueprint / JSON / HTTP 状态码
类比 SpringBoot：
App Factory ≈ Spring 上下文容器初始化；
Blueprint ≈ 拆分的 Controller 模块；
json 接口、http 状态码就是后端接口标准返回。
1.App Factory应用工厂模式
作用：把 Flask app 实例的创建逻辑封装成一个函数，不要全局直接写app = Flask(__name__)
为什么要用工厂
- 方便单元测试：每次测试可以生成全新 app 实例
- 多实例、多配置：开发 / 测试 / 生产环境加载不同配置
- 配合 Blueprint 做模块化项目，大型项目必备
App Factory 工厂模式
# app/__init__.py
from flask import Flask
def create_app():
# 工厂函数：内部创建app
app = Flask(__name__)
# 加载配置
app.config.from_object("config.Config")
# 在这里注册蓝图
from .routes.user import bp as user_bp
app.register_blueprint(user_bp)
return app
启动入口 run.py
from app import create_app
app = create_app()
if __name__ == "__main__":
app.run(debug=True)
2.Blueprint蓝图（模块化路由）
类比 SpringBoot 把接口拆分到不同 Controller； 大项目接口很多，全部堆在一个文件会爆炸，Blueprint 用来拆分模块。
- 可以给蓝图加统一 url 前缀，例如
/user/api - 每个蓝图拥有自己的路由、模板、静态资源、错误处理
示例：用户模块蓝图
# app/routes/user.py
from flask import Blueprint
# 创建蓝图对象
bp = Blueprint("user", __name__, url_prefix="/user")
@bp.route("/info")
def user_info():
return {"id":1, "name":"test"}
然后在create_app()里面 app.register_blueprint(bp) 访问地址：/user/info
要点：
- Blueprint 本身不能直接 run，必须注册到主 app 上；
url_prefix给这一组接口统一加路径前缀。
3.Flask JSON返回
方式1：jsonify() --推荐
自动设置响应头 Content-Type: application/json
from flask import jsonify
@bp.route("/demo")
def demo():
return jsonify(code=0, msg="ok", data={"name":"xxx"})
方式2：直接返回dict(flask2.0 + 新特性)
@bp.route("/demo2")
def demo2():
return {"code":0,"msg":"ok"}
Flask 会自动转 json，设置正确 Content‑Type。
4. HTTP 常用状态码（后端接口必记）
| 状态码 | 含义 | 业务场景 |
|---|---|---|
| 2xx 成功 | ||
| 200 OK | 请求成功 | 查询接口正常返回数据 |
| 201 Created | 创建成功 | POST 新增数据成功 |
| 状态码 | 名称 | 含义 | 典型场景 |
|---|---|---|---|
| 400 | Bad Request | 请求参数错误 | 参数格式不对、JSON 解析失败、必填字段缺失 |
| 401 | Unauthorized | 未认证 | 没登录、token 缺失或失效 |
| 403 | Forbidden | 权限不足 | 已登录，但没有访问该资源的权限 |
| 404 | Not Found | 资源不存在 | URL 写错、查询的记录不存在 |
| 405 | Method Not Allowed | 请求方法不允许 | 接口只允许 POST，客户端发了 GET |
| 状态码 | 名称 | 含义 | 典型场景 |
|---|---|---|---|
| 500 | Internal Server Error | 服务器内部异常 | 代码抛异常未捕获、空指针、数据库连接失败 |
| 503 | Service Unavailable | 服务不可用 | 服务过载、停机维护、网关找不到后端实例 |
速记口诀
2xx 成功，4xx 你错，5xx 我错
- 400 参数错，401 没登录，403 没权限，404 找不到，405 方法错
- 500 代码崩了，503 服务挂了
对比 SpringBoot 快速理解：
- create_app → Spring 容器初始化
- Blueprint → @RestController 模块
- jsonify → 返回 ResponseBody JSON
- http 状态码和 Web 完全一致。
Flask Mock 机械臂后端：把 MockArm(ArmPort) 包装成 HTTP 服务
"""Flask Mock 机械臂后端：把 MockArm(ArmPort) 包装成 HTTP 服务（第 2 周 D1/D2）。"""
from __future__ import annotations
import os
import uuid
from flask import Flask, Response, jsonify, request
from ..backend.mock_arm import ArmPort, MockArm
API_KEY = os.environ.get("ARM_API_KEY", "demo-key")
def _err(code: str, message: str, http_status: int) -> tuple[Response, int]:
return jsonify({"code": code, "message": message, "retryable": False, "details": {}}), http_status
def create_app(arm: ArmPort | None = None) -> Flask:
app = Flask(__name__)
arm = arm if arm is not None else MockArm()
@app.before_request
def require_api_key() -> tuple[Response, int] | None:
if request.headers.get("X-API-Key") != API_KEY:
return _err("UNAUTHORIZED", "无效或缺失 API Key", 401)
return None
@app.after_request
def add_request_id(resp: Response) -> Response:
resp.headers["X-Request-ID"] = request.headers.get("X-Request-ID") or uuid.uuid4().hex
return resp
@app.errorhandler(Exception)
def handle_unexpected(_exc: Exception) -> tuple[Response, int]:
return _err("INTERNAL_ERROR", "系统内部错误", 500)
@app.post("/v1/arm/actions")
def actions() -> tuple[Response, int]:
data = request.get_json(silent=True)
if not isinstance(data, dict):
return _err("INVALID_TASK", "请求体必须是 JSON 对象", 400)
action = data.get("action")
if action == "gripper_open":
ok = arm.gripper("open")
elif action == "gripper_close":
ok = arm.gripper("close")
elif action == "move_to_pose":
ok = arm.move_to_pose(
float(data.get("x", 0.0)),
float(data.get("y", 0.0)),
float(data.get("z", 0.0)),
float(data.get("speed", 0.3)),
)
elif action == "estop":
arm.estop()
ok = True
else:
return _err("INVALID_TASK", f"未知动作: {action}", 400)
return jsonify({"success": ok, "message": "ok" if ok else "step failed", "action": action}), 200
@app.get("/v1/arm/status")
def status() -> tuple[Response, int]:
return jsonify(arm.status()), 200
return app
整体功能：把内存里的模拟机械臂 MockArm，包装成 HTTP 接口服务，外部通过发 HTTP 请求就能控制虚拟机械臂 使用 Flask 工厂模式 create_app()，不是全局 app。
依赖：
MockArm是底层模拟机械臂对象，ArmPort是它的抽象接口；这个 flask 只做 http 转发，真正控制机械臂的逻辑不在这层代码里。
全局变量
API_KEY = os.environ.get("ARM_API_KEY", "demo-key")
- 接口鉴权密钥，优先读环境变量
ARM_API_KEY，没设置就默认demo‑key - 调用接口请求头必须带
X‑API‑Key: demo‑key，否则直接 401 拒绝。
工具函数 _err ()
def _err(code: str, message: str, http_status: int) -> tuple[Response, int]:
统一错误返回工具：返回标准化 json + HTTP 状态码。 示例：_err("UNAUTHORIZED","无效key",401) 输出 HTTP 401，body：
{"code":"UNAUTHORIZED","message":"无效或缺失 API Key","retryable":false,"details":{}}
create_app (arm) 工厂函数【主入口】
def create_app(arm: ArmPort | None = None) -> Flask:
- 创建 Flask app 实例
- 如果外部没有传入机械臂实例
arm，就自己新建一个MockArm()模拟机械臂
好处：单元测试的时候，可以传入自己写的 mock 对象，不用真的新建 MockArm。
① before_request：请求拦截器（每一个接口进来最先执行）
@app.before_request
def require_api_key()
每一次 HTTP 请求，在进入路由函数之前先跑这个函数。 逻辑：读取请求头
X‑API‑Key，和全局API_KEY对比。
- 对不上：调用
_err()返回 401 未认证，直接拦截，不再往下走到接口； - 校验通过：return None，放行，继续执行接口函数。
② after_request：响应后置钩子（接口处理完，返回给客户端之前执行）
@app.after_request
def add_request_id(resp: Response)
接口业务逻辑跑完，准备返回给浏览器 / 客户端的时候执行。 给 HTTP 响应头加上 X‑Request‑ID：
- 如果客户端请求带了这个 ID，就沿用；
- 没有就自动生成一个 uuid 随机字符串。 作用：日志排查，每个请求有唯一 ID，方便追踪问题。
③ @app.errorhandler (Exception) 全局异常捕获
@app.errorhandler(Exception)
def handle_unexpected(_exc: Exception)
兜底：只要接口代码抛出任何没捕获的异常，都会跑到这里。 不会直接抛堆栈给前端，统一返回
INTERNAL_ERROR，HTTP 500。 对应 HTTP 状态码 500，服务端内部错误。
④ POST /v1/arm/actions 动作执行接口【写操作，控制机械臂】
@app.post("/v1/arm/actions")
def actions()
客户端 POST 发 JSON，告诉机械臂做什么动作。
执行链路：
request.get_json(silent=True)拿请求体 JSON。- 如果请求不是合法 json 对象，返回 400：
请求体必须是 JSON 对象
- 如果请求不是合法 json 对象，返回 400：
- 读取字段
action，判断是什么动作：gripper_open→arm.gripper("open")打开夹爪gripper_close→arm.gripper("close")关闭夹爪move_to_pose→ 解析 x/y/z/speed，调用arm.move_to_pose()移动机械臂到坐标点estop→ 急停arm.estop()- 其他 action 值 → 返回 400，
未知动作
- 拿到底层 arm 返回的 ok 布尔值，包装 json 返回，HTTP 200。
{"success": true/false, "message":"ok / step failed", "action":"gripper_open"}
注意：哪怕动作执行失败 ok=False，HTTP 状态码依旧返回 200；业务错误放在 json 内部 success 字段，不是用 4xx。
⑤ GET /v1/arm/status 获取机械臂状态【查询接口】
@app.get("/v1/arm/status")
def status()
调用底层arm.status()拿到机械臂当前状态字典，直接 json 返回，HTTP200。 比如：夹爪状态、当前坐标、是否急停。
全部注册完成，返回 Flask app 实例。
Panthera： backend/app.py概览
#!/usr/bin/env python3
"""
Digital Twin Backend - Flask + WebSocket Server
Connects to Panthera robot and streams real-time data to web interface.
Usage:
python app.py # Uses default config
python app.py --config path/to/robot.yaml # Custom config
python app.py --demo # Demo mode without robot
"""
import sys
import os
import time
import threading
import logging
import argparse
import yaml
import numpy as np
import pinocchio as pin
from flask import Flask, jsonify, request, send_from_directory, Response
from flask_socketio import SocketIO, emit
from flask_cors import CORS
# Disable Flask's request logging
log = logging.getLogger('werkzeug')
log.setLevel(logging.ERROR)
# Add Panthera SDK to path
SDK_PATH = os.path.join(os.path.dirname(__file__), '..', '..', 'panthera_python')
sys.path.insert(0, SDK_PATH)
sys.path.insert(0, os.path.join(SDK_PATH, 'scripts'))
# Local config path (self-contained in digital_twin folder)
LOCAL_CONFIG_PATH = os.path.join(os.path.dirname(__file__), '..', 'robot_param', 'Follower.yaml')
app = Flask(__name__, static_folder='../frontend/dist', static_url_path='')
CORS(app, origins="*")
socketio = SocketIO(app, cors_allowed_origins="*", async_mode='threading')
# ============== GLOBAL SETTINGS ==============
CONTROL_FREQ = 200 # Hz - control loop frequency
BROADCAST_FREQ = 30 # Hz - WebSocket broadcast frequency
END_EFFECTOR_OFFSET = 0.07 # meters - offset from Link_6 origin to actual tool tip
ARM_JOINT_COUNT = 6
GRIPPER_JOINT_NAME = "gripper"
GRIPPER_DEFAULT_TARGET = 0.0
# =============================================
# Robot instance
robot = None
robot_config = None
demo_mode = False
script_mode = False # True when external script is pushing state
script_state = {
'positions': [0.0] * 6,
'velocities': [0.0] * 6,
'torques': [0.0] * 6,
'fk': None
}
# Control state
target_positions = [0.0] * 6
target_velocity = 0.6
max_torque = [10.0, 10.0, 10.0, 10.0, 10.0, 2.0]
reset_profile_active = False
reset_profile_target = [0.0] * 6
reset_profile_started_at = 0.0
reset_profile_handoff_until = 0.0
reset_profile_handoff_positions = [0.0] * 6
reset_gripper_active = False
reset_gripper_target = 0.0
RESET_MAX_VELOCITY = 0.7
RESET_START_VELOCITY = 0.04
RESET_MIN_VELOCITY = 0.12
RESET_VELOCITY_GAIN = 1.8
RESET_VELOCITY_OFFSET = 0.06
RESET_NEAR_ZERO_THRESHOLD = 0.01
RESET_ACCEL_DURATION = 0.45
RESET_HANDOFF_DURATION = 0.18
# Current state
current_positions = [0.0] * 6
current_velocities = [0.0] * 6
current_torques = [0.0] * 6
# ============== CONTROL MODE SETTINGS ==============
# Modes: 'position', 'gravity_comp', 'gravity_friction', 'impedance'
control_mode = 'position'
# Gravity compensation parameters
gravity_gain = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
joint_offset = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
tau_limit = np.array([10.0, 10.0, 10.0, 10.0, 10.0, 3.7])
# Impedance control parameters (PD + gravity)
# impedance_K = np.array([10.0, 21.0, 21.0, 16.0, 13.0, 1.0]) # Stiffness
# impedance_B = np.array([1.0, 2.0, 2.0, 0.9, 0.8, 0.1]) # Damping
impedance_K = np.array([5.0, 12.0, 12.0, 4.0, 4.0, 2.0])
impedance_B = np.array([0.5, 1.0, 1.0, 0.4, 0.4, 0.2])
impedance_target = np.array([0.0, 0.7, 0.7, -0.1, 0.0, 0.0]) # Target position
# Joint configuration
JOINT_CONFIG = []
URDF_PATH = None
# Thread control
target_lock = threading.Lock()
loop_running = False
connected_clients = set()
# Forward kinematics data
current_fk = {
'position': [0.0, 0.0, 0.0],
'euler': [0.0, 0.0, 0.0], # Roll, Pitch, Yaw in degrees
'rotation': [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
}
# Waypoints for trajectory execution (max 6)
MAX_WAYPOINTS = 6
waypoints = []
trajectory_running = False
trajectory_progress = 0.0
TRAJECTORY_START_BLEND_DURATION = 1.0
def rotation_matrix_to_euler(R):
"""Convert rotation matrix to euler angles (ZYX intrinsic order, degrees)
Uses scipy.spatial.transform.Rotation for robust conversion.
ZYX intrinsic = Yaw-Pitch-Roll convention.
Returns: [roll, pitch, yaw] in degrees
Note: For this robot's coordinate frame:
- Roll = Y rotation (lateral tilt)
- Pitch = X rotation (forward/back tilt)
- Yaw = Z rotation (heading)
"""
from scipy.spatial.transform import Rotation
R = np.array(R)
rot = Rotation.from_matrix(R)
# ZYX intrinsic returns [Z, Y, X] = [yaw, pitch, roll] order
zyx_angles = rot.as_euler('ZYX', degrees=True)
# Swap to match robot's coordinate frame convention
roll = zyx_angles[1] # Y rotation
pitch = zyx_angles[2] # X rotation
yaw = zyx_angles[0] # Z rotation
return [roll, pitch, yaw]
# Timing stats
timing_stats = {
"loop_count": 0,
"avg_cmd_time": 0.0,
"overruns": 0
}
# External wrench (force/torque estimation)
current_wrench = [0.0] * 6
FT_LAMBDA = 0.05 # DLS damping for force estimation
FT_Fc = np.array([0.10, 0.12, 0.12, 0.08, 0.03, 0.02])
FT_Fv = np.array([0.04, 0.06, 0.06, 0.04, 0.02, 0.02])
FT_VEL_THRESH = 0.02
TOOL_OFFSET = np.array([0.165, 0.0, 0.0])
def compute_external_wrench(robot, joint_angles, joint_velocities, joint_torques):
"""Estimate external Cartesian force/torque from joint torque measurements.
Returns: [Fx, Fy, Fz, Mx, My, Mz]
"""
try:
q = np.asarray(joint_angles)
dq = np.asarray(joint_velocities)
tau_measured = np.asarray(joint_torques)
# Gravity compensation
G = robot.get_Gravity(q)
# Friction compensation
F_friction = robot.get_friction_compensation(dq, FT_Fc, FT_Fv, FT_VEL_THRESH)
# External joint torque = measured - model
tau_ext = tau_measured - G - F_friction
# Compute Jacobian at last joint
q_pin = np.zeros(robot.model.nq)
for i, name in enumerate(robot.joint_names):
jid = robot.model.getJointId(name)
q_pin[robot.model.joints[jid].idx_q] = q[i]
pin.computeJointJacobians(robot.model, robot.data, q_pin)
last_jid = robot.model.getJointId(robot.joint_names[-1])
J_full = pin.getJointJacobian(
robot.model, robot.data, last_jid,
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
# Tool offset adjustment
T_last = robot.data.oMi[last_jid]
r_world = T_last.rotation @ TOOL_OFFSET
skew_r = np.array([[0, -r_world[2], r_world[1]],
[r_world[2], 0, -r_world[0]],
[-r_world[1], r_world[0], 0]])
J_tcp = J_full.copy()
J_tcp[:3, :] -= skew_r @ J_full[3:, :]
# Select 6 controlled joints
cols = [robot.model.joints[robot.model.getJointId(n)].idx_v
for n in robot.joint_names]
J6 = J_tcp[:, cols]
# DLS: F = J^T (J J^T + λ²I)^(-1) τ_ext
JJT = J6 @ J6.T
F_ext = J6.T @ np.linalg.solve(JJT + FT_LAMBDA**2 * np.eye(6), tau_ext)
return F_ext.tolist()
except Exception as e:
# Silently return zero on any estimation error
return [0.0] * 6
def precise_sleep(duration):
"""High precision sleep function"""
if duration <= 0:
return
end_time = time.perf_counter() + duration
# Use sleep for most of the time (leave 1ms margin)
if duration > 0.001:
time.sleep(duration - 0.001)
# Busy wait for final precision
while time.perf_counter() < end_time:
pass
def _current_arm_positions():
if robot is not None and not demo_mode:
try:
robot.send_get_motor_state_cmd()
pos = robot.get_current_pos()
return (pos.tolist() if hasattr(pos, 'tolist') else list(pos))[:ARM_JOINT_COUNT]
except Exception:
pass
return current_positions.copy()
def _prepare_trajectory_from_current_pose():
with target_lock:
_cancel_reset_profile()
current_start = _clamp_arm_positions(_current_arm_positions())
waypoint_list = [wp['positions'] for wp in waypoints]
durations = [wp['duration'] for wp in waypoints[:-1]]
if waypoint_list:
first_distance = max(
abs(current_start[i] - waypoint_list[0][i])
for i in range(min(len(current_start), len(waypoint_list[0])))
)
if first_distance > 0.01:
waypoint_list = [current_start] + waypoint_list
durations = [TRAJECTORY_START_BLEND_DURATION] + durations
return waypoint_list, durations
def execute_trajectory_thread(waypoint_list, durations, control_rate=100):
"""Execute trajectory in a separate thread"""
global trajectory_running, trajectory_progress, target_positions, control_mode
if len(waypoint_list) < 2:
print("Need at least 2 waypoints for trajectory")
return False
if robot is None or demo_mode:
print("Cannot execute trajectory: robot not available or in demo mode")
# In demo mode, simulate trajectory execution
if demo_mode:
trajectory_running = True
total_duration = sum(durations)
elapsed = 0
for seg_idx, duration in enumerate(durations):
start_pos = waypoint_list[seg_idx]
end_pos = waypoint_list[seg_idx + 1]
steps = int(duration * 30) # Lower rate for demo
for step in range(steps):
if not trajectory_running:
return False
t = step / steps
# Smooth interpolation
s = 3 * t**2 - 2 * t**3
pos = [start_pos[i] + s * (end_pos[i] - start_pos[i]) for i in range(len(start_pos))]
with target_lock:
target_positions[:] = pos
elapsed += duration / steps
trajectory_progress = elapsed / total_duration
socketio.emit('trajectory_progress', {'progress': trajectory_progress})
time.sleep(1.0 / 30)
trajectory_progress = 1.0
socketio.emit('trajectory_progress', {'progress': 1.0})
socketio.emit('trajectory_complete', {'success': True})
trajectory_running = False
return True
return False
# Switch to trajectory mode - control_loop will skip, letting this thread have exclusive control.
control_mode = 'trajectory'
trajectory_running = True
trajectory_progress = 0.0
dt = 1.0 / control_rate
total_duration = sum(durations)
elapsed_total = 0
try:
for segment in range(len(durations)):
start_pos = waypoint_list[segment]
end_pos = waypoint_list[segment + 1]
duration = durations[segment]
steps = int(duration * control_rate)
segment_start = time.perf_counter()
for step in range(steps):
if not trajectory_running:
# Trajectory was cancelled
socketio.emit('trajectory_complete', {'success': False, 'cancelled': True})
return False
target_time = segment_start + (step + 1) * dt
current_time = step * dt
# Generate interpolated trajectory using septic polynomial
pos, vel, _ = robot.septic_interpolation(start_pos, end_pos, duration, current_time)
# Send control command
robot.Joint_Pos_Vel(pos, vel, max_torque)
# Update progress
elapsed_total = sum(durations[:segment]) + current_time
trajectory_progress = elapsed_total / total_duration
# Broadcast progress to clients
socketio.emit('trajectory_progress', {'progress': trajectory_progress})
# High precision wait
wait_time = target_time - time.perf_counter()
if wait_time > 0:
precise_sleep(wait_time)
# Move to final position
final_pos = waypoint_list[-1]
robot.Joint_Pos_Vel(final_pos, [0.0] * robot.motor_count, max_torque)
trajectory_progress = 1.0
socketio.emit('trajectory_progress', {'progress': 1.0})
socketio.emit('trajectory_complete', {'success': True})
except Exception as e:
print(f"Trajectory execution error: {e}")
socketio.emit('trajectory_complete', {'success': False, 'error': str(e)})
finally:
trajectory_running = False
control_mode = 'position'
with target_lock:
if waypoint_list:
target_positions[:] = _clamp_arm_positions(waypoint_list[-1])
return True
def load_config(config_path):
"""Load robot configuration from YAML file"""
global robot_config, JOINT_CONFIG, URDF_PATH
with open(config_path, 'r', encoding='utf-8') as f:
robot_config = yaml.safe_load(f)
config_dir = os.path.dirname(os.path.abspath(config_path))
# Load URDF path
urdf_relative = robot_config['urdf']['file_path']
URDF_PATH = os.path.normpath(os.path.join(config_dir, urdf_relative))
# Load joint configuration
joint_names = robot_config['kinematics']['joint_names']
lower_limits = robot_config['robot']['joint_limits']['lower']
upper_limits = robot_config['robot']['joint_limits']['upper']
JOINT_CONFIG = []
for i, name in enumerate(joint_names):
JOINT_CONFIG.append({
"name": name,
"index": i,
"min": lower_limits[i],
"max": upper_limits[i],
"kind": "arm"
})
gripper_limits = robot_config.get('robot', {}).get('gripper_limits')
if gripper_limits:
JOINT_CONFIG.append({
"name": GRIPPER_JOINT_NAME,
"index": len(joint_names),
"min": float(gripper_limits.get('lower', 0.0)),
"max": float(gripper_limits.get('upper', 1.8)),
"kind": "gripper"
})
print(f"Config loaded: {robot_config['robot']['name']}")
print(f"URDF: {URDF_PATH}")
print(f"Joints: {len(JOINT_CONFIG)}")
return robot_config
def _arm_joint_config():
return [jc for jc in JOINT_CONFIG if jc.get("kind") != "gripper"]
def _gripper_config():
for jc in JOINT_CONFIG:
if jc.get("kind") == "gripper" or jc.get("name") == GRIPPER_JOINT_NAME:
return jc
return None
def _gripper_limits_list():
cfg = _gripper_config()
if not cfg:
return None
return [cfg["min"], cfg["max"]]
def _clamp_gripper_position(position):
cfg = _gripper_config()
position = float(position)
if cfg:
position = max(cfg["min"], min(cfg["max"], position))
return position
def _set_gripper_target(position, velocity=0.3):
global _gripper_target
position = _clamp_gripper_position(position)
_gripper_target = position
if robot is not None and not demo_mode:
try:
robot.gripper_control(position, velocity, 0.15)
except Exception:
pass
return position
def _set_default_gripper_mit_hold():
global _gripper_target
_gripper_target = 0.0
if robot is not None and not demo_mode:
try:
robot.gripper_control_MIT(0.0, 0.0, 0.0, 0.9, 0.06)
except Exception:
pass
def _release_gripper():
"""Release gripper torque so it can be moved freely by hand."""
if robot is not None and not demo_mode:
try:
robot.gripper_control_MIT(0.0, 0.0, 0.0, 0.0, 0.0)
except Exception:
pass
def _hold_gripper_impedance():
"""Keep gripper in a light MIT hold while arm impedance is active."""
if robot is not None and not demo_mode:
try:
robot.gripper_control_MIT(1.0, 0.0, 0.0, 0.65, 0.06)
except Exception:
pass
def _read_gripper_position():
global _gripper_target
if robot is not None and not demo_mode:
try:
state = robot.get_current_state_gripper()
_gripper_target = _clamp_gripper_position(state.position)
except Exception:
pass
return _gripper_target
def _is_gripper_index(joint_index):
cfg = _gripper_config()
return cfg is not None and joint_index == cfg["index"]
def _reset_velocity_from_error(error):
return max(
RESET_MIN_VELOCITY,
min(RESET_MAX_VELOCITY, RESET_VELOCITY_GAIN * error + RESET_VELOCITY_OFFSET)
)
def _smoothstep(progress):
progress = max(0.0, min(1.0, progress))
return progress * progress * progress * (progress * (progress * 6.0 - 15.0) + 10.0)
def _reset_accel_velocity_cap(elapsed):
if RESET_ACCEL_DURATION <= 0:
return RESET_MAX_VELOCITY
progress = _smoothstep(elapsed / RESET_ACCEL_DURATION)
return RESET_START_VELOCITY + (RESET_MAX_VELOCITY - RESET_START_VELOCITY) * progress
def _clamp_arm_positions(positions):
arm_config = _arm_joint_config()
next_positions = list(positions[:len(target_positions)])
for i, pos in enumerate(next_positions):
if i < len(arm_config):
jc = arm_config[i]
next_positions[i] = max(jc["min"], min(jc["max"], pos))
return next_positions
def _cancel_reset_profile():
global reset_profile_active, reset_gripper_active, reset_profile_handoff_until
reset_profile_active = False
reset_gripper_active = False
reset_profile_handoff_until = 0.0
def _start_smooth_position_move(positions, gripper_target=None, with_handoff=False):
global target_positions, target_velocity, reset_profile_active, reset_profile_target, reset_profile_started_at
global reset_profile_handoff_until, reset_profile_handoff_positions
global reset_gripper_active, reset_gripper_target
reset_profile_target[:] = _clamp_arm_positions(positions)
target_positions[:] = reset_profile_target.copy()
if gripper_target is not None:
reset_gripper_target = _clamp_gripper_position(gripper_target)
reset_gripper_active = True
else:
reset_gripper_active = False
now = time.time()
reset_profile_started_at = now
reset_profile_handoff_until = 0.0
reset_profile_handoff_positions[:] = current_positions[:len(reset_profile_handoff_positions)]
if with_handoff:
reset_profile_handoff_positions[:] = _clamp_arm_positions(_current_arm_positions())
reset_profile_handoff_until = now + RESET_HANDOFF_DURATION
reset_profile_started_at = reset_profile_handoff_until
target_velocity = RESET_MAX_VELOCITY
reset_profile_active = True
def _start_smooth_position_reset(with_handoff=False):
_start_smooth_position_move(
[0.0] * len(target_positions),
gripper_target=0.0,
with_handoff=with_handoff
)
def _hold_current_position_target():
global target_positions
hold_positions = _clamp_arm_positions(_current_arm_positions())
target_positions[:] = hold_positions
_cancel_reset_profile()
def _set_control_mode(mode):
global control_mode, impedance_target, reset_profile_active, reset_gripper_active
previous_mode = control_mode
control_mode = mode
if mode == 'position':
if previous_mode in ['gravity_comp', 'gravity_friction', 'impedance']:
_hold_current_position_target()
return
_cancel_reset_profile()
if mode == 'impedance':
impedance_target = np.array(current_positions)
def init_robot(config_path):
"""Initialize robot connection"""
global robot, demo_mode
try:
if demo_mode:
# In demo mode, use PantheraSim for FK kinematics (no hardware needed)
print("Running in DEMO mode (no motor control)")
try:
from panthera_sim import PantheraSim
robot = PantheraSim(config_path)
print(f"Kinematics initialized for {robot.motor_count} joints (FK available)")
except Exception as e:
print(f"Could not initialize kinematics: {e}")
print("FK will not be available in demo mode")
robot = None
return True
from scripts.Panthera_lib.Panthera import Panthera
robot = Panthera(config_path)
print(f"Robot initialized with {robot.motor_count} motors")
# Read fresh motor state before getting positions
robot.send_get_motor_state_cmd()
robot.motor_send_cmd()
time.sleep(0.1)
pos = robot.get_current_pos()
# Validate that positions are reasonable (not uninitialized 999)
pos_list = pos.tolist() if hasattr(pos, 'tolist') else list(pos)
if any(abs(p) > 100 for p in pos_list):
print(f"[WARN] Motor positions look invalid ({pos_list}), defaulting to zero")
pos_list = [0.0] * len(pos_list)
global target_positions, current_positions, _gripper_target
target_positions = pos_list
current_positions = pos_list
_set_default_gripper_mit_hold()
return True
except Exception as e:
print(f"Failed to initialize robot: {e}")
print("Falling back to DEMO mode")
demo_mode = True
return True
def control_loop():
"""Main control loop - sends commands to robot based on control mode"""
global current_positions, current_velocities, current_torques, timing_stats
global control_mode, impedance_target, reset_profile_active, reset_gripper_active
dt = 1.0 / CONTROL_FREQ
# Zero arrays for torque-only control
zero_pos = [0.0] * 6
zero_vel = [0.0] * 6
zero_kp = [0.0] * 6
zero_kd = [0.0] * 6
while loop_running:
loop_start = time.time()
try:
if script_mode:
# Let the script thread control the robot exclusively
pass
elif not demo_mode and robot is not None:
with target_lock:
mode = control_mode
targets = target_positions.copy()
vel_target = target_velocity
reset_active = reset_profile_active
reset_target = reset_profile_target.copy()
reset_started_at = reset_profile_started_at
reset_handoff_until = reset_profile_handoff_until
reset_handoff_positions = reset_profile_handoff_positions.copy()
gripper_reset_active = reset_gripper_active
gripper_reset_target = reset_gripper_target
imp_target = impedance_target.copy()
# Process keyboard input (nudges targets)
_process_keyboard()
t1 = time.time()
if mode == 'position':
# Position control mode - direct joint control
# Safety clamp
arm_config = _arm_joint_config()
if arm_config:
for i in range(min(len(targets), len(arm_config))):
jc = arm_config[i]
targets[i] = max(jc['min'], min(jc['max'], targets[i]))
if reset_active:
now = time.time()
if reset_handoff_until > now:
targets = reset_handoff_positions[:len(targets)]
vel = [0.0] * len(targets)
else:
targets = reset_target[:len(targets)]
errors = [abs(targets[i] - current_positions[i]) for i in range(len(targets))]
max_error = max(errors) if errors else 0.0
accel_cap = _reset_accel_velocity_cap(now - reset_started_at)
vel = [min(accel_cap, _reset_velocity_from_error(error)) for error in errors]
if max_error < RESET_NEAR_ZERO_THRESHOLD:
vel = [RESET_MIN_VELOCITY] * len(targets)
else:
vel = [vel_target] * len(targets)
robot.Joint_Pos_Vel(targets, vel, max_torque, iswait=False)
if gripper_reset_active:
current_gripper_position = _read_gripper_position()
gripper_error = abs(gripper_reset_target - current_gripper_position)
accel_cap = _reset_accel_velocity_cap(time.time() - reset_started_at)
gripper_velocity = min(accel_cap, _reset_velocity_from_error(gripper_error))
if gripper_error < RESET_NEAR_ZERO_THRESHOLD:
gripper_velocity = RESET_MIN_VELOCITY
_set_gripper_target(gripper_reset_target, velocity=gripper_velocity)
elif mode == 'gravity_comp':
# Gravity compensation mode - robot floats freely
robot.send_get_motor_state_cmd()
q = robot.get_current_pos() + joint_offset
tor = robot.get_Gravity(q) * gravity_gain
tor = np.clip(tor, -tau_limit, tau_limit)
robot.pos_vel_tqe_kp_kd(zero_pos, zero_vel, tor.tolist(), zero_kp, zero_kd)
_release_gripper()
elif mode == 'gravity_friction':
# Gravity + friction compensation mode
robot.send_get_motor_state_cmd()
q = robot.get_current_pos() + joint_offset
dq = robot.get_current_vel()
tor_g = robot.get_Gravity(q) * gravity_gain
tor_f = robot.get_friction_compensation(dq, FT_Fc, FT_Fv, FT_VEL_THRESH)
tor = np.clip(tor_g + tor_f, -tau_limit, tau_limit)
robot.pos_vel_tqe_kp_kd(zero_pos, zero_vel, tor.tolist(), zero_kp, zero_kd)
_release_gripper()
elif mode == 'impedance':
# Impedance control mode - PD + gravity compensation
robot.send_get_motor_state_cmd()
q_current = robot.get_current_pos()
vel_current = robot.get_current_vel()
# PD torque
tor_pd = impedance_K * (imp_target - q_current) + impedance_B * (0.0 - vel_current)
# Gravity compensation
G = robot.get_Gravity(q_current + joint_offset)
# Total torque
tor = tor_pd + G * gravity_gain
tor = np.clip(tor, -tau_limit, tau_limit)
robot.pos_vel_tqe_kp_kd(zero_pos, zero_vel, tor.tolist(), zero_kp, zero_kd)
_hold_gripper_impedance()
elif mode == 'trajectory':
# Trajectory mode - control handled by execute_trajectory_thread
# Just skip, don't send any commands
pass
cmd_time = (time.time() - t1) * 1000
timing_stats["loop_count"] += 1
timing_stats["avg_cmd_time"] = (
timing_stats["avg_cmd_time"] * 0.95 + cmd_time * 0.05
)
except Exception as e:
print(f"Control loop error: {e}")
elapsed = time.time() - loop_start
if elapsed > dt:
timing_stats["overruns"] += 1
sleep_time = dt - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
def state_broadcast_loop():
"""Broadcast robot state to connected clients"""
global current_positions, current_velocities, current_torques, current_fk
dt = 1.0 / BROADCAST_FREQ
while loop_running:
loop_start = time.time()
try:
if script_mode:
# Script is running — still read real robot state for frontend
# (the script thread controls motors; we only observe)
if not demo_mode and robot is not None:
try:
pos = robot.get_current_pos()
vel = robot.get_current_vel()
tqe = robot.get_current_torque()
current_positions[:] = pos.tolist() if hasattr(pos, 'tolist') else list(pos)
current_velocities[:] = vel.tolist() if hasattr(vel, 'tolist') else list(vel)
current_torques[:] = tqe.tolist() if hasattr(tqe, 'tolist') else list(tqe)
except Exception:
pass
elif not demo_mode and robot is not None:
# Read fresh state from real robot
robot.send_get_motor_state_cmd()
robot.motor_send_cmd()
pos = robot.get_current_pos()
vel = robot.get_current_vel()
tqe = robot.get_current_torque()
current_positions = pos.tolist() if hasattr(pos, 'tolist') else list(pos)
current_velocities = vel.tolist() if hasattr(vel, 'tolist') else list(vel)
current_torques = tqe.tolist() if hasattr(tqe, 'tolist') else list(tqe)
# Calculate forward kinematics
try:
fk = robot.forward_kinematics(pos)
if fk:
current_fk['position'] = fk['position'].tolist() if hasattr(fk['position'], 'tolist') else list(fk['position'])
rotation = fk['rotation']
current_fk['rotation'] = rotation.tolist() if hasattr(rotation, 'tolist') else [list(row) for row in rotation]
current_fk['euler'] = rotation_matrix_to_euler(rotation)
except Exception as fk_error:
pass # FK calculation failed, keep previous values
else:
# Demo mode: smoothly interpolate to target
with target_lock:
targets = target_positions.copy()
for i in range(len(current_positions)):
diff = targets[i] - current_positions[i]
current_positions[i] += diff * 0.1 # Smooth interpolation
# Demo mode FK: use robot if available, otherwise skip
if robot is not None:
try:
fk = robot.forward_kinematics(np.array(current_positions))
if fk:
current_fk['position'] = fk['position'].tolist() if hasattr(fk['position'], 'tolist') else list(fk['position'])
rotation = fk['rotation']
current_fk['rotation'] = rotation.tolist() if hasattr(rotation, 'tolist') else [list(row) for row in rotation]
current_fk['euler'] = rotation_matrix_to_euler(rotation)
except Exception:
pass
# External wrench estimation (only in live mode)
if not demo_mode and robot is not None:
try:
wrench = compute_external_wrench(
robot, current_positions, current_velocities, current_torques
)
current_wrench[:] = wrench
except Exception:
pass
# Broadcast to all connected clients
if connected_clients:
with target_lock:
mode = control_mode
imp_target = impedance_target.tolist()
gripper_position = _read_gripper_position()
socketio.emit('robot_state', {
'positions': current_positions,
'velocities': current_velocities,
'torques': current_torques,
'target_positions': target_positions,
'gripper_position': gripper_position,
'control_mode': mode,
'impedance_target': imp_target,
'forward_kinematics': current_fk,
'ee_position': current_fk['position'],
'ee_euler': current_fk['euler'],
'external_wrench': current_wrench,
'timestamp': time.time()
})
except Exception as e:
print(f"Broadcast loop error: {e}")
elapsed = time.time() - loop_start
sleep_time = dt - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
def start_loops():
"""Start control and broadcast loops"""
global loop_running
loop_running = True
# Control loop (high frequency)
control_thread = threading.Thread(target=control_loop, daemon=True)
control_thread.start()
# Broadcast loop (medium frequency)
broadcast_thread = threading.Thread(target=state_broadcast_loop, daemon=True)
broadcast_thread.start()
print(f"Control loop started at {CONTROL_FREQ} Hz")
print(f"Broadcast loop started at {BROADCAST_FREQ} Hz")
# ============== Static Files ==============
ARM_DESCRIPTION_PATH = os.path.join(os.path.dirname(__file__), '..', 'arm_description')
PANTHERA_HT_PATH = os.path.join(os.path.dirname(__file__), '..', '..', 'panthera_python', 'Panthera-HT_description')
@app.route('/arm_description/<path:filename>')
def serve_arm_description(filename):
"""Serve files from arm_description folder"""
return send_from_directory(ARM_DESCRIPTION_PATH, filename)
@app.route('/Panthera-HT_description/<path:filename>')
def serve_panthera_ht(filename):
"""Serve files from Panthera-HT_description folder"""
return send_from_directory(PANTHERA_HT_PATH, filename)
# ============== REST API Routes ==============
@app.route('/')
def index():
"""Serve frontend"""
return send_from_directory(app.static_folder, 'index.html')
@app.route('/api/config')
def get_config():
"""Get robot configuration for frontend"""
return jsonify({
"robot_name": robot_config['robot']['name'] if robot_config else "Unknown",
"joints": JOINT_CONFIG,
"urdf_path": URDF_PATH,
"demo_mode": demo_mode,
"control_freq": CONTROL_FREQ,
"connected": robot is not None or demo_mode,
"control_mode": control_mode,
"end_effector_link": robot_config.get('urdf', {}).get('end_effector_link') if robot_config else None,
"end_effector_offset": END_EFFECTOR_OFFSET,
"gripper_limits": _gripper_limits_list(),
"impedance_kp": impedance_K.tolist(),
})
@app.route('/api/arm_description_files')
def get_arm_description_files():
"""Get list of files in arm_description and Panthera-HT_description folders for auto-loading"""
files = {}
def scan_directory(path, prefix='', url_prefix=''):
"""Recursively scan directory and build file list"""
for entry in os.scandir(path):
rel_path = os.path.join(prefix, entry.name) if prefix else entry.name
if entry.is_file():
url_path = rel_path
if url_prefix:
url_path = f'{url_prefix}/{rel_path}'
files[rel_path] = url_path
elif entry.is_dir():
scan_directory(entry.path, rel_path, url_prefix)
try:
# Scan arm_description
scan_directory(ARM_DESCRIPTION_PATH, url_prefix='/arm_description')
# Also scan Panthera-HT_description
scan_directory(PANTHERA_HT_PATH, url_prefix='/Panthera-HT_description')
return jsonify({
"success": True,
"files": files,
"base_url": "/arm_description"
})
except Exception as e:
return jsonify({
"success": False,
"error": str(e)
}), 500
@app.route('/api/status')
def get_status():
"""Get current robot status (REST fallback)"""
return jsonify({
"connected": robot is not None or demo_mode,
"demo_mode": demo_mode,
"script_mode": script_mode,
"script_running": _script_is_running(),
"current_script": _script_name,
"positions": current_positions,
"velocities": current_velocities,
"torques": current_torques,
"target_positions": target_positions,
"target_velocity": target_velocity,
"gripper_position": _read_gripper_position()
})
@app.route('/api/move_joint', methods=['POST'])
def move_joint():
"""Move a single joint"""
global target_positions
data = request.json
joint_index = data.get('joint')
position = data.get('position')
if joint_index is not None and position is not None:
joint_index = int(joint_index)
if _is_gripper_index(joint_index):
_cancel_reset_profile()
_set_gripper_target(position)
return jsonify({"success": True})
# Clamp to joint limits
arm_config = _arm_joint_config()
if arm_config and joint_index < len(arm_config):
jc = arm_config[joint_index]
position = max(jc['min'], min(jc['max'], position))
if joint_index < len(target_positions):
with target_lock:
_cancel_reset_profile()
target_positions[joint_index] = position
return jsonify({"success": True})
@app.route('/api/move', methods=['POST'])
def move_all():
"""Move all joints"""
global target_positions, target_velocity
data = request.json
gripper_velocity = data.get('velocity', 0.3)
with target_lock:
_cancel_reset_profile()
if 'positions' in data:
positions = list(data['positions'])
target_positions[:] = _clamp_arm_positions(positions)
gripper_cfg = _gripper_config()
if gripper_cfg and len(positions) > gripper_cfg["index"]:
_set_gripper_target(positions[gripper_cfg["index"]], velocity=gripper_velocity)
if 'gripper' in data:
_set_gripper_target(data['gripper'], velocity=gripper_velocity)
if 'velocity' in data:
target_velocity = data['velocity']
return jsonify({"success": True})
@app.route('/api/home', methods=['POST'])
def go_home():
"""Move to home position"""
with target_lock:
_start_smooth_position_reset()
return jsonify({"success": True})
@app.route('/api/stop', methods=['POST'])
def stop():
"""Stop at current position"""
global target_positions
with target_lock:
_cancel_reset_profile()
target_positions[:] = current_positions.copy()
return jsonify({"success": True})
# ── Script execution state ─────────────────────────────────────────
import subprocess as _subprocess
import contextlib
import io
_script_process = None
_script_name = None
_script_stop_event = threading.Event() # signal to stop in-process script
_backend_config_path = None # set at startup
SCRIPTS_DIR = os.path.normpath(os.path.join(os.path.dirname(__file__),
'..', '..', 'panthera_python', 'scripts'))
SCRIPT_LOG_DIR = os.path.join(os.path.dirname(__file__), 'logs')
SCRIPT_LOG_PATH = os.path.join(SCRIPT_LOG_DIR, 'script_runner.log')
_script_output_lock = threading.Lock()
_SCRIPT_OUTPUT_LIMIT = 1000
def _append_script_output(text):
if text is None:
return
text = str(text)
if not text:
return
os.makedirs(SCRIPT_LOG_DIR, exist_ok=True)
if not text.endswith('\n'):
text += '\n'
with _script_output_lock:
with open(SCRIPT_LOG_PATH, 'a', encoding='utf-8', errors='replace') as log_file:
log_file.write(text)
log_file.flush()
def _clear_script_output():
os.makedirs(SCRIPT_LOG_DIR, exist_ok=True)
with _script_output_lock:
with open(SCRIPT_LOG_PATH, 'w', encoding='utf-8'):
pass
def _get_script_output():
if not os.path.exists(SCRIPT_LOG_PATH):
return []
with _script_output_lock:
with open(SCRIPT_LOG_PATH, 'r', encoding='utf-8', errors='replace') as log_file:
lines = log_file.read().splitlines()
return lines[-_SCRIPT_OUTPUT_LIMIT:]
def _script_is_running():
return (_script_process is not None and
((isinstance(_script_process, _subprocess.Popen) and _script_process.poll() is None) or
(isinstance(_script_process, threading.Thread) and _script_process.is_alive())))
def _discover_scripts():
scripts = []
if not os.path.isdir(SCRIPTS_DIR):
return scripts
for filename in sorted(os.listdir(SCRIPTS_DIR)):
full_path = os.path.normpath(os.path.join(SCRIPTS_DIR, filename))
if not os.path.isfile(full_path) or not filename.endswith('.py') or filename.startswith('__'):
continue
name = filename[:-3]
label = name.replace('_', ' ')
scripts.append({
'name': name,
'file': filename,
'label': label,
})
scripts.sort(key=lambda item: item['file'])
return scripts
def _resolve_script_path(script_name):
script_name = (script_name or '').replace('\\', '/').lstrip('/')
if not script_name.endswith('.py'):
script_name += '.py'
script_path = os.path.normpath(os.path.join(SCRIPTS_DIR, script_name))
scripts_root = os.path.abspath(SCRIPTS_DIR)
abs_script_path = os.path.abspath(script_path)
if os.path.commonpath([scripts_root, abs_script_path]) != scripts_root:
return None, script_name
return script_path, script_name
@app.route('/api/scripts', methods=['GET'])
def list_scripts():
"""List available Python scripts from the SDK scripts directory."""
is_running = _script_is_running()
scripts = _discover_scripts()
return jsonify({
'scripts': scripts,
'scripts_count': len(scripts),
'scripts_dir': os.path.abspath(SCRIPTS_DIR),
'running': is_running,
'current_script': _script_name
})
@app.route('/api/scripts/output', methods=['GET'])
def get_script_output():
"""Get captured output from the current or last script run."""
return jsonify({
'success': True,
'running': _script_is_running(),
'current_script': _script_name,
'log_path': os.path.abspath(SCRIPT_LOG_PATH),
'output': _get_script_output()
})
@app.route('/api/scripts/log', methods=['GET'])
def get_script_log():
"""Read captured script output directly from the log file."""
lines = _get_script_output()
return Response('\n'.join(lines), mimetype='text/plain; charset=utf-8')
@app.route('/api/scripts/run', methods=['POST'])
def run_script():
"""Launch a script. Demo→subprocess(PantheraSim). Real→in-process thread."""
global _script_process, _script_name, script_mode
data = request.json or {}
script_name = data.get('script', '')
if not script_name:
return jsonify({'success': False, 'error': 'No script specified'}), 400
script_path, script_name = _resolve_script_path(script_name)
if not script_path:
return jsonify({'success': False, 'error': f'Invalid script path: {script_name}'}), 403
if not os.path.exists(script_path):
return jsonify({'success': False, 'error': f'Script not found: {script_name}'}), 404
if robot is None and not demo_mode:
return jsonify({'success': False, 'error': 'Robot is not connected'}), 409
_clear_script_output()
_append_script_output(f"[ScriptRunner] Starting {script_name}")
# Stop any running script first
if _script_process is not None:
if isinstance(_script_process, _subprocess.Popen) and _script_process.poll() is None:
_script_process.terminate()
try: _script_process.wait(timeout=2)
except Exception: _script_process.kill()
elif isinstance(_script_process, threading.Thread) and _script_process.is_alive():
# In-process thread — we can't force-kill reliably,
# but setting script_mode=False lets the control loop resume
script_mode = False
_script_process = None
_script_name = None
# ── Demo / simulation mode: subprocess with PantheraSim ──────────
if demo_mode or robot is None:
runner = os.path.join(os.path.dirname(__file__), 'run_script.py')
cmd = [sys.executable, runner, '--demo', script_name]
try:
log_file = open(SCRIPT_LOG_PATH, 'a', encoding='utf-8', buffering=1)
_script_process = _subprocess.Popen(
cmd,
cwd=os.path.dirname(__file__),
stdout=log_file,
stderr=log_file,
text=True,
bufsize=1,
)
_script_name = script_name
script_mode = True
threading.Thread(target=_watch_script_process, args=(_script_process, log_file), daemon=True).start()
return jsonify({'success': True, 'script': script_name, 'pid': _script_process.pid})
except Exception as e:
return jsonify({'success': False, 'error': str(e)}), 500
# ── Real robot mode: run in-process thread to avoid serial-port
# conflicts with the backend's existing Panthera instance ─────
try:
_script_name = script_name
script_mode = True
thr = threading.Thread(
target=_run_script_in_thread,
args=(script_path,),
daemon=True,
)
thr.start()
_script_process = thr
return jsonify({'success': True, 'script': script_name, 'thread': True})
except Exception as e:
script_mode = False
return jsonify({'success': False, 'error': str(e)}), 500
def _watch_script_process(process, log_file=None):
"""Clear running state when a subprocess exits."""
global _script_process, _script_name, script_mode
try:
return_code = process.wait()
_append_script_output(f"[ScriptRunner] Script exited with code {return_code}")
except Exception as e:
_append_script_output(f"[ScriptRunner] Process watcher error: {e}")
finally:
if log_file:
try:
log_file.close()
except Exception:
pass
if _script_process is process:
_script_process = None
_script_name = None
script_mode = False
class _ScriptOutputCapture(io.TextIOBase):
def __init__(self, stream):
super().__init__()
self.stream = stream
self._buffer = ''
def writable(self):
return True
def write(self, text):
if not text:
return 0
self.stream.write(text)
self.stream.flush()
self._buffer += text
while '\n' in self._buffer:
line, self._buffer = self._buffer.split('\n', 1)
_append_script_output(line)
return len(text)
def flush(self):
self.stream.flush()
if self._buffer:
_append_script_output(self._buffer)
self._buffer = ''
class _ScriptStop(Exception):
"""Raised inside a script thread to abort execution."""
pass
def _run_script_in_thread(script_path):
"""Execute a script in-process using the backend's real robot instance."""
global script_mode, _script_name, _script_process, robot, current_positions, current_velocities
global current_torques, current_fk, _script_stop_event
import types
_script_stop_event.clear()
real_robot = robot # NEVER patch this — control/broadcast loops use it
# ── joint limits for position clamping ──────────────────────────
arm_config = _arm_joint_config()
if arm_config:
jl_lower = np.array([jc['min'] for jc in arm_config])
jl_upper = np.array([jc['max'] for jc in arm_config])
else:
jl_lower = np.array([-3.14]*6)
jl_upper = np.array([3.14]*6)
def _check_stop():
if _script_stop_event.is_set():
raise _ScriptStop("Script stopped by user")
def _safe_pos():
raw = real_robot.get_current_pos()
if hasattr(raw, 'tolist'):
raw = raw.tolist()
raw = np.asarray(raw[:len(jl_lower)])
return np.clip(raw, jl_lower + 1e-6, jl_upper - 1e-6)
def _push_state():
"""Sync broadcast variables from real robot so frontend sees live position."""
try:
pos = real_robot.get_current_pos()
vel = real_robot.get_current_vel()
tqe = real_robot.get_current_torque()
current_positions[:] = pos.tolist() if hasattr(pos, 'tolist') else list(pos)
current_velocities[:] = vel.tolist() if hasattr(vel, 'tolist') else list(vel)
current_torques[:] = tqe.tolist() if hasattr(tqe, 'tolist') else list(tqe)
try:
fk = real_robot.forward_kinematics(pos)
if fk:
current_fk['position'] = fk['position'].tolist() if hasattr(fk['position'], 'tolist') else list(fk['position'])
Rmat = fk['rotation']
current_fk['rotation'] = Rmat.tolist() if hasattr(Rmat, 'tolist') else [list(row) for row in Rmat]
from scipy.spatial.transform import Rotation
rot = Rotation.from_matrix(np.array(Rmat))
zyx = rot.as_euler('ZYX', degrees=True)
current_fk['euler'] = [zyx[1], zyx[2], zyx[0]]
except Exception:
pass
except Exception:
pass
# ── Build a wrapper around the real robot ───────────────────────
# The wrapper adds: stop-check, position-clamping, auto-push.
# The real robot object is never modified.
class _ScriptRobotWrapper:
def __init__(self):
pass
def __getattr__(self, name):
# Forward everything to the real robot
attr = getattr(real_robot, name)
if not callable(attr):
return attr
def checked_method(*args, **kwargs):
_check_stop()
result = attr(*args, **kwargs)
_check_stop()
return result
return checked_method
# ── position getters (clamped) ──────────────────────────
def get_current_pos(self):
_check_stop()
return _safe_pos()
# ── control methods (checked + auto-push) ───────────────
def Joint_Pos_Vel(self, *a, **kw):
_check_stop()
r = real_robot.Joint_Pos_Vel(*a, **kw)
_push_state()
_check_stop()
return r
def pos_vel_tqe_kp_kd(self, *a, **kw):
_check_stop()
r = real_robot.pos_vel_tqe_kp_kd(*a, **kw)
_push_state()
_check_stop()
return r
def moveJ(self, *a, **kw):
_check_stop()
r = real_robot.moveJ(*a, **kw)
_push_state()
_check_stop()
return r
def Joint_Vel(self, *a, **kw):
_check_stop()
r = real_robot.Joint_Vel(*a, **kw)
_push_state()
return r
def moveL(self, *a, **kw):
_check_stop()
r = real_robot.moveL(*a, **kw)
_push_state()
return r
wrapper = _ScriptRobotWrapper()
# Make sure script imports see the current panthera_python/scripts tree.
scripts_path = os.path.abspath(SCRIPTS_DIR)
if scripts_path in sys.path:
sys.path.remove(scripts_path)
sys.path.insert(0, scripts_path)
# Import a fresh Panthera module so static helpers match the updated scripts.
for module_name in list(sys.modules):
if module_name == 'Panthera_lib' or module_name.startswith('Panthera_lib.'):
del sys.modules[module_name]
from Panthera_lib.Panthera import Panthera as RealPantheraClass
# Inject Panthera→wrapper into Panthera_lib
class _ScriptPantheraProxy:
def __new__(cls, *args, **kwargs):
return wrapper
panthera_lib = types.ModuleType('Panthera_lib')
panthera_lib.Panthera = _ScriptPantheraProxy
panthera_lib.TrajectoryRecorder = None
sys.modules['Panthera_lib'] = panthera_lib
sys.modules['Panthera_lib.Panthera'] = types.ModuleType('Panthera_lib.Panthera')
sys.modules['Panthera_lib.Panthera'].Panthera = _ScriptPantheraProxy
for attr_name in dir(RealPantheraClass):
if attr_name.startswith('__'):
continue
attr = getattr(RealPantheraClass, attr_name)
if isinstance(RealPantheraClass.__dict__.get(attr_name), staticmethod):
setattr(_ScriptPantheraProxy, attr_name, staticmethod(attr))
try:
with open(script_path) as f:
code = compile(f.read(), script_path, 'exec')
exec_globals = {'__name__': '__main__', '__file__': script_path}
capture = _ScriptOutputCapture(sys.stdout)
with contextlib.redirect_stdout(capture), contextlib.redirect_stderr(capture):
exec(code, exec_globals)
capture.flush()
except _ScriptStop:
print("[ScriptRunner] Script stopped by user")
_append_script_output("[ScriptRunner] Script stopped by user")
except Exception as e:
import traceback
print(f"[ScriptRunner] Error: {e}")
_append_script_output(f"[ScriptRunner] Error: {e}")
traceback.print_exc()
_append_script_output(traceback.format_exc())
finally:
script_mode = False
_script_name = None
_script_process = None
print("[ScriptRunner] Script finished")
_append_script_output("[ScriptRunner] Script finished")
@app.route('/api/scripts/stop', methods=['POST'])
def stop_script():
"""Stop the currently running script (subprocess or in-process thread)."""
global _script_process, _script_name, script_mode
if _script_process is None:
_script_name = None
script_mode = False
return jsonify({'success': True, 'status': 'not_running'})
# Subprocess
if isinstance(_script_process, _subprocess.Popen):
if _script_process.poll() is not None:
_script_process = None; _script_name = None; script_mode = False
return jsonify({'success': True, 'status': 'not_running'})
try:
_script_process.terminate()
try: _script_process.wait(timeout=3)
except Exception: _script_process.kill(); _script_process.wait(timeout=2)
_script_process = None; _script_name = None; script_mode = False
return jsonify({'success': True, 'status': 'stopped'})
except Exception as e:
return jsonify({'success': False, 'error': str(e)}), 500
# In-process thread — signal stop via event
if isinstance(_script_process, threading.Thread):
if not _script_process.is_alive():
_script_process = None; _script_name = None; script_mode = False
return jsonify({'success': True, 'status': 'not_running'})
_script_stop_event.set()
# Give the thread a moment to react
_script_process.join(timeout=3)
if _script_process is not None and _script_process.is_alive():
return jsonify({'success': False, 'status': 'stopping', 'error': 'Script did not stop within timeout'}), 202
script_mode = False
_script_process = None
_script_name = None
return jsonify({'success': True, 'status': 'stopped'})
return jsonify({'success': False, 'error': 'Unknown process type'}), 500
@app.route('/api/script_state', methods=['POST'])
def script_state_update():
"""Receive joint state from external simulation scripts (PantheraSim)."""
global script_mode, script_state, current_positions, current_velocities
global current_torques, current_fk
try:
data = request.json
if 'positions' in data:
script_state['positions'] = data['positions']
current_positions[:] = data['positions']
if 'velocities' in data:
script_state['velocities'] = data['velocities']
current_velocities[:] = data['velocities']
if 'torques' in data:
script_state['torques'] = data['torques']
current_torques[:] = data['torques']
if 'fk' in data and data['fk'] is not None:
current_fk = data['fk']
script_state['fk'] = data['fk']
script_mode = True
return jsonify({"success": True})
except Exception as e:
return jsonify({"success": False, "error": str(e)}), 400
@app.route('/api/set_zero', methods=['POST'])
def set_zero():
"""Reset encoder positions to zero (set current position as zero reference)"""
global robot, current_positions, target_positions
if robot is None:
return jsonify({"success": False, "error": "Robot not connected"}), 400
try:
# Call the robot's set_reset_zero method
robot.set_reset_zero()
robot.motor_send_cmd()
# Reset our tracked positions to zero
with target_lock:
current_positions[:] = [0.0] * len(current_positions)
target_positions[:] = [0.0] * len(target_positions)
print("[Set Zero] Encoder positions reset to zero")
# Broadcast the update to all clients
socketio.emit('joint_positions', {
'positions': [0.0] * len(current_positions),
'velocities': [0.0] * len(current_positions),
'torques': [0.0] * len(current_positions)
})
return jsonify({"success": True, "message": "Encoder positions reset to zero"})
except Exception as e:
print(f"[Set Zero] Error: {e}")
return jsonify({"success": False, "error": str(e)}), 500
@app.route('/api/set_velocity', methods=['POST'])
def set_velocity():
"""Set movement velocity"""
global target_velocity
with target_lock:
target_velocity = request.json.get('velocity', 0.5)
return jsonify({"success": True, "velocity": target_velocity})
@app.route('/api/set_mode', methods=['POST'])
def set_mode():
"""Set control mode: 'position', 'gravity_comp', 'gravity_friction', 'impedance'"""
data = request.json
mode = data.get('mode', 'position')
if mode not in ['position', 'gravity_comp', 'gravity_friction', 'impedance']:
return jsonify({"success": False, "error": "Invalid mode"}), 400
with target_lock:
_set_control_mode(mode)
print(f"Control mode changed to: {mode}")
return jsonify({"success": True, "mode": mode})
@app.route('/api/get_mode', methods=['GET'])
def get_mode():
"""Get current control mode and parameters"""
return jsonify({
"mode": control_mode,
"impedance": {
"K": impedance_K.tolist(),
"B": impedance_B.tolist(),
"target": impedance_target.tolist()
},
"gravity_comp": {
"gain": gravity_gain.tolist(),
"offset": joint_offset.tolist()
}
})
@app.route('/api/set_impedance_params', methods=['POST'])
def set_impedance_params():
"""Set impedance control parameters"""
global impedance_K, impedance_B, impedance_target
data = request.json
with target_lock:
if 'K' in data:
impedance_K = np.array(data['K'])
if 'B' in data:
impedance_B = np.array(data['B'])
if 'target' in data:
impedance_target = np.array(data['target'])
return jsonify({
"success": True,
"K": impedance_K.tolist(),
"B": impedance_B.tolist(),
"target": impedance_target.tolist()
})
@app.route('/api/set_impedance_target', methods=['POST'])
def set_impedance_target():
"""Set impedance control target position"""
global impedance_target
data = request.json
with target_lock:
if 'target' in data:
impedance_target = np.array(data['target'])
elif 'joint' in data and 'position' in data:
# Set single joint target
joint_index = data['joint']
impedance_target[joint_index] = data['position']
return jsonify({"success": True, "target": impedance_target.tolist()})
# ============== Waypoint API Routes ==============
@app.route('/api/waypoints', methods=['GET'])
def get_waypoints():
"""Get all waypoints"""
return jsonify({
"success": True,
"waypoints": waypoints,
"max_waypoints": MAX_WAYPOINTS,
"trajectory_running": trajectory_running
})
@app.route('/api/waypoints/add', methods=['POST'])
def add_waypoint():
"""Add current position as a new waypoint"""
global waypoints
if len(waypoints) >= MAX_WAYPOINTS:
return jsonify({"success": False, "error": f"Maximum {MAX_WAYPOINTS} waypoints allowed"}), 400
data = request.json
if 'positions' in data:
# Use provided positions
positions = data['positions']
else:
# Use current robot positions
positions = current_positions.copy()
duration = data.get('duration', 1.0)
waypoint = {
'positions': positions,
'duration': duration,
'index': len(waypoints)
}
waypoints.append(waypoint)
print(f"Added waypoint {len(waypoints)}: {positions}")
return jsonify({"success": True, "waypoint": waypoint, "total": len(waypoints)})
@app.route('/api/waypoints/update', methods=['POST'])
def update_waypoint():
"""Update a waypoint"""
global waypoints
data = request.json
index = data.get('index')
if index is None or index < 0 or index >= len(waypoints):
return jsonify({"success": False, "error": "Invalid waypoint index"}), 400
if 'positions' in data:
waypoints[index]['positions'] = data['positions']
if 'duration' in data:
waypoints[index]['duration'] = data['duration']
return jsonify({"success": True, "waypoint": waypoints[index]})
@app.route('/api/waypoints/delete', methods=['POST'])
def delete_waypoint():
"""Delete a waypoint"""
global waypoints
data = request.json
index = data.get('index')
if index is None or index < 0 or index >= len(waypoints):
return jsonify({"success": False, "error": "Invalid waypoint index"}), 400
deleted = waypoints.pop(index)
# Update indices
for i, wp in enumerate(waypoints):
wp['index'] = i
return jsonify({"success": True, "deleted": deleted, "total": len(waypoints)})
@app.route('/api/waypoints/clear', methods=['POST'])
def clear_waypoints():
"""Clear all waypoints"""
global waypoints
waypoints = []
return jsonify({"success": True})
@app.route('/api/waypoints/go_to', methods=['POST'])
def go_to_waypoint():
"""Move robot to a specific waypoint"""
data = request.json
index = data.get('index')
if index is None or index < 0 or index >= len(waypoints):
return jsonify({"success": False, "error": "Invalid waypoint index"}), 400
positions = waypoints[index]['positions']
switched_to_position = False
with target_lock:
switched_to_position = control_mode in ['gravity_comp', 'gravity_friction']
if switched_to_position:
_set_control_mode('position')
_start_smooth_position_move(positions, with_handoff=switched_to_position)
if switched_to_position:
socketio.emit('mode_changed', {'mode': 'position'})
return jsonify({"success": True, "target": positions})
@app.route('/api/trajectory/run', methods=['POST'])
def run_trajectory():
"""Execute trajectory through all waypoints"""
global trajectory_running
if trajectory_running:
return jsonify({"success": False, "error": "Trajectory already running"}), 400
if len(waypoints) < 2:
return jsonify({"success": False, "error": "Need at least 2 waypoints"}), 400
data = request.json
control_rate = data.get('control_rate', 100)
waypoint_list, durations = _prepare_trajectory_from_current_pose()
# Start trajectory in separate thread
traj_thread = threading.Thread(
target=execute_trajectory_thread,
args=(waypoint_list, durations, control_rate),
daemon=True
)
traj_thread.start()
return jsonify({"success": True, "message": "Trajectory started"})
@app.route('/api/trajectory/stop', methods=['POST'])
def stop_trajectory():
"""Stop running trajectory"""
global trajectory_running
trajectory_running = False
return jsonify({"success": True})
@app.route('/api/trajectory/status', methods=['GET'])
def trajectory_status():
"""Get trajectory execution status"""
return jsonify({
"running": trajectory_running,
"progress": trajectory_progress
})
# ============== WebSocket Events ==============
@socketio.on('connect')
def handle_connect():
"""Handle new WebSocket connection"""
connected_clients.add(request.sid)
print(f"Client connected: {request.sid} (total: {len(connected_clients)})")
# Send initial config
emit('config', {
"robot_name": robot_config['robot']['name'] if robot_config else "Unknown",
"joints": JOINT_CONFIG,
"demo_mode": demo_mode,
"connected": robot is not None or demo_mode,
"control_mode": control_mode,
"end_effector_link": robot_config.get('urdf', {}).get('end_effector_link') if robot_config else None,
"gripper_limits": _gripper_limits_list(),
"impedance": {
"K": impedance_K.tolist(),
"B": impedance_B.tolist(),
"target": impedance_target.tolist()
},
"end_effector_offset": END_EFFECTOR_OFFSET
})
@socketio.on('disconnect')
def handle_disconnect():
"""Handle WebSocket disconnection"""
connected_clients.discard(request.sid)
print(f"Client disconnected: {request.sid} (total: {len(connected_clients)})")
@socketio.on('move_joint')
def handle_move_joint(data):
"""Handle joint movement command via WebSocket"""
global target_positions
joint_index = data.get('joint')
position = data.get('position')
if joint_index is not None and position is not None:
joint_index = int(joint_index)
if _is_gripper_index(joint_index):
_cancel_reset_profile()
_set_gripper_target(position)
return
arm_config = _arm_joint_config()
if arm_config and joint_index < len(arm_config):
jc = arm_config[joint_index]
position = max(jc['min'], min(jc['max'], position))
if joint_index < len(target_positions):
with target_lock:
_cancel_reset_profile()
target_positions[joint_index] = position
@socketio.on('move_all')
def handle_move_all(data):
"""Handle all joints movement via WebSocket"""
global target_positions, target_velocity
with target_lock:
gripper_velocity = data.get('velocity', 0.3)
_cancel_reset_profile()
if 'positions' in data:
positions = list(data['positions'])
target_positions[:] = _clamp_arm_positions(positions)
gripper_cfg = _gripper_config()
if gripper_cfg and len(positions) > gripper_cfg["index"]:
_set_gripper_target(positions[gripper_cfg["index"]], velocity=gripper_velocity)
if 'gripper' in data:
_set_gripper_target(data['gripper'], velocity=gripper_velocity)
if 'velocity' in data:
target_velocity = data['velocity']
@socketio.on('home')
def handle_home():
"""Handle home command via WebSocket"""
with target_lock:
_start_smooth_position_reset()
@socketio.on('reset_all')
def handle_reset_all():
"""Handle smooth reset command via WebSocket"""
with target_lock:
_start_smooth_position_reset()
@socketio.on('stop')
def handle_stop():
"""Handle stop command via WebSocket"""
global target_positions
with target_lock:
_cancel_reset_profile()
target_positions[:] = current_positions.copy()
@socketio.on('set_zero')
def handle_set_zero():
"""Handle set zero command via WebSocket - reset encoder positions to zero"""
global robot, current_positions, target_positions
if robot is None:
emit('set_zero_result', {'success': False, 'error': 'Robot not connected'})
return
try:
# Call the robot's set_reset_zero method
robot.set_reset_zero()
robot.motor_send_cmd()
# Reset our tracked positions to zero
with target_lock:
_cancel_reset_profile()
current_positions[:] = [0.0] * len(current_positions)
target_positions[:] = [0.0] * len(target_positions)
print("[Set Zero] Encoder positions reset to zero")
# Broadcast the update to all clients
socketio.emit('joint_positions', {
'positions': [0.0] * len(current_positions),
'velocities': [0.0] * len(current_positions),
'torques': [0.0] * len(current_positions)
})
emit('set_zero_result', {'success': True, 'message': 'Encoder positions reset to zero'})
except Exception as e:
print(f"[Set Zero] Error: {e}")
emit('set_zero_result', {'success': False, 'error': str(e)})
@socketio.on('set_mode')
def handle_set_mode(data):
"""Handle control mode change via WebSocket"""
mode = data.get('mode', 'position')
if mode in ['position', 'gravity_comp', 'gravity_friction', 'impedance']:
with target_lock:
_set_control_mode(mode)
print(f"Control mode changed to: {mode}")
# Broadcast mode change to all clients
socketio.emit('mode_changed', {'mode': mode})
@socketio.on('set_impedance_target')
def handle_set_impedance_target(data):
"""Handle impedance target change via WebSocket"""
global impedance_target
with target_lock:
if 'target' in data:
impedance_target = np.array(data['target'])
elif 'joint' in data and 'position' in data:
joint_index = data['joint']
impedance_target[joint_index] = data['position']
@socketio.on('set_impedance_params')
def handle_set_impedance_params(data):
"""Handle impedance parameter change via WebSocket"""
global impedance_K, impedance_B
with target_lock:
if 'K' in data:
impedance_K = np.array(data['K'])
if 'B' in data:
impedance_B = np.array(data['B'])
@socketio.on('add_waypoint')
def handle_add_waypoint(data):
"""Add waypoint via WebSocket"""
global waypoints
if len(waypoints) >= MAX_WAYPOINTS:
emit('waypoint_error', {'error': f'Maximum {MAX_WAYPOINTS} waypoints allowed'})
return
if 'positions' in data:
positions = data['positions']
else:
positions = current_positions.copy()
duration = data.get('duration', 1.0)
waypoint = {
'positions': positions,
'duration': duration,
'index': len(waypoints)
}
waypoints.append(waypoint)
# Broadcast to all clients
socketio.emit('waypoints_updated', {'waypoints': waypoints})
@socketio.on('delete_waypoint')
def handle_delete_waypoint(data):
"""Delete waypoint via WebSocket"""
global waypoints
index = data.get('index')
if index is None or index < 0 or index >= len(waypoints):
emit('waypoint_error', {'error': 'Invalid waypoint index'})
return
waypoints.pop(index)
# Update indices
for i, wp in enumerate(waypoints):
wp['index'] = i
socketio.emit('waypoints_updated', {'waypoints': waypoints})
@socketio.on('clear_waypoints')
def handle_clear_waypoints():
"""Clear all waypoints via WebSocket"""
global waypoints
waypoints = []
socketio.emit('waypoints_updated', {'waypoints': waypoints})
@socketio.on('update_waypoint_duration')
def handle_update_waypoint_duration(data):
"""Update waypoint duration via WebSocket"""
global waypoints
index = data.get('index')
duration = data.get('duration')
if index is None or index < 0 or index >= len(waypoints):
return
if duration is not None:
waypoints[index]['duration'] = duration
socketio.emit('waypoints_updated', {'waypoints': waypoints})
@socketio.on('run_trajectory')
def handle_run_trajectory(data):
"""Run trajectory via WebSocket"""
global trajectory_running
if trajectory_running:
emit('trajectory_error', {'error': 'Trajectory already running'})
return
if len(waypoints) < 2:
emit('trajectory_error', {'error': 'Need at least 2 waypoints'})
return
control_rate = data.get('control_rate', 100)
waypoint_list, durations = _prepare_trajectory_from_current_pose()
traj_thread = threading.Thread(
target=execute_trajectory_thread,
args=(waypoint_list, durations, control_rate),
daemon=True
)
traj_thread.start()
@socketio.on('stop_trajectory')
def handle_stop_trajectory():
"""Stop trajectory via WebSocket"""
global trajectory_running
trajectory_running = False
@socketio.on('go_to_waypoint')
def handle_go_to_waypoint(data):
"""Move to waypoint via WebSocket"""
index = data.get('index')
if index is None or index < 0 or index >= len(waypoints):
return
positions = waypoints[index]['positions']
switched_to_position = False
with target_lock:
switched_to_position = control_mode in ['gravity_comp', 'gravity_friction']
if switched_to_position:
_set_control_mode('position')
_start_smooth_position_move(positions, with_handoff=switched_to_position)
if switched_to_position:
socketio.emit('mode_changed', {'mode': 'position'})
# ============== Keyboard Control ==============
_key_state = {}
_key_lock = threading.Lock()
_valid_keys = {'w','s','a','d','q','e','i','k','j','l','u','o','z','x'}
_cmd_queue = []
_cmd_queue_lock = threading.Lock()
KEY_STEP = 0.015 # rad per control cycle at 200Hz (~3 rad/s max)
GRIPPER_STEP = 0.02 # rad per control cycle
_gripper_target = 0.0
@socketio.on('key_down')
def handle_key_down(data):
key = data.get('key', '').lower()
if key in _valid_keys:
with _key_lock:
_key_state[key] = True
@socketio.on('key_up')
def handle_key_up(data):
key = data.get('key', '').lower()
if key in _valid_keys:
with _key_lock:
_key_state[key] = False
@socketio.on('command')
def handle_command(data):
action = data.get('action', '')
if action in ('home', 'zero_ft', 'print_pose'):
with _cmd_queue_lock:
_cmd_queue.append(action)
def _process_keyboard():
"""Apply active keyboard keys to control targets.
Call from control loop (200 Hz) when in position or impedance mode."""
global target_positions, impedance_target, _gripper_target, current_positions
with _key_lock:
keys = dict(_key_state)
with _cmd_queue_lock:
cmds = list(_cmd_queue)
_cmd_queue.clear()
for cmd in cmds:
if cmd == 'home':
_start_smooth_position_reset()
impedance_target[:] = [0.0] * 6
_gripper_target = 0.0
elif cmd == 'print_pose':
print(f"[KB] pos={current_positions} imp_target={impedance_target}")
if not keys:
return
if control_mode == 'position':
with target_lock:
t = target_positions
if keys.get('w'): t[0] += KEY_STEP
if keys.get('s'): t[0] -= KEY_STEP
if keys.get('a'): t[1] += KEY_STEP
if keys.get('d'): t[1] -= KEY_STEP
if keys.get('q'): t[2] += KEY_STEP
if keys.get('e'): t[2] -= KEY_STEP
if keys.get('i'): t[3] += KEY_STEP
if keys.get('k'): t[3] -= KEY_STEP
if keys.get('j'): t[4] += KEY_STEP
if keys.get('l'): t[4] -= KEY_STEP
if keys.get('u'): t[5] += KEY_STEP
if keys.get('o'): t[5] -= KEY_STEP
arm_config = _arm_joint_config()
if arm_config:
for i in range(min(len(t), len(arm_config))):
t[i] = max(arm_config[i]['min'], min(arm_config[i]['max'], t[i]))
elif control_mode == 'impedance':
t = impedance_target
if keys.get('w'): t[0] += KEY_STEP
if keys.get('s'): t[0] -= KEY_STEP
if keys.get('a'): t[1] += KEY_STEP
if keys.get('d'): t[1] -= KEY_STEP
if keys.get('q'): t[2] += KEY_STEP
if keys.get('e'): t[2] -= KEY_STEP
if keys.get('i'): t[3] += KEY_STEP
if keys.get('k'): t[3] -= KEY_STEP
if keys.get('j'): t[4] += KEY_STEP
if keys.get('l'): t[4] -= KEY_STEP
if keys.get('u'): t[5] += KEY_STEP
if keys.get('o'): t[5] -= KEY_STEP
arm_config = _arm_joint_config()
if arm_config:
for i in range(min(len(t), len(arm_config))):
t[i] = max(arm_config[i]['min'], min(arm_config[i]['max'], t[i]))
# Gripper keys (work in any mode)
if keys.get('z') or keys.get('x'):
if keys.get('z'): _gripper_target += GRIPPER_STEP
if keys.get('x'): _gripper_target -= GRIPPER_STEP
# Clamp
gl = _gripper_config()
if gl:
_gripper_target = max(gl['min'], min(gl['max'], _gripper_target))
if robot is not None and not demo_mode:
try:
robot.gripper_control(_gripper_target, 0.3, 0.5)
except Exception:
pass
# ============== Main ==============
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Digital Twin Backend Server')
parser.add_argument('--config', '-c', type=str,
default=LOCAL_CONFIG_PATH,
help='Path to robot config YAML')
parser.add_argument('--demo', action='store_true',
help='Run in demo mode without robot connection')
parser.add_argument('--port', '-p', type=int, default=5000,
help='Server port (default: 5000)')
args = parser.parse_args()
demo_mode = args.demo
print("=" * 50)
print("Digital Twin Backend Server")
print("=" * 50)
# Load configuration
config_path = args.config
if not os.path.isabs(config_path):
config_path = os.path.join(os.getcwd(), config_path)
_backend_config_path = config_path
print(f"\n1. Loading configuration from: {config_path}")
try:
load_config(config_path)
except Exception as e:
print(f"Failed to load config: {e}")
print("Using default joint configuration")
JOINT_CONFIG = [
{"name": f"Joint {i+1}", "index": i, "min": -3.14, "max": 3.14}
for i in range(6)
]
# Initialize robot
print("\n2. Initializing robot...")
init_robot(config_path)
# Start control loops
print("\n3. Starting control loops...")
start_loops()
# Start server
print(f"\n4. Starting server on port {args.port}...")
print(f"\n Backend API: http://localhost:{args.port}")
print(f" WebSocket: ws://localhost:{args.port}")
print("=" * 50)
socketio.run(app, host='0.0.0.0', port=args.port, debug=False,
allow_unsafe_werkzeug=True)
app.py 中文备注 + 接口文档,与源码对应概览
# Panthera-HT 数字孪生后端 `app.py` 中文备注 + 接口文档
> 来源：`Panthera-HT_Host/Panthera_digital_twin-main/backend/app.py`
> 说明：本文件是 Panthera-HT 的 **上位机 Web 后端**（Flask + Socket.IO），连接真机并把实时状态推给前端页面；同时暴露一套 HTTP API 供外部脚本/上层 Agent 调用。
## 一、文件概览（中文备注导读）
| 模块 | 作用 | 关键点 |
|---|---|---|
| 全局配置 | Flask app + SocketIO + CORS | `async_mode='threading'`，`CORS("*")` |
| SDK 路径注入 | 把 `panthera_python` 加入 `sys.path` | 让后端能 `import Panthera` |
| 控制参数 | `CONTROL_FREQ=200Hz` 控制环、`BROADCAST_FREQ=30Hz` 状态广播 | 控制环与广播环分离 |
| 状态变量 | `current_positions/velocities/torques`、`target_positions`、`control_mode`、`waypoints` | 多线程共享，用 `target_lock` 保护 |
| 控制模式 | `position` / `gravity_comp` / `gravity_friction` / `impedance` | 通过 `/api/set_mode` 切换 |
| 正运动学 | `rotation_matrix_to_euler()` | 旋转矩阵 → RPY（度） |
| 核心线程 | `control_loop()`（200Hz 控制）、`state_broadcast_loop()`（30Hz 推送）、`execute_trajectory_thread()` | 轨迹在独立线程执行 |
| HTTP API | 31 个路由 | 详见第三节 |
| Socket.IO | 21 个 `on` 事件 + 10 个 `emit` 事件 | 详见第四节 |
**启动方式**：
```bash
python app.py # 默认配置，端口 5000
python app.py --demo # 无真机 Demo 模式
python app.py -p 8000 # 指定端口
python app.py --config robot.yaml # 自定义配置
```
## 二、模块级设置常量（备注）
| 常量 | 含义 |
|---|---|
| `CONTROL_FREQ=200` | 控制环频率（Hz） |
| `BROADCAST_FREQ=30` | WebSocket 状态广播频率（Hz） |
| `END_EFFECTOR_OFFSET=0.07` | Link6 原点到工具尖端的偏移（m） |
| `ARM_JOINT_COUNT=6` | 机械臂关节数 |
| `GRIPPER_JOINT_NAME="gripper"` | 夹爪关节名 |
| `MAX_WAYPOINTS=6` | 轨迹路点上限 |
| `KEY_STEP=0.015` | 键盘单步关节增量（rad） |
| `GRIPPER_STEP=0.02` | 键盘单步夹爪增量（rad） |
| `RESET_*` | 回原点/复位运动规划参数（速度、加速度、交接时长） |
## 三、HTTP API 接口文档
> 除静态资源与 `/api/config`、`/api/status` 等 GET 外，写接口均为 POST + JSON。
### 3.1 状态/配置
| 方法/路径 | 入参 | 出参 | 说明 |
|---|---|---|---|
| `GET /api/config` | — | `robot_name, urdf_path, end_effector_link, joints, control_mode, control_freq, connected, demo_mode` | 当前配置 |
| `GET /api/arm_description_files` | — | `base_url, files, success` | 描述文件列表 |
| `GET /api/status` | — | `connected, demo_mode, script_mode, script_running, current_script, positions[6], velocities[6], torques[6], target_positions[6], target_velocity, gripper_position` | 机械臂状态（REST 兜底） |
| `GET /api/get_mode` | — | `mode` | 当前控制模式 |
### 3.2 运动控制
| 方法/路径 | 入参 | 出参 | 说明 |
|---|---|---|---|
| `POST /api/move_joint` | `{joint, position}` | `success` | 单关节运动 |
| `POST /api/move` | `{positions[6], velocity}` | `success` | 全关节运动 |
| `POST /api/home` | — | `success` | 回原点 |
| `POST /api/stop` | — | `success` | 急停/停止 |
| `POST /api/set_zero` | — | `success, message, error` | 设零位 |
| `POST /api/set_velocity` | `{velocity}` | `success, velocity` | 设目标速度 |
| `POST /api/set_mode` | `{mode}` | `success, mode, error` | 切控制模式（position/gravity_comp/gravity_friction/impedance） |
| `POST /api/set_impedance_params` | `{K[6], B[6]}` | — | 设阻抗 PD 参数 |
| `POST /api/set_impedance_target` | `{positions[6]}` | — | 设阻抗目标 |
### 3.3 脚本执行（外部脚本推送状态）
| 方法/路径 | 入参 | 出参 | 说明 |
|---|---|---|---|
| `GET /api/scripts` | — | 脚本列表 | 枚举可运行脚本 |
| `GET /api/scripts/output` | — | 脚本输出 | 读输出缓冲 |
| `GET /api/scripts/log` | — | 日志内容 | 读日志文件 |
| `POST /api/scripts/run` | `{script}` | `success, pid, thread, script, error` | 后台运行脚本 |
| `POST /api/scripts/stop` | — | `success, status, error` | 停止脚本 |
| `POST /api/script_state` | `{positions, velocities, torques, fk}` | `success` | 外部脚本上报状态（script_mode） |
### 3.4 路点与轨迹
| 方法/路径 | 入参 | 出参 | 说明 |
|---|---|---|---|
| `GET /api/waypoints` | — | `waypoints, max_waypoints, trajectory_running, success` | 路点列表 |
| `POST /api/waypoints/add` | `{positions[6], duration}` | `success, error` | 加路点（≤6） |
| `POST /api/waypoints/update` | `{index, ...}` | `success, waypoint, error` | 更新路点 |
| `POST /api/waypoints/delete` | `{index}` | `success, error` | 删除路点 |
| `POST /api/waypoints/clear` | — | `success` | 清空路点 |
| `POST /api/waypoints/go_to` | `{index}` | `success, target, error` | 运动到某路点 |
| `POST /api/trajectory/run` | `{control_rate}` | `success, message, error` | 按路点跑轨迹 |
| `POST /api/trajectory/stop` | — | `success` | 停止轨迹 |
| `GET /api/trajectory/status` | — | `running, progress` | 轨迹进度 |
## 四、Socket.IO 接口文档
### 4.1 客户端 → 服务端（`on` 事件）
| 事件 | 入参 | 说明 |
|---|---|---|
| `connect` | — | 连接建立 |
| `disconnect` | — | 断开 |
| `move_joint` | `{joint, position}` | 单关节 |
| `move_all` | `{positions[6], velocity}` | 全关节 |
| `home` / `reset_all` / `stop` | — | 回原点/复位/停止 |
| `set_zero` | — | 设零 |
| `set_mode` | `{mode}` | 切模式 |
| `set_impedance_target` / `set_impedance_params` | 目标/PD 参数 | 阻抗 |
| `add_waypoint` | `{positions[6], duration}` | 加路点 |
| `delete_waypoint` | `{index}` | 删路点 |
| `clear_waypoints` | — | 清空 |
| `update_waypoint_duration` | `{index, duration}` | 改路点时长 |
| `run_trajectory` | `{control_rate}` | 跑轨迹 |
| `stop_trajectory` | — | 停轨迹 |
| `go_to_waypoint` | `{index}` | 去路点 |
| `key_down` / `key_up` | `{key}` | 键盘遥操 |
| `command` | `{action, w/a/s/d/q/e/i/k/j/l/u/o/z/x}` | 组合指令（含 `home`/`print_pose`） |
### 4.2 服务端 → 客户端（`emit` 事件）
| 事件 | 字段 | 说明 |
|---|---|---|
| `robot_state` | `positions, velocities, torques, target_positions, gripper_position, control_mode, impedance_target, forward_kinematics, ee_position, ee_euler, external_wrench, timestamp` | **主状态流（30Hz）** |
| `joint_positions` | 关节位置 | 关节状态 |
| `mode_changed` | 新模式 | 模式切换通知 |
| `config` | 配置 | 配置下发 |
| `set_zero_result` | 设零结果 | 设零回调 |
| `waypoints_updated` | 路点列表 | 路点变更 |
| `waypoint_error` | 错误 | 路点错误 |
| `trajectory_progress` | 进度 | 轨迹进度 |
| `trajectory_complete` | — | 轨迹完成 |
| `trajectory_error` | 错误 | 轨迹错误 |
## 五、与上层 Agent 的对接建议（本项目视角）
| 上层 ArmPort | 可映射的 Panthera-HT Host 接口 |
|---|---|
| `move_to_joint(joints, scaling)` | `POST /api/move`（`positions` + `velocity`） |
| `status()` | `GET /api/status` 或订阅 `robot_state`（30Hz） |
| `move_to_pose`（笛卡尔） | Host 只暴露关节空间；笛卡尔走 `Panthera-HT_ROS2 /move_to_pose` |
| `gripper` | Host 用 `gripper_position`（数字孪生夹爪目标） |
| `estop` | `POST /api/stop` |
> 注：Host 是"数字孪生 Web 上位机"，偏演示/遥操；**上层 Agent 的正式后端应优先用 Panthera-HT_ROS2 服务**（`/move_to_pose`、`/gripper_control`、`/arm_status` 等，已在《上层Agent系统接口文档》中记录）。Host 的 `app.py` 主要参考其"Flask + Socket.IO 状态推送"的组织方式。
---
## 六、HTTP API 方法详细说明（作用 / 入参 / 返回参数）
> 单位约定：`positions` 为关节角（rad），夹爪为归一化开度；`velocity` 为速度标量；`mode` 为控制模式字符串。
### 6.1 状态与配置
#### GET `/api/config`
- **作用**：返回前端初始化所需的机器人配置。
- **返回**：
- `robot_name`(string)：机器人名
- `joints`(list)：关节配置列表，每项 `{name, index, min, max}`
- `urdf_path`(string|null)：URDF 路径
- `demo_mode`(bool)：是否 Demo 模式
- `control_freq`(int)：控制环频率（200）
- `connected`(bool)：是否连接
- `control_mode`(string)：当前控制模式
- `end_effector_link`(string|null)：末端执行器连杆名
- `end_effector_offset`(float)：末端工具偏移（m）
- `gripper_limits`(list)：夹爪限位 `[min, max]`
- `impedance_kp`(list[6])：阻抗 K 参数
#### GET `/api/arm_description_files`
- **作用**：枚举描述文件（arm_description / Panthera-HT_description 目录递归）。
- **返回**：`success`(bool)、`files`(dict：相对路径→URL)、`base_url`(string)。
#### GET `/api/status`
- **作用**：获取机械臂状态（REST 兜底，与 `robot_state` 推送等价）。
- **返回**：
- `connected`(bool)、`demo_mode`(bool)、`script_mode`(bool)、`script_running`(bool)、`current_script`(string|null)
- `positions`(list[6])：当前关节角（rad）
- `velocities`(list[6])：当前关节速度
- `torques`(list[6])：当前关节力矩
- `target_positions`(list[6])：目标关节角
- `target_velocity`(float)：目标速度
- `gripper_position`(float)：夹爪位置
#### GET `/api/get_mode`
- **作用**：获取当前控制模式与相关参数。
- **返回**：`mode`(string)、`impedance{K,B,target}`(各 list[6])、`gravity_comp{gain,offset}`(各 list[6])。
### 6.2 运动控制
#### POST `/api/move_joint`
- **作用**：单关节运动（夹爪关节则设夹爪目标）。
- **入参**：`joint`(int 关节号)、`position`(float 目标角，rad)。
- **返回**：`success`(bool)。
#### POST `/api/move`
- **作用**：全关节运动。
- **入参**：`positions`(list[6]，目标关节角 rad)、`velocity`(float，可选，默认 0.3)。
- **返回**：`success`(bool)。
#### POST `/api/home`
- **作用**：回原点（平滑复位）。
- **入参**：无。
- **返回**：`success`(bool)。
#### POST `/api/stop`
- **作用**：把当前位置设为目标，实现立即停止。
- **入参**：无。
- **返回**：`success`(bool)。
#### POST `/api/set_zero`
- **作用**：设当前位姿为零位（真机 `set_reset_zero()`）。
- **入参**：无。
- **返回**：`success`(bool)；未连接 → `400 {success:false, error}`。
#### POST `/api/set_velocity`
- **作用**：设置运动目标速度。
- **入参**：`velocity`(float，默认 0.5)。
- **返回**：`success`(bool)、`velocity`(float)。
#### POST `/api/set_mode`
- **作用**：切换控制模式。
- **入参**：`mode`(string：`position`/`gravity_comp`/`gravity_friction`/`impedance`)。
- **返回**：`success`(bool)、`mode`(string)；非法模式 → `400 {success:false, error}`。
#### POST `/api/set_impedance_params`
- **作用**：设置阻抗控制 PD 参数（可选更新）。
- **入参**：`K`(list[6])、`B`(list[6])、`target`(list[6]) 均可选。
- **返回**：`success`(bool)、`K`、`B`、`target`。
#### POST `/api/set_impedance_target`
- **作用**：设置阻抗目标位置（整体或单关节）。
- **入参**：`target`(list[6])，或 `joint`(int)+`position`(float) 单关节。
- **返回**：`success`(bool)、`target`(list[6])。
### 6.3 脚本执行
#### GET `/api/scripts`
- **作用**：列出 SDK scripts 目录下可运行脚本。
- **返回**：`scripts`(list)、`scripts_count`(int)、`scripts_dir`(string)、`running`(bool)、`current_script`(string|null)。
#### GET `/api/scripts/output`
- **作用**：读取脚本运行输出缓冲。
- **返回**：`success`、`running`、`current_script`、`log_path`、`output`(string)。
#### GET `/api/scripts/log`
- **作用**：以纯文本返回脚本日志。
- **返回**：`text/plain` 响应体（逐行日志）。
#### POST `/api/scripts/run`
- **作用**：后台运行脚本（Demo→子进程；真机→进程内线程，避免串口冲突）。
- **入参**：`script`(string 脚本名)。
- **返回**：`success`、`script`、`pid`(Demo 模式) 或 `thread`(真机模式)；错误 `400/403/404/409/500` + `error`。
#### POST `/api/scripts/stop`
- **作用**：停止脚本。
- **返回**：`success`(bool)、`status`(string：`not_running`/`stopped`/`stopping`)、`error`(可选)。
#### POST `/api/script_state`
- **作用**：外部脚本（如 PantheraSim）上报关节状态。
- **入参**：`positions`(list[6])、`velocities`(list[6])、`torques`(list[6])、`fk`(dict，正运动学结果) 均可选。
- **返回**：`success`(bool)；解析失败 `400 + error`。
### 6.4 路点与轨迹
#### GET `/api/waypoints`
- **返回**：`success`、`waypoints`(list，每项 `{index, positions[6], duration}`)、`max_waypoints`(6)、`trajectory_running`(bool)。
#### POST `/api/waypoints/add`
- **作用**：添加路点。
- **入参**：`positions`(list[6]，可选，缺省=当前关节)、`duration`(float，可选，默认 1.0)。
- **返回**：`success`、`waypoint`、`total`；超过 6 个 → `400 + error`。
#### POST `/api/waypoints/update`
- **入参**：`index`(int)、`positions`(可选)、`duration`(可选)。
- **返回**：`success`、`waypoint`；非法 index → `400`。
#### POST `/api/waypoints/delete`
- **入参**：`index`(int)。
- **返回**：`success`、`deleted`、`total`；非法 → `400`。
#### POST `/api/waypoints/clear`
- **返回**：`success`。
#### POST `/api/waypoints/go_to`
- **入参**：`index`(int)。
- **返回**：`success`、`target`(list[6])；非法 → `400`。
#### POST `/api/trajectory/run`
- **作用**：按路点顺序执行轨迹（独立线程）。
- **入参**：`control_rate`(int，默认 100)。
- **返回**：`success`、`message`；已在跑/少于 2 路点 → `400 + error`。
#### POST `/api/trajectory/stop`
- **返回**：`success`。
#### GET `/api/trajectory/status`
- **返回**：`running`(bool)、`progress`(float)。
---
## 七、Socket.IO 事件方法详细说明
### 7.1 客户端 → 服务端（`on` 事件入参）
| 事件 | 入参字段 | 类型/说明 |
|---|---|---|
| `connect` / `disconnect` | — | 连接建立/断开 |
| `move_joint` | `joint`(int)、`position`(float) | 单关节运动 |
| `move_all` | `positions`(list[6])、`velocity`(float) | 全关节运动 |
| `home` | — | 回原点 |
| `reset_all` | — | 复位全部关节 |
| `stop` | — | 停止 |
| `set_zero` | — | 设零位 |
| `set_mode` | `mode`(string) | 切控制模式 |
| `set_impedance_target` | `target`(list[6]) 或 `joint`+`position` | 阻抗目标 |
| `set_impedance_params` | `K`、`B`、`target`(各 list[6]) | 阻抗参数 |
| `add_waypoint` | `positions`(可选)、`duration`(float) | 加路点 |
| `delete_waypoint` | `index`(int) | 删路点 |
| `clear_waypoints` | — | 清空路点 |
| `update_waypoint_duration` | `index`(int)、`duration`(float) | 改路点时长 |
| `run_trajectory` | `control_rate`(int) | 跑轨迹 |
| `stop_trajectory` | — | 停轨迹 |
| `go_to_waypoint` | `index`(int) | 去某路点 |
| `key_down` / `key_up` | `key`(string，单字符) | 键盘遥操 |
| `command` | `action`(string：`home`/`zero_ft`/`print_pose`) | 组合命令 |
### 7.2 服务端 → 客户端（`emit` 事件出参）
| 事件 | 出参字段 | 说明 |
|---|---|---|
| `robot_state` | `positions, velocities, torques, target_positions, gripper_position, control_mode, impedance_target, forward_kinematics, ee_position, ee_euler, external_wrench, timestamp` | 30Hz 主状态流 |
| `joint_positions` | 关节位置 | 关节状态 |
| `mode_changed` | `mode`(string) | 模式切换通知 |
| `config` | 配置对象 | 配置下发 |
| `set_zero_result` | 设零结果 | 设零回调 |
| `waypoints_updated` | `waypoints`(list) | 路点变更广播 |
| `waypoint_error` | `error`(string) | 路点错误 |
| `trajectory_progress` | `progress`(float) | 轨迹进度 |
| `trajectory_complete` | — | 轨迹完成 |
| `trajectory_error` | `error`(string) | 轨迹错误 |
扫一扫
199
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
