Fix portable paths, optional dependencies and project documentation
This commit is contained in:
@@ -12,3 +12,5 @@ runtime/arm/out/*
|
||||
runtime/arm/data/observation/sparse_plan.json
|
||||
runtime/hug/handeye_calibration_kit/logs/
|
||||
*.log
|
||||
|
||||
runtime/arm/build/
|
||||
|
||||
@@ -56,6 +56,4 @@ bash run.sh cup --place-open-return --execute
|
||||
|
||||
## 验证状态
|
||||
|
||||
本仓库整理自 2026-10-26 的实际杯子流程,保留当天代码的控制策略。最近一次对话记录中,中指第 6 槽故障码 64 连续三次刷新未消失,因此没有继续抬升。不能把代码齐全或历史局部成功当成当天整套流程成功。
|
||||
|
||||
本次交付只做离线打包检查,没有连接或控制机器人;验证范围见 [验证记录](docs/VALIDATION.md)。
|
||||
已提供默认杯子流程的离线检查。最近记录的抓取尝试在手部故障后终止,尚不能据此认定整套流程稳定成功;故障详情及验证范围见 [验证记录](docs/VALIDATION.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 推导。
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
-r requirements.txt
|
||||
ruckig==0.19.4
|
||||
@@ -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)
|
||||
@@ -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 <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
|
||||
#include <cmath>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
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<Eigen::VectorXd> points;
|
||||
for (size_t i=0; i<count; ++i) {
|
||||
Eigen::VectorXd q(7);
|
||||
for (int j=0; j<7; ++j) {
|
||||
if (!(std::cin >> 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<size_t>(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;
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user