diff --git a/.gitignore b/.gitignore index 68a03c6..68dba59 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,5 @@ runtime/arm/out/* runtime/arm/data/observation/sparse_plan.json runtime/hug/handeye_calibration_kit/logs/ *.log + +runtime/arm/build/ diff --git a/README.md b/README.md index f56b434..494aea3 100644 --- a/README.md +++ b/README.md @@ -56,6 +56,4 @@ bash run.sh cup --place-open-return --execute ## 验证状态 -本仓库整理自 2026-10-26 的实际杯子流程,保留当天代码的控制策略。最近一次对话记录中,中指第 6 槽故障码 64 连续三次刷新未消失,因此没有继续抬升。不能把代码齐全或历史局部成功当成当天整套流程成功。 - -本次交付只做离线打包检查,没有连接或控制机器人;验证范围见 [验证记录](docs/VALIDATION.md)。 +已提供默认杯子流程的离线检查。最近记录的抓取尝试在手部故障后终止,尚不能据此认定整套流程稳定成功;故障详情及验证范围见 [验证记录](docs/VALIDATION.md)。 diff --git a/docs/INSTALL.md b/docs/INSTALL.md index 60d8151..fd61536 100644 --- a/docs/INSTALL.md +++ b/docs/INSTALL.md @@ -39,3 +39,15 @@ config/camera_extrinsics_current.json 为共同 base_link 外参,右臂标定 ## 可选新物体 HUG 模式 --object-mode pointcloud 不是默认杯子流程。额外需要 HUG 源码、hug_full.safetensors 权重和其独立推理环境,以及 retarget_l20_v2.py 所在 retargeting 目录、dex-retargeting / pinocchio / PyYAML 环境。通过 HUG_ROOT、RETARGETING_ROOT、RETARGET_PYTHON 配置;权重和这些独立环境不包含在本仓库。本次不宣称此可选模式完成独立安装验收。 + +## 可选通用轨迹回放 + +默认杯子观察位路线不需要此组依赖。使用 runtime/arm 的通用 follow 轨迹规划时,先安装并编译离线 MoveIt TOTG 辅助程序: + +~~~bash +sudo apt install g++ libeigen3-dev ros-jazzy-moveit-core ros-jazzy-ruckig +.venv/bin/python -m pip install -r requirements-replay.txt +.venv/bin/python runtime/arm/build_totg.py +~~~ + +源文件为 runtime/arm/cpp/totg_offline.cpp,输出 runtime/arm/build/totg_offline。构建与离线定时均不连接机器人;实际执行仍受原控制检查约束。CUP_ROS_PREFIX 可覆盖 ROS 安装目录,默认从 CUP_ROS_SETUP 推导。 diff --git a/requirements-replay.txt b/requirements-replay.txt new file mode 100644 index 0000000..b67bf01 --- /dev/null +++ b/requirements-replay.txt @@ -0,0 +1,2 @@ +-r requirements.txt +ruckig==0.19.4 diff --git a/runtime/arm/build_totg.py b/runtime/arm/build_totg.py new file mode 100644 index 0000000..5ea8f27 --- /dev/null +++ b/runtime/arm/build_totg.py @@ -0,0 +1,19 @@ +"""Build the offline helper against the installed MoveIt library, no downloads.""" +from pathlib import Path +import subprocess +import os + +root = Path(__file__).resolve().parent +ros = Path(os.environ.get('CUP_ROS_PREFIX', Path(os.environ.get('CUP_ROS_SETUP', '/opt/ros/jazzy/setup.bash')).parent)).expanduser() +output = root/'build/totg_offline' +output.parent.mkdir(exist_ok=True) +include = [str(p) for p in (ros/'include').iterdir() if p.is_dir()] +cmd = ['g++', '-O2', '-std=c++17', '-I/usr/include/eigen3'] +cmd += ['-I'+p for p in include] +cmd += [str(root/'cpp/totg_offline.cpp'), '-L'+str(ros/'lib'), + '-L'+str(ros/'lib/x86_64-linux-gnu'), '-lmoveit_trajectory_processing', '-lruckig', + '-Wl,-rpath,'+str(ros/'lib'), '-Wl,-rpath,'+str(ros/'lib/x86_64-linux-gnu'), + '-Wl,-rpath-link,'+str(ros/'lib'), '-Wl,-rpath-link,'+str(ros/'lib/x86_64-linux-gnu'), + '-o', str(output)] +subprocess.run(cmd, check=True) +print(output) diff --git a/runtime/arm/cpp/totg_offline.cpp b/runtime/arm/cpp/totg_offline.cpp new file mode 100644 index 0000000..d52baeb --- /dev/null +++ b/runtime/arm/cpp/totg_offline.cpp @@ -0,0 +1,55 @@ +// Offline-only adapter to the INSTALLED MoveIt TOTG implementation. +// No ROS node, executor, RViz, CAN, or controller connection is created. +#include +#include +#include +#include +#include +#include + +int main() { + try { + size_t count; double velocity, acceleration, tolerance, sample_dt; + if (!(std::cin >> count >> velocity >> acceleration >> tolerance >> sample_dt) || + count < 2 || count > 10000 || !std::isfinite(velocity) || velocity <= 0 || + !std::isfinite(acceleration) || acceleration <= 0 || + !std::isfinite(tolerance) || tolerance <= 0 || tolerance > .01 || + !std::isfinite(sample_dt) || sample_dt < .001 || sample_dt > 1.) + throw std::runtime_error("Invalid TOTG input"); + std::vector points; + for (size_t i=0; i> q[j]) || !std::isfinite(q[j])) + throw std::runtime_error("Nonfinite/incomplete waypoint"); + } + if (points.empty() || (q-points.back()).norm() > 1e-10) points.push_back(q); + } + if (points.size()<2) throw std::runtime_error("No moving geometric path"); + auto path=trajectory_processing::Path::create(points, tolerance); + if (!path) throw std::runtime_error("MoveIt TOTG path failed"); + auto trajectory=trajectory_processing::Trajectory::create(*path, + Eigen::VectorXd::Constant(7,velocity), Eigen::VectorXd::Constant(7,acceleration), .001); + if (!trajectory) throw std::runtime_error("MoveIt TOTG parameterization failed"); + const double duration=trajectory->getDuration(); + if (!std::isfinite(duration) || duration<=0 || duration>1800) + throw std::runtime_error("Invalid TOTG duration"); + size_t n=static_cast(std::ceil(duration/sample_dt)); + // Uniform intervals including the exact endpoint avoid a tiny final fit + // interval. These are fit knots, not the final 100 Hz execution samples. + std::cout << std::setprecision(17) << duration << ' ' << path->getLength() << '\n'; + for (size_t i=0; i<=n; ++i) { + double t=duration*double(i)/double(n); + auto q=trajectory->getPosition(t); + auto v=trajectory->getVelocity(t); + auto a=trajectory->getAcceleration(t); + std::cout << t; + for (const auto& value:{q,v,a}) + for (int j=0; j<7; ++j) std::cout << ' ' << value[j]; + std::cout << '\n'; + } + } catch (const std::exception& e) { + std::cerr << "TOTG failed: " << e.what() << '\n'; + return 1; + } +} diff --git a/runtime/arm/src/a7_left/totg_timing.py b/runtime/arm/src/a7_left/totg_timing.py index 79964d5..dc8f80d 100644 --- a/runtime/arm/src/a7_left/totg_timing.py +++ b/runtime/arm/src/a7_left/totg_timing.py @@ -19,7 +19,7 @@ from .local_timing import allocate def totg_samples(q, cfg, sample_dt=.3): helper = ROOT/'build/totg_offline' if not helper.is_file(): - raise ValueError('MoveIt TOTG helper missing; run .venv/bin/python build_totg.py') + raise ValueError('MoveIt TOTG helper missing; run .venv/bin/python runtime/arm/build_totg.py from the repository root') q = np.asarray(q, float) if q.ndim != 2 or q.shape[1] != 7 or len(q)<2 or not np.isfinite(q).all(): raise ValueError('TOTG needs finite seven-axis waypoints') @@ -28,7 +28,8 @@ def totg_samples(q, cfg, sample_dt=.3): data = ' '.join(map(str, header))+'\n'+'\n'.join(' '.join(map(str, row)) for row in q)+'\n' env = os.environ.copy() # Isolate ROS's Ruckig from the different Python wheel version. - env['LD_LIBRARY_PATH'] = '/opt/ros/jazzy/lib:/opt/ros/jazzy/lib/x86_64-linux-gnu:'+env.get('LD_LIBRARY_PATH', '') + ros = Path(env.get('CUP_ROS_PREFIX', Path(env.get('CUP_ROS_SETUP', '/opt/ros/jazzy/setup.bash')).parent)) + env['LD_LIBRARY_PATH'] = os.pathsep.join([str(ros/'lib'), str(ros/'lib/x86_64-linux-gnu'), env.get('LD_LIBRARY_PATH', '')]) try: result = subprocess.run([str(helper)], input=data, capture_output=True, text=True, env=env, timeout=60, check=True)