diff --git a/CMakeLists.txt b/CMakeLists.txt index cd8ee90d..92bcb5b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,6 @@ set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE) set(CMAKE_BUILD_RPATH "\$ORIGIN:\$ORIGIN/../lib") set(CMAKE_INSTALL_RPATH "\$ORIGIN:\$ORIGIN/../lib") -# Use RUNPATH (new dtags) generally preferable set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) @@ -29,38 +28,6 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") include(FindExternalLib) set(ARCH "x86") setup_external_libs(${ARCH}) - -# Intel oneVPL / VA-API runtime. The shared libraries in lib/ are installed -# by setup_external_libs(); the VA-API driver plugin directory is installed -# separately because it must retain its dri layout. -set(INTEL_MEDIA_STACK_ROOT - "${PROJECT_SOURCE_DIR}/dependency/${ARCH}/third_party/intel-media-stack/vpl-2.17" -) -set(INTEL_MEDIA_DRIVER_DIR "${INTEL_MEDIA_STACK_ROOT}/lib/dri") -set(INTEL_IHD_DRIVER "${INTEL_MEDIA_DRIVER_DIR}/iHD_drv_video.so") - -if(NOT EXISTS "${INTEL_IHD_DRIVER}") - message(FATAL_ERROR "Intel iHD VA-API driver not found: ${INTEL_IHD_DRIVER}") -endif() - -message(STATUS "Intel media stack: ${INTEL_MEDIA_STACK_ROOT}") -install( - DIRECTORY "${INTEL_MEDIA_DRIVER_DIR}/" - DESTINATION lib/dri -) -install(CODE [=[ - find_program(CMVR_PATCHELF_EXECUTABLE patchelf REQUIRED) - set(_cmvr_ihd_driver - "${CMAKE_INSTALL_PREFIX}/lib/dri/iHD_drv_video.so" - ) - execute_process( - COMMAND "${CMVR_PATCHELF_EXECUTABLE}" - --set-rpath "$ORIGIN/.." - "${_cmvr_ihd_driver}" - COMMAND_ERROR_IS_FATAL ANY - ) -]=]) - # 在调用 setup_external_libs 之后 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}") message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}") @@ -140,7 +107,7 @@ target_link_libraries(cmvr_es PRIVATE cmvr_es::runtime cmvr_es::proto cmvr_es::logging - service + cmvr_es::quic_edge_task ${GLOG_LIBRARIES} jsoncpp cmvr_es::service diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index a4d133e9..0ff7c443 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -8,7 +8,9 @@ add_subdirectory(simulate) add_subdirectory(devices) add_subdirectory(manager/device_manager) add_subdirectory(manager/media_source_hub) +add_subdirectory(service/quic_edge) add_subdirectory(task) +add_subdirectory(task/quic_edge_task) add_subdirectory(manager/task_manager) add_subdirectory(service) add_subdirectory(runtime) diff --git a/cmvr-es/algorithms/CMakeLists.txt b/cmvr-es/algorithms/CMakeLists.txt index 53f2415c..471f0dfe 100644 --- a/cmvr-es/algorithms/CMakeLists.txt +++ b/cmvr-es/algorithms/CMakeLists.txt @@ -2,3 +2,4 @@ add_subdirectory(motion_planner) add_subdirectory(kinematics/ik_solver) add_subdirectory(perception) add_subdirectory(controllers) +add_subdirectory(collision_detection) diff --git a/cmvr-es/algorithms/README.md b/cmvr-es/algorithms/README.md new file mode 100644 index 00000000..90e64f19 --- /dev/null +++ b/cmvr-es/algorithms/README.md @@ -0,0 +1,125 @@ +# Algorithms 模块开发指南 + +`algorithms/` 保存与具体厂商协议无关的运动学、规划、控制和感知算法。算法接受通用类型或显式接口输入，不应直接解析设备报文，也不应承担 gRPC/QUIC 传输职责。 + +返回[项目总览](../../README.md)。 + +## 当前结构 + +| 目录 | 主要能力 | 主要 CMake target | +| --- | --- | --- | +| `kinematics/ik_solver/` | Pinocchio DLS/QP、SRS、LAWBA 逆运动学 | `cmvr_es::ik_solver` | +| `motion_planner/base_motion/` | TOPPRA、S 曲线、笛卡尔速度限制 | `cmvr_es::base_motion` | +| `motion_planner/arm_motion/` | MoveJ、MoveL、SpeedL 机械臂规划 | `cmvr_es::algorithms::arm_motion` | +| `controllers/` | PID、IBVS、笛卡尔速度控制 | `cmvr_es::algorithms::controller`、`cmvr_es::algorithms::arm_control` | +| `perception/` | AprilTag 和视觉定位 | `cmvr_es::perception` | + +顶层入口是 [`CMakeLists.txt`](CMakeLists.txt)。 + +## 依赖边界 + +- 算法层可以依赖 `common/`、Eigen、Pinocchio、OSQP、TOPPRA、OpenCV、ViSP 等； +- 不包含串口、CAN、HTTP 或厂商 SDK 协议处理； +- 不启动 gRPC/QUIC 服务或管理设备生命周期； +- 不从算法内部读取全局配置文件，构造或 `configure` 时显式传入配置； +- 可复用算法不应主动取得 `DeviceManager` 单例。 + +当前部分 controller target 仍链接 `device_manager`，这是现有耦合。新增算法应优先通过参数、回调或窄接口注入设备状态，避免继续扩大该依赖。 + +## 扩展已有算法类别 + +### 1. 定义或复用抽象接口 + +常用接口： + +- [`IKSolver`](kinematics/ik_solver/common/include/ik_solver.h) +- [`JointMotionPlanner`](motion_planner/arm_motion/joint_motion/joint_motion_planner.h) +- [`CartesianMotionPlanner`](motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h) + +接口应明确： + +- 输入输出单位和坐标系； +- 是否修改内部状态； +- 是否线程安全； +- 失败时输出是否保持不变； +- 是否支持实时循环，以及最大允许耗时。 + +### 2. 增加配置 + +在 [`../../protos/README.md`](../../protos/README.md) 指导下： + +1. 为算法增加独立配置 message； +2. 在所属 `oneof algorithm` 中增加新字段和新 tag； +3. 不复用已发布 tag； +4. 为迭代次数、容差、速度和加速度设置有效范围； +5. 在默认设备配置中给出显式参数。 + +### 3. 实现与工厂注册 + +将实现放在对应类别子目录，并修改实际工厂： + +- IK：[`ik_solver_factory.h`](kinematics/ik_solver/ik_solver_factory.h) +- MoveJ：[`joint_motion_planner_factory.h`](motion_planner/arm_motion/joint_motion/joint_motion_planner_factory.h) +- MoveL / SpeedL：[`cartesian_motion_planner_factory.h`](motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h) + +工厂失败应返回 `nullptr` 并记录清晰原因，不能静默回退到另一个算法。MoveL 和 SpeedL 的实现必须保持配置组合一致。 + +### 4. 更新 CMake + +- 将实现 `.cpp` 加入对应 library； +- 使用项目已有 alias target； +- 通过 `target_include_directories` 暴露公共头； +- 将依赖放入使用它的最小 target； +- 测试源文件不能加入生产共享库； +- 新增三方依赖时同步根依赖发现逻辑和 `request.txt`。 + +## 数值与机器人语义 + +算法扩展至少需要明确： + +- 关节位置单位为 rad，速度为 rad/s； +- 笛卡尔平移为 m，旋转和角速度为 rad； +- base、tool、world、user frame 的转换方向； +- URDF base frame、tip frame 和关节顺序； +- 位置、速度、加速度和 jerk 限制； +- 奇异点、不可达目标和求解超时行为； +- measured state 与算法内部 seed 的更新时机。 + +IK 在求解前应使用真实关节角更新 seed。MoveL 连续求解时，应使用上一步解更新下一步状态，不能一直使用初始状态。 + +## 测试要求 + +每个新算法至少覆盖： + +1. 正常输入； +2. 空输入、自由度不匹配和 NaN/Inf； +3. 关节限位与速度限制； +4. 不可达目标和不收敛； +5. 坐标系转换； +6. 确定性和重复调用； +7. 若用于实时控制，统计最坏执行时间； +8. 与一个已知模型或离线参考结果对比。 + +当前不少算法测试只通过 `add_executable()` 构建，没有登记到 CTest。新增无设备测试应放在 `BUILD_TESTING` 条件内，并使用 `add_test()`；需要图形界面、RealSense 或 MuJoCo 的测试应明确标为集成测试，不得阻塞默认无设备测试。 + +## 新增算法类别 + +如果现有类别无法承载： + +1. 在 `algorithms//` 新建目录； +2. 定义协议无关抽象接口； +3. 定义配置 Proto 和工厂； +4. 提供单独 CMake library 与 `cmvr_es::...` alias； +5. 在 [`algorithms/CMakeLists.txt`](CMakeLists.txt) 添加子目录； +6. 由设备或任务层注入使用，不让算法反向控制服务层； +7. 添加无设备单元测试和真实设备/仿真集成测试。 + +## 提交检查 + +- [ ] 厂商协议没有进入算法接口 +- [ ] 单位、坐标系和关节顺序明确 +- [ ] 工厂已注册且配置组合经过校验 +- [ ] 不可达、超时和数值异常可观测 +- [ ] 测试没有被编入生产共享库 +- [ ] 无设备测试已登记到 CTest +- [ ] 实时路径没有日志洪泛和无界内存分配 diff --git a/cmvr-es/algorithms/collision_detection/CMakeLists.txt b/cmvr-es/algorithms/collision_detection/CMakeLists.txt new file mode 100644 index 00000000..f6401175 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/CMakeLists.txt @@ -0,0 +1,48 @@ +add_library(self_collision_checker SHARED + self_collision/src/self_collision_checker.cpp + self_collision/src/distance_sampling_policy.cpp +) + +target_include_directories(self_collision_checker PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_compile_definitions(self_collision_checker PRIVATE + PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION + PINOCCHIO_WITH_HPP_FCL + COAL_DISABLE_HPP_FCL_WARNINGS +) + +target_link_libraries(self_collision_checker PUBLIC + pinocchio_default + pinocchio_parsers + pinocchio_collision + coal +) + +add_library(cmvr_es::self_collision_checker ALIAS self_collision_checker) + +add_executable(self_collision_checker_test + self_collision/test/self_collision_checker_test.cpp +) +target_link_libraries(self_collision_checker_test PRIVATE + cmvr_es::self_collision_checker + gtest + gtest_main + pthread +) +target_compile_definitions(self_collision_checker_test PRIVATE + CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}" +) + +add_executable(self_collision_benchmark + self_collision/benchmark/self_collision_benchmark.cpp +) +target_link_libraries(self_collision_benchmark PRIVATE + cmvr_es::self_collision_checker +) +target_compile_definitions(self_collision_benchmark PRIVATE + CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}" +) + +install(TARGETS self_collision_checker LIBRARY DESTINATION lib) diff --git a/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp b/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp new file mode 100644 index 00000000..dae09d71 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp @@ -0,0 +1,53 @@ +#include +#include +#include +#include +#include + +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +int main() +{ + const std::string urdf_path = std::string(CMVR_ES_SOURCE_DIR) + + "/model/xiaoyan_description/dual_arm_collision.urdf"; + const std::vector joint_names{ + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", + "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R", + }; + + cmvr::SelfCollisionChecker checker; + std::string error; + if (!checker.init(urdf_path, joint_names, {}, &error)) { + std::cerr << "Initialization failed: " << error << '\n'; + return 1; + } + + constexpr std::size_t kIterations = 2000; + std::vector samples_us; + samples_us.reserve(kIterations); + std::vector q(joint_names.size(), 0.0); + for (std::size_t iteration = 0; iteration < kIterations; ++iteration) { + q[0] = 0.2 * static_cast(iteration % 100) / 100.0; + const auto begin = std::chrono::steady_clock::now(); + const auto result = checker.check(q); + const auto end = std::chrono::steady_clock::now(); + if (!result.valid) { + std::cerr << "Collision check failed: " << result.error << '\n'; + return 1; + } + samples_us.push_back(std::chrono::duration(end - begin).count()); + } + + std::sort(samples_us.begin(), samples_us.end()); + double total_us = 0.0; + for (const double sample : samples_us) { + total_us += sample; + } + const std::size_t p99_index = static_cast(0.99 * (samples_us.size() - 1)); + std::cout << "active_pairs=" << checker.activePairCount() << '\n' + << "iterations=" << samples_us.size() << '\n' + << "average_us=" << total_us / samples_us.size() << '\n' + << "p99_us=" << samples_us[p99_index] << '\n' + << "max_us=" << samples_us.back() << '\n'; + return 0; +} diff --git a/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h b/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h new file mode 100644 index 00000000..f2b022af --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h @@ -0,0 +1,46 @@ +#ifndef CMVR_ES_DISTANCE_SAMPLING_POLICY_H +#define CMVR_ES_DISTANCE_SAMPLING_POLICY_H + +#include +#include + +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +namespace cmvr { + +struct DistanceSamplingOptions { + double max_geometry_displacement_m{0.002}; + double max_check_period_s{0.01}; +}; + +class DistanceSamplingPolicy { +public: + using Clock = std::chrono::steady_clock; + + bool configure(const DistanceSamplingOptions& options, + std::string* error = nullptr); + + bool shouldCheck(const CollisionGeometrySnapshot& current, + Clock::time_point now) const; + + void markChecked(const CollisionGeometrySnapshot& current, + Clock::time_point now); + + void reset(); + + double displacementSinceLastCheck( + const CollisionGeometrySnapshot& current) const; + + bool hasBaseline() const { return has_baseline_; } + +private: + DistanceSamplingOptions options_{}; + CollisionGeometrySnapshot last_checked_{}; + Clock::time_point last_check_time_{}; + bool configured_{false}; + bool has_baseline_{false}; +}; + +} // namespace cmvr + +#endif // CMVR_ES_DISTANCE_SAMPLING_POLICY_H diff --git a/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h b/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h new file mode 100644 index 00000000..60fae78b --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h @@ -0,0 +1,82 @@ +#ifndef CMVR_ES_SELF_COLLISION_CHECKER_H +#define CMVR_ES_SELF_COLLISION_CHECKER_H + +#include +#include +#include +#include + +#include + +namespace cmvr { + +struct CollisionPair { + std::string first; + std::string second; +}; + +struct SelfCollisionOptions { + std::vector ignored_pairs; +}; + +struct CollisionObjectPose { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::size_t geometry_index{0}; + Eigen::Vector3d position{Eigen::Vector3d::Zero()}; + Eigen::Quaterniond orientation{Eigen::Quaterniond::Identity()}; + double bounding_radius_m{0.0}; +}; + +struct CollisionGeometrySnapshot { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::vector> objects; +}; + +struct SelfCollisionResult { + bool valid{false}; + bool in_collision{false}; + double minimum_distance_m{0.0}; + std::string first; + std::string second; + std::string error; +}; + +// Instances cache Pinocchio work data and are not thread-safe. +class SelfCollisionChecker { +public: + SelfCollisionChecker(); + ~SelfCollisionChecker(); + + SelfCollisionChecker(SelfCollisionChecker&&) noexcept; + SelfCollisionChecker& operator=(SelfCollisionChecker&&) noexcept; + + SelfCollisionChecker(const SelfCollisionChecker&) = delete; + SelfCollisionChecker& operator=(const SelfCollisionChecker&) = delete; + + bool init(const std::string& urdf_path, + const std::vector& active_joint_names, + const SelfCollisionOptions& options, + std::string* error = nullptr); + + bool makeSnapshot(const std::vector& joint_positions, + CollisionGeometrySnapshot* snapshot, + std::string* error = nullptr); + + SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot); + SelfCollisionResult check(const std::vector& joint_positions); + + bool initialized() const; + std::size_t dof() const; + std::size_t activePairCount() const; + const std::vector& jointNames() const; + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace cmvr + +#endif // CMVR_ES_SELF_COLLISION_CHECKER_H diff --git a/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp b/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp new file mode 100644 index 00000000..e45df763 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp @@ -0,0 +1,100 @@ +#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h" + +#include +#include +#include + +namespace cmvr { +namespace { + +void setError(std::string* error, const std::string& message) +{ + if (error) { + *error = message; + } +} + +double rotationAngle(const Eigen::Quaterniond& first, + const Eigen::Quaterniond& second) +{ + const double dot = std::clamp( + std::abs(first.normalized().dot(second.normalized())), 0.0, 1.0); + return 2.0 * std::acos(dot); +} + +} // namespace + +bool DistanceSamplingPolicy::configure(const DistanceSamplingOptions& options, + std::string* error) +{ + if (!std::isfinite(options.max_geometry_displacement_m) || + options.max_geometry_displacement_m <= 0.0) { + setError(error, "max_geometry_displacement_m must be finite and positive"); + return false; + } + if (!std::isfinite(options.max_check_period_s) || + options.max_check_period_s <= 0.0) { + setError(error, "max_check_period_s must be finite and positive"); + return false; + } + options_ = options; + configured_ = true; + reset(); + if (error) { + error->clear(); + } + return true; +} + +bool DistanceSamplingPolicy::shouldCheck(const CollisionGeometrySnapshot& current, + const Clock::time_point now) const +{ + if (!configured_ || !has_baseline_) { + return true; + } + const double elapsed_s = std::chrono::duration(now - last_check_time_).count(); + if (elapsed_s >= options_.max_check_period_s) { + return true; + } + return displacementSinceLastCheck(current) >= options_.max_geometry_displacement_m; +} + +void DistanceSamplingPolicy::markChecked(const CollisionGeometrySnapshot& current, + const Clock::time_point now) +{ + last_checked_ = current; + last_check_time_ = now; + has_baseline_ = true; +} + +void DistanceSamplingPolicy::reset() +{ + last_checked_.objects.clear(); + last_check_time_ = Clock::time_point{}; + has_baseline_ = false; +} + +double DistanceSamplingPolicy::displacementSinceLastCheck( + const CollisionGeometrySnapshot& current) const +{ + if (!has_baseline_ || current.objects.size() != last_checked_.objects.size()) { + return std::numeric_limits::infinity(); + } + + double maximum_displacement = 0.0; + for (std::size_t index = 0; index < current.objects.size(); ++index) { + const auto& previous = last_checked_.objects[index]; + const auto& now = current.objects[index]; + if (previous.geometry_index != now.geometry_index) { + return std::numeric_limits::infinity(); + } + const double translation = (now.position - previous.position).norm(); + const double radius = std::max(previous.bounding_radius_m, now.bounding_radius_m); + const double swept_distance = + translation + radius * rotationAngle(previous.orientation, now.orientation); + maximum_displacement = std::max(maximum_displacement, swept_distance); + } + return maximum_displacement; +} + +} // namespace cmvr diff --git a/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp b/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp new file mode 100644 index 00000000..8606bd22 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp @@ -0,0 +1,403 @@ +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr { +namespace { + +using LinkPairKey = std::pair; + +LinkPairKey canonicalPair(std::string first, std::string second) +{ + if (second < first) { + std::swap(first, second); + } + return {std::move(first), std::move(second)}; +} + +void setError(std::string* error, const std::string& message) +{ + if (error) { + *error = message; + } +} + +} // namespace + +class SelfCollisionChecker::Impl { +public: + bool init(const std::string& urdf_path, + const std::vector& active_joint_names, + const SelfCollisionOptions& options, + std::string* error) + { + reset(); + if (urdf_path.empty()) { + setError(error, "URDF path is empty"); + return false; + } + if (!std::filesystem::is_regular_file(urdf_path)) { + setError(error, "URDF file does not exist: " + urdf_path); + return false; + } + if (active_joint_names.empty()) { + setError(error, "Active joint list is empty"); + return false; + } + + try { + pinocchio::urdf::buildModel(urdf_path, model_); + pinocchio::urdf::buildGeom( + model_, urdf_path, pinocchio::COLLISION, geometry_model_); + } catch (const std::exception& exception) { + setError(error, "Failed to load collision URDF: " + std::string(exception.what())); + reset(); + return false; + } + + if (geometry_model_.ngeoms == 0) { + setError(error, "URDF contains no collision geometry: " + urdf_path); + reset(); + return false; + } + + std::unordered_set active_joint_ids; + std::unordered_set unique_joint_names; + joint_names_.reserve(active_joint_names.size()); + joint_q_indices_.reserve(active_joint_names.size()); + for (const auto& joint_name : active_joint_names) { + if (joint_name.empty() || !unique_joint_names.insert(joint_name).second) { + setError(error, "Active joint names must be non-empty and unique"); + reset(); + return false; + } + if (!model_.existJointName(joint_name)) { + setError(error, "Joint not found in URDF: " + joint_name); + reset(); + return false; + } + const pinocchio::JointIndex joint_id = model_.getJointId(joint_name); + const auto& joint = model_.joints[joint_id]; + if (joint.nq() != 1) { + setError(error, "Only one-DoF active joints are supported: " + joint_name); + reset(); + return false; + } + active_joint_ids.insert(joint_id); + joint_names_.push_back(joint_name); + joint_q_indices_.push_back(joint.idx_q()); + } + + geometry_link_names_.resize(geometry_model_.ngeoms); + std::unordered_set selected_link_names; + for (pinocchio::GeomIndex geometry_id = 0; + geometry_id < geometry_model_.ngeoms; + ++geometry_id) { + auto& geometry = geometry_model_.geometryObjects[geometry_id]; + const std::string link_name = geometry.parentFrame < model_.frames.size() + ? model_.frames[geometry.parentFrame].name + : geometry.name; + geometry_link_names_[geometry_id] = link_name; + + const bool is_static = geometry.parentJoint == 0; + const bool belongs_to_active_arm = active_joint_ids.count(geometry.parentJoint) != 0; + if (!is_static && !belongs_to_active_arm) { + continue; + } + + if (!geometry.geometry) { + setError(error, "Collision geometry is null for link: " + link_name); + reset(); + return false; + } + geometry.geometry->computeLocalAABB(); + selected_geometry_indices_.push_back(geometry_id); + selected_link_names.insert(link_name); + } + + if (selected_geometry_indices_.size() < 2) { + setError(error, "Fewer than two collision geometries remain after arm filtering"); + reset(); + return false; + } + + std::set ignored_pairs; + for (const auto& pair : options.ignored_pairs) { + if (pair.first.empty() || pair.second.empty() || pair.first == pair.second) { + setError(error, "Ignored collision pairs require two different non-empty links"); + reset(); + return false; + } + if (!selected_link_names.count(pair.first) || !selected_link_names.count(pair.second)) { + setError(error, + "Ignored collision pair references an inactive or unknown link: " + + pair.first + ", " + pair.second); + reset(); + return false; + } + ignored_pairs.insert(canonicalPair(pair.first, pair.second)); + } + + geometry_model_.removeAllCollisionPairs(); + for (std::size_t first_index = 0; + first_index < selected_geometry_indices_.size(); + ++first_index) { + const auto first_geometry_id = selected_geometry_indices_[first_index]; + const auto& first_geometry = geometry_model_.geometryObjects[first_geometry_id]; + for (std::size_t second_index = first_index + 1; + second_index < selected_geometry_indices_.size(); + ++second_index) { + const auto second_geometry_id = selected_geometry_indices_[second_index]; + const auto& second_geometry = geometry_model_.geometryObjects[second_geometry_id]; + + if (first_geometry.parentJoint == second_geometry.parentJoint) { + continue; + } + if (model_.parents[first_geometry.parentJoint] == second_geometry.parentJoint || + model_.parents[second_geometry.parentJoint] == first_geometry.parentJoint) { + continue; + } + + const auto link_pair = canonicalPair( + geometry_link_names_[first_geometry_id], + geometry_link_names_[second_geometry_id]); + if (ignored_pairs.count(link_pair)) { + continue; + } + geometry_model_.addCollisionPair( + pinocchio::CollisionPair(first_geometry_id, second_geometry_id)); + } + } + + if (geometry_model_.collisionPairs.empty()) { + setError(error, "No active collision pairs remain after filtering"); + reset(); + return false; + } + + data_ = std::make_unique(model_); + geometry_data_ = std::make_unique(geometry_model_); + for (auto& request : geometry_data_->distanceRequests) { + request.enable_signed_distance = true; + } + neutral_q_ = pinocchio::neutral(model_); + initialized_ = true; + if (error) { + error->clear(); + } + return true; + } + + bool makeSnapshot(const std::vector& joint_positions, + CollisionGeometrySnapshot* snapshot, + std::string* error) + { + if (!initialized_) { + setError(error, "SelfCollisionChecker is not initialized"); + return false; + } + if (!snapshot) { + setError(error, "Collision snapshot output is null"); + return false; + } + if (joint_positions.size() != joint_names_.size()) { + std::ostringstream stream; + stream << "Joint position size mismatch: expected " << joint_names_.size() + << ", got " << joint_positions.size(); + setError(error, stream.str()); + return false; + } + + Eigen::VectorXd q = neutral_q_; + for (std::size_t index = 0; index < joint_positions.size(); ++index) { + if (!std::isfinite(joint_positions[index])) { + setError(error, "Joint position contains a non-finite value: " + joint_names_[index]); + return false; + } + q[joint_q_indices_[index]] = joint_positions[index]; + } + + try { + pinocchio::updateGeometryPlacements( + model_, *data_, geometry_model_, *geometry_data_, q); + } catch (const std::exception& exception) { + setError(error, "Failed to update collision geometry: " + std::string(exception.what())); + return false; + } + + snapshot->objects.clear(); + snapshot->objects.reserve(selected_geometry_indices_.size()); + for (const auto geometry_id : selected_geometry_indices_) { + const auto& placement = geometry_data_->oMg[geometry_id]; + const auto& geometry = geometry_model_.geometryObjects[geometry_id]; + CollisionObjectPose pose; + pose.geometry_index = geometry_id; + pose.position = placement.translation(); + pose.orientation = Eigen::Quaterniond(placement.rotation()).normalized(); + pose.bounding_radius_m = std::max(0.0, geometry.geometry->aabb_radius); + snapshot->objects.push_back(std::move(pose)); + } + if (error) { + error->clear(); + } + return true; + } + + SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot) + { + SelfCollisionResult result; + if (!initialized_) { + result.error = "SelfCollisionChecker is not initialized"; + return result; + } + if (snapshot.objects.size() != selected_geometry_indices_.size()) { + result.error = "Collision snapshot size does not match initialized geometry"; + return result; + } + + for (std::size_t index = 0; index < snapshot.objects.size(); ++index) { + const auto& pose = snapshot.objects[index]; + if (pose.geometry_index != selected_geometry_indices_[index] || + pose.geometry_index >= geometry_data_->oMg.size()) { + result.error = "Collision snapshot geometry order is invalid"; + return result; + } + if (!pose.position.allFinite() || !pose.orientation.coeffs().allFinite() || + pose.orientation.norm() <= std::numeric_limits::epsilon()) { + result.error = "Collision snapshot contains an invalid pose"; + return result; + } + geometry_data_->oMg[pose.geometry_index] = pinocchio::SE3( + pose.orientation.normalized().toRotationMatrix(), pose.position); + } + + try { + const std::size_t pair_index = + pinocchio::computeDistances(geometry_model_, *geometry_data_); + if (pair_index >= geometry_model_.collisionPairs.size()) { + result.error = "Collision distance computation returned no active pair"; + return result; + } + const auto& pair = geometry_model_.collisionPairs[pair_index]; + result.minimum_distance_m = geometry_data_->distanceResults[pair_index].min_distance; + result.first = geometry_link_names_[pair.first]; + result.second = geometry_link_names_[pair.second]; + result.in_collision = result.minimum_distance_m <= 0.0; + result.valid = std::isfinite(result.minimum_distance_m); + if (!result.valid) { + result.error = "Collision distance is not finite"; + } + } catch (const std::exception& exception) { + result.error = "Collision distance computation failed: " + std::string(exception.what()); + } + return result; + } + + SelfCollisionResult check(const std::vector& joint_positions) + { + CollisionGeometrySnapshot snapshot; + std::string error; + if (!makeSnapshot(joint_positions, &snapshot, &error)) { + SelfCollisionResult result; + result.error = std::move(error); + return result; + } + return check(snapshot); + } + + void reset() + { + initialized_ = false; + joint_names_.clear(); + joint_q_indices_.clear(); + selected_geometry_indices_.clear(); + geometry_link_names_.clear(); + geometry_data_.reset(); + data_.reset(); + model_ = pinocchio::Model{}; + geometry_model_ = pinocchio::GeometryModel{}; + neutral_q_.resize(0); + } + + bool initialized_{false}; + std::vector joint_names_; + std::vector joint_q_indices_; + std::vector selected_geometry_indices_; + std::vector geometry_link_names_; + pinocchio::Model model_; + pinocchio::GeometryModel geometry_model_; + std::unique_ptr data_; + std::unique_ptr geometry_data_; + Eigen::VectorXd neutral_q_; +}; + +SelfCollisionChecker::SelfCollisionChecker() + : impl_(std::make_unique()) +{ +} + +SelfCollisionChecker::~SelfCollisionChecker() = default; +SelfCollisionChecker::SelfCollisionChecker(SelfCollisionChecker&&) noexcept = default; +SelfCollisionChecker& SelfCollisionChecker::operator=(SelfCollisionChecker&&) noexcept = default; + +bool SelfCollisionChecker::init(const std::string& urdf_path, + const std::vector& active_joint_names, + const SelfCollisionOptions& options, + std::string* error) +{ + return impl_->init(urdf_path, active_joint_names, options, error); +} + +bool SelfCollisionChecker::makeSnapshot(const std::vector& joint_positions, + CollisionGeometrySnapshot* snapshot, + std::string* error) +{ + return impl_->makeSnapshot(joint_positions, snapshot, error); +} + +SelfCollisionResult SelfCollisionChecker::check(const CollisionGeometrySnapshot& snapshot) +{ + return impl_->check(snapshot); +} + +SelfCollisionResult SelfCollisionChecker::check(const std::vector& joint_positions) +{ + return impl_->check(joint_positions); +} + +bool SelfCollisionChecker::initialized() const +{ + return impl_->initialized_; +} + +std::size_t SelfCollisionChecker::dof() const +{ + return impl_->joint_names_.size(); +} + +std::size_t SelfCollisionChecker::activePairCount() const +{ + return impl_->geometry_model_.collisionPairs.size(); +} + +const std::vector& SelfCollisionChecker::jointNames() const +{ + return impl_->joint_names_; +} + +} // namespace cmvr diff --git a/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp b/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp new file mode 100644 index 00000000..5c5d75b4 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp @@ -0,0 +1,236 @@ +#include +#include +#include + +#include + +#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h" +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +namespace cmvr { +namespace { + +const std::vector kRightArmJoints{ + "R_SHOULDER_P", + "R_SHOULDER_R", + "R_SHOULDER_Y", + "R_ELBOW_R", + "R_WRIST_P", + "R_WRIST_Y", + "R_WRIST_R", +}; + +const std::vector kGen2RightArmJoints{ + "right_arm_J1", + "right_arm_J2", + "right_arm_J3", + "right_arm_J4", + "right_arm_J5", + "right_arm_J6", + "right_arm_J7", +}; + +const std::vector kGen2SetupPose{ + 0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0, +}; + +const std::vector kGen2WarningPose{ + 2.45028525340088, + 0.413065394330014, + -1.78610031118294, + 2.3232081721811, + -2.96828882895788, + -1.59350098130002, + 0.582912411114367, +}; + +const std::vector kGen2StopPose{ + 2.13758633436379, + 1.61835160165575, + -2.3836142221041, + 0.964538527544213, + -0.00382525077004825, + 1.74586899135531, + -0.336868659266887, +}; + +const std::vector kGen2CollisionPose{ + -0.42656969579233, + 1.41426471041774, + -2.67949400419915, + 2.45814854129954, + -2.35907388079205, + 1.14125209449898, + 1.53232912981414, +}; + +const std::vector kGen2TorsoCollisionPose{ + 1.57607137794121, + 2.06613762981425, + -1.76915077905899, + 0.959251437141443, + -0.725973209527894, + 1.79390262120717, + 0.2223354372144, +}; + +std::string collisionUrdfPath() +{ + return std::string(CMVR_ES_SOURCE_DIR) + + "/model/xiaoyan_description/dual_arm_collision.urdf"; +} + +std::string gen2CollisionUrdfPath() +{ + return std::string(CMVR_ES_SOURCE_DIR) + + "/model/gen2/collision/robot_collision.urdf"; +} + +SelfCollisionOptions gen2CollisionOptions() +{ + SelfCollisionOptions options; + options.ignored_pairs.push_back({"arm_link_5_2", "arm_link_7_2"}); + options.ignored_pairs.push_back({"body_link", "arm_link_2_2"}); + return options; +} + +CollisionGeometrySnapshot singleObjectSnapshot(double x, + double angle, + double radius) +{ + CollisionGeometrySnapshot snapshot; + CollisionObjectPose pose; + pose.geometry_index = 1; + pose.position = Eigen::Vector3d(x, 0.0, 0.0); + pose.orientation = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()); + pose.bounding_radius_m = radius; + snapshot.objects.push_back(pose); + return snapshot; +} + +TEST(SelfCollisionCheckerTest, LoadsRightArmFromDualArmUrdf) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error; + EXPECT_EQ(checker.dof(), 7U); + EXPECT_GT(checker.activePairCount(), 0U); + + CollisionGeometrySnapshot snapshot; + ASSERT_TRUE(checker.makeSnapshot(std::vector(7, 0.0), &snapshot, &error)) << error; + EXPECT_EQ(snapshot.objects.size(), 11U); + + const SelfCollisionResult result = checker.check(snapshot); + ASSERT_TRUE(result.valid) << result.error; + EXPECT_TRUE(result.first.rfind("L_", 0) != 0); + EXPECT_TRUE(result.second.rfind("L_", 0) != 0); +} + +TEST(SelfCollisionCheckerTest, RejectsWrongJointVectorSize) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error; + + CollisionGeometrySnapshot snapshot; + EXPECT_FALSE(checker.makeSnapshot(std::vector(6, 0.0), &snapshot, &error)); + EXPECT_NE(error.find("size mismatch"), std::string::npos); +} + +TEST(SelfCollisionCheckerTest, RemovesConfiguredIgnoredPair) +{ + SelfCollisionChecker baseline; + SelfCollisionChecker filtered; + std::string error; + ASSERT_TRUE(baseline.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error; + + SelfCollisionOptions options; + options.ignored_pairs.push_back({"base_link", "R_ELBOW_R_S"}); + ASSERT_TRUE(filtered.init(collisionUrdfPath(), kRightArmJoints, options, &error)) << error; + EXPECT_EQ(filtered.activePairCount() + 1, baseline.activePairCount()); +} + +TEST(SelfCollisionCheckerTest, LoadsGen2RightArmCollisionModel) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init( + gen2CollisionUrdfPath(), + kGen2RightArmJoints, + gen2CollisionOptions(), + &error)) << error; + EXPECT_EQ(checker.dof(), 7U); + EXPECT_EQ(checker.activePairCount(), 19U); + + CollisionGeometrySnapshot snapshot; + ASSERT_TRUE(checker.makeSnapshot(kGen2SetupPose, &snapshot, &error)) << error; + EXPECT_EQ(snapshot.objects.size(), 8U); + + const SelfCollisionResult setup_result = checker.check(snapshot); + ASSERT_TRUE(setup_result.valid) << setup_result.error; + EXPECT_FALSE(setup_result.in_collision); + EXPECT_GT(setup_result.minimum_distance_m, 0.02); +} + +TEST(SelfCollisionCheckerTest, ClassifiesGen2SafetyDistances) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init( + gen2CollisionUrdfPath(), + kGen2RightArmJoints, + gen2CollisionOptions(), + &error)) << error; + + const SelfCollisionResult warning_result = checker.check(kGen2WarningPose); + ASSERT_TRUE(warning_result.valid) << warning_result.error; + EXPECT_FALSE(warning_result.in_collision); + EXPECT_GT(warning_result.minimum_distance_m, 0.005); + EXPECT_LE(warning_result.minimum_distance_m, 0.02); + + const SelfCollisionResult stop_result = checker.check(kGen2StopPose); + ASSERT_TRUE(stop_result.valid) << stop_result.error; + EXPECT_FALSE(stop_result.in_collision); + EXPECT_GT(stop_result.minimum_distance_m, 0.0); + EXPECT_LE(stop_result.minimum_distance_m, 0.005); + + const SelfCollisionResult collision_result = checker.check(kGen2CollisionPose); + ASSERT_TRUE(collision_result.valid) << collision_result.error; + EXPECT_TRUE(collision_result.in_collision); + EXPECT_LE(collision_result.minimum_distance_m, 0.0); + + const SelfCollisionResult torso_result = + checker.check(kGen2TorsoCollisionPose); + ASSERT_TRUE(torso_result.valid) << torso_result.error; + EXPECT_TRUE(torso_result.in_collision); + EXPECT_LE(torso_result.minimum_distance_m, 0.0); + EXPECT_TRUE(torso_result.first == "body_link" || + torso_result.second == "body_link"); +} + +TEST(DistanceSamplingPolicyTest, SamplesByAccumulatedGeometryDisplacement) +{ + DistanceSamplingPolicy policy; + DistanceSamplingOptions options; + options.max_geometry_displacement_m = 0.002; + options.max_check_period_s = 0.01; + std::string error; + ASSERT_TRUE(policy.configure(options, &error)) << error; + + const auto start = DistanceSamplingPolicy::Clock::now(); + const auto initial = singleObjectSnapshot(0.0, 0.0, 0.2); + EXPECT_TRUE(policy.shouldCheck(initial, start)); + policy.markChecked(initial, start); + + EXPECT_FALSE(policy.shouldCheck( + singleObjectSnapshot(0.001, 0.0, 0.2), start + std::chrono::milliseconds(1))); + EXPECT_TRUE(policy.shouldCheck( + singleObjectSnapshot(0.0021, 0.0, 0.2), start + std::chrono::milliseconds(2))); + EXPECT_TRUE(policy.shouldCheck( + singleObjectSnapshot(0.0, 0.011, 0.2), start + std::chrono::milliseconds(2))); + EXPECT_TRUE(policy.shouldCheck( + initial, start + std::chrono::milliseconds(10))); +} + +} // namespace +} // namespace cmvr diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt index eb26409f..33026787 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt @@ -14,3 +14,14 @@ target_link_libraries(arm_motion add_library(cmvr_es::arm_motion ALIAS arm_motion) add_library(cmvr_es::algorithms::arm_motion ALIAS arm_motion) install(TARGETS arm_motion LIBRARY DESTINATION lib) + +add_executable(toppra_joint_motion_planner_test + joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp +) + +target_link_libraries(toppra_joint_motion_planner_test + PRIVATE + cmvr_es::algorithms::arm_motion + gtest + gtest_main +) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h index ea500ea4..a2f77c51 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h @@ -1,18 +1,15 @@ #ifndef CMVR_ES_JOINT_MOTION_PLANNER_H #define CMVR_ES_JOINT_MOTION_PLANNER_H +#include +#include #include +#include "common/base/logging/logger.h" #include "common/types/arm/arm_types.h" namespace cmvr::device { -struct JointTrajectorySample { - double t{0.0}; - std::vector position; - std::vector velocity; -}; - class JointMotionPlanner { public: virtual ~JointMotionPlanner() = default; @@ -23,9 +20,137 @@ public: const JointPositionCommand& target, const MotionOptions& options, double speed_scaling, - std::vector& samples) = 0; + JointTrajectory& trajectory) = 0; + + virtual bool planReplay(const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + JointTrajectory& replay_trajectory) = 0; + + bool validateJointTrajectory(const JointTrajectory& trajectory, + std::size_t expected_dof, + const MotionOptions& limits) const; }; +inline bool JointMotionPlanner::validateJointTrajectory( + const JointTrajectory& trajectory, + const std::size_t expected_dof, + const MotionOptions& limits) const +{ + if (trajectory.size() < 2 || expected_dof == 0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory must contain at least " + "two points and have a non-zero DOF"; + return false; + } + if (!std::isfinite(limits.velocity) || limits.velocity <= 0.0 || + !std::isfinite(limits.acceleration) || limits.acceleration <= 0.0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity or acceleration limit is invalid"; + return false; + } + if (!limits.joint_velocity_limits.empty() && + limits.joint_velocity_limits.size() != expected_dof) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] joint velocity limit count does not match DOF"; + return false; + } + + constexpr double kVelocityTolerance = 1e-6; + constexpr double kAccelerationTolerance = 1e-3; + double maximum_velocity = 0.0; + double maximum_acceleration = 0.0; + double maximum_position_velocity = 0.0; + double maximum_position_acceleration = 0.0; + double maximum_jerk = 0.0; + std::vector previous_position_velocity(expected_dof, 0.0); + std::vector previous_acceleration(expected_dof, 0.0); + + for (std::size_t i = 0; i < trajectory.size(); ++i) { + const auto& point = trajectory[i]; + if (!std::isfinite(point.time_s) || + point.position.size() != expected_dof || + point.velocity.size() != expected_dof) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid trajectory point at index=" << i; + return false; + } + + double dt = 0.0; + if (i > 0) { + dt = point.time_s - trajectory[i - 1].time_s; + if (!std::isfinite(dt) || dt <= 0.0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory time is not increasing at index=" + << i; + return false; + } + } + + for (std::size_t joint = 0; joint < expected_dof; ++joint) { + if (!std::isfinite(point.position[joint]) || + !std::isfinite(point.velocity[joint])) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] non-finite trajectory value at point=" + << i << ", joint=" << joint; + return false; + } + + const double velocity = std::abs(point.velocity[joint]); + const double velocity_limit = limits.joint_velocity_limits.empty() + ? limits.velocity + : limits.joint_velocity_limits[joint]; + if (!std::isfinite(velocity_limit) || velocity_limit <= 0.0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid velocity limit for joint=" + << joint; + return false; + } + maximum_velocity = std::max(maximum_velocity, velocity); + if (velocity > velocity_limit + kVelocityTolerance) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity limit exceeded at point=" + << i << ", joint=" << joint + << ", actual=" << velocity + << ", limit=" << velocity_limit; + return false; + } + + if (i > 0) { + const double position_velocity = + (point.position[joint] - trajectory[i - 1].position[joint]) / dt; + const double acceleration = + (point.velocity[joint] - trajectory[i - 1].velocity[joint]) / dt; + maximum_position_velocity = std::max( + maximum_position_velocity, std::abs(position_velocity)); + maximum_acceleration = std::max( + maximum_acceleration, std::abs(acceleration)); + if (std::abs(acceleration) > + limits.acceleration + kAccelerationTolerance) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] acceleration limit exceeded at point=" + << i << ", joint=" << joint + << ", actual=" << std::abs(acceleration) + << ", limit=" << limits.acceleration; + return false; + } + if (i > 1) { + maximum_position_acceleration = std::max( + maximum_position_acceleration, + std::abs(position_velocity - + previous_position_velocity[joint]) / dt); + maximum_jerk = std::max( + maximum_jerk, + std::abs(acceleration - previous_acceleration[joint]) / dt); + } + previous_position_velocity[joint] = position_velocity; + previous_acceleration[joint] = acceleration; + } + } + } + + CMVR_LOG(INFO) << "[JointMotionPlanner] trajectory validated" + << ", points=" << trajectory.size() + << ", max_velocity_rad_s=" << maximum_velocity + << ", max_discrete_acceleration_rad_s2=" << maximum_acceleration + << ", max_position_velocity_rad_s=" << maximum_position_velocity + << ", max_position_acceleration_rad_s2=" + << maximum_position_acceleration + << ", max_discrete_jerk_rad_s3=" << maximum_jerk; + return true; +} + } // namespace cmvr::device #endif // CMVR_ES_JOINT_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h index 8c6db577..7aadd085 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h @@ -21,9 +21,19 @@ public: const JointPositionCommand& target, const MotionOptions& options, double speed_scaling, - std::vector& samples) override; + JointTrajectory& trajectory) override; + + bool planReplay(const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + JointTrajectory& replay_trajectory) override; private: + bool sampleTrajectory_( + const std::shared_ptr& planner, + const cmvr::TrajPtr& raw_trajectory, + JointTrajectory& trajectory) const; + std::shared_ptr planner_; cmvr::PathType path_type_{cmvr::PathType::Quintic}; double sample_period_s_{0.001}; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp index 8ee1e8e2..1fe882fa 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp @@ -1,6 +1,9 @@ #include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h" +#include + #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" +#include "common/base/logging/logger.h" namespace cmvr::device { @@ -39,36 +42,215 @@ bool ToppraJointMotionPlanner::init() return true; } +bool ToppraJointMotionPlanner::sampleTrajectory_( + const std::shared_ptr& planner, + const cmvr::TrajPtr& raw_trajectory, + JointTrajectory& trajectory) const +{ + const auto raw_samples = planner->sampleTrajectory( + raw_trajectory, sample_period_s_); + if (raw_samples.size() < 2) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] trajectory sampling returned fewer than " + "two points: count=" + << raw_samples.size(); + return false; + } + trajectory.clear(); + trajectory.reserve(raw_samples.size()); + for (std::size_t i = 0; i < raw_samples.size(); ++i) { + const auto& sample = raw_samples[i]; + if (!std::isfinite(sample.t) || !sample.q.allFinite() || + !sample.qd.allFinite()) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] sampled trajectory contains " + "a non-finite value at point=" + << i; + trajectory.clear(); + return false; + } + JointTrajectoryPoint point; + point.time_s = sample.t; + point.position = toStdVector(sample.q); + point.velocity = toStdVector(sample.qd); + trajectory.push_back(std::move(point)); + } + return true; +} + bool ToppraJointMotionPlanner::planMoveJ(const std::vector& start, const JointPositionCommand& target, const MotionOptions& options, const double speed_scaling, - std::vector& samples) + JointTrajectory& trajectory) { - samples.clear(); + trajectory.clear(); if (!planner_ || start.empty() || start.size() != target.position.size() || options.velocity <= 0.0 || options.acceleration <= 0.0) { return false; } - cmvr::TrajPtr trajectory; + cmvr::TrajPtr raw_trajectory; planner_->setPathType(path_type_); planner_->setGridSizes(grid_size_, high_grid_size_); planner_->setSymmetricLimits( std::vector(start.size(), options.velocity * speed_scaling), std::vector(start.size(), options.acceleration)); - if (!planner_->plan(start, target.position, trajectory)) { + if (!planner_->plan(start, target.position, raw_trajectory)) { return false; } - const auto raw_samples = planner_->sampleTrajectory(trajectory, sample_period_s_); - samples.reserve(raw_samples.size()); - for (const auto& sample : raw_samples) { - JointTrajectorySample dst; - dst.t = sample.t; - dst.position = toStdVector(sample.q); - dst.velocity = toStdVector(sample.qd); - samples.push_back(std::move(dst)); + return sampleTrajectory_(planner_, raw_trajectory, trajectory); +} + +bool ToppraJointMotionPlanner::planReplay( + const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + JointTrajectory& replay_trajectory) +{ + replay_trajectory.clear(); + if (current_position.empty() || recorded_trajectory.size() < 2 || + options.velocity <= 0.0 || + options.acceleration <= 0.0 || !std::isfinite(options.velocity) || + !std::isfinite(options.acceleration)) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid trajectory or options"; + return false; + } + + const std::size_t dof = current_position.size(); + if (!options.joint_velocity_limits.empty() && + options.joint_velocity_limits.size() != dof) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid DOF or joint limits"; + return false; + } + for (const double position : current_position) { + if (!std::isfinite(position)) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite current position"; + return false; + } + } + for (std::size_t i = 0; i < recorded_trajectory.size(); ++i) { + const auto& point = recorded_trajectory[i]; + if (!std::isfinite(point.time_s) || point.position.size() != dof || + (i > 0 && point.time_s <= recorded_trajectory[i - 1].time_s)) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid recorded point: " + << i; + return false; + } + for (std::size_t joint = 0; joint < dof; ++joint) { + if (!std::isfinite(point.position[joint])) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite recorded point: " + << i; + return false; + } + } + } + + std::vector velocity_limits = options.joint_velocity_limits; + if (velocity_limits.empty()) { + velocity_limits.assign(dof, options.velocity); + } + for (std::size_t joint = 0; joint < velocity_limits.size(); ++joint) { + const double limit = velocity_limits[joint]; + if (!std::isfinite(limit) || limit <= 0.0) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid joint velocity limit"; + return false; + } + } + + const double ramp_duration_s = std::max( + sample_period_s_, options.velocity / options.acceleration); + replay_trajectory.reserve(recorded_trajectory.size() + 2); + replay_trajectory.push_back(JointTrajectoryPoint{ + 0.0, current_position, std::vector(dof, 0.0)}); + + double replay_time_s = ramp_duration_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory.back().position, + std::vector(dof, 0.0)}); + for (std::size_t i = recorded_trajectory.size() - 1; i > 0; --i) { + replay_time_s += recorded_trajectory[i].time_s - + recorded_trajectory[i - 1].time_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory[i - 1].position, + std::vector(dof, 0.0)}); + } + replay_time_s += ramp_duration_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory.front().position, + std::vector(dof, 0.0)}); + + const auto update_velocities = [&] { + for (auto& point : replay_trajectory) { + std::fill(point.velocity.begin(), point.velocity.end(), 0.0); + } + for (std::size_t i = 1; i + 1 < replay_trajectory.size(); ++i) { + const double dt = replay_trajectory[i + 1].time_s - + replay_trajectory[i - 1].time_s; + for (std::size_t joint = 0; joint < dof; ++joint) { + replay_trajectory[i].velocity[joint] = + (replay_trajectory[i + 1].position[joint] - + replay_trajectory[i - 1].position[joint]) / dt; + } + } + }; + + for (int iteration = 0; iteration < 3; ++iteration) { + update_velocities(); + double required_scale = 1.0; + std::vector previous_position_velocity(dof, 0.0); + for (std::size_t i = 0; i < replay_trajectory.size(); ++i) { + const auto& point = replay_trajectory[i]; + for (std::size_t joint = 0; joint < dof; ++joint) { + required_scale = std::max( + required_scale, + std::abs(point.velocity[joint]) / velocity_limits[joint]); + if (i == 0) { + continue; + } + + const double dt = point.time_s - + replay_trajectory[i - 1].time_s; + const double position_velocity = + (point.position[joint] - + replay_trajectory[i - 1].position[joint]) / dt; + const double acceleration = + (point.velocity[joint] - + replay_trajectory[i - 1].velocity[joint]) / dt; + required_scale = std::max( + required_scale, + std::abs(position_velocity) / velocity_limits[joint]); + required_scale = std::max( + required_scale, + std::sqrt(std::abs(acceleration) / + options.acceleration)); + if (i > 1) { + const double position_acceleration = + (position_velocity - + previous_position_velocity[joint]) / dt; + required_scale = std::max( + required_scale, + std::sqrt(std::abs(position_acceleration) / + options.acceleration)); + } + previous_position_velocity[joint] = position_velocity; + } + } + + if (required_scale <= 1.0 + 1e-9) { + break; + } + required_scale *= 1.001; + for (auto& point : replay_trajectory) { + point.time_s *= required_scale; + } + } + update_velocities(); + if (!validateJointTrajectory(replay_trajectory, dof, options)) { + replay_trajectory.clear(); + return false; } return true; } diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp new file mode 100644 index 00000000..40ad8414 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp @@ -0,0 +1,139 @@ +#include +#include +#include +#include +#include + +#include + +#include "joint_motion/toppra/include/toppra_joint_motion_planner.h" + +namespace cmvr::device { +namespace { + +constexpr std::size_t kDof = 7; + +JointTrajectory makeRecordedTrajectory(const std::size_t point_count) +{ + JointTrajectory trajectory; + trajectory.reserve(point_count); + for (std::size_t i = 0; i < point_count; ++i) { + const double s = static_cast(i) / + static_cast(point_count - 1); + JointTrajectoryPoint point; + point.time_s = static_cast(i) * 0.002; + point.position = { + 0.40 * s, + -0.25 * s + 0.03 * std::sin(3.141592653589793 * s), + 0.20 * s * s, + 0.30 * std::sin(1.5707963267948966 * s), + -0.12 * s, + 0.15 * s, + -0.08 * std::sin(3.141592653589793 * s), + }; + point.velocity.assign(kDof, 0.0); + trajectory.push_back(std::move(point)); + } + return trajectory; +} + +double maximumPositionError(const std::vector& lhs, + const std::vector& rhs) +{ + if (lhs.size() != rhs.size()) { + return std::numeric_limits::infinity(); + } + double maximum = 0.0; + for (std::size_t i = 0; i < lhs.size(); ++i) { + maximum = std::max(maximum, std::abs(lhs[i] - rhs[i])); + } + return maximum; +} + +TEST(ToppraJointMotionPlannerTest, PlansBoundedReverseReplay) +{ + ToppraJointMotionPlanner planner( + cmvr::PathType::Quintic, 0.001, 150, 300); + ASSERT_TRUE(planner.init()); + + const JointTrajectory recorded = makeRecordedTrajectory(300); + MotionOptions options; + options.velocity = 0.15; + options.acceleration = 5.0; + + JointTrajectory replay; + ASSERT_TRUE(planner.planReplay( + recorded.back().position, recorded, options, replay)); + ASSERT_EQ(replay.size(), recorded.size() + 2); + EXPECT_LT(maximumPositionError( + replay.front().position, recorded.back().position), + 1e-9); + EXPECT_LT(maximumPositionError( + replay.back().position, recorded.front().position), + 1e-9); + for (std::size_t i = 0; i < recorded.size(); ++i) { + EXPECT_LT(maximumPositionError( + replay[i + 1].position, + recorded[recorded.size() - 1 - i].position), + 1e-9); + } + + double maximum_velocity = 0.0; + double maximum_acceleration = 0.0; + for (std::size_t i = 0; i < replay.size(); ++i) { + ASSERT_EQ(replay[i].position.size(), kDof); + ASSERT_EQ(replay[i].velocity.size(), kDof); + for (std::size_t joint = 0; joint < kDof; ++joint) { + maximum_velocity = std::max( + maximum_velocity, std::abs(replay[i].velocity[joint])); + if (i > 0) { + const double dt = replay[i].time_s - replay[i - 1].time_s; + ASSERT_GT(dt, 0.0); + maximum_acceleration = std::max( + maximum_acceleration, + std::abs(replay[i].velocity[joint] - + replay[i - 1].velocity[joint]) / dt); + } + } + } + EXPECT_LE(maximum_velocity, options.velocity + 1e-6); + EXPECT_LE(maximum_acceleration, options.acceleration + 1e-3); +} + +TEST(ToppraJointMotionPlannerTest, RejectsNonIncreasingRecordedTime) +{ + ToppraJointMotionPlanner planner( + cmvr::PathType::Quintic, 0.001, 150, 300); + ASSERT_TRUE(planner.init()); + + JointTrajectory recorded = makeRecordedTrajectory(10); + recorded[5].time_s = recorded[4].time_s; + MotionOptions options; + options.velocity = 0.15; + options.acceleration = 5.0; + + JointTrajectory replay; + EXPECT_FALSE(planner.planReplay( + recorded.back().position, recorded, options, replay)); + EXPECT_TRUE(replay.empty()); +} + +TEST(ToppraJointMotionPlannerTest, ValidationRejectsInvalidOutputTrajectory) +{ + ToppraJointMotionPlanner planner( + cmvr::PathType::Quintic, 0.001, 150, 300); + MotionOptions options; + options.velocity = 0.15; + options.acceleration = 5.0; + + JointTrajectory trajectory = makeRecordedTrajectory(10); + trajectory[5].velocity[2] = options.velocity + 0.01; + EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options)); + + trajectory[5].velocity[2] = 0.0; + trajectory[5].time_s = trajectory[4].time_s; + EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options)); +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt index bd84d4b9..62fb599c 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -20,4 +20,15 @@ target_link_libraries(base_motion PUBLIC ) add_library(cmvr_es::base_motion ALIAS base_motion) -install(TARGETS base_motion LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS base_motion LIBRARY DESTINATION lib) + +add_executable(toppra_multi_waypoint_test + joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp +) + +target_link_libraries(toppra_multi_waypoint_test + PRIVATE + cmvr_es::base_motion + gtest + gtest_main +) diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h index 39865020..f344fc1e 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h @@ -109,37 +109,18 @@ namespace cmvr { static void sanitizeVsq(toppra::Vector &v); - // centripetal 弦长（alpha=0.5），生成严格递增 S + // Joint-space chord length keeps the parameterization independent of + // how densely the same geometric path is sampled. static std::vector - makeS_centripetal(const std::vector &q) { + makeSChordLength(const std::vector &q) { const size_t M = q.size(); std::vector S(M, 0.0); - auto chord = [](const Eigen::VectorXd &a, const Eigen::VectorXd &b) { - double d = (a - b).norm(); - return std::pow(std::max(d, 1e-16), 0.5); - }; for (size_t i = 1; i < M; ++i) { - S[i] = S[i - 1] + chord(q[i], q[i - 1]); - if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12; + S[i] = S[i - 1] + (q[i] - q[i - 1]).norm(); } return S; } - // 等距参数（简单稳妥） - static inline std::vector makeS_equal(size_t M) { - std::vector S(M); - for (size_t i = 0; i < M; ++i) S[i] = static_cast(i); - return S; - } - - // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds - static inline void normalize_and_floor_S(std::vector &S, double ds_min = 0.2) { - for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0]; - double L = S.back(); - if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L; - for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min; - } - // Catmull–Rom（centripetal）估计结点几何速度 v（端点=0） static std::vector estimateVelsCatmull(const std::vector &q, @@ -159,14 +140,16 @@ namespace cmvr { // 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]） static void clampNodeVels(std::vector &v, const std::vector &q, + const std::vector &S, double k = 1.0) { const size_t M = q.size(); if (M <= 2) return; for (size_t i = 1; i + 1 < M; ++i) { - double d0 = (q[i] - q[i - 1]).norm(); - double d1 = (q[i + 1] - q[i]).norm(); - double d = std::max(std::min(d0, d1), 1e-12); - double vmax = k * d; + const double ds0 = std::max(S[i] - S[i - 1], 1e-12); + const double ds1 = std::max(S[i + 1] - S[i], 1e-12); + const double slope0 = (q[i] - q[i - 1]).norm() / ds0; + const double slope1 = (q[i + 1] - q[i]).norm() / ds1; + const double vmax = k * std::min(slope0, slope1); double n = v[i].norm(); if (n > vmax) v[i] *= (vmax / n); } diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp index e3637b8b..040b59e1 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp @@ -5,10 +5,117 @@ #include #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" +#include +#include #include #include namespace cmvr { + namespace { + + class TimeScaledTrajectory final : public ITrajectory { + public: + TimeScaledTrajectory(TrajPtr source, const double scale) + : source_(std::move(source)), scale_(scale), source_interval_(source_->timeInterval()) + { + } + + toppra::Bound timeInterval() const override + { + toppra::Bound interval; + interval << source_interval_[0], + source_interval_[0] + + (source_interval_[1] - source_interval_[0]) * scale_; + return interval; + } + + Eigen::VectorXd q(const double t) const override + { + return source_->q(sourceTime_(t)); + } + + Eigen::VectorXd qd(const double t) const override + { + return source_->qd(sourceTime_(t)) / scale_; + } + + Eigen::VectorXd qdd(const double t) const override + { + return source_->qdd(sourceTime_(t)) / (scale_ * scale_); + } + + private: + double sourceTime_(const double output_time) const + { + return std::clamp( + source_interval_[0] + + (output_time - source_interval_[0]) / scale_, + source_interval_[0], + source_interval_[1]); + } + + TrajPtr source_; + double scale_{1.0}; + toppra::Bound source_interval_; + }; + + bool enforceSampledLimits(const TrajPtr& source, + const std::vector& velocity_limits, + const std::vector& acceleration_limits, + const std::size_t waypoint_count, + TrajPtr& output) + { + if (!source || velocity_limits.empty() || + velocity_limits.size() != acceleration_limits.size()) { + return false; + } + const auto interval = source->timeInterval(); + const double duration = interval[1] - interval[0]; + if (!std::isfinite(duration) || duration <= 0.0) { + return false; + } + + const std::size_t time_samples = static_cast( + std::ceil(duration / 0.001)) + 1; + const std::size_t path_samples = waypoint_count * 20; + const std::size_t sample_count = std::clamp( + std::max({std::size_t{1000}, time_samples, path_samples}), + std::size_t{1000}, + std::size_t{200000}); + + double required_scale = 1.0; + for (std::size_t sample = 0; sample < sample_count; ++sample) { + const double ratio = static_cast(sample) / + static_cast(sample_count - 1); + const double time = interval[0] + duration * ratio; + const Eigen::VectorXd velocity = source->qd(time); + const Eigen::VectorXd acceleration = source->qdd(time); + if (!velocity.allFinite() || !acceleration.allFinite() || + velocity.size() != static_cast(velocity_limits.size()) || + acceleration.size() != + static_cast(acceleration_limits.size())) { + return false; + } + for (Eigen::Index joint = 0; joint < velocity.size(); ++joint) { + const std::size_t index = static_cast(joint); + required_scale = std::max( + required_scale, + std::abs(velocity[joint]) / velocity_limits[index]); + required_scale = std::max( + required_scale, + std::sqrt(std::abs(acceleration[joint]) / + acceleration_limits[index])); + } + } + + constexpr double kNumericalMargin = 1.001; + output = std::make_shared( + source, required_scale * kNumericalMargin); + return true; + } + + } // namespace + // ===== ConstAccelTraj ===== ConstAccelTraj::ConstAccelTraj(std::shared_ptr p) : impl_(std::move(p)) { @@ -54,22 +161,39 @@ namespace cmvr { bool ToppraJointTrajectoryPlanner::plan(const std::vector>& waypoints, TrajPtr& traj_out) { traj_out.reset(); - const size_t M = waypoints.size(); - if (M < 2) return false; + if (waypoints.size() < 2) return false; const size_t DoF = waypoints.front().size(); - for (const auto& w : waypoints) if (w.size()!=DoF) return false; + if (DoF == 0) return false; + for (const auto& w : waypoints) { + if (w.size() != DoF) return false; + for (const double value : w) { + if (!std::isfinite(value)) return false; + } + } if (!ensureLimitsSized(DoF)) return false; + for (size_t joint = 0; joint < DoF; ++joint) { + if (!std::isfinite(v_max_[joint]) || v_max_[joint] <= 0.0 || + !std::isfinite(a_max_[joint]) || a_max_[joint] <= 0.0) { + return false; + } + } - // 组装 - std::vector q; q.reserve(M); - for (const auto& w : waypoints) - q.emplace_back(Eigen::Map(w.data(), DoF)); + std::vector q; + q.reserve(waypoints.size()); + constexpr double kDuplicateDistance = 1e-10; + for (const auto& waypoint : waypoints) { + Eigen::VectorXd value = Eigen::Map( + waypoint.data(), static_cast(DoF)); + if (q.empty() || (value - q.back()).norm() > kDuplicateDistance) { + q.push_back(std::move(value)); + } + } + if (q.size() < 2) return false; + const size_t M = q.size(); - // 生成 S -// std::vector S = (M==2) ? std::vector{0.0,1.0} -// : makeS_centripetal(q); - std::vector S = (M==2) ? std::vector{0.0,1.0} - : makeS_equal(M); + const std::vector S = M == 2 + ? std::vector{0.0, 1.0} + : makeSChordLength(q); // 几何路径 auto path = buildPathUnified(q, S); @@ -86,8 +210,23 @@ namespace cmvr { // TOPPRA toppra::algorithm::TOPPRA algo{constraints, path}; - auto solve_once = [&](int N)->bool{ - algo.setN(N); + auto solve_once = [&](const int requested_intervals)->bool{ + const int segment_count = static_cast(M - 1); + const int subdivisions = std::max( + 1, (requested_intervals + segment_count - 1) / segment_count); + toppra::Vector grid(segment_count * subdivisions + 1); + Eigen::Index index = 0; + for (int segment = 0; segment < segment_count; ++segment) { + const double start = S[static_cast(segment)]; + const double length = S[static_cast(segment + 1)] - start; + for (int subdivision = 0; subdivision < subdivisions; ++subdivision) { + grid[index++] = start + length * + static_cast(subdivision) / + static_cast(subdivisions); + } + } + grid[index] = S.back(); + algo.setGridpoints(grid); algo.solver(std::make_shared()); return algo.computePathParametrization(0.0, 0.0) == toppra::ReturnCode::OK; }; @@ -99,19 +238,20 @@ namespace cmvr { toppra::Vector grid = data.gridpoints; toppra::Vector vsq = data.parametrization; + TrajPtr candidate; auto ca = std::make_shared(path, grid, vsq); if (ca->validate()) { - traj_out = std::make_shared(std::move(ca)); - return true; - } - sanitizeVsq(vsq); - try { - traj_out = std::make_shared(path, grid, vsq); - (void) traj_out->timeInterval(); - return true; - } catch (...) { - return false; + candidate = std::make_shared(std::move(ca)); + } else { + sanitizeVsq(vsq); + try { + candidate = std::make_shared(path, grid, vsq); + (void) candidate->timeInterval(); + } catch (...) { + return false; + } } + return enforceSampledLimits(candidate, v_max_, a_max_, M, traj_out); } @@ -225,7 +365,7 @@ namespace cmvr { double ds = std::max(S[k+1]-S[k], 1e-12); toppra::Matrix seg(2, DoF); Eigen::RowVectorXd A1 = ((q[k+1]-q[k])/ds).transpose(); - Eigen::RowVectorXd A0 = (q[k] - A1.transpose()*S[k]).transpose(); + Eigen::RowVectorXd A0 = q[k].transpose(); seg.row(0)=A1; seg.row(1)=A0; segs.emplace_back(std::move(seg)); } @@ -237,7 +377,7 @@ namespace cmvr { ToppraJointTrajectoryPlanner::buildCubicHermiteMulti(const std::vector& q, const std::vector& S) { auto v = estimateVelsCatmull(q, S); - clampNodeVels(v, q, /*k=*/1.0); + clampNodeVels(v, q, S, /*k=*/1.0); toppra::Vectors pos(q.begin(), q.end()); toppra::Vectors vel(v.begin(), v.end()); auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, S); @@ -282,7 +422,7 @@ namespace cmvr { const std::vector& S) { const size_t M = q.size(), DoF = q[0].size(); auto v = estimateVelsCatmull(q, S); - clampNodeVels(v, q, /*k=*/1.0); + clampNodeVels(v, q, S, /*k=*/1.0); auto a = estimateAccelsSecondDiff(q, S); toppra::Matrices segs; segs.reserve(M-1); @@ -301,11 +441,11 @@ namespace cmvr { Eigen::VectorXd C5 = ( 6.0*dq - (3.0*A1 + 0.5*(a0*ds*ds)) - (3.0*(v1*ds) - 0.5*(a1*ds*ds)) ); toppra::Matrix seg(6, DoF); - seg.row(0)=C5.transpose(); - seg.row(1)=C4.transpose(); - seg.row(2)=C3.transpose(); - seg.row(3)=A2.transpose(); - seg.row(4)=A1.transpose(); + seg.row(0)=(C5 / std::pow(ds, 5)).transpose(); + seg.row(1)=(C4 / std::pow(ds, 4)).transpose(); + seg.row(2)=(C3 / std::pow(ds, 3)).transpose(); + seg.row(3)=(a0 / 2.0).transpose(); + seg.row(4)=v0.transpose(); seg.row(5)=A0.transpose(); segs.emplace_back(std::move(seg)); } @@ -384,4 +524,4 @@ namespace cmvr { } return true; } -} // namespace cmvr \ No newline at end of file +} // namespace cmvr diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp new file mode 100644 index 00000000..87c6f3cb --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp @@ -0,0 +1,228 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" + +namespace cmvr { +namespace { + +constexpr std::size_t kDof = 7; +constexpr double kVelocityLimit = 0.15; +constexpr double kAccelerationLimit = 0.3; +constexpr double kSamplePeriodS = 0.002; + +std::vector> makeSmoothWaypoints(const std::size_t count) +{ + constexpr double kPi = 3.14159265358979323846; + std::vector> waypoints; + waypoints.reserve(count); + for (std::size_t i = 0; i < count; ++i) { + const double s = static_cast(i) / + static_cast(count - 1); + std::vector q(kDof, 0.0); + q[0] = 0.40 * s + 0.03 * std::sin(2.0 * kPi * s); + q[1] = -0.25 * s + 0.04 * std::sin(kPi * s); + q[2] = 0.20 * s * s; + q[3] = 0.30 * std::sin(0.5 * kPi * s); + q[4] = -0.12 * s + 0.02 * std::sin(3.0 * kPi * s); + q[5] = 0.15 * s; + q[6] = -0.08 * std::sin(kPi * s); + waypoints.push_back(std::move(q)); + } + return waypoints; +} + +double maxAbs(const Eigen::VectorXd& value) +{ + double result = 0.0; + for (Eigen::Index i = 0; i < value.size(); ++i) { + result = std::max(result, std::abs(value[i])); + } + return result; +} + +double positionError(const Eigen::VectorXd& actual, + const std::vector& expected) +{ + if (actual.size() != static_cast(expected.size())) { + return std::numeric_limits::infinity(); + } + double squared_error = 0.0; + for (Eigen::Index i = 0; i < actual.size(); ++i) { + const double error = actual[i] - expected[static_cast(i)]; + squared_error += error * error; + } + return std::sqrt(squared_error); +} + +struct PlanMetrics { + bool success{false}; + double planning_ms{0.0}; + double duration_s{0.0}; + double max_velocity{0.0}; + double max_acceleration{0.0}; + double max_waypoint_error{0.0}; + double start_error{0.0}; + double end_error{0.0}; + std::size_t sample_count{0}; +}; + +PlanMetrics planAndMeasure(const std::vector>& waypoints, + const PathType path_type = PathType::Linear) +{ + PlanMetrics metrics; + ToppraJointTrajectoryPlanner planner(path_type); + planner.setSymmetricLimits( + std::vector(kDof, kVelocityLimit), + std::vector(kDof, kAccelerationLimit)); + planner.setGridSizes(150, 300); + + TrajPtr trajectory; + const auto start = std::chrono::steady_clock::now(); + metrics.success = planner.plan(waypoints, trajectory); + metrics.planning_ms = std::chrono::duration( + std::chrono::steady_clock::now() - start).count(); + if (!metrics.success || !trajectory) { + return metrics; + } + + const auto interval = trajectory->timeInterval(); + metrics.duration_s = interval[1] - interval[0]; + const auto samples = planner.sampleTrajectory(trajectory, kSamplePeriodS); + metrics.sample_count = samples.size(); + if (samples.empty()) { + metrics.success = false; + return metrics; + } + metrics.start_error = positionError(samples.front().q, waypoints.front()); + metrics.end_error = positionError(samples.back().q, waypoints.back()); + + for (const auto& sample : samples) { + if (!std::isfinite(sample.t) || !sample.q.allFinite() || + !sample.qd.allFinite() || !sample.qdd.allFinite()) { + metrics.success = false; + return metrics; + } + metrics.max_velocity = std::max(metrics.max_velocity, maxAbs(sample.qd)); + metrics.max_acceleration = std::max( + metrics.max_acceleration, maxAbs(sample.qdd)); + } + + std::size_t sample_index = 0; + for (const auto& waypoint : waypoints) { + while (sample_index + 1 < samples.size() && + positionError(samples[sample_index + 1].q, waypoint) <= + positionError(samples[sample_index].q, waypoint)) { + ++sample_index; + } + metrics.max_waypoint_error = std::max( + metrics.max_waypoint_error, + positionError(samples[sample_index].q, waypoint)); + } + return metrics; +} + +const char* pathTypeName(const PathType path_type) +{ + switch (path_type) { + case PathType::Linear: return "Linear"; + case PathType::CubicHermite: return "CubicHermite"; + case PathType::Quintic: return "Quintic"; + case PathType::Natural: return "Natural"; + } + return "Unknown"; +} + +void printMetrics(const std::size_t waypoint_count, const PlanMetrics& metrics) +{ + std::cout << "[ToppraMultiWaypointTest] waypoints=" << waypoint_count + << ", success=" << metrics.success + << ", planning_ms=" << metrics.planning_ms + << ", duration_s=" << metrics.duration_s + << ", samples=" << metrics.sample_count + << ", max_qd=" << metrics.max_velocity + << ", max_qdd=" << metrics.max_acceleration + << ", max_waypoint_error=" << metrics.max_waypoint_error + << ", start_error=" << metrics.start_error + << ", end_error=" << metrics.end_error + << std::endl; +} + +TEST(ToppraMultiWaypointTest, SmoothSevenDofPathScalesToThousandsOfWaypoints) +{ + double reference_duration_s = 0.0; + for (const std::size_t count : {10U, 100U, 300U, 1000U, 3000U}) { + const auto metrics = planAndMeasure(makeSmoothWaypoints(count)); + printMetrics(count, metrics); + ASSERT_TRUE(metrics.success) << "waypoint_count=" << count; + EXPECT_GT(metrics.duration_s, 0.0) << "waypoint_count=" << count; + EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6) + << "waypoint_count=" << count; + EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5) + << "waypoint_count=" << count; + EXPECT_LT(metrics.max_waypoint_error, 0.002) + << "waypoint_count=" << count; + if (reference_duration_s == 0.0) { + reference_duration_s = metrics.duration_s; + } else { + EXPECT_NEAR(metrics.duration_s, reference_duration_s, + reference_duration_s * 0.10) + << "waypoint_count=" << count; + } + } +} + +TEST(ToppraMultiWaypointTest, RepeatedWaypointsRemainPlannable) +{ + const auto smooth = makeSmoothWaypoints(300); + std::vector> repeated; + repeated.reserve(smooth.size() * 2); + for (const auto& waypoint : smooth) { + repeated.push_back(waypoint); + repeated.push_back(waypoint); + } + + const auto metrics = planAndMeasure(repeated); + printMetrics(repeated.size(), metrics); + EXPECT_TRUE(metrics.success); + if (metrics.success) { + EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6); + EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5); + EXPECT_LT(metrics.max_waypoint_error, 0.002); + } +} + +TEST(ToppraMultiWaypointTest, CompareInterpolationModesAtThreeHundredWaypoints) +{ + const auto waypoints = makeSmoothWaypoints(300); + for (const auto path_type : { + PathType::CubicHermite, + PathType::Quintic, + PathType::Natural}) { + const auto metrics = planAndMeasure(waypoints, path_type); + std::cout << "[ToppraMultiWaypointTest] path_type=" + << pathTypeName(path_type) << std::endl; + printMetrics(waypoints.size(), metrics); + EXPECT_TRUE(metrics.success) << pathTypeName(path_type); + if (metrics.success) { + EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6) + << pathTypeName(path_type); + EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5) + << pathTypeName(path_type); + EXPECT_LT(metrics.max_waypoint_error, 0.002) + << pathTypeName(path_type); + EXPECT_LT(metrics.start_error, 1e-9) << pathTypeName(path_type); + EXPECT_LT(metrics.end_error, 1e-9) << pathTypeName(path_type); + } + } +} + +} // namespace +} // namespace cmvr diff --git a/cmvr-es/common/CMakeLists.txt b/cmvr-es/common/CMakeLists.txt index 3bedb230..a9e3f852 100644 --- a/cmvr-es/common/CMakeLists.txt +++ b/cmvr-es/common/CMakeLists.txt @@ -29,6 +29,12 @@ target_link_libraries(common PUBLIC add_library(cmvr_es::common ALIAS common) install(TARGETS common LIBRARY DESTINATION lib) +add_executable(support_functions_test + math/support_functions_test.cpp +) +target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es) +target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog) + #add_executable(image_display_test # utils/visualization/image_display_test.cpp #) diff --git a/cmvr-es/common/README.md b/cmvr-es/common/README.md new file mode 100644 index 00000000..cf60bb8d --- /dev/null +++ b/cmvr-es/common/README.md @@ -0,0 +1,111 @@ +# Common 模块开发指南 + +`common/` 保存可被设备、算法、管理器和协议层复用的基础能力。这里适合放稳定、协议无关、厂商无关的类型与工具，不适合放设备连接、业务服务或任务调度逻辑。 + +返回[项目总览](../../README.md)。 + +## 目录职责 + +| 目录 | 职责 | +| --- | --- | +| `base/` | 日志、基础常量、gRPC 辅助函数和线程安全缓冲区 | +| `config/` | 配置根目录解析和 Proto Text 配置加载 | +| `io/` | Protobuf 二进制与 TextFormat 文件读写 | +| `math/` | 坐标变换、关节限制、QP 和运动数学 | +| `media/` | 协议无关媒体模型以及 FFmpeg 采集、编码、写文件能力 | +| `types/` | 跨后端共享的领域类型，例如 AGV、机械臂和几何类型 | +| `vision/` | 图像显示、投影等视觉辅助代码 | + +## 依赖边界 + +新增公共组件时应遵守： + +- 不依赖 `service/`、`task/` 或具体厂商设备实现； +- 不保存 gRPC/QUIC 连接、session 或客户端状态； +- 通用类型不包含厂商报文字段、端口号和私有错误码； +- 需要调用设备的逻辑应放在 manager adapter、service 或 task； +- 需要第三方库的 `.cpp` 组件应通过明确的 CMake target 暴露依赖； +- 避免在公共头文件中使用全局 `using namespace` 或引入大体量实现头。 + +当前 `common` 共享库目标是 `cmvr_es::common`，日志是独立目标 `cmvr_es::logging`。新增 `.cpp` 文件时，需要更新 [`CMakeLists.txt`](CMakeLists.txt) 或对应子目录 CMake；纯头文件不需要加入 `add_library` 源文件列表。 + +## 新增共享类型 + +1. 选择 `types//` 或已有领域文件； +2. 类型使用明确单位，例如米、弧度、秒、纳秒； +3. 为容器长度、自由度和数值范围提供校验函数； +4. 保持控制器无关，将厂商字段转换为通用枚举或结果； +5. 确认不会迫使所有调用方引入设备 SDK； +6. 增加边界值和错误输入测试。 + +AGV 通用类型应参考 [`types/agv/agv_types.h`](types/agv/agv_types.h)，机械臂通用类型应参考 [`types/arm/arm_types.h`](types/arm/arm_types.h)。不要为了一个具体控制器把协议结构塞回 `abstract_*.h`。 + +## 媒体模型 + +[`media/media_frame.h`](media/media_frame.h) 中的 `TrackDescriptor`、`MediaFrame` 及 payload 在构造后不可变，可被多个协议消费者共享。 + +扩展媒体字段时需要保持： + +- `TrackDescriptor::generation` 非零，编码参数变化时创建新 descriptor； +- PTS、DTS 和 duration 使用 descriptor 的 `time_base`； +- `capture_time_ns` 使用单调时钟，供节奏控制和延迟统计； +- `capture_utc_ns` 只作为可选墙上时间，不能用于计算持续时间； +- H.264/H.265 明确 `ANNEX_B` 或 `AVCC`； +- AAC、Opus、PCM 明确 payload format、采样率和声道数； +- 不把 QUIC、gRPC 或浏览器专有字段加入通用帧。 + +设备媒体接入流程见 [`../manager/README.md`](../manager/README.md) 的 MediaSourceHub 章节。 + +## 环形队列选择 + +[`base/ring_buffer.h`](base/ring_buffer.h) 当前包含三类缓冲区： + +| 类型 | 使用场景 | 重要约束 | +| --- | --- | --- | +| `RingBuffer` | 只需要保存最近 N 项并批量读取 | 覆盖最旧项，没有阻塞读取 | +| `SPMCRingBuffer` | 历史单生产者场景 | 独立 `reader_tail` 只能由一个线程拥有 | +| `BroadcastFrameRing` | 新的媒体或广播式多消费者场景 | 每个消费者使用独立 Cursor，保存不可变共享对象 | + +新的实时多消费者模块优先使用 `BroadcastFrameRing`： + +- capacity 必须大于零； +- 同一 Cursor 不得被多个线程同时读取或移动； +- 慢消费者落后时会跳到最旧保留项，并得到精确 dropped count； +- `reset()` 开启新 generation，旧 Cursor 在下一次成功读取时看到变化； +- `close()` 唤醒等待者，关闭后不能继续发布； +- 不要先读取 head 再无锁读取槽位，应使用队列提供的原子读取接口。 + +## 配置和文件路径 + +[`config/config_files.h`](config/config_files.h) 提供： + +- `resolveConfigFile()`：相对根配置目录解析业务配置； +- `resolveResourceFile()`：在配置根及其父目录中查找模型等资源； +- `loadConfigFile()` / `saveConfigFile()`：读写 Proto Text 配置。 + +进程启动后配置根由 `main.cpp` 设置。公共组件不应自行使用当前工作目录拼接配置路径。 + +[`io/proto_file_io.h`](io/proto_file_io.h) 写出的 TextFormat 文件权限为 `0600`。保存运行时配置前，应确认目标目录存在，并避免把生产密钥写入仓库。 + +## 新增公共组件 + +1. 确认能力确实会被两个及以上模块复用； +2. 定义最小 API 和所有权、线程安全、错误语义； +3. 将头文件放入合适子目录，将实现放入相邻 `.cpp`； +4. 更新 CMake target 和 `target_link_libraries`； +5. 不使用未声明的传递依赖； +6. 增加无设备单元测试； +7. 对并发组件增加关闭、超时、覆盖、取消和析构测试； +8. 使用 ASan/TSan 时检查生命周期和数据竞争。 + +推荐测试目标放在组件相邻的 `tests/`，并在 `BUILD_TESTING` 下通过 `add_test()` 登记。仅创建 `_test` 可执行文件不会自动进入 CTest。 + +## 提交检查 + +- [ ] API 不依赖具体设备或传输协议 +- [ ] 公共类型有明确单位和有效性规则 +- [ ] 所有权及线程安全写入注释 +- [ ] 新增 `.cpp` 和依赖已经加入 CMake +- [ ] 缓冲区关闭能够唤醒等待线程 +- [ ] 不记录密码、私钥或大块媒体 payload +- [ ] 无设备测试可以在开发主机运行 diff --git a/cmvr-es/common/math/support_functions.h b/cmvr-es/common/math/support_functions.h index f6c38923..2be9b19e 100644 --- a/cmvr-es/common/math/support_functions.h +++ b/cmvr-es/common/math/support_functions.h @@ -3,6 +3,7 @@ // #pragma once +#include #include #include #include @@ -14,6 +15,25 @@ class SupportFunctions { private: static constexpr double EPS = 1e-9; public: + static constexpr std::int64_t absoluteDifference(const std::int32_t lhs, + const std::int32_t rhs) noexcept { + return lhs >= rhs + ? static_cast(lhs) - static_cast(rhs) + : static_cast(rhs) - static_cast(lhs); + } + + static constexpr std::int64_t cyclicAbsoluteDifference( + const std::int32_t lhs, + const std::int32_t rhs, + const std::int64_t period) noexcept { + const auto linear_distance = absoluteDifference(lhs, rhs); + if (period <= 0) { + return linear_distance; + } + const auto wrapped_distance = linear_distance % period; + return std::min(wrapped_distance, period - wrapped_distance); + } + static std::vector eigen_to_vector(const Eigen::VectorXd &v) { return std::vector(v.data(), v.data() + v.size()); } diff --git a/cmvr-es/common/math/support_functions_test.cpp b/cmvr-es/common/math/support_functions_test.cpp new file mode 100644 index 00000000..cf1a7c4e --- /dev/null +++ b/cmvr-es/common/math/support_functions_test.cpp @@ -0,0 +1,34 @@ +#include +#include + +#include + +#include "common/math/support_functions.h" + +TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceTreatsFullTurnsAsEquivalent) +{ + constexpr std::int64_t period = 65536LL * 101LL; + + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(5254257, -1364879, period), 0); + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(-10883488, -17502624, period), 0); +} + +TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceUsesShortestWrappedDistance) +{ + constexpr std::int64_t period = 100; + + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(3, 97, period), 6); + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(97, 3, period), 6); + EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(10, 40, period), 30); +} + +TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceHandlesInt32Range) +{ + constexpr std::int64_t period = 65536LL * 101LL; + + const auto distance = SupportFunctions::cyclicAbsoluteDifference( + std::numeric_limits::min(), + std::numeric_limits::max(), period); + EXPECT_GE(distance, 0); + EXPECT_LE(distance, period / 2); +} diff --git a/cmvr-es/common/media/ffmpeg/camera_capture.cpp b/cmvr-es/common/media/ffmpeg/camera_capture.cpp index 06ea1fb6..d8f2c61a 100644 --- a/cmvr-es/common/media/ffmpeg/camera_capture.cpp +++ b/cmvr-es/common/media/ffmpeg/camera_capture.cpp @@ -62,14 +62,14 @@ int CameraCapture::initialize(const Config& config) { } int CameraCapture::init_device() { - // AVInputFormat* input_fmt = nullptr; + AVInputFormat* input_fmt = nullptr; std::string device_path; #ifdef _WIN32 - auto input_fmt = av_find_input_format("dshow"); + input_fmt = av_find_input_format("dshow"); device_path = "video=" + config_.device_name; #else - auto input_fmt = av_find_input_format("v4l2"); + input_fmt = av_find_input_format("v4l2"); device_path = config_.device_name; #endif diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h new file mode 100644 index 00000000..5c7ef69c --- /dev/null +++ b/cmvr-es/common/types/agv/agv_types.h @@ -0,0 +1,420 @@ +#ifndef CMVR_ES_AGV_TYPES_H +#define CMVR_ES_AGV_TYPES_H + +#include +#include +#include +#include +#include + +#include "common/types/geometry_types.h" + +namespace cmvr::device { + +/** + * @brief AGV 通用命令/结果错误类别。 + * + * 这些枚举描述框架层面的通用结果。厂商或控制器特有错误码应由具体 + * AGV 实现转换，或保存在该实现私有的适配参数/细节中。 + */ +enum class AgvErrorCode { + OK = 0, + NotConnected, + AlreadyConnected, + ConnectionFailed, + Timeout, + InvalidArgument, + LocalizationLost, + MapNotLoaded, + TaskRejected, + TaskFailed, + TaskCanceled, + CommandFailed, + EmergencyStopped, + Fault, + UnsupportedCommand, + UnknownError +}; + +/** + * @brief AGV 命令的标准返回值。 + */ +struct AgvResult { + AgvErrorCode code{AgvErrorCode::OK}; + std::string message{"OK"}; + + bool ok() const { return code == AgvErrorCode::OK; } + static AgvResult success() { return {AgvErrorCode::OK, "OK"}; } + static AgvResult failure(AgvErrorCode c, const std::string& msg) { return {c, msg}; } +}; + +/** + * @brief AGV 粗粒度运行模式。 + */ +enum class AgvMode { + Unknown = 0, + Disconnected, + Idle, + Manual, + Auto, + Charging, + Paused, + Stopped, + Fault, + EmergencyStop +}; + +/** + * @brief 当前跟踪的导航任务状态。 + */ +enum class AgvTaskState { + None = 0, + Waiting, + Running, + Paused, + Completed, + Failed, + Canceled +}; + +/** + * @brief 当前跟踪的导航任务类型。 + */ +enum class AgvTaskType { + None = 0, + NavigateToPose, + NavigateToStation, + FollowPath, + Dock, + Charge, + Custom +}; + +/** + * @brief AGV 车体坐标系下的平面速度。 + * + * 线速度单位为米/秒，角速度单位为弧度/秒。 + */ +struct AgvVelocity { + double vx{0.0}; + double vy{0.0}; + double wz{0.0}; +}; + +/** + * @brief 导航通用运动约束和执行选项。 + * + * 除非具体实现另有说明，数值限制为 0 表示使用设备或控制器默认值。 + */ +struct AgvMotionOptions { + double max_speed{0.0}; + double max_angular_speed{0.0}; + double max_acceleration{0.0}; + double max_angular_acceleration{0.0}; + double reach_distance{0.0}; + double reach_angle{0.0}; + double speed_ratio{1.0}; + bool asynchronous{true}; +}; + +/** + * @brief AGV 适配器可选的实现特定参数。 + * + * 该结构用于避免抽象接口绑定某一个控制器协议。具体 AGV 驱动可以按需 + * 解释操作名、特殊运动模式、设备特定标志等键值。 + */ +struct AgvAdapterParams { + std::unordered_map values; + + bool empty() const { return values.empty(); } + + std::optional getString(const std::string& key) const + { + const auto it = values.find(key); + if (it == values.end()) { + return std::nullopt; + } + return it->second; + } + + std::optional getDouble(const std::string& key) const + { + const auto value = getString(key); + if (!value) { + return std::nullopt; + } + try { + return std::stod(*value); + } catch (...) { + return std::nullopt; + } + } + + std::optional getBool(const std::string& key) const + { + const auto value = getString(key); + if (!value) { + return std::nullopt; + } + if (*value == "1" || *value == "true" || *value == "yes" || *value == "on") { + return true; + } + if (*value == "0" || *value == "false" || *value == "no" || *value == "off") { + return false; + } + return std::nullopt; + } +}; + +/** + * @brief 作为 AgvRuntimeState 一部分暴露的电池信息。 + */ +struct AgvBatteryState { + double percentage{0.0}; + double voltage{0.0}; + double current{0.0}; + double temperature{0.0}; + bool charging{false}; +}; + +/** + * @brief AGV 当前运行状态快照。 + * + * 这是 AGV 设备的主要状态查询对象。抽象接口中应避免派生出的便利 + * getter；调用方可直接从该快照读取字段。 + */ +struct AgvRuntimeState { + double timestamp{0.0}; + AgvMode mode{AgvMode::Unknown}; + bool connected{false}; + bool localized{false}; + bool moving{false}; + bool fault{false}; + bool emergency_stopped{false}; + math::Pose2d pose{}; + AgvVelocity velocity{}; + AgvBatteryState battery{}; + std::string current_map; + std::string current_station; + std::string last_error; +}; + +/** + * @brief AGV 抽象层可见的地图站点/路径点。 + */ +struct AgvStation { + std::string id; + std::string type; + math::Pose2d pose{}; + std::string description; +}; + +/** + * @brief 显式导航路径中的一段站点到站点路径。 + */ +struct AgvPathSegment { + std::string source_station; + std::string target_station; +}; + +/** + * @brief AGV 扫图过程中产生的数据文件。 + * + * content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。 + */ +struct AgvMappingDataFile { + std::string name; + std::string content; +}; + +/** + * @brief 从控制器增量获取的扫图数据批次。 + */ +struct AgvMappingData { + int start_index{0}; + int next_index{0}; + std::vector files; +}; + +/** + * @brief 上位机请求的统一地图维度。 + * + * 该枚举只表示上位机希望得到 2D、3D 或两者都要；不表示厂商文件格式。 + * 厂商原始地图必须由具体 AGV 驱动转换为下面的统一地图结构。 + */ +enum class AgvMapDimension { + Unspecified = 0, + Map2D, + Map3D, + Map2DAnd3D +}; + +/** + * @brief 地图流中的更新类型。 + */ +enum class AgvMapUpdateType { + Unspecified = 0, + Snapshot, + Incremental, + Reset +}; + +/** + * @brief 统一语义地图对象类型。 + */ +enum class AgvMapObjectType { + Unspecified = 0, + Station, + Line, + Area, + QrTag, + Reflector, + BinLocation, + ExternalDevice +}; + +/** + * @brief 地图坐标系下的三维点，单位：米。 + */ +struct AgvMapPoint3D { + double x{0.0}; + double y{0.0}; + double z{0.0}; +}; + +/** + * @brief 统一语义对象。几何点均使用地图坐标系，单位：米。 + */ +struct AgvMapObject { + std::string id; + AgvMapObjectType type{AgvMapObjectType::Unspecified}; + std::vector points; + double heading{0.0}; + std::unordered_map properties; +}; + +/** + * @brief 统一 2D 地图。 + * + * data 采用行优先顺序，取值约定为 -1 未知、0 空闲、100 占据。 + * 当厂商地图只提供矢量/语义元素时，data 可以为空，objects 仍然有效。 + */ +struct AgvUnifiedMap2D { + std::string frame_id{"map"}; + double timestamp{0.0}; + double resolution{0.0}; + std::uint32_t width{0}; + std::uint32_t height{0}; + math::Pose2d origin{}; + std::vector data; + std::vector objects; +}; + +/** + * @brief 统一 3D 点样本，坐标单位：米。 + */ +struct AgvMapPointSample3D { + double x{0.0}; + double y{0.0}; + double z{0.0}; + float intensity{0.0F}; + std::uint32_t ring{0}; + double time_offset{0.0}; +}; + +/** + * @brief 统一 3D 占据体素。 + */ +struct AgvMapVoxel3D { + std::int32_t x{0}; + std::int32_t y{0}; + std::int32_t z{0}; + float probability{-1.0F}; +}; + +/** + * @brief 统一 3D 平面特征。 + */ +struct AgvMapPlane3D { + AgvMapPoint3D center; + AgvMapPoint3D normal; + double d{0.0}; + double radius{0.0}; +}; + +/** + * @brief 统一 3D 地图。 + */ +struct AgvUnifiedMap3D { + std::string frame_id{"map"}; + double timestamp{0.0}; + double voxel_resolution{0.0}; + std::vector points; + std::vector voxels; + std::vector planes; + std::vector objects; +}; + +/** + * @brief 地图流读取参数。 + */ +struct AgvMapStreamOptions { + AgvMapDimension dimension{AgvMapDimension::Unspecified}; + std::string map_name; + std::string resume_token; + bool snapshot{true}; + bool incremental{false}; + int max_chunk_bytes{0}; + int wait_timeout_ms{1000}; +}; + +/** + * @brief 建图/扫图启动参数。 + */ +struct AgvMappingOptions { + AgvMapDimension dimension{AgvMapDimension::Unspecified}; + std::string map_name; + bool real_time{false}; +}; + +/** + * @brief 统一地图流中的单条更新。 + */ +struct AgvUnifiedMapUpdate { + std::string map_id; + std::string session_id; + std::uint64_t sequence{0}; + std::string resume_token; + AgvMapDimension dimension{AgvMapDimension::Unspecified}; + AgvMapUpdateType update_type{AgvMapUpdateType::Unspecified}; + std::string frame_id{"map"}; + double timestamp{0.0}; + bool snapshot_begin{false}; + bool snapshot_end{false}; + std::uint32_t chunk_index{0}; + std::uint32_t chunk_count{0}; + std::optional map_2d; + std::optional map_3d; +}; + +/** + * @brief 当前导航任务状态。 + * + * 该状态独立于 AgvRuntimeState，因为导航任务可能处于排队、暂停、 + * 完成或失败状态，而车辆本体仍然保持连接并处于正常状态。 + */ +struct AgvNavigationStatus { + AgvTaskState state{AgvTaskState::None}; + AgvTaskType type{AgvTaskType::None}; + double progress{0.0}; + std::string message; +}; + +/** + * @brief 兼容旧 AGV 状态 API 命名的别名。 + */ +using AGVState = AgvRuntimeState; + +} // namespace cmvr::device + +#endif // CMVR_ES_AGV_TYPES_H diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 567b503c..4728236c 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -112,6 +112,14 @@ struct JointGroupState { } }; +struct JointTrajectoryPoint { + double time_s{0.0}; + std::vector position; + std::vector velocity; +}; + +using JointTrajectory = std::vector; + struct JointPositionCommand { std::vector position; diff --git a/cmvr-es/common/types/geometry_types.h b/cmvr-es/common/types/geometry_types.h index e1378bf6..76fa0054 100644 --- a/cmvr-es/common/types/geometry_types.h +++ b/cmvr-es/common/types/geometry_types.h @@ -49,7 +49,7 @@ namespace cmvr::math { typedef struct { double x; //* unit: m double y; - double theta; + double theta; //* unit: rad } Pose2d; } diff --git a/cmvr-es/config/README.md b/cmvr-es/config/README.md new file mode 100644 index 00000000..c1414757 --- /dev/null +++ b/cmvr-es/config/README.md @@ -0,0 +1,130 @@ +# Config 模块开发指南 + +`config/` 保存 CMVR-ES 的默认运行配置。配置格式是 Protobuf TextFormat，Schema 位于 [`../../protos/cmvr/config/`](../../protos/cmvr/config/)。 + +返回[项目总览](../../README.md)。 + +## 配置树 + +```text +cmvr_es.pb.txt +├── logger/logger.pb.txt +├── manager/device_manager.pb.txt +│ └── devices//*.pb.txt +└── manager/task_manager.pb.txt + └── tasks//*.pb.txt +``` + +入口文件： + +- [`cmvr_es.pb.txt`](cmvr_es.pb.txt) +- [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) +- [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) + +## 路径规则 + +无参数运行时，程序读取： + +```text +/config/cmvr_es.pb.txt +``` + +安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时： + +```bash +./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt +``` + +设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。 + +日志配置中的相对 `directory` 以可执行文件目录解析，不以配置根解析。 + +## 新增设备配置 + +增加同类设备后端时： + +1. 在 `protos/cmvr/config/_config/` 增加后端 message； +2. 在类别设备 message 的 `oneof backend` 中增加字段； +3. 在 `devices//` 的 `.pb.txt` 中增加实例； +4. 实例外层 `id` 必须唯一； +5. 在 [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) 增加相同 `id`、正确 `type` 和配置路径； +6. 开发默认保持 `enable: false`； +7. 同步类别 factory 和 CMake； +8. 在无硬件环境验证关闭状态，在真机环境单独开启。 + +设备集合中的 ID 与 DeviceManager 条目 ID 不一致时，工厂会拒绝创建。 + +## 新增任务配置 + +1. 在 `protos/cmvr/config/` 增加任务配置和 root message； +2. 在 `tasks//` 增加默认 `.pb.txt`； +3. 在 `task_manager.pb.txt` 增加唯一任务 ID； +4. 配置正确的 `TaskType` 和 `TaskRunMode`； +5. 周期任务设置大于零的 `control_period_s`； +6. 服务任务使用 `TASK_RUN_MODE_BLOCKING_SERVICE`； +7. 默认关闭依赖网络、证书或硬件的新任务。 + +任务实现流程见 [`../task/README.md`](../task/README.md)。 + +## 默认值与校验 + +- 不依赖 proto3 数值零值表达危险的生产默认值； +- timeout、队列大小、帧大小和周期应在代码中校验； +- 新增 loader 对不认识的 enum 和未设置的 oneof 必须明确失败；当前个别历史路径仍有退化默认行为，不应复制； +- 设备端口、坐标系、速度和单位写入注释； +- `enable` 应由 manager 层控制，后端内部的 enable 字段不能替代 manager 开关； +- QUIC 需要 TaskManager 与 `QuicEdgeConfig.enable` 同时开启； +- QUIC 零媒体轨道是合法配置。 + +### gRPC 相机实时流 + +[`tasks/grpc_server_task/grpc_server_task.pb.txt`](tasks/grpc_server_task/grpc_server_task.pb.txt) +中的两个低延迟参数仅作用于 gRPC RGB 编码流，不改变机械臂、AGV 等控制 RPC： + +- `camera_stream_max_pending_frames`：单个客户端允许的待发送帧数，超过后清空该 + 客户端积压；默认 2； +- `camera_stream_max_frame_age_ms`：从设备回调进入边缘系统起计算的最大帧龄， + 超过后不再发送；默认 250 ms。 + +两个字段填 0 或旧配置未包含字段时使用默认值。丢弃 H.264/H.265 帧后服务会请求 +IDR 并等待关键帧恢复。如果现场采集、编码本身稳定超过 250 ms，应根据日志中的 +`age_ms` 调高帧龄阈值，而不是增大环形队列。 + +新二进制可以读取未包含这两个字段的旧配置；旧二进制不能解析包含新字段的 +TextFormat。部署时必须同步更新程序与配置，不能只把新版 +`grpc_server_task.pb.txt` 复制给旧的 `output/bin/cmvr_es`。 + +## 配置验证 + +构建后可以用 `protoc --encode` 对单个 TextFormat 文件做语法和字段验证。例如： + +```bash +output/bin/protoc \ + -I protos \ + --encode=cmvr.config.QuicEdgeRootConfig \ + protos/cmvr/config/quic_edge_config/quic_edge_config.proto \ + < cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt \ + > /tmp/quic_edge_config.pb +``` + +该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。 + +## 生产配置 + +`cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。 + +- 不提交真实设备密码、token、私钥和生产地址； +- 证书与私钥放在独立 `certs/`，使用最小读取权限； +- 为不同站点维护独立配置根，不在运行时修改仓库样例； +- 发布前检查所有 `enable`、IP、端口和设备 ID； +- 变更配置 Schema 时同步 Proto 兼容性文档和平台生成代码。 + +## 提交检查 + +- [ ] TextFormat 可以被对应 root message 解析 +- [ ] ID、类别和引用路径完全一致 +- [ ] 新硬件和新网络任务默认关闭 +- [ ] 参数单位、范围和安全默认值明确 +- [ ] 没有生产凭据 +- [ ] 安装覆盖不会丢失现场配置 +- [ ] 无设备启动仍然成功 diff --git a/cmvr-es/config/certs/cmvr-quic-ca.crt b/cmvr-es/config/certs/cmvr-quic-ca.crt new file mode 100644 index 00000000..91e2619a --- /dev/null +++ b/cmvr-es/config/certs/cmvr-quic-ca.crt @@ -0,0 +1,25 @@ +-----BEGIN CERTIFICATE----- +MIIEKzCCApOgAwIBAgIUTtZCyKM8INYxZKkfwbKdpHKLjn4wDQYJKoZIhvcNAQEL +BQAwHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENBMB4XDTI2MDcyNDA2NDQy +MFoXDTM2MDcyMTA2NDQyMFowHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENB +MIIBojANBgkqhkiG9w0BAQEFAAOCAY8AMIIBigKCAYEAmq2rHldOobaemqNfWggS +OVj3inKy6AYjfgtcXUfKs48DDbpZ9gyEd/YPJXA8C2jGPXpxzgnc7a4UCUVQZ8ah +ddoJtFcC+Q6BgjeMVqUdUubu5Y9HpkfU3lvnp4KhzvOeFnkKtrCzYIPa2nK3zLc7 +uCiuLlB+91KQSRXPFbc6N7H/EAfGmUHIwlZGysAkRN7b2TAoR4C7E96JLVtuUQsS +VtlEGpunSfuefFzeeZCMS6avLbB+a8Q6yUzLt6pqnheNsDB+jCCXodlJs5XS1AOB +W2GOpGFMj7dLoTD+eBIlAlrhWFcwKjzFmtp6LGl/Jy0O+E99X4TL72oNSOdTrO2O +iavXABz9IvWR2BrAyo5AKlTJqO6tmZw77iVti8jYi+HsXIVGQKMYwvv5k0jdHeIZ +FaDToUbFPP/zj0m8ZraMy+8eNAhScnx6Zs56fcncBuDti6pT+zKisjV1rH/sFvZY +wO1UvJlOEbvXrfYLPp58Aqe/toG2nV45a2Q4+x6AETV1AgMBAAGjYzBhMB0GA1Ud +DgQWBBRdHz+g2vVmqghDZi2kEHtZfoyHZzAfBgNVHSMEGDAWgBRdHz+g2vVmqghD +Zi2kEHtZfoyHZzAPBgNVHRMBAf8EBTADAQH/MA4GA1UdDwEB/wQEAwIBBjANBgkq +hkiG9w0BAQsFAAOCAYEASuEoMFiVcg4iTMxO2kshFTJ6LIrqqGXBn+1j+yQ1ennG +mqPMo5fBOe/Kp3YWCnREQWu0+EEPEC9qWgIDOIm3v7ch4mZiW31GUOqae6bjprBe +er7ySElKGZ5GefKAq++we19A6WHnNxtNAT9BE1VSKUmkxEsnIkuwd+QMmQ9eaIRm +8RvzshWdUcyiBJg07sI3rPzPD/YpnfcYlAa2a0+oXJ3o3zlhdbs2S+9fIl+Y/zFN +wBaUV6ZNk0RFzOCA+jUq2jU5Y1pODcot3Mp2jCHzp4uTFyVqsoqc5USRB2TUILCP +acSbEjD9GnKaNU4miTcftnC85fnBAiZ5btZkXIs72ij+J3VW7QnbcFQG8bElngoR +mWizh6ByPQrsvyT0evJjdjpuOzILsVTAn64m0cYpnlR4g5orcEkIJHv4DzMhpXc2 +CvpcWwk2vJrUuws/LGxDATTAcqtIUIxAkxffqUsdB/6j8fdykNCvXFwNh/uPcawd +4kzh6dr+RFzHSMQ6oN16 +-----END CERTIFICATE----- diff --git a/cmvr-es/config/certs/cmvr-quic-ca.key b/cmvr-es/config/certs/cmvr-quic-ca.key new file mode 100644 index 00000000..4c2b3ac9 --- /dev/null +++ b/cmvr-es/config/certs/cmvr-quic-ca.key @@ -0,0 +1,40 @@ +-----BEGIN PRIVATE KEY----- +MIIG/wIBADANBgkqhkiG9w0BAQEFAASCBukwggblAgEAAoIBgQCaraseV06htp6a +o19aCBI5WPeKcrLoBiN+C1xdR8qzjwMNuln2DIR39g8lcDwLaMY9enHOCdztrhQJ +RVBnxqF12gm0VwL5DoGCN4xWpR1S5u7lj0emR9TeW+engqHO854WeQq2sLNgg9ra +crfMtzu4KK4uUH73UpBJFc8Vtzo3sf8QB8aZQcjCVkbKwCRE3tvZMChHgLsT3okt +W25RCxJW2UQam6dJ+558XN55kIxLpq8tsH5rxDrJTMu3qmqeF42wMH6MIJeh2Umz +ldLUA4FbYY6kYUyPt0uhMP54EiUCWuFYVzAqPMWa2nosaX8nLQ74T31fhMvvag1I +51Os7Y6Jq9cAHP0i9ZHYGsDKjkAqVMmo7q2ZnDvuJW2LyNiL4exchUZAoxjC+/mT +SN0d4hkVoNOhRsU8//OPSbxmtozL7x40CFJyfHpmznp9ydwG4O2LqlP7MqKyNXWs +f+wW9ljA7VS8mU4Ru9et9gs+nnwCp7+2gbadXjlrZDj7HoARNXUCAwEAAQKCAYAi +e49/pNqWghoTItM9xLldWAhdcMsSH1Y3wgweDoRxqbL2U0I9cFZy0OPZBo+YQowZ +RgwLcRbz1MBKPc3aSMWTep95uQEkaVe1YjFS2p3yLqH5AstoFjDuPmJjLWPpuVVX +sLXS+wsOO+7lDriLdpjlagpEsHTRqbIZXPeM4YtkwbV5SyZ64ZfCPU4sYo/jW6R6 +43nDUP9Dw3Nk7XJnNlbpDigY33T4sRPIqUJ+qtsf/WGlx6gzWax6Vni+8gqxQlIe +80XG6dHCM3UGFEdGF64PB2FuprsPWBSSau7L7I/gagiIuQbhs9VYXOanz1MQizMU +ZdK8cqmG/bpP+uY4VQLJ5Lxu87boEllGFHBOYTCjWq0zatzGMt1/DeggLnjgfC90 +5kOzlxl403j+4K8nqiHoKpAbOatwTxFz1up6XSecsarE1RKsCq8sIH+FjjCfO1Db +hBfv2a7/XemdcxHtl9Q/EOSDaAFOUQfFMDAc3rCH7/vonTzaaZq5ucWmjlBsv2EC +gcEAz+cH+3x09pXVOnM7zlWrGAQk69Fj/0BGs7g3GFf8ZvI/kkbez+9J1QGItxj+ +7Dm4Ay0kgxYi3DxFWbPLHTbFpVg2CyL08IQZSXJzokwz2V/HTd1a+OgzgB/dBNDc +V+XnxJI89+SI3VhOQZJG625l//lhHWCy2u0ULE7ZlK3ao95L86bSXQHI4dZ2nWXl +0f4ISJmm4qkEJAY+DkQeF3UZLumd1NRxy0LBdbYVM/wZIN+sGsRdQqQ9f+ik6+aX +YrjhAoHBAL52dJzIPIN1Z/Y9qxUrVLPE7K8Mm8KLaFIl/ztl3TFqaUvErHi66nx3 +MTwEnrQIBI31OKxB6Or2JLxYMvDTSVRV1K2tY53+K6Q8giWU4x3zFc/svyl35gkd +U1bLdfxXqKMLrvlmVWj6/RMoH7vd/aTcidYVvwOZgtL+6RahS4LfZ9qXAyREFAJa +VSI0HW5LLiOBLTegztT1CBVe7xb78bWFMiUkxre4XsLZYwFPzXzVPh6HloDcACDL +FPwLrfJrFQKBwQCvGDl90UzEnF4v0vsshLQLDvp1bS1VvTGOjPhB1WBq510o+e0P +nM1Gyvr0keWo19elPTDCAjOr3kreCHFpEkcVQRyK9o7pvad6Vx0SNDF6wpKdfm7u +sMknAC7prmnU0XkH8c3NTTkDiiqmSObXw2u+UK48ysL3ZLIXuvS+pkk8t6yp8Pa8 +hBNGOJQ/baFH4TXixx1pScWF/YfoBfB9+w4Rl4loxN9tu7QpSgfDd29GY3qUNIsC +5EYzYqD7WIJpD6ECgcEAkdIfdenYas14yw5r7ck/EGO00lDU8B3LwRlWUCOtNihC +dcAeTFDPNnwLNehTmYKJ+iXFPh04Nqw9c/YTCk651dfg/RfDLTNsNlIdUqirOkLi +cE7SDO2/MTtCkzEzI//5HNvVGx0+RyHioMgXg75yc8ZlwYLku9zMTL7dtnXHWmux +F6qGvT1iFGsUwxsjbU4iBQzhkbWMpX70sWf9pZs/c7qGqel+OyrtYkENi/ONYAXj +iXxFvmKxtmnFpzNJ+lABAoHBAJjs8jfEXJnJP80t93Bxg6NmfKhxUEm4ovcIeR8U +lEfb8Ind1ICrBWGVGOAkirTFZgFAAlEjne+lT1FYgf19oC+XbxFdl/e02M8KfflS +01+0/t6lEpefiU0dfW0dWtXOPaAVvpNgMkeMgBzB2Yz7ZNUrMypfCGk289qveWxB +8UbO+QLjTI+cFvPsmR95Eti/nAjd2fXGeqbR+z+DdQnz4BvXLSCQCw4on2+T+mpw +BprrEQmcdeZA8LE1zgcKtkZR1w== +-----END PRIVATE KEY----- diff --git a/cmvr-es/config/certs/cmvr-quic-ca.srl b/cmvr-es/config/certs/cmvr-quic-ca.srl new file mode 100644 index 00000000..a423a9bb --- /dev/null +++ b/cmvr-es/config/certs/cmvr-quic-ca.srl @@ -0,0 +1 @@ +2C945D70B02014891B6E09D57E377CEFB6D18498 diff --git a/cmvr-es/config/certs/quic-gateway.crt b/cmvr-es/config/certs/quic-gateway.crt new file mode 100644 index 00000000..5a249638 --- /dev/null +++ b/cmvr-es/config/certs/quic-gateway.crt @@ -0,0 +1,22 @@ +-----BEGIN CERTIFICATE----- +MIIDuzCCAiOgAwIBAgIULJRdcLAgFIkbbgnVfjd877bRhJgwDQYJKoZIhvcNAQEL +BQAwHTEbMBkGA1UEAwwSQ01WUiBRVUlDIExvY2FsIENBMB4XDTI2MDcyNDA2NDQz +NloXDTI4MTAyNjA2NDQzNlowGDEWMBQGA1UEAwwNMTkyLjE2OC4wLjIyMjCCASIw +DQYJKoZIhvcNAQEBBQADggEPADCCAQoCggEBAKdM3i1FYFKqNWJzhfhsD9nRUAuK +pzilz5uqCKAt8lKYYC9WnLHOYdiEjcHGnGr02yd6sWFH/LBbxNhzx8M7h4S4izuO +bhSlG1EIhkMiojzVD1e3P7YzXdEoVxTCfmMgBZQJG63GNOfzRawFYtEeGv7ndFVw +kitCYlyTza5KlBNlWpiNOPmmx4dLTdGLUk8a5TUm0zJ+b/LyzVsWUtr9sxKmKeG8 +0/77AeiL0hQE3xUt5QROTZjRTVhNHowv410dFMJyIfY4sab8ndc4SIwE9PCKg068 +4705vbFInBS3eTvQur5VLSZLPatnXGKzCXjci1lIQX2p/QIqADMGFWIN9OECAwEA +AaN4MHYwDwYDVR0RBAgwBocEwKgA3jAOBgNVHQ8BAf8EBAMCBaAwEwYDVR0lBAww +CgYIKwYBBQUHAwEwHQYDVR0OBBYEFLmoEtsglxm3mXh4l8OF31bt2ZhnMB8GA1Ud +IwQYMBaAFF0fP6Da9WaqCENmLaQQe1l+jIdnMA0GCSqGSIb3DQEBCwUAA4IBgQAR +eK1mD9rJkzHe4OusimQfcuDQW+0J32e4T/34RHlW+lIj7botFaElXIzO9S80tDwq +4d4ozNPKysqgJN9hv/BBMzJpZLwP2XozPaGLTNl1jRTCc9UhFUPrUeu0LbpQGBfC +6Ghq42V94zPAw4lnMujnkq8botk21hclbJORQ9kblXP31IdWCgiKSFLy1NTBmQmc +IxmR+SldMWYrWGWv/0I85AeMu6HR3+NKHmzDblm1HUHFekyC1f7sypNG+D1r8ab2 +GSijoCMKHSEOm81Vl/j5bgWQygnnIOhsLOUf2DZO6jC+VZmTKpMEJNHgwk23WBlu +FYz/X9p5z9ZucL8aBxegj7G1fI5Ik2O05+LLeqJMfspAm6ZcnRDxCVZWQ+K/dAle +fz+gTASzhHTsjEBeiX46LP0L2PVyBiPvNtD66e52LoM5ZigechZq91niLO+ipWdh +xq7ryJzmaAEQKPuYaoswriWzJam07ywX9yupGHzBA6vVgGy0jPAsnGCuZXTfDyA= +-----END CERTIFICATE----- diff --git a/cmvr-es/config/certs/quic-gateway.csr b/cmvr-es/config/certs/quic-gateway.csr new file mode 100644 index 00000000..c767f90f --- /dev/null +++ b/cmvr-es/config/certs/quic-gateway.csr @@ -0,0 +1,17 @@ +-----BEGIN CERTIFICATE REQUEST----- +MIICpDCCAYwCAQAwGDEWMBQGA1UEAwwNMTkyLjE2OC4wLjIyMjCCASIwDQYJKoZI +hvcNAQEBBQADggEPADCCAQoCggEBAKdM3i1FYFKqNWJzhfhsD9nRUAuKpzilz5uq +CKAt8lKYYC9WnLHOYdiEjcHGnGr02yd6sWFH/LBbxNhzx8M7h4S4izuObhSlG1EI +hkMiojzVD1e3P7YzXdEoVxTCfmMgBZQJG63GNOfzRawFYtEeGv7ndFVwkitCYlyT +za5KlBNlWpiNOPmmx4dLTdGLUk8a5TUm0zJ+b/LyzVsWUtr9sxKmKeG80/77AeiL +0hQE3xUt5QROTZjRTVhNHowv410dFMJyIfY4sab8ndc4SIwE9PCKg0684705vbFI +nBS3eTvQur5VLSZLPatnXGKzCXjci1lIQX2p/QIqADMGFWIN9OECAwEAAaBHMEUG +CSqGSIb3DQEJDjE4MDYwDwYDVR0RBAgwBocEwKgA3jAOBgNVHQ8BAf8EBAMCBaAw +EwYDVR0lBAwwCgYIKwYBBQUHAwEwDQYJKoZIhvcNAQELBQADggEBAGIcLeCE344z +PENI1/oONVHBzMMt5VN0P8jbkJOFgZ3a6AUhfqAmDNBr+8SBym+cX2Y9Q2BsaWAu +TKOBN+fs+fh5/NqF0hTNvmXzp89NFK5SlsTjoC21HJvK1HTNuNW8drOxNfWgFW3/ +gSCutcsWS9hVtYrV2FHQzOvVXvXKfmuTkZE8g92P3BCkvRm+ORxI0QfVS81/Ibnv +Yl+t/o2QCphdP+1OWzU6+Pccqi2xCyIc6jqCmh0b01zg265PTTOfICO6WQNtYcOn +308avDDpTneGl5vWRVdmmiXPqkc+dCesoguIEDUlerFpDKKY81P2GTpfjki93K7Z +2DJPs4P1cU0= +-----END CERTIFICATE REQUEST----- diff --git a/cmvr-es/config/devices/agv/agv.pb.txt b/cmvr-es/config/devices/agv/agv.pb.txt deleted file mode 100644 index 45881d7a..00000000 --- a/cmvr-es/config/devices/agv/agv.pb.txt +++ /dev/null @@ -1,9 +0,0 @@ -agv { - agvs { - id: "agv_1" - my_agv { - ip: "127.0.0.1" - port: 8080 - } - } -} diff --git a/cmvr-es/config/devices/agv/src1100.pb.txt b/cmvr-es/config/devices/agv/src1100.pb.txt new file mode 100644 index 00000000..833bde58 --- /dev/null +++ b/cmvr-es/config/devices/agv/src1100.pb.txt @@ -0,0 +1,45 @@ +agv { + agvs { + id: "agv_1" + my_agv { + ip: "127.0.0.1" + port: 8080 + } + } + + agvs { + id: "src1100" + src1100_agv { + ip: "192.168.192.5" + port_status: 19204 + port_control: 19205 + port_nav: 19206 + port_config: 19207 + port_other: 19210 + port_push: 19301 + recv_timeout_ms: 1000 + enable_state_push: true + state_push_interval_ms: 200 + state_push_included_fields: "x" + state_push_included_fields: "y" + state_push_included_fields: "angle" + state_push_included_fields: "vx" + state_push_included_fields: "vy" + state_push_included_fields: "w" + state_push_included_fields: "battery_level" + state_push_included_fields: "battery_temp" + state_push_included_fields: "charging" + state_push_included_fields: "voltage" + state_push_included_fields: "current" + state_push_included_fields: "current_map" + state_push_included_fields: "current_station" + state_push_included_fields: "confidence" + state_push_included_fields: "emergency" + state_push_included_fields: "fatals" + state_push_included_fields: "errors" + enable_map_update: true + map_update_interval_ms: 1000 + map_update_history_size: 8 + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt new file mode 100644 index 00000000..f8c0a3b8 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt @@ -0,0 +1,151 @@ +arm { + robot_arms { + id: "mujoco_right_arm" + + motor { + motor_system_id: "mujoco_motors" + motor_group_ids: "mujoco_right_arm" + dof: 7 + joint_names: "right_arm_J1" + joint_names: "right_arm_J2" + joint_names: "right_arm_J3" + joint_names: "right_arm_J4" + joint_names: "right_arm_J5" + joint_names: "right_arm_J6" + joint_names: "right_arm_J7" + upd_freq: 1000 + buffer_size: 50 + default_vel: 0.6 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/gen2/robot.urdf" + base_frame_name: "body_link" + flange_frame_name: "arm_link_7_2" + max_iters: 200 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-5 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + } + soft_limit { + enable: true + margin_ratio: 0.01 + min_margin_rad: 0.01 + } + avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + weight: 2.0 + } + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.005 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 4.0 + max_joint_acceleration_rad_s2: 100.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 10.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 10.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + } + + speed_l { + pinocchio_cartesian_motion_planner { + linear_velocity_max: 0.5 + linear_acceleration_max: 2.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + enforce_joint_acceleration_limits: true + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.005 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 4.0 + max_joint_acceleration_rad_s2: 100.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 10.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 10.0 + min_desired_linear_speed: 0.01 + min_desired_angular_speed: 1e-4 + } + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + stop_acceleration: 2.0 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/aubo_arm.pb.txt b/cmvr-es/config/devices/arm/aubo_arm.pb.txt index b5696f9c..551f31df 100644 --- a/cmvr-es/config/devices/arm/aubo_arm.pb.txt +++ b/cmvr-es/config/devices/arm/aubo_arm.pb.txt @@ -4,7 +4,7 @@ arm { vendor { brand: VENDOR_ROBOT_ARM_BRAND_AUBO_ARM - ip: "192.168.1.100" + ip: "192.168.192.18" port: 30004 dof: 6 joint_names: "joint_1" diff --git a/cmvr-es/config/devices/camera/camera.pb.txt b/cmvr-es/config/devices/camera/camera.pb.txt index 1eb69fdd..25d29ab9 100644 --- a/cmvr-es/config/devices/camera/camera.pb.txt +++ b/cmvr-es/config/devices/camera/camera.pb.txt @@ -101,7 +101,27 @@ camera { } } - + cameras { + id: "left_eye_cam" + uvc { + usb: "/dev/uvc_left_camera" + camera_mode: CAMERA_MODE_VIDEO + capture { + width: 640 + height: 480 + fps: 30 + stream_mode: STREAM_MODE_RGB + } + encoder { + width: 640 + height: 480 + fps: 30 + codec: "H265" + enable_stream_timestamp: true + buffer_size: 30 + } + } + } cameras { id: "cam5" @@ -165,50 +185,4 @@ camera { buffer_size: 30 } } - - cameras { - id: "real_cam1" - realsense { - serialNumber: "332522076896" - camera_mode: CAMERA_MODE_VIDEO - capture { - width: 640 - height: 480 - fps: 30 - stream_mode: STREAM_MODE_RGBD - } - encoder { - width: 640 - height: 480 - fps: 30 - codec: "h265_qsv" - enable_stream_timestamp: true - buffer_size: 30 - } - align_mode: ALIGN_MODE_COLOR - sync: false - } - } - - cameras { - id: "usb_cam1" - uvc { - usb: "/dev/video0" - camera_mode: CAMERA_MODE_VIDEO - capture { - width: 640 - height: 480 - fps: 30 - stream_mode: STREAM_MODE_RGB - } - encoder { - width: 640 - height: 480 - fps: 30 - codec: "h265_qsv" - enable_stream_timestamp: true - buffer_size: 30 - } - } - } } diff --git a/cmvr-es/config/devices/microphone/microphone.pb.txt b/cmvr-es/config/devices/microphone/microphone.pb.txt index 90d71fa2..9ceba156 100644 --- a/cmvr-es/config/devices/microphone/microphone.pb.txt +++ b/cmvr-es/config/devices/microphone/microphone.pb.txt @@ -5,7 +5,7 @@ microphone { channels: 2 sampleRate: 48000 volume: 100 - input_device: "plughw:CARD=XFMDPV0018,DEV=0" + input_device: "default" } } } diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt new file mode 100644 index 00000000..9fda778a --- /dev/null +++ b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt @@ -0,0 +1,72 @@ +motor { + id: "ethercat_motors" + + motor_groups { + id: "right_arm_ethercat" + bus_type: MOTOR_BUS_ETHERCAT + vendor: MOTOR_VENDOR_EYOU + protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 + + ethercat { + master_index: 0 + cycle_us: 1000 + slave_op_timeout_ms: 12000 + slave_state_poll_period_ms: 10 + + cia402 { + state_transition_timeout_ms: 1200 + velocity_stop_timeout_ms: 2000 + status_poll_period_ms: 10 + stopped_velocity_tolerance_rad_s: 0.001 + } + + zero_calibration { + timeout_ms: 2000 + poll_period_ms: 10 + stable_sample_count: 5 + position_tolerance_counts: 10000 + stable_delta_counts: 1000 + } + + dc { + enable: true + reference_motor_id: 1 + sync0_cycle_us: 1000 + sync0_shift_us: 0 + sync_reference_clock_period: 1 + assign_activate: 768 + sync_monitor_period_ms: 1000 + } + + slaves { motor_id: 1 alias: 0 position: 0 } + slaves { motor_id: 2 alias: 0 position: 1 } + slaves { motor_id: 3 alias: 0 position: 2 } + slaves { motor_id: 4 alias: 0 position: 3 } + slaves { motor_id: 5 alias: 0 position: 4 } + slaves { motor_id: 6 alias: 0 position: 5 } + slaves { motor_id: 7 alias: 0 position: 6 } + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + } + + motors { + motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + } + } +} diff --git a/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt new file mode 100644 index 00000000..8e69905a --- /dev/null +++ b/cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt @@ -0,0 +1,63 @@ +motor { + id: "ethercat_motors" + + motor_groups { + id: "right_arm_ethercat" + bus_type: MOTOR_BUS_ETHERCAT + vendor: MOTOR_VENDOR_EYOU + protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 + + ethercat { + master_index: 0 + cycle_us: 1000 + slave_op_timeout_ms: 15000 + slave_state_poll_period_ms: 10 + + cia402 { + state_transition_timeout_ms: 1200 + velocity_stop_timeout_ms: 2000 + status_poll_period_ms: 10 + stopped_velocity_tolerance_rad_s: 0.001 + } + + zero_calibration { + timeout_ms: 2000 + poll_period_ms: 10 + stable_sample_count: 5 + position_tolerance_counts: 10000 + stable_delta_counts: 1000 + } + + dc { + enable: false + reference_motor_id: 1 + sync0_cycle_us: 1000 + sync0_shift_us: 0 + sync_reference_clock_period: 1 + assign_activate: 768 + sync_monitor_period_ms: 1000 + } + + slaves { motor_id: 1 alias: 0 position: 0 } + slaves { motor_id: 2 alias: 0 position: 1 } + slaves { motor_id: 3 alias: 0 position: 2 } + slaves { motor_id: 4 alias: 0 position: 3 } + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } + } + + motors { + motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + } + } +} diff --git a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt index 4ebaac09..7271eba6 100644 --- a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt +++ b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt @@ -6,7 +6,6 @@ motor { bus_type: MOTOR_BUS_MUJOCO vendor: MOTOR_VENDOR_MUJOCO protocol: MOTOR_PROTOCOL_MUJOCO - tool_frame: "R_FINGER_TIP" mujoco { world_id: "mujoco_world" } diff --git a/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt new file mode 100644 index 00000000..8bd88329 --- /dev/null +++ b/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt @@ -0,0 +1,35 @@ +motor { + id: "mujoco_motors" + + motor_groups { + id: "mujoco_right_arm" + bus_type: MOTOR_BUS_MUJOCO + vendor: MOTOR_VENDOR_MUJOCO + protocol: MOTOR_PROTOCOL_MUJOCO + mujoco { + world_id: "mujoco_world" + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + } + + motors { + motors { id: 1 joint_name: "right_arm_J1" } + motors { id: 2 joint_name: "right_arm_J2" } + motors { id: 3 joint_name: "right_arm_J3" } + motors { id: 4 joint_name: "right_arm_J4" } + motors { id: 5 joint_name: "right_arm_J5" } + motors { id: 6 joint_name: "right_arm_J6" } + motors { id: 7 joint_name: "right_arm_J7" } + } + } +} diff --git a/cmvr-es/config/devices/motor/ti5_motors.pb.txt b/cmvr-es/config/devices/motor/ti5_motors.pb.txt index ca50e0ec..20a28aff 100644 --- a/cmvr-es/config/devices/motor/ti5_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ti5_motors.pb.txt @@ -6,7 +6,6 @@ motor { bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "L_FINGER_TIP" can { channel_id: 0 } @@ -16,13 +15,13 @@ motor { urdf_path: "model/xiaoyan_description/dual_arm.urdf" } motors { - motors { id: 23 joint_name: "L_SHOULDER_P" } - motors { id: 24 joint_name: "L_SHOULDER_R" } - motors { id: 25 joint_name: "L_SHOULDER_Y" } - motors { id: 26 joint_name: "L_ELBOW_R" } - motors { id: 27 joint_name: "L_WRIST_P" } - motors { id: 28 joint_name: "L_WRIST_Y" } - motors { id: 29 joint_name: "L_WRIST_R" } + motors { id: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 24 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 25 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 26 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 27 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 28 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 29 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -31,7 +30,6 @@ motor { bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "R_FINGER_TIP" can { channel_id: 1 } @@ -47,13 +45,13 @@ motor { joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } } motors { - motors { id: 16 joint_name: "R_SHOULDER_P" } - motors { id: 17 joint_name: "R_SHOULDER_R" } - motors { id: 18 joint_name: "R_SHOULDER_Y" } - motors { id: 19 joint_name: "R_ELBOW_R" } - motors { id: 20 joint_name: "R_WRIST_P" } - motors { id: 21 joint_name: "R_WRIST_Y" } - motors { id: 22 joint_name: "R_WRIST_R" } + motors { id: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 17 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 18 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 19 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 20 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 21 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 22 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -73,9 +71,9 @@ motor { joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } motors { - motors { id: 32 joint_name: "HEAD_Y" } - motors { id: 30 joint_name: "HEAD_P" } - motors { id: 31 joint_name: "HEAD_R" } + motors { id: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 31 joint_name: "HEAD_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -94,8 +92,8 @@ motor { joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } motors { - motors { id: 4 joint_name: "WAIST_Y" } - motors { id: 15 joint_name: "WAIST_P" } + motors { id: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 5475ff16..3506a85a 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -89,20 +89,6 @@ device_manager { enable: false } - devices { - id: "eyou_arm" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/arm.pb.txt" - enable: false - } - - devices { - id: "eyou_left_arm" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/arm_eyou_left.pb.txt" - enable: false - } - devices { id: "aubo_arm" type: DEVICE_TYPE_ROBOT_ARM @@ -161,20 +147,4 @@ device_manager { # Host-development default: keep physical audio devices disabled. enable: false } - - - devices { - id: "real_cam1" - type: DEVICE_TYPE_CAMERA - config_file: "devices/camera/camera.pb.txt" - # Host-development default: keep physical cameras disabled. - enable: false - } - devices { - id: "usb_cam1" - type: DEVICE_TYPE_CAMERA - config_file: "devices/camera/camera.pb.txt" - # Host-development default: keep physical cameras disabled. - enable: false - } } diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index db13dfa9..d6569ea6 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -14,4 +14,20 @@ task_manager { config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt" enable: true } + tasks { + id: "right_arm_self_collision" + type: TASK_TYPE_SELF_COLLISION + run_mode: TASK_RUN_MODE_PERIODIC_STEP + control_period_s: 0.002 + config_file: "tasks/self_collision_task/self_collision_task.pb.txt" + enable: false + } + tasks { + id: "quic_edge" + type: TASK_TYPE_QUIC_EDGE + run_mode: TASK_RUN_MODE_BLOCKING_SERVICE + config_file: "tasks/quic_edge_task/quic_edge_task.pb.txt" + # Host-development default: no QUIC Gateway or physical media devices. + enable: true + } } diff --git a/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt new file mode 100644 index 00000000..fd4b000d --- /dev/null +++ b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt @@ -0,0 +1,65 @@ +quic_edge { + id: "quic_edge" + + # Task enablement is controlled by manager/task_manager.pb.txt. Configure a + # reachable QUIC Gateway and TLS policy before enabling the task there. + server_host: "quic-gateway.example.com" + server_port: 4433 + alpn: "cmvr-quic-edge/1" + node_id: "cmvr-edge" + software_version: "0.1" + + # The existing cmvr-es gRPC server remains the robot-control endpoint. "auto" + # selects a usable address from the interface snapshot sent at registration + # and on every heartbeat. + grpc_endpoint_host: "auto" + grpc_endpoint_port: 50052 + grpc_endpoint_tls: false + include_loopback_interfaces: false + + # Local heartbeat period. The Gateway keeps this value when its registration + # response returns heartbeat_interval_ms=0; a non-zero response overrides it. + heartbeat_interval_ms: 5000 + control_response_timeout_ms: 1000 + + tls { + ca_file: "certs/quic_gateway_ca.pem" + certificate_file: "certs/cmvr_edge_cert.pem" + private_key_file: "certs/cmvr_edge_key.pem" + server_name: "quic-gateway.example.com" + allow_insecure: false + } + + reconnect { + initial_delay_ms: 500 + maximum_delay_ms: 30000 + multiplier: 2.0 + jitter_percent: 20 + connect_timeout_ms: 5000 + } + + maximum_datagram_bytes: 1200 + maximum_control_frame_bytes: 1048576 + # With 1200-byte DATAGRAMs and a 512-entry queue, 524288 stays below + # the atomic batch capacity while reserving slots for control messages. + maximum_frame_bytes: 524288 + datagram_send_queue_depth: 512 + media_poll_interval_ms: 2 + + # Zero media tracks is valid and keeps registration, IP reporting and + # heartbeat active. Add tracks only for devices enabled in DeviceManager. + # tracks { + # track_id: 1 + # source_kind: SOURCE_KIND_CAMERA + # device_id: "right_hand_cam" + # source_track_id: "right_hand_cam/video/color" + # enable: true + # } + # tracks { + # track_id: 2 + # source_kind: SOURCE_KIND_MICROPHONE + # device_id: "mic1" + # source_track_id: "mic1/audio/main" + # enable: true + # } +} diff --git a/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt b/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt new file mode 100644 index 00000000..db9fe0e8 --- /dev/null +++ b/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt @@ -0,0 +1,34 @@ +self_collision_task { + id: "right_arm_self_collision" + arm_id: "mujoco_right_arm" + + checker { + urdf_path: "model/xiaoyan_description/dual_arm_collision.urdf" + + # Simplified compact-wrist bodies overlap in the normal assembled pose. + ignored_pairs { + first: "R_WRIST_P_S" + second: "R_WRIST_R_S" + } + } + + sampling { + max_geometry_displacement_m: 0.002 + max_check_period_s: 0.01 + } + + safety { + warning_distance_m: 0.02 + stop_distance_m: 0.005 + } + + + recovery { + clear_distance_m: 0.025 + stable_period_s: 0.1 + max_joint_velocity_rad_s: 0.15 + max_joint_acceleration_rad_s2: 0.3 + history_duration_s: 10.0 + max_distance_regression_m: 0.001 + } +} diff --git a/cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt b/cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt new file mode 100644 index 00000000..49f5cfb3 --- /dev/null +++ b/cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt @@ -0,0 +1,37 @@ +self_collision_task { + id: "gen2_right_arm_self_collision" + arm_id: "mujoco_right_arm" + + checker { + urdf_path: "model/gen2/collision/robot_collision.urdf" + + # These second-neighbor mounting bodies overlap in normal assembled poses. + ignored_pairs { + first: "arm_link_5_2" + second: "arm_link_7_2" + } + ignored_pairs { + first: "body_link" + second: "arm_link_2_2" + } + } + + sampling { + max_geometry_displacement_m: 0.002 + max_check_period_s: 0.01 + } + + safety { + warning_distance_m: 0.02 + stop_distance_m: 0.005 + } + + recovery { + clear_distance_m: 0.05 + stable_period_s: 0.1 + max_joint_velocity_rad_s: 0.3 + max_joint_acceleration_rad_s2: 5.0 + history_duration_s: 10.0 + max_distance_regression_m: 0.001 + } +} diff --git a/cmvr-es/devices/abstract_device.h b/cmvr-es/devices/abstract_device.h index e697fbeb..73768977 100644 --- a/cmvr-es/devices/abstract_device.h +++ b/cmvr-es/devices/abstract_device.h @@ -38,6 +38,20 @@ namespace cmvr::device { response_json = R"({"success":false,"error_message":"JSON command unsupported"})"; return false; } + + // This hook is sampled by DeviceManager while building heartbeats. It + // must be thread-safe and complete in bounded time while only copying + // in-memory state through atomics or a dedicated short-held state + // lock. Implementations must not perform device I/O, network requests, + // or wait on a lifecycle lock held across such I/O. + // + // The method is intentionally non-const because several legacy device + // categories expose non-const state getters. The returned object is a + // value and does not expose the device lifetime to callers. + virtual DeviceHealthSnapshot healthSnapshot() { + return {}; + } + protected: std::string id_; // 设备名称 }; diff --git a/cmvr-es/devices/agv/CMakeLists.txt b/cmvr-es/devices/agv/CMakeLists.txt index d3963c05..a0ca27e4 100644 --- a/cmvr-es/devices/agv/CMakeLists.txt +++ b/cmvr-es/devices/agv/CMakeLists.txt @@ -1,4 +1,5 @@ add_subdirectory(my_agv) +add_subdirectory(src1100) add_library(agv INTERFACE) @@ -7,6 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(agv INTERFACE cmvr_es::device::my_agv + cmvr_es::device::src1100_agv cmvr_es::proto ) diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 788ae2a1..b8ad4a61 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -6,32 +6,235 @@ #define CMVR_ES_ABSTRACT_AGV_H #pragma once +#include +#include +#include + +#include "common/types/agv/agv_types.h" #include "devices/abstract_device.h" -namespace cmvr::device{ - class AbstractAGV: public AbstractDevice { - public: - AbstractAGV() = default; - ~AbstractAGV() override=default; +namespace cmvr::device { - DeviceKind kind() const noexcept override { return DeviceKind::AGV; } - virtual bool getState(AGVState &state) { return true; } +/** + * @brief AGV/移动底盘设备抽象基类。 + * + * 该接口只描述通用 AGV 能力。控制器特有的请求/响应字段应放在具体 + * 驱动类中；当公共 API 需要扩展点时，可通过 AgvAdapterParams 传递。 + */ +class AbstractAGV : public AbstractDevice { +public: + AbstractAGV() = default; + ~AbstractAGV() override = default; - // navigation - virtual bool eStop() { return true; } - virtual bool goHome() { return true; } - virtual bool moveto(math::Pose2d &location, double speed_ratio) { return true; } - virtual bool setVelocity(math::Vec3 linear, math::Vec3 angular) { return true; } + DeviceKind kind() const noexcept override { return DeviceKind::AGV; } - // map - virtual bool initMap(float resolution, int width, int height) { return true; } - virtual bool updateMap() { return true; } - virtual bool saveMap(const std::string& file_path) { return true; } - virtual bool loadMap(const std::string& file_path) { return true; } + /** + * @brief 获取 AGV 运行状态快照。 + */ + virtual AgvRuntimeState runtimeState() const { return {}; } - protected: - AGVState state_; - }; -} + /** + * @brief 获取当前导航任务状态。 + */ + virtual AgvNavigationStatus navigationStatus() const { return {}; } -#endif //CMVR_ES_ABSTRACT_AGV_H + /** + * @brief 触发 AGV 急停行为。 + */ + virtual AgvResult emergencyStop() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "emergencyStop not implemented"); + } + + /** + * @brief 清除可恢复的 AGV 故障或告警。 + */ + virtual AgvResult clearFault() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "clearFault not implemented"); + } + + /** + * @brief 发起到世界/地图位姿的导航任务。 + */ + virtual AgvResult navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) + { + (void)pose; + (void)options; + (void)adapter_params; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToPose not implemented"); + } + + /** + * @brief 发起到指定地图站点的导航任务。 + */ + virtual AgvResult navigateToStation( + const std::string& station_id, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) + { + (void)station_id; + (void)options; + (void)adapter_params; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "navigateToStation not implemented"); + } + + /** + * @brief 发起显式站点到站点路径导航任务。 + */ + virtual AgvResult followPath(const std::vector& path) + { + (void)path; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented"); + } + + /** + * @brief 暂停当前导航任务，如果设备支持。 + */ + virtual AgvResult pauseNavigation() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "pauseNavigation not implemented"); + } + + /** + * @brief 恢复已暂停的导航任务，如果设备支持。 + */ + virtual AgvResult resumeNavigation() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "resumeNavigation not implemented"); + } + + /** + * @brief 取消当前导航任务，如果设备支持。 + */ + virtual AgvResult cancelNavigation() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "cancelNavigation not implemented"); + } + + /** + * @brief 向 AGV 下发低层速度控制指令。 + * + * 该接口不同于导航命令。具体实现应明确速度控制在导航过程中是中断 + * 导航、与导航共存，还是被拒绝执行。 + */ + virtual AgvResult setVelocity(const AgvVelocity& velocity) + { + (void)velocity; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "setVelocity not implemented"); + } + + /** + * @brief 通过下发零速度停止低层速度控制。 + * + * 该接口不表示取消正在执行的导航任务；取消导航请使用 + * cancelNavigation()。 + */ + virtual AgvResult stopVelocityControl() + { + return setVelocity(AgvVelocity{}); + } + + /** + * @brief 查询 AGV 可用地图名称列表。 + */ + virtual AgvResult listMaps(std::vector& maps) const + { + (void)maps; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listMaps not implemented"); + } + + /** + * @brief 查询当前活动地图中的站点列表。 + */ + virtual AgvResult listStations(std::vector& stations) const + { + (void)stations; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "listStations not implemented"); + } + + /** + * @brief 切换当前活动地图。 + */ + virtual AgvResult switchMap(const std::string& map_name) + { + (void)map_name; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "switchMap not implemented"); + } + + /** + * @brief 按名称上传或替换地图。 + */ + virtual AgvResult uploadMap(const std::string& map_name, const std::string& content) + { + (void)map_name; + (void)content; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "uploadMap not implemented"); + } + + /** + * @brief 按名称下载地图内容。 + */ + virtual AgvResult downloadMap(const std::string& map_name, std::string& content) const + { + (void)map_name; + (void)content; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "downloadMap not implemented"); + } + + /** + * @brief 开始扫图/建图。 + */ + virtual AgvResult startMapping(const AgvMappingOptions& options = {}) + { + (void)options; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "startMapping not implemented"); + } + + /** + * @brief 从指定下标开始获取厂商原始扫图数据。 + * + * 该接口主要保留给具体驱动内部使用。对外 gRPC 地图流应优先使用 + * getUnifiedMapUpdate()，避免把厂商文件格式暴露给上位机。 + */ + virtual AgvResult getMappingData(int start_index, AgvMappingData& data) const + { + (void)start_index; + (void)data; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getMappingData not implemented"); + } + + /** + * @brief 获取统一地图更新。 + * + * after_sequence 为 0 时通常返回最近可用的全量快照；大于 0 时返回 + * 指定序号之后的下一条更新。如果当前没有新地图，具体实现可在 + * options.wait_timeout_ms 内等待后台更新线程写入缓存。 + */ + virtual AgvResult getUnifiedMapUpdate( + std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const + { + (void)after_sequence; + (void)options; + (void)update; + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "getUnifiedMapUpdate not implemented"); + } + + /** + * @brief 停止扫图/建图。 + */ + virtual AgvResult stopMapping() + { + return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "stopMapping not implemented"); + } + +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ABSTRACT_AGV_H diff --git a/cmvr-es/devices/agv/agv_factory.h b/cmvr-es/devices/agv/agv_factory.h index b866face..79b2f0a3 100644 --- a/cmvr-es/devices/agv/agv_factory.h +++ b/cmvr-es/devices/agv/agv_factory.h @@ -7,6 +7,7 @@ #include "common/base/logging/logger.h" #include "devices/agv/abstract_agv.h" #include "devices/agv/my_agv/include/my_agv.h" +#include "devices/agv/src1100/include/src1100_agv.h" namespace cmvr::device { @@ -30,6 +31,16 @@ public: backend.set_id(cfg.id()); return std::make_shared(backend); } + case config::AGVDeviceConfig::kSrc1100Agv: + { + if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) { + CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id(); + return nullptr; + } + auto backend = cfg.src1100_agv(); + backend.set_id(cfg.id()); + return std::make_shared(backend); + } case config::AGVDeviceConfig::BACKEND_NOT_SET: default: diff --git a/cmvr-es/devices/agv/my_agv/include/my_agv.h b/cmvr-es/devices/agv/my_agv/include/my_agv.h index 8f0a788a..f8f69f9f 100644 --- a/cmvr-es/devices/agv/my_agv/include/my_agv.h +++ b/cmvr-es/devices/agv/my_agv/include/my_agv.h @@ -20,15 +20,13 @@ public: bool stop() override; bool update() override; - bool getState(AGVState& state) override; - bool eStop() override; - bool goHome() override; - bool moveto(math::Pose2d& location, double speed_ratio) override; - bool setVelocity(math::Vec3 linear, math::Vec3 angular) override; - bool initMap(float resolution, int width, int height) override; - bool updateMap() override; - bool saveMap(const std::string& file_path) override; - bool loadMap(const std::string& file_path) override; + AgvRuntimeState runtimeState() const override; + AgvResult emergencyStop() override; + AgvResult navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; + AgvResult setVelocity(const AgvVelocity& velocity) override; private: config::MyAgvConfig config_; diff --git a/cmvr-es/devices/agv/my_agv/src/my_agv.cpp b/cmvr-es/devices/agv/my_agv/src/my_agv.cpp index a1c6ea77..dd9b6b7a 100644 --- a/cmvr-es/devices/agv/my_agv/src/my_agv.cpp +++ b/cmvr-es/devices/agv/my_agv/src/my_agv.cpp @@ -27,49 +27,27 @@ bool MyAgv::update() return true; } -bool MyAgv::getState(AGVState&) +AgvRuntimeState MyAgv::runtimeState() const { - return true; + return {}; } -bool MyAgv::eStop() +AgvResult MyAgv::emergencyStop() { - return true; + return AgvResult::success(); } -bool MyAgv::goHome() +AgvResult MyAgv::navigateToPose( + const math::Pose2d&, + const AgvMotionOptions&, + const AgvAdapterParams&) { - return true; + return AgvResult::success(); } -bool MyAgv::moveto(math::Pose2d&, double) +AgvResult MyAgv::setVelocity(const AgvVelocity&) { - return true; -} - -bool MyAgv::setVelocity(math::Vec3, math::Vec3) -{ - return true; -} - -bool MyAgv::initMap(float, int, int) -{ - return true; -} - -bool MyAgv::updateMap() -{ - return true; -} - -bool MyAgv::saveMap(const std::string&) -{ - return true; -} - -bool MyAgv::loadMap(const std::string&) -{ - return true; + return AgvResult::success(); } } // namespace cmvr::device diff --git a/cmvr-es/devices/agv/src1100/CMakeLists.txt b/cmvr-es/devices/agv/src1100/CMakeLists.txt new file mode 100644 index 00000000..6ad1f3fe --- /dev/null +++ b/cmvr-es/devices/agv/src1100/CMakeLists.txt @@ -0,0 +1,12 @@ +add_library(src1100_agv SHARED src/src1100_agv.cpp) + +target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) + +target_link_libraries(src1100_agv + PUBLIC + cmvr_es::proto + jsoncpp +) + +add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv) +install(TARGETS src1100_agv LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h new file mode 100644 index 00000000..ab5bcb92 --- /dev/null +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -0,0 +1,179 @@ +#ifndef CMVR_ES_SRC1100_AGV_H +#define CMVR_ES_SRC1100_AGV_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/config/agv_config/agv_config.pb.h" +#include "devices/agv/abstract_agv.h" + +namespace cmvr::device { + +class Src1100Agv final : public AbstractAGV { +public: + explicit Src1100Agv(const config::Src1100AgvConfig& cfg); + ~Src1100Agv() override; + + std::string typeName() const override { return "Src1100Agv"; } + + bool init() override; + bool start() override; + bool stop() override; + bool update() override; + + AgvRuntimeState runtimeState() const override; + AgvNavigationStatus navigationStatus() const override; + + AgvResult emergencyStop() override; + AgvResult clearFault() override; + + AgvResult navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; + AgvResult navigateToStation( + const std::string& station_id, + const AgvMotionOptions& options = {}, + const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; + AgvResult followPath(const std::vector& path) override; + AgvResult pauseNavigation() override; + AgvResult resumeNavigation() override; + AgvResult cancelNavigation() override; + + AgvResult setVelocity(const AgvVelocity& velocity) override; + + AgvResult listMaps(std::vector& maps) const override; + AgvResult listStations(std::vector& stations) const override; + AgvResult switchMap(const std::string& map_name) override; + AgvResult uploadMap(const std::string& map_name, const std::string& content) override; + AgvResult downloadMap(const std::string& map_name, std::string& content) const override; + AgvResult startMapping(const AgvMappingOptions& options = {}) override; + AgvResult getMappingData(int start_index, AgvMappingData& data) const override; + AgvResult getUnifiedMapUpdate( + std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const override; + AgvResult stopMapping() override; + +private: + struct Ports { + int status{19204}; + int control{19205}; + int navigation{19206}; + int config{19207}; + int other{19210}; + int push{19301}; + }; + + AgvResult connect_(); + AgvResult disconnect_(); + AgvResult connectSocket_(int& sock, int port); + AgvResult ensureOtherSocket_(); + void closeSocket_(int& sock) const; + bool connected_() const; + + AgvResult sendCommand_(int sock, + std::uint16_t command, + const Json::Value& payload, + Json::Value* response) const; + AgvResult sendCommandRaw_(int sock, + std::uint16_t command, + const Json::Value& payload, + std::string* response_payload) const; + AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const; + AgvResult configurePush_(); + void startPushThread_(); + void stopPushThread_(); + void pushLoop_(); + AgvRuntimeState queryRuntimeState_() const; + void updateCachedRuntimeState_(const Json::Value& payload); + void startMapUpdateThread_(); + void stopMapUpdateThread_(); + void mapUpdateLoop_(); + AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const; + AgvResult parseMapFileToUpdates_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const; + AgvResult parseSrc1100MapArchive_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const; + AgvResult parseSrc1100Map2D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const; + AgvResult parseSrc1100Map3D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const; + void cacheMapUpdates_(std::vector updates) const; + bool findCachedMapUpdate_( + std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const; + bool mapUpdateMatches_( + const AgvUnifiedMapUpdate& update, + const AgvMapStreamOptions& options) const; + + static std::vector buildFrame_(std::uint16_t command, const std::string& payload); + static std::string toJsonString_(const Json::Value& value); + static bool parseJson_(const std::string& input, Json::Value& output, std::string& error); + static std::string extractJson_(const std::string& raw); + static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload); + static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback); + static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback); + static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options); + static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); + static AgvResult resultFromResponse_(const Json::Value& response); + + config::Src1100AgvConfig config_; + std::string ip_; + int recv_timeout_ms_{1000}; + Ports ports_; + bool state_push_enabled_{false}; + bool map_update_enabled_{false}; + int map_update_interval_ms_{1000}; + std::size_t map_update_history_size_{8}; + + mutable std::mutex mutex_; + int sock_status_{-1}; + int sock_control_{-1}; + int sock_navigation_{-1}; + int sock_config_{-1}; + int sock_other_{-1}; + int sock_push_{-1}; + std::string last_error_; + + std::atomic push_running_{false}; + std::thread push_thread_; + mutable std::mutex runtime_state_mutex_; + AgvRuntimeState cached_runtime_state_; + bool cached_runtime_state_valid_{false}; + + mutable std::atomic map_update_running_{false}; + mutable std::thread map_update_thread_; + mutable std::mutex map_update_mutex_; + mutable std::condition_variable map_update_cv_; + mutable std::deque cached_map_updates_; + mutable std::uint64_t map_sequence_{0}; + mutable int next_mapping_index_{0}; + mutable std::size_t last_map_content_hash_{0}; + mutable std::string map_session_id_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_SRC1100_AGV_H diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp new file mode 100644 index 00000000..7c1b1b2b --- /dev/null +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -0,0 +1,1898 @@ +#include "devices/agv/src1100/include/src1100_agv.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "common/base/logging/logger.h" +#include "rbk/protocol/src1100_map3d.pb.h" + +namespace cmvr::device { + +namespace { + +constexpr std::uint16_t kRobotStatusLoc = 1004; +constexpr std::uint16_t kRobotStatusBattery = 1007; +constexpr std::uint16_t kRobotStatusTask = 1020; +constexpr std::uint16_t kRobotStatusMap = 1300; +constexpr std::uint16_t kRobotStatusStation = 1301; +constexpr std::uint16_t kRobotStatusMappingFileList = 1780; +constexpr std::uint16_t kRobotStatusDownloadFile = 1800; +constexpr std::uint16_t kRobotControlStop = 2000; +constexpr std::uint16_t kRobotControlMotion = 2010; +constexpr std::uint16_t kRobotControlLoadMap = 2022; +constexpr std::uint16_t kRobotTaskPause = 3001; +constexpr std::uint16_t kRobotTaskResume = 3002; +constexpr std::uint16_t kRobotTaskCancel = 3003; +constexpr std::uint16_t kRobotTaskGoTarget = 3051; +constexpr std::uint16_t kRobotTaskGoTargetList = 3066; +constexpr std::uint16_t kRobotConfigUploadMap = 4010; +constexpr std::uint16_t kRobotConfigDownloadMap = 4011; +constexpr std::uint16_t kRobotOtherStartMapping = 6100; +constexpr std::uint16_t kRobotOtherStopMapping = 6101; +constexpr std::uint16_t kRobotPushConfigReq = 9300; +constexpr std::uint16_t kRobotPushConfigRes = 19300; +constexpr std::uint16_t kRobotPush = 19301; +constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U; +constexpr int kDefaultMapUpdateIntervalMs = 1000; +constexpr std::size_t kDefaultMapUpdateHistorySize = 8; +constexpr std::uint64_t kMapSnapshotSequenceStart = 1; + +namespace fs = std::filesystem; + +std::string systemError() +{ + return std::strerror(errno); +} + +bool wants2D(const AgvMapDimension dimension) +{ + return dimension == AgvMapDimension::Unspecified + || dimension == AgvMapDimension::Map2D + || dimension == AgvMapDimension::Map2DAnd3D; +} + +bool wants3D(const AgvMapDimension dimension) +{ + return dimension == AgvMapDimension::Unspecified + || dimension == AgvMapDimension::Map3D + || dimension == AgvMapDimension::Map2DAnd3D; +} + +bool contentLooksLikeZip(const std::string& content) +{ + return content.size() >= 4 + && static_cast(content[0]) == 0x50U + && static_cast(content[1]) == 0x4BU + && static_cast(content[2]) == 0x03U + && static_cast(content[3]) == 0x04U; +} + +bool contentLooksLikeJson(const std::string& content) +{ + const auto pos = content.find_first_not_of(" \t\r\n"); + return pos != std::string::npos && (content[pos] == '{' || content[pos] == '['); +} + +std::string shellQuote(const std::string& value) +{ + std::string quoted = "'"; + for (const char ch : value) { + if (ch == '\'') { + quoted += "'\\''"; + } else { + quoted += ch; + } + } + quoted += "'"; + return quoted; +} + +bool writeBinaryFile(const fs::path& path, const std::string& content) +{ + std::ofstream output(path, std::ios::binary); + if (!output) { + return false; + } + output.write(content.data(), static_cast(content.size())); + return output.good(); +} + +bool readBinaryFile(const fs::path& path, std::string& content) +{ + std::ifstream input(path, std::ios::binary); + if (!input) { + return false; + } + std::ostringstream buffer; + buffer << input.rdbuf(); + content = buffer.str(); + return true; +} + +fs::path makeTempDirectory() +{ + auto pattern = fs::temp_directory_path() / "cmvr_src1100_map_XXXXXX"; + std::string path = pattern.string(); + char* created = ::mkdtemp(path.data()); + if (!created) { + return {}; + } + return fs::path(created); +} + +Json::Value& jsonMember(Json::Value& value, const char* key) +{ + return *value.demand(key, key + std::strlen(key)); +} + +Json::Value& jsonMember(Json::Value& value, const std::string& key) +{ + return *value.demand(key.data(), key.data() + key.size()); +} + +const Json::Value* jsonFind(const Json::Value& value, const char* key) +{ + return value.find(key, key + std::strlen(key)); +} + +Json::Value jsonGet(const Json::Value& value, const char* key, const Json::Value& fallback) +{ + const auto* found = jsonFind(value, key); + return found ? *found : fallback; +} + +double nowSeconds() +{ + const auto now = std::chrono::system_clock::now().time_since_epoch(); + return std::chrono::duration(now).count(); +} + +bool jsonHas(const Json::Value& value, const char* key) +{ + return jsonFind(value, key) != nullptr; +} + +bool hasFaultArray(const Json::Value& value, const char* key) +{ + const auto* found = jsonFind(value, key); + return found && found->isArray() && !found->empty(); +} + +std::string jsonValueToString(const Json::Value& value) +{ + if (value.isString()) return value.asString(); + if (value.isBool()) return value.asBool() ? "true" : "false"; + if (value.isInt64() || value.isInt()) return std::to_string(value.asInt64()); + if (value.isUInt64() || value.isUInt()) return std::to_string(value.asUInt64()); + if (value.isDouble()) return std::to_string(value.asDouble()); + if (value.isNull()) return {}; + + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + return Json::writeString(builder, value); +} + +void putPropertyIfPresent( + std::unordered_map& properties, + const Json::Value& value, + const char* json_key, + const char* property_key) +{ + const auto* found = jsonFind(value, json_key); + if (!found || found->isNull()) { + return; + } + properties[property_key] = jsonValueToString(*found); +} + +void appendMapProperties( + std::unordered_map& properties, + const Json::Value& value, + const char* key) +{ + const auto* list = jsonFind(value, key); + if (!list || !list->isArray()) { + return; + } + + for (const auto& item : *list) { + const std::string property_key = jsonGet(item, "key", "").asString(); + if (property_key.empty()) { + continue; + } + + const char* value_keys[] = { + "string_value", + "bool_value", + "int32_value", + "uint32_value", + "int64_value", + "uint64_value", + "float_value", + "double_value", + "bytes_value", + "value" + }; + for (const char* value_key : value_keys) { + const auto* found = jsonFind(item, value_key); + if (found && !found->isNull()) { + properties[property_key] = jsonValueToString(*found); + break; + } + } + } +} + +AgvMapPoint3D jsonPoint3D(const Json::Value& value) +{ + AgvMapPoint3D point; + point.x = jsonGet(value, "x", 0.0).asDouble(); + point.y = jsonGet(value, "y", 0.0).asDouble(); + point.z = jsonGet(value, "z", 0.0).asDouble(); + return point; +} + +void appendObject( + AgvUnifiedMap2D& map, + std::string id, + const AgvMapObjectType type, + std::vector points, + const double heading, + const Json::Value& source) +{ + AgvMapObject object; + object.id = std::move(id); + object.type = type; + object.points = std::move(points); + object.heading = heading; + putPropertyIfPresent(object.properties, source, "class_name", "class_name"); + putPropertyIfPresent(object.properties, source, "type", "type"); + putPropertyIfPresent(object.properties, source, "description", "description"); + appendMapProperties(object.properties, source, "property"); + map.objects.push_back(std::move(object)); +} + +void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField& strings) +{ + if (strings.empty()) { + return; + } + Json::Value array(Json::arrayValue); + for (const auto& item : strings) { + array.append(item); + } + jsonMember(value, key) = array; +} + +AgvMode modeFromTaskState(const int state) +{ + switch (state) { + case 2: + return AgvMode::Auto; + case 3: + return AgvMode::Paused; + case 5: + return AgvMode::Fault; + case 6: + return AgvMode::Stopped; + default: + return AgvMode::Idle; + } +} + +AgvTaskState toTaskState(const int value) +{ + switch (value) { + case 1: + return AgvTaskState::Waiting; + case 2: + return AgvTaskState::Running; + case 3: + return AgvTaskState::Paused; + case 4: + return AgvTaskState::Completed; + case 5: + return AgvTaskState::Failed; + case 6: + return AgvTaskState::Canceled; + case 0: + default: + return AgvTaskState::None; + } +} + +AgvTaskType toTaskType(const int value) +{ + switch (value) { + case 1: + return AgvTaskType::NavigateToPose; + case 2: + return AgvTaskType::NavigateToStation; + case 3: + return AgvTaskType::FollowPath; + case 100: + return AgvTaskType::Custom; + default: + return AgvTaskType::None; + } +} + +} // namespace + +Src1100Agv::Src1100Agv(const config::Src1100AgvConfig& cfg) + : config_(cfg), + ip_(cfg.ip()), + recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000), + state_push_enabled_(cfg.enable_state_push()), + map_update_enabled_(cfg.enable_map_update()), + map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs), + map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize) +{ + id_ = cfg.id(); + if (cfg.port_status() > 0) ports_.status = cfg.port_status(); + if (cfg.port_control() > 0) ports_.control = cfg.port_control(); + if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav(); + if (cfg.port_config() > 0) ports_.config = cfg.port_config(); + if (cfg.port_other() > 0) ports_.other = cfg.port_other(); + if (cfg.port_push() > 0) ports_.push = cfg.port_push(); + + const auto result = connect_(); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[Src1100Agv] Auto connect failed" + << ", id=" << id_ + << ", ip=" << ip_ + << ", error=" << result.message; + } +} + +Src1100Agv::~Src1100Agv() +{ + (void)disconnect_(); +} + +bool Src1100Agv::init() +{ + return !id_.empty() && !ip_.empty(); +} + +bool Src1100Agv::start() +{ + return true; +} + +bool Src1100Agv::stop() +{ + return true; +} + +bool Src1100Agv::update() +{ + return true; +} + +AgvRuntimeState Src1100Agv::runtimeState() const +{ + if (state_push_enabled_) { + std::lock_guard lock(runtime_state_mutex_); + if (cached_runtime_state_valid_) { + auto state = cached_runtime_state_; + state.connected = connected_(); + state.last_error = last_error_; + if (!state.connected) { + state.mode = AgvMode::Disconnected; + } + return state; + } + } + + return queryRuntimeState_(); +} + +AgvRuntimeState Src1100Agv::queryRuntimeState_() const +{ + AgvRuntimeState state; + state.connected = connected_(); + state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; + state.last_error = last_error_; + + Json::Value loc; + if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) { + state.pose.x = jsonGet(loc, "x", 0.0).asDouble(); + state.pose.y = jsonGet(loc, "y", 0.0).asDouble(); + state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble(); + state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0; + state.current_station = jsonGet(loc, "current_station", "").asString(); + } + + Json::Value battery; + if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) { + state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble(); + state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble(); + state.battery.charging = jsonGet(battery, "charging", false).asBool(); + state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble(); + state.battery.current = jsonGet(battery, "current", 0.0).asDouble(); + } + + Json::Value map; + if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) { + state.current_map = jsonGet(map, "current_map", "").asString(); + } + + const auto nav = navigationStatus(); + state.moving = nav.state == AgvTaskState::Running; + state.fault = nav.state == AgvTaskState::Failed; + state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast(nav.state)); + return state; +} + +AgvNavigationStatus Src1100Agv::navigationStatus() const +{ + AgvNavigationStatus status; + Json::Value payload(Json::objectValue); + jsonMember(payload, "simple") = false; + + Json::Value response; + const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response); + if (!result.ok()) { + status.state = AgvTaskState::Failed; + status.message = result.message; + return status; + } + + status.state = toTaskState(jsonGet(response, "task_status", 0).asInt()); + status.type = toTaskType(jsonGet(response, "task_type", 0).asInt()); + status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString(); + if (const auto* task_status_package = jsonFind(response, "task_status_package")) { + status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble(); + } + return status; +} + +AgvResult Src1100Agv::connect_() +{ + stopPushThread_(); + stopMapUpdateThread_(); + + { + std::lock_guard lock(mutex_); + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + + if (ip_.empty()) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 AGV ip is empty"); + } + + const auto close_all = [this]() { + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + }; + + if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) { + close_all(); + return result; + } + + if (state_push_enabled_) { + const auto result = connectSocket_(sock_push_, ports_.push); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[Src1100Agv] Connect push port failed" + << ", id=" << id_ + << ", port=" << ports_.push + << ", error=" << result.message; + closeSocket_(sock_push_); + } + } + last_error_.clear(); + } + + if (state_push_enabled_ && sock_push_ >= 0) { + const auto result = configurePush_(); + if (result.ok()) { + startPushThread_(); + } else { + CMVR_LOG(ERROR) << "[Src1100Agv] Configure push failed" + << ", id=" << id_ + << ", error=" << result.message; + std::lock_guard lock(mutex_); + closeSocket_(sock_push_); + } + } + if (map_update_enabled_) { + startMapUpdateThread_(); + } + return AgvResult::success(); +} + +AgvResult Src1100Agv::disconnect_() +{ + stopMapUpdateThread_(); + stopPushThread_(); + std::lock_guard lock(mutex_); + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + return AgvResult::success(); +} + +AgvResult Src1100Agv::emergencyStop() +{ + return cancelNavigation(); +} + +AgvResult Src1100Agv::clearFault() +{ + return AgvResult::success(); +} + +AgvResult Src1100Agv::navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options, + const AgvAdapterParams& adapter_params) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); + jsonMember(payload, "id") = adapter_params.getString("target_id").value_or(""); + jsonMember(payload, "skill_name") = adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); + auto& free_go = jsonMember(payload, "freeGo"); + jsonMember(free_go, "x") = pose.x; + jsonMember(free_go, "y") = pose.y; + jsonMember(free_go, "theta") = pose.theta; + applyMotionOptions_(payload, options); + applyAdapterParams_(payload, adapter_params); + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::navigateToStation( + const std::string& station_id, + const AgvMotionOptions& options, + const AgvAdapterParams& adapter_params) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); + jsonMember(payload, "id") = station_id; + applyMotionOptions_(payload, options); + applyAdapterParams_(payload, adapter_params); + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::followPath(const std::vector& path) +{ + Json::Value payload(Json::objectValue); + Json::Value tasks(Json::arrayValue); + int index = 0; + for (const auto& segment : path) { + Json::Value task(Json::objectValue); + jsonMember(task, "task_id") = id_ + "_path_" + std::to_string(index++); + jsonMember(task, "source_id") = segment.source_station; + jsonMember(task, "id") = segment.target_station; + tasks.append(task); + } + jsonMember(payload, "move_task_list") = tasks; + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::pauseNavigation() +{ + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::resumeNavigation() +{ + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::cancelNavigation() +{ + Json::Value response; + auto result = sendCommand_(sock_navigation_, kRobotTaskCancel, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "vx") = velocity.vx; + jsonMember(payload, "vy") = velocity.vy; + jsonMember(payload, "w") = velocity.wz; + jsonMember(payload, "duration") = -1; + Json::Value response; + auto result = sendCommand_(sock_control_, kRobotControlMotion, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::listMaps(std::vector& maps) const +{ + Json::Value response; + auto result = sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &response); + if (!result.ok()) return result; + maps.clear(); + if (const auto* values = jsonFind(response, "maps"); values && values->isArray()) { + for (const auto& value : *values) { + maps.push_back(value.asString()); + } + } + return resultFromResponse_(response); +} + +AgvResult Src1100Agv::listStations(std::vector& stations) const +{ + Json::Value response; + auto result = sendCommand_(sock_status_, kRobotStatusStation, Json::Value(Json::objectValue), &response); + if (!result.ok()) return result; + stations.clear(); + if (const auto* values = jsonFind(response, "stations"); values && values->isArray()) { + for (const auto& value : *values) { + AgvStation station; + station.id = jsonGet(value, "id", "").asString(); + station.type = jsonGet(value, "type", "").asString(); + station.pose.x = jsonGet(value, "x", 0.0).asDouble(); + station.pose.y = jsonGet(value, "y", 0.0).asDouble(); + station.pose.theta = jsonGet(value, "r", 0.0).asDouble(); + station.description = jsonGet(value, "desc", "").asString(); + stations.push_back(station); + } + } + return resultFromResponse_(response); +} + +AgvResult Src1100Agv::switchMap(const std::string& map_name) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + Json::Value response; + auto result = sendCommand_(sock_control_, kRobotControlLoadMap, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::uploadMap(const std::string& map_name, const std::string& content) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + jsonMember(payload, "map_content") = content; + Json::Value response; + auto result = sendCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::downloadMap(const std::string& map_name, std::string& content) const +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + Json::Value response; + auto result = sendCommand_(sock_config_, kRobotConfigDownloadMap, payload, &response); + if (!result.ok()) return result; + content = jsonGet(response, "map_content", jsonGet(response, "content", "")).asString(); + return resultFromResponse_(response); +} + +AgvResult Src1100Agv::startMapping(const AgvMappingOptions& options) +{ + auto result = ensureOtherSocket_(); + if (!result.ok()) return result; + + Json::Value payload(Json::objectValue); + jsonMember(payload, "slam_type") = options.dimension == AgvMapDimension::Map2D ? 2 : 4; + jsonMember(payload, "real_time") = options.real_time; + if (!options.map_name.empty()) { + jsonMember(payload, "map_name") = options.map_name; + } + + Json::Value response; + result = sendCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); + result = result.ok() ? resultFromResponse_(response) : result; + if (result.ok()) { + { + std::lock_guard lock(map_update_mutex_); + cached_map_updates_.clear(); + next_mapping_index_ = 0; + last_map_content_hash_ = 0; + map_sequence_ = 0; + map_session_id_ = id_ + "_mapping_" + std::to_string(static_cast(nowSeconds() * 1000.0)); + } + if (map_update_enabled_ || options.real_time) { + startMapUpdateThread_(); + } + } + return result; +} + +AgvResult Src1100Agv::getMappingData(const int start_index, AgvMappingData& data) const +{ + if (start_index < 0) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "mapping data start_index must be >= 0"); + } + + Json::Value list_payload(Json::objectValue); + jsonMember(list_payload, "index") = start_index; + + Json::Value list_response; + auto result = sendCommand_(sock_status_, kRobotStatusMappingFileList, list_payload, &list_response); + if (!result.ok()) return result; + result = resultFromResponse_(list_response); + if (!result.ok()) return result; + + data = {}; + data.start_index = start_index; + data.next_index = start_index; + + const auto* list = jsonFind(list_response, "list"); + if (!list || !list->isArray()) { + return AgvResult::success(); + } + + for (const auto& item : *list) { + const std::string file_name = item.asString(); + if (file_name.empty()) { + continue; + } + + Json::Value download_payload(Json::objectValue); + jsonMember(download_payload, "type") = "users"; + jsonMember(download_payload, "file_path") = file_name; + + std::string content; + result = sendCommandRaw_(sock_status_, kRobotStatusDownloadFile, download_payload, &content); + if (!result.ok()) return result; + + Json::Value maybe_error; + std::string parse_error; + if (parseJson_(content, maybe_error, parse_error) && maybe_error.isObject()) { + result = resultFromResponse_(maybe_error); + if (!result.ok()) return result; + content = jsonGet(maybe_error, "content", jsonGet(maybe_error, "file_content", content)).asString(); + } + + AgvMappingDataFile file; + file.name = file_name; + file.content = std::move(content); + data.files.push_back(std::move(file)); + } + + data.next_index = data.start_index + static_cast(data.files.size()); + return AgvResult::success(); +} + +AgvResult Src1100Agv::getUnifiedMapUpdate( + const std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + if (findCachedMapUpdate_(after_sequence, options, update)) { + return AgvResult::success(); + } + + const auto refresh_result = refreshMapCacheOnce_(options); + if (findCachedMapUpdate_(after_sequence, options, update)) { + return AgvResult::success(); + } + if (!refresh_result.ok() && refresh_result.code != AgvErrorCode::Timeout) { + return refresh_result; + } + + const auto wait_ms = options.wait_timeout_ms > 0 ? options.wait_timeout_ms : 1000; + std::unique_lock lock(map_update_mutex_); + const auto effective_after = [&]() { + if (after_sequence != 0 || options.resume_token.empty()) { + return after_sequence; + } + try { + return static_cast(std::stoull(options.resume_token)); + } catch (...) { + return std::uint64_t{0}; + } + }(); + const auto find_locked = [&]() { + for (const auto& candidate : cached_map_updates_) { + if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { + update = candidate; + return true; + } + } + return false; + }; + + if (find_locked()) { + return AgvResult::success(); + } + const bool ready = map_update_cv_.wait_for( + lock, + std::chrono::milliseconds(wait_ms), + find_locked); + if (ready) { + return AgvResult::success(); + } + return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 unified map update timeout"); +} + +void Src1100Agv::startMapUpdateThread_() +{ + if (map_update_running_.exchange(true)) { + return; + } + map_update_thread_ = std::thread(&Src1100Agv::mapUpdateLoop_, this); +} + +void Src1100Agv::stopMapUpdateThread_() +{ + const bool was_running = map_update_running_.exchange(false); + if (was_running) { + map_update_cv_.notify_all(); + } + if (map_update_thread_.joinable()) { + map_update_thread_.join(); + } +} + +void Src1100Agv::mapUpdateLoop_() +{ + while (map_update_running_) { + AgvMapStreamOptions options; + options.dimension = AgvMapDimension::Map2DAnd3D; + options.snapshot = true; + options.incremental = true; + options.wait_timeout_ms = 0; + + const auto result = refreshMapCacheOnce_(options); + if (!result.ok() && result.code != AgvErrorCode::Timeout) { + std::lock_guard lock(mutex_); + last_error_ = result.message; + } + + std::unique_lock lock(map_update_mutex_); + map_update_cv_.wait_for( + lock, + std::chrono::milliseconds(map_update_interval_ms_), + [this]() { return !map_update_running_; }); + } +} + +AgvResult Src1100Agv::refreshMapCacheOnce_(const AgvMapStreamOptions& options) const +{ + int start_index = 0; + { + std::lock_guard lock(map_update_mutex_); + start_index = next_mapping_index_; + } + + AgvMappingData mapping_data; + auto result = getMappingData(start_index, mapping_data); + if (result.ok() && !mapping_data.files.empty()) { + std::vector updates; + for (const auto& file : mapping_data.files) { + std::vector file_updates; + const auto parse_result = parseMapFileToUpdates_(file.name, file.content, options, file_updates); + if (!parse_result.ok()) { + CMVR_LOG(ERROR) << "[Src1100Agv] Parse mapping file failed" + << ", id=" << id_ + << ", file=" << file.name + << ", error=" << parse_result.message; + continue; + } + updates.insert( + updates.end(), + std::make_move_iterator(file_updates.begin()), + std::make_move_iterator(file_updates.end())); + } + { + std::lock_guard lock(map_update_mutex_); + next_mapping_index_ = std::max(next_mapping_index_, mapping_data.next_index); + } + if (!updates.empty()) { + cacheMapUpdates_(std::move(updates)); + return AgvResult::success(); + } + } + + std::string map_name = options.map_name; + if (map_name.empty()) { + const auto state = runtimeState(); + map_name = state.current_map; + } + if (map_name.empty()) { + std::vector maps; + if (listMaps(maps).ok() && !maps.empty()) { + map_name = maps.back(); + } + } + if (map_name.empty()) { + return result.ok() + ? AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 no map file is available") + : result; + } + + std::string content; + result = downloadMap(map_name, content); + if (!result.ok()) { + return result; + } + const auto content_hash = std::hash{}(content); + std::size_t last_map_content_hash = 0; + { + std::lock_guard lock(map_update_mutex_); + last_map_content_hash = last_map_content_hash_; + } + AgvUnifiedMapUpdate cached; + if (content_hash == last_map_content_hash && findCachedMapUpdate_(0, options, cached)) { + return AgvResult::success(); + } + + std::vector updates; + result = parseMapFileToUpdates_(map_name, content, options, updates); + if (!result.ok()) { + return result; + } + if (updates.empty()) { + return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 map file has no requested dimension"); + } + + { + std::lock_guard lock(map_update_mutex_); + last_map_content_hash_ = content_hash; + } + cacheMapUpdates_(std::move(updates)); + return AgvResult::success(); +} + +AgvResult Src1100Agv::parseMapFileToUpdates_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const +{ + if (content.empty()) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 map file is empty: " + file_name); + } + + if (contentLooksLikeZip(content)) { + return parseSrc1100MapArchive_(file_name, content, options, updates); + } + + if (contentLooksLikeJson(content)) { + if (wants2D(options.dimension)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map2D_(file_name, content, options, update); + if (!result.ok()) { + return result; + } + updates.push_back(std::move(update)); + } + return AgvResult::success(); + } + + if (wants3D(options.dimension)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map3D_(file_name, content, options, update); + if (!result.ok()) { + return result; + } + updates.push_back(std::move(update)); + return AgvResult::success(); + } + + return AgvResult::success(); +} + +AgvResult Src1100Agv::parseSrc1100MapArchive_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const +{ + const auto temp_dir = makeTempDirectory(); + if (temp_dir.empty()) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "create temporary map directory failed: " + systemError()); + } + + const auto archive_path = temp_dir / "map.smap"; + if (!writeBinaryFile(archive_path, content)) { + fs::remove_all(temp_dir); + return AgvResult::failure(AgvErrorCode::CommandFailed, "write temporary map archive failed"); + } + + const std::string command = "unzip -qq -o " + + shellQuote(archive_path.string()) + + " -d " + + shellQuote(temp_dir.string()); + const int unzip_result = std::system(command.c_str()); + if (unzip_result != 0) { + fs::remove_all(temp_dir); + return AgvResult::failure(AgvErrorCode::CommandFailed, "unzip SRC1100 smap archive failed: " + file_name); + } + + if (wants2D(options.dimension)) { + std::string map2d_content; + if (readBinaryFile(temp_dir / "0.smap", map2d_content)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map2D_(file_name, map2d_content, options, update); + if (result.ok()) { + updates.push_back(std::move(update)); + } else { + CMVR_LOG(ERROR) << "[Src1100Agv] Parse 0.smap failed" + << ", id=" << id_ + << ", file=" << file_name + << ", error=" << result.message; + } + } + } + + if (wants3D(options.dimension)) { + std::string map3d_content; + if (readBinaryFile(temp_dir / "0.3dsmap", map3d_content)) { + AgvUnifiedMapUpdate update; + const auto result = parseSrc1100Map3D_(file_name, map3d_content, options, update); + if (result.ok()) { + updates.push_back(std::move(update)); + } else { + CMVR_LOG(ERROR) << "[Src1100Agv] Parse 0.3dsmap failed" + << ", id=" << id_ + << ", file=" << file_name + << ", error=" << result.message; + } + } + } + + fs::remove_all(temp_dir); + return updates.empty() + ? AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 smap archive has no requested map data: " + file_name) + : AgvResult::success(); +} + +AgvResult Src1100Agv::parseSrc1100Map2D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + Json::Value root; + std::string error; + if (!parseJson_(content, root, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SRC1100 2D map json failed: " + error); + } + if (!root.isObject()) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 2D map json root is not object"); + } + + const auto* header_ptr = jsonFind(root, "header"); + const Json::Value& header = header_ptr && header_ptr->isObject() ? *header_ptr : root; + + AgvUnifiedMap2D map; + map.frame_id = "map"; + map.timestamp = nowSeconds(); + map.resolution = jsonGet(header, "resolution", 0.0).asDouble(); + if (const auto* min_pos = jsonFind(header, "min_pos")) { + map.origin.x = jsonGet(*min_pos, "x", 0.0).asDouble(); + map.origin.y = jsonGet(*min_pos, "y", 0.0).asDouble(); + map.origin.theta = 0.0; + } + if (const auto* max_pos = jsonFind(header, "max_pos"); + max_pos && map.resolution > 0.0) { + const double width_m = jsonGet(*max_pos, "x", map.origin.x).asDouble() - map.origin.x; + const double height_m = jsonGet(*max_pos, "y", map.origin.y).asDouble() - map.origin.y; + if (width_m > 0.0 && height_m > 0.0) { + map.width = static_cast(std::ceil(width_m / map.resolution)); + map.height = static_cast(std::ceil(height_m / map.resolution)); + } + } + + const auto make_id = [](const Json::Value& value, const char* prefix, const int index) { + std::string id = jsonGet(value, "instance_name", "").asString(); + if (id.empty()) id = jsonGet(value, "id", "").asString(); + if (id.empty()) id = jsonGet(value, "name", "").asString(); + if (id.empty()) id = jsonGet(value, "point_name", "").asString(); + if (id.empty() && jsonHas(value, "tag_value")) { + id = std::to_string(jsonGet(value, "tag_value", 0).asUInt()); + } + if (id.empty()) id = std::string(prefix) + "_" + std::to_string(index); + return id; + }; + + if (const auto* list = jsonFind(root, "advanced_point_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + const auto* pos = jsonFind(item, "pos"); + appendObject( + map, + make_id(item, "station", index++), + AgvMapObjectType::Station, + pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, + jsonGet(item, "dir", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "normal_line_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* start = jsonFind(item, "start_pos")) points.push_back(jsonPoint3D(*start)); + if (const auto* end = jsonFind(item, "end_pos")) points.push_back(jsonPoint3D(*end)); + appendObject(map, make_id(item, "normal_line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_line_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* line = jsonFind(item, "line")) { + if (const auto* start = jsonFind(*line, "start_pos")) points.push_back(jsonPoint3D(*start)); + if (const auto* end = jsonFind(*line, "end_pos")) points.push_back(jsonPoint3D(*end)); + } + appendObject(map, make_id(item, "line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_curve_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* start = jsonFind(item, "start_pos")) { + if (const auto* pos = jsonFind(*start, "pos")) points.push_back(jsonPoint3D(*pos)); + } + if (const auto* control = jsonFind(item, "control_pos1")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos2")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos3")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos4")) points.push_back(jsonPoint3D(*control)); + if (const auto* end = jsonFind(item, "end_pos")) { + if (const auto* pos = jsonFind(*end, "pos")) points.push_back(jsonPoint3D(*pos)); + } + appendObject(map, make_id(item, "curve", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_area_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* pos_group = jsonFind(item, "pos_group"); pos_group && pos_group->isArray()) { + for (const auto& pos : *pos_group) points.push_back(jsonPoint3D(pos)); + } + appendObject( + map, + make_id(item, "area", index++), + AgvMapObjectType::Area, + std::move(points), + jsonGet(item, "dir", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "reflector_pos_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "reflector", index++), + AgvMapObjectType::Reflector, + {jsonPoint3D(item)}, + 0.0, + item); + } + } + + if (const auto* list = jsonFind(root, "tag_pos_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "tag", index++), + AgvMapObjectType::QrTag, + {jsonPoint3D(item)}, + jsonGet(item, "angle", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "external_device_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "external_device", index++), + AgvMapObjectType::ExternalDevice, + {}, + 0.0, + item); + } + } + + if (const auto* groups = jsonFind(root, "bin_locations_list"); groups && groups->isArray()) { + int index = 0; + for (const auto& group : *groups) { + const auto* list = jsonFind(group, "bin_location_list"); + if (!list || !list->isArray()) { + continue; + } + for (const auto& item : *list) { + const auto* pos = jsonFind(item, "pos"); + appendObject( + map, + make_id(item, "bin_location", index++), + AgvMapObjectType::BinLocation, + pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, + 0.0, + item); + } + } + } + + std::string map_id = options.map_name; + if (map_id.empty()) map_id = jsonGet(header, "map_name", "").asString(); + if (map_id.empty()) map_id = file_name; + + update = {}; + update.map_id = map_id; + update.dimension = AgvMapDimension::Map2D; + update.update_type = AgvMapUpdateType::Snapshot; + update.frame_id = map.frame_id; + update.timestamp = map.timestamp; + update.snapshot_begin = true; + update.snapshot_end = true; + update.chunk_index = 0; + update.chunk_count = 1; + update.map_2d = std::move(map); + return AgvResult::success(); +} + +AgvResult Src1100Agv::parseSrc1100Map3D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + rbk::protocol::Message_Map3D src; + if (!src.ParseFromString(content)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SRC1100 3D map protobuf failed: " + file_name); + } + + AgvUnifiedMap3D map; + map.frame_id = "map"; + map.timestamp = nowSeconds(); + if (src.has_feature_map_3d() && src.feature_map_3d().has_params()) { + map.voxel_resolution = src.feature_map_3d().params().max_voxel_size(); + } else if (src.has_header()) { + map.voxel_resolution = src.header().resolution(); + } + + map.points.reserve(static_cast(src.normal_pos3d_list_size())); + for (const auto& point : src.normal_pos3d_list()) { + AgvMapPointSample3D sample; + sample.x = point.x(); + sample.y = point.y(); + sample.z = point.z(); + map.points.push_back(sample); + } + + if (src.has_feature_map_3d()) { + const auto& feature_map = src.feature_map_3d(); + map.planes.reserve(static_cast(feature_map.planes_size())); + for (const auto& plane : feature_map.planes()) { + AgvMapPlane3D dst; + dst.center = {plane.center().x(), plane.center().y(), plane.center().z()}; + dst.normal = {plane.normal().x(), plane.normal().y(), plane.normal().z()}; + dst.d = plane.d(); + dst.radius = plane.radius(); + map.planes.push_back(dst); + } + + map.voxels.reserve(static_cast(feature_map.voxel_locs_size())); + for (const auto& voxel : feature_map.voxel_locs()) { + AgvMapVoxel3D dst; + dst.x = voxel.x(); + dst.y = voxel.y(); + dst.z = voxel.z(); + dst.probability = 1.0F; + map.voxels.push_back(dst); + } + } + + std::string map_id = options.map_name; + if (map_id.empty() && src.has_header()) map_id = src.header().map_name(); + if (map_id.empty()) map_id = src.map_directory(); + if (map_id.empty()) map_id = file_name; + + update = {}; + update.map_id = map_id; + update.dimension = AgvMapDimension::Map3D; + update.update_type = AgvMapUpdateType::Snapshot; + update.frame_id = map.frame_id; + update.timestamp = map.timestamp; + update.snapshot_begin = true; + update.snapshot_end = true; + update.chunk_index = 0; + update.chunk_count = 1; + update.map_3d = std::move(map); + return AgvResult::success(); +} + +void Src1100Agv::cacheMapUpdates_(std::vector updates) const +{ + if (updates.empty()) { + return; + } + + { + std::lock_guard lock(map_update_mutex_); + if (map_session_id_.empty()) { + map_session_id_ = id_ + "_map"; + } + if (map_sequence_ == 0) { + map_sequence_ = kMapSnapshotSequenceStart - 1; + } + for (auto& update : updates) { + update.sequence = ++map_sequence_; + update.session_id = map_session_id_; + update.resume_token = std::to_string(update.sequence); + if (update.timestamp <= 0.0) update.timestamp = nowSeconds(); + if (update.frame_id.empty()) update.frame_id = "map"; + if (update.map_id.empty()) update.map_id = id_; + if (update.update_type == AgvMapUpdateType::Unspecified) { + update.update_type = AgvMapUpdateType::Snapshot; + } + cached_map_updates_.push_back(std::move(update)); + } + while (cached_map_updates_.size() > map_update_history_size_) { + cached_map_updates_.pop_front(); + } + } + map_update_cv_.notify_all(); +} + +bool Src1100Agv::findCachedMapUpdate_( + const std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + std::uint64_t effective_after = after_sequence; + if (effective_after == 0 && !options.resume_token.empty()) { + try { + effective_after = static_cast(std::stoull(options.resume_token)); + } catch (...) { + effective_after = 0; + } + } + + std::lock_guard lock(map_update_mutex_); + for (const auto& candidate : cached_map_updates_) { + if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { + update = candidate; + return true; + } + } + return false; +} + +bool Src1100Agv::mapUpdateMatches_( + const AgvUnifiedMapUpdate& update, + const AgvMapStreamOptions& options) const +{ + if (!options.map_name.empty() && update.map_id != options.map_name) { + return false; + } + + switch (options.dimension) { + case AgvMapDimension::Map2D: + return update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value(); + case AgvMapDimension::Map3D: + return update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value(); + case AgvMapDimension::Map2DAnd3D: + return (update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value()) + || (update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value()); + case AgvMapDimension::Unspecified: + default: + return update.map_2d.has_value() || update.map_3d.has_value(); + } +} + +AgvResult Src1100Agv::stopMapping() +{ + auto result = ensureOtherSocket_(); + if (!result.ok()) return result; + + Json::Value response; + result = sendCommand_(sock_other_, kRobotOtherStopMapping, Json::Value(Json::objectValue), &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult Src1100Agv::connectSocket_(int& sock, const int port) +{ + sock = ::socket(AF_INET, SOCK_STREAM, 0); + if (sock < 0) { + last_error_ = "create socket failed: " + systemError(); + return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(static_cast(port)); + if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) { + closeSocket_(sock); + last_error_ = "invalid SRC1100 ip: " + ip_; + return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_); + } + + if (::connect(sock, reinterpret_cast(&address), sizeof(address)) < 0) { + closeSocket_(sock); + last_error_ = "connect SRC1100 port " + std::to_string(port) + " failed: " + systemError(); + return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); + } + + timeval timeout{}; + timeout.tv_sec = recv_timeout_ms_ / 1000; + timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000; + ::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + return AgvResult::success(); +} + +AgvResult Src1100Agv::ensureOtherSocket_() +{ + std::lock_guard lock(mutex_); + if (sock_other_ >= 0) { + return AgvResult::success(); + } + return connectSocket_(sock_other_, ports_.other); +} + +void Src1100Agv::closeSocket_(int& sock) const +{ + if (sock >= 0) { + ::close(sock); + sock = -1; + } +} + +bool Src1100Agv::connected_() const +{ + return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0; +} + +AgvResult Src1100Agv::sendCommand_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + Json::Value* response) const +{ + std::string response_payload; + auto result = sendCommandRaw_(sock, command, payload, &response_payload); + if (!result.ok()) { + return result; + } + if (!response) { + return AgvResult::success(); + } + + Json::Value parsed; + std::string error; + if (!parseJson_(response_payload, parsed, error)) { + const std::string json_text = extractJson_(response_payload); + if (json_text.empty() || !parseJson_(json_text, parsed, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, error); + } + } + + *response = std::move(parsed); + return AgvResult::success(); +} + +AgvResult Src1100Agv::sendCommandRaw_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + std::string* response_payload) const +{ + std::lock_guard lock(mutex_); + if (sock < 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket not connected"); + } + + const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload); + const auto frame = buildFrame_(command, payload_text); + if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError()); + } + + std::uint16_t response_command = 0; + std::string payload_text_response; + const auto result = receiveFrame_(sock, response_command, payload_text_response); + if (!result.ok()) { + return result; + } + (void)response_command; + if (response_payload) { + *response_payload = std::move(payload_text_response); + } + return AgvResult::success(); +} + +AgvResult Src1100Agv::sendCommandNoResponse_( + const int sock, + const std::uint16_t command, + const Json::Value& payload) const +{ + return sendCommand_(sock, command, payload, nullptr); +} + +AgvResult Src1100Agv::configurePush_() +{ + if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 push included_fields and excluded_fields cannot both be set"); + } + + Json::Value payload(Json::objectValue); + if (config_.state_push_interval_ms() > 0) { + jsonMember(payload, "interval") = config_.state_push_interval_ms(); + } + appendStringArray(payload, "included_fields", config_.state_push_included_fields()); + appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields()); + + if (payload.empty()) { + return AgvResult::success(); + } + + const std::string payload_text = toJsonString_(payload); + const auto frame = buildFrame_(kRobotPushConfigReq, payload_text); + + std::lock_guard lock(mutex_); + if (sock_push_ < 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 push socket not connected"); + } + if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send push config failed: " + systemError()); + } + + while (true) { + std::uint16_t command = 0; + std::string response_payload; + const auto result = receiveFrame_(sock_push_, command, response_payload); + if (!result.ok()) { + return result; + } + + Json::Value response; + std::string error; + if (!response_payload.empty() && !parseJson_(response_payload, response, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, error); + } + + if (command == kRobotPushConfigRes) { + return resultFromResponse_(response); + } + if (command == kRobotPush && response.isObject()) { + updateCachedRuntimeState_(response); + } + } +} + +void Src1100Agv::startPushThread_() +{ + if (!state_push_enabled_) { + return; + } + if (push_running_.exchange(true)) { + return; + } + if (sock_push_ < 0) { + push_running_ = false; + return; + } + push_thread_ = std::thread(&Src1100Agv::pushLoop_, this); +} + +void Src1100Agv::stopPushThread_() +{ + const bool was_running = push_running_.exchange(false); + if (was_running) { + int sock = -1; + { + std::lock_guard lock(mutex_); + sock = sock_push_; + } + if (sock >= 0) { + ::shutdown(sock, SHUT_RDWR); + } + } + if (push_thread_.joinable()) { + push_thread_.join(); + } +} + +void Src1100Agv::pushLoop_() +{ + while (push_running_) { + int sock = -1; + { + std::lock_guard lock(mutex_); + sock = sock_push_; + } + if (sock < 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + + std::uint16_t command = 0; + std::string payload; + const auto result = receiveFrame_(sock, command, payload); + if (!push_running_) { + break; + } + if (!result.ok()) { + if (result.code != AgvErrorCode::Timeout) { + std::lock_guard lock(mutex_); + last_error_ = result.message; + } + continue; + } + if (command != kRobotPush || payload.empty()) { + continue; + } + + Json::Value parsed; + std::string error; + if (!parseJson_(payload, parsed, error)) { + std::lock_guard lock(mutex_); + last_error_ = error; + continue; + } + updateCachedRuntimeState_(parsed); + } +} + +void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) +{ + std::lock_guard lock(runtime_state_mutex_); + auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{}; + state.timestamp = nowSeconds(); + state.connected = true; + state.last_error.clear(); + + if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble(); + if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble(); + if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble(); + if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble(); + if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble(); + if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble(); + if (jsonHas(payload, "battery_level")) { + state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble(); + } + if (jsonHas(payload, "battery_temp")) { + state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble(); + } + if (jsonHas(payload, "charging")) { + state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool(); + } + if (jsonHas(payload, "voltage")) { + state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble(); + } + if (jsonHas(payload, "current")) { + state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble(); + } + if (jsonHas(payload, "current_map")) { + state.current_map = jsonGet(payload, "current_map", state.current_map).asString(); + } + if (jsonHas(payload, "current_station")) { + state.current_station = jsonGet(payload, "current_station", state.current_station).asString(); + } + if (jsonHas(payload, "confidence")) { + state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0; + } + if (jsonHas(payload, "emergency")) { + state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool(); + } + + state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; + state.fault = hasFaultArray(payload, "fatals") || hasFaultArray(payload, "errors"); + if (state.emergency_stopped) { + state.mode = AgvMode::EmergencyStop; + } else if (state.fault) { + state.mode = AgvMode::Fault; + } else if (state.battery.charging) { + state.mode = AgvMode::Charging; + } else if (state.moving) { + state.mode = AgvMode::Auto; + } else { + state.mode = AgvMode::Idle; + } + + cached_runtime_state_ = state; + cached_runtime_state_valid_ = true; +} + +std::vector Src1100Agv::buildFrame_( + const std::uint16_t command, + const std::string& payload) +{ + std::vector frame(16 + payload.size(), 0); + frame[0] = 0x5A; + frame[1] = 0x01; + frame[2] = 0x00; + frame[3] = 0x01; + const auto length = static_cast(payload.size()); + frame[4] = static_cast((length >> 24U) & 0xFFU); + frame[5] = static_cast((length >> 16U) & 0xFFU); + frame[6] = static_cast((length >> 8U) & 0xFFU); + frame[7] = static_cast(length & 0xFFU); + frame[8] = static_cast((command >> 8U) & 0xFFU); + frame[9] = static_cast(command & 0xFFU); + std::copy(payload.begin(), payload.end(), frame.begin() + 16); + return frame; +} + +std::string Src1100Agv::toJsonString_(const Json::Value& value) +{ + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + return Json::writeString(builder, value); +} + +bool Src1100Agv::parseJson_(const std::string& input, Json::Value& output, std::string& error) +{ + Json::CharReaderBuilder builder; + std::unique_ptr reader(builder.newCharReader()); + return reader->parse(input.data(), input.data() + input.size(), &output, &error); +} + +std::string Src1100Agv::extractJson_(const std::string& raw) +{ + const auto begin = raw.find('{'); + const auto end = raw.rfind('}'); + if (begin == std::string::npos || end == std::string::npos || end < begin) { + return {}; + } + return raw.substr(begin, end - begin + 1); +} + +AgvResult Src1100Agv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload) +{ + const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult { + std::size_t offset = 0; + while (offset < size) { + const ssize_t count = ::recv(fd, data + offset, size - offset, 0); + if (count > 0) { + offset += static_cast(count); + continue; + } + if (count == 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket closed"); + } + if (errno == EINTR) { + continue; + } + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 receive timeout"); + } + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 receive failed: " + systemError()); + } + return AgvResult::success(); + }; + + std::uint8_t header[16]{}; + auto result = recv_exact(sock, header, sizeof(header)); + if (!result.ok()) { + return result; + } + if (header[0] != 0x5A) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 frame header is invalid"); + } + + const auto length = (static_cast(header[4]) << 24U) + | (static_cast(header[5]) << 16U) + | (static_cast(header[6]) << 8U) + | static_cast(header[7]); + command = static_cast((static_cast(header[8]) << 8U) | header[9]); + payload.clear(); + if (length == 0) { + return AgvResult::success(); + } + if (length > kMaxFramePayloadBytes) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 frame payload is too large"); + } + + std::vector buffer(length); + result = recv_exact(sock, buffer.data(), buffer.size()); + if (!result.ok()) { + return result; + } + payload.assign(reinterpret_cast(buffer.data()), buffer.size()); + return AgvResult::success(); +} + +int Src1100Agv::optionalInt_(const AgvAdapterParams& params, const std::string& key, const int fallback) +{ + const auto value = params.getDouble(key); + return value ? static_cast(*value) : fallback; +} + +double Src1100Agv::optionalDouble_(const AgvAdapterParams& params, const std::string& key, const double fallback) +{ + const auto value = params.getDouble(key); + return value ? *value : fallback; +} + +void Src1100Agv::applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options) +{ + if (options.max_speed > 0.0) jsonMember(payload, "max_speed") = options.max_speed; + if (options.max_angular_speed > 0.0) jsonMember(payload, "max_wspeed") = options.max_angular_speed; + if (options.max_acceleration > 0.0) jsonMember(payload, "max_acc") = options.max_acceleration; + if (options.max_angular_acceleration > 0.0) jsonMember(payload, "max_wacc") = options.max_angular_acceleration; + if (options.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance; + if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle; +} + +void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params) +{ + for (const auto& [key, value] : params.values) { + if (key.rfind("port_", 0) == 0) { + continue; + } + jsonMember(payload, key) = value; + } + jsonMember(payload, "jack_height") = optionalDouble_( + params, + "jack_height", + jsonGet(payload, "jack_height", 0.0).asDouble()); +} + +AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) +{ + const int ret_code = jsonGet(response, "ret_code", 0).asInt(); + const std::string message = jsonGet(response, "err_msg", "").asString(); + if (ret_code == 0) { + return AgvResult::success(); + } + return AgvResult::failure(AgvErrorCode::CommandFailed, + message.empty() ? "SRC1100 command failed: " + std::to_string(ret_code) : message); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index 0a98cfe5..d56d09cb 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -1,13 +1,61 @@ add_library(aubo_arm SHARED - src/aubo_arm.cpp + aubo_arm.cpp ) target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include) -set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib) +set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1) +set(AUBO_SDK_INCLUDE_DIR ${AUBO_SDK_ROOT}/include) +set(AUBO_SDK_LIB_DIR ${AUBO_SDK_ROOT}/lib) -if (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h") +if (EXISTS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk/aubo_sdkConfig.cmake") + list(APPEND CMAKE_PREFIX_PATH "${AUBO_SDK_LIB_DIR}/cmake") + find_package(Qt5Core QUIET) + if (NOT Qt5Core_FOUND AND NOT TARGET Qt5::Core) + find_library(QT5_CORE_LIBRARY + NAMES Qt5Core libQt5Core.so.5 + PATHS /lib /usr/lib /usr/local/lib /lib/x86_64-linux-gnu /usr/lib/x86_64-linux-gnu + ) + if (QT5_CORE_LIBRARY) + add_library(Qt5::Core UNKNOWN IMPORTED) + set_target_properties(Qt5::Core PROPERTIES + IMPORTED_LOCATION "${QT5_CORE_LIBRARY}" + ) + endif() + endif() + find_package(aubo_sdk REQUIRED CONFIG PATHS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk" NO_DEFAULT_PATH) + + # The vendor directory contains an old private libstdc++. Keep it out of + # consumers' RUNPATH by staging only the AUBO runtime libraries. + set(AUBO_CLEAN_LIB_DIR "${CMAKE_CURRENT_BINARY_DIR}/aubo_sdk_runtime") + file(MAKE_DIRECTORY "${AUBO_CLEAN_LIB_DIR}") + foreach(AUBO_LIB + libaubo_sdk.so + libaubo_sdkd.so + librobot_proxy.so + librobot_proxyd.so) + file(COPY_FILE + "${AUBO_SDK_LIB_DIR}/${AUBO_LIB}" + "${AUBO_CLEAN_LIB_DIR}/${AUBO_LIB}" + ONLY_IF_DIFFERENT + ) + endforeach() + + set_target_properties(aubo_sdk::aubo_sdk aubo_sdk::robot_proxy PROPERTIES + MAP_IMPORTED_CONFIG_DEBUG Release + ) + set_target_properties(aubo_sdk::aubo_sdk PROPERTIES + IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/libaubo_sdk.so" + IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/libaubo_sdkd.so" + ) + set_target_properties(aubo_sdk::robot_proxy PROPERTIES + IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/librobot_proxy.so" + IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/librobot_proxyd.so" + ) + target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK) + target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR}) + target_link_libraries(aubo_arm PRIVATE aubo_sdk::aubo_sdk) +elseif (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h") target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK) target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR}) if (EXISTS "${AUBO_SDK_LIB_DIR}") diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp new file mode 100644 index 00000000..41dedc9f --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -0,0 +1,1065 @@ +#include "devices/arm/aubo_arm/aubo_arm.h" + +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +#include "aubo_sdk/rpc.h" + +namespace cmvr::device { +namespace { + +struct BusyGuard { + std::atomic& busy; + ~BusyGuard() { busy.store(false); } +}; + +std::vector defaultJointNames(const std::size_t dof) +{ + std::vector names; + names.reserve(dof); + for (std::size_t i = 0; i < dof; ++i) { + names.push_back("joint_" + std::to_string(i + 1)); + } + return names; +} + +std::string vendorBrandName(const config::VendorRobotArmBrand brand) +{ + switch (brand) { + case config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM: + return "AuboARM"; + case config::VENDOR_ROBOT_ARM_BRAND_UNKNOWN: + default: + return "Unknown"; + } +} + +using arcs::common_interface::RobotModeType; +using arcs::aubo_sdk::RobotInterfacePtr; + +constexpr int kAuboServoMode = 3; + +RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr& rpc_client, + const std::string& context, + Result& result) +{ + const auto robot_names = rpc_client->getRobotNames(); + if (robot_names.empty()) { + result = Result::failure(ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + " failed: robot name list is empty"); + return nullptr; + } + + auto robot_interface = rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + result = Result::failure(ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + " failed: robot interface is null"); + return nullptr; + } + + result = Result::success(); + return robot_interface; +} + +bool waitForRobotMode(const RobotInterfacePtr& robot_interface, + const RobotModeType& target_mode) +{ + const auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(20)) { + const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); + if (current_mode == target_mode) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return false; +} + +int waitArrival(const RobotInterfacePtr& robot_interface) +{ + int retry_count = 0; + int exec_id = robot_interface->getMotionControl()->getExecId(); + while (exec_id == -1 && retry_count++ < 5) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + exec_id = robot_interface->getMotionControl()->getExecId(); + } + if (exec_id == -1) { + return -1; + } + while (robot_interface->getMotionControl()->getExecId() != -1) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + return 0; +} + +bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode) +{ + for (int i = 0; i < 20; ++i) { + if (robot_interface->getMotionControl()->getServoModeSelect() == mode) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return false; +} + +CartesianPose poseFromVector(const std::vector& values) +{ + CartesianPose pose; + if (values.size() >= 6) { + pose.x = values[0]; + pose.y = values[1]; + pose.z = values[2]; + pose.rx = values[3]; + pose.ry = values[4]; + pose.rz = values[5]; + } + return pose; +} + +} // namespace + +struct AuboArm::SdkState { + std::shared_ptr rpc_client; +}; + +AuboArm::AuboArm(const config::RobotArmConfig& cfg) + : cfg_(cfg) +{ + id_ = cfg.id(); + if (cfg.has_vendor()) { + vendor_cfg_ = cfg.vendor(); + } + + ip_ = vendor_cfg_.ip(); + port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 30004; + username_ = vendor_cfg_.username().empty() ? "aubo" : vendor_cfg_.username(); + password_ = vendor_cfg_.password().empty() ? "123456" : vendor_cfg_.password(); + + const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; + model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model(); + model_.manufacturer = vendorBrandName(vendor_cfg_.brand()); + model_.dof = dof; + model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); + if (model_.joint_names.empty()) { + model_.joint_names = defaultJointNames(dof); + } + if (model_.joint_names.size() != dof) { + CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_; + model_.joint_names = defaultJointNames(dof); + } +} + +AuboArm::~AuboArm() +{ + (void)disconnect(); +} + +bool AuboArm::init() +{ + if (ip_.empty()) { + CMVR_LOG(ERROR) << "[AuboArm] ip is empty, id=" << id_; + return false; + } + const auto result = connect(ip_, port_); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[AuboArm] init failed: " << result.message; + return false; + } + return true; +} + +bool AuboArm::stop() +{ + return stopMotion().ok(); +} + +ArmState AuboArm::getRobotState() const +{ + ArmState state; + state.connected = connected_.load(); + state.powered_on = state.connected; + state.brake_released = state.connected; + state.moving = busy_.load(); + state.robot_mode = getRobotMode(); + state.safety_mode = getSafetyMode(); + state.control_mode = getControlMode(); + state.emergency_stopped = emergency_stopped_; + state.speed_scaling = speed_scaling_; + state.actual_joint_state = getJointState(); + state.target_joint_state = state.actual_joint_state; + return state; +} + +JointGroupState AuboArm::getJointState() const +{ + JointGroupState state; + state.position.assign(model_.dof, 0.0); + state.velocity.assign(model_.dof, 0.0); + state.effort.assign(model_.dof, 0.0); + + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return state; + } + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot name list is empty"; + return state; + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot interface is null"; + return state; + } + const auto robot_state = robot_interface->getRobotState(); + const auto positions = robot_state->getJointPositions(); + const auto velocities = robot_state->getJointSpeeds(); + const auto n = std::min(model_.dof, positions.size()); + for (std::size_t i = 0; i < n; ++i) { + state.position[i] = positions[i]; + } + const auto vn = std::min(model_.dof, velocities.size()); + for (std::size_t i = 0; i < vn; ++i) { + state.velocity[i] = velocities[i]; + } + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: " << e.what(); + } + return state; +} + +CartesianPose AuboArm::getTcpPose(FrameType frame) const +{ + (void)frame; + CartesianPose pose; + + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return pose; + } + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot name list is empty"; + return pose; + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot interface is null"; + return pose; + } + const auto pose_values = robot_interface->getRobotState()->getTcpPose(); + if (pose_values.size() >= 6) { + pose.x = pose_values[0]; + pose.y = pose_values[1]; + pose.z = pose_values[2]; + pose.rx = pose_values[3]; + pose.ry = pose_values[4]; + pose.rz = pose_values[5]; + } + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: " << e.what(); + } + + return pose; +} + +RobotMode AuboArm::getRobotMode() const +{ + if (!connected_.load()) { + return RobotMode::Disconnected; + } + if (emergency_stopped_) { + return RobotMode::Stopped; + } + return busy_.load() ? RobotMode::Running : RobotMode::Idle; +} + +Result AuboArm::torqueOn() +{ + const auto ready = ensureConnected_("torqueOn"); + if (!ready.ok()) { + return ready; + } + + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + + double mass = 0.0; + std::vector cog(3, 0.0); + std::vector aom(3, 0.0); + std::vector inertia(6, 0.0); + robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); + + const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); + if (current_mode != arcs::common_interface::RobotModeType::Running) { + robot_interface->getRobotManage()->poweron(); + if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) { + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle"); + } + robot_interface->getRobotManage()->startup(); + if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) { + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running"); + } + } + emergency_stopped_ = false; + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); + } +} + +Result AuboArm::torqueOff() +{ + const auto ready = ensureConnected_("torqueOff"); + if (!ready.ok()) { + return ready; + } + + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + robot_interface->getRobotManage()->poweroff(); + if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::PowerOff)) { + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOff failed: timeout waiting for PowerOff"); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOff failed: ") + e.what()); + } +} + +Result AuboArm::calibrateZeroQ(const std::string& joint_name) +{ + (void)joint_name; + return unsupported_("calibrateZeroQ"); +} + +Result AuboArm::emergencyStop() +{ + emergency_stopped_ = true; + return stopMotion(); +} + +Result AuboArm::setSpeedScaling(const double scaling) +{ + if (scaling < 0.0 || scaling > 1.0) { + return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]"); + } + speed_scaling_ = scaling; + return Result::success(); +} + +Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options) +{ + std::string error; + if (!validDof_(target.position.size(), error)) { + return Result::failure(ArmErrorCode::InvalidDof, error); + } + const auto ready = ensureConnected_("moveJ"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + robot_interface->getMotionControl()->moveJoint( + target.position, + options.acceleration > 0.0 ? options.acceleration : 0.5, + options.velocity > 0.0 ? options.velocity : 0.5, + options.blend_radius, + 0); + if (waitArrival(robot_interface) != 0) { + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ did not complete"); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what()); + } +} + +Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) +{ + std::string error; + if (!validDof_(velocity.velocity.size(), error)) { + return Result::failure(ArmErrorCode::InvalidDof, error); + } + const auto ready = ensureConnected_("speedJ"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedJ", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5; + const double resolved_duration = duration > 0.0 ? duration : 100.0; + const int ret = robot_interface->getMotionControl()->speedJoint( + velocity.velocity, + resolved_acceleration, + resolved_duration); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] speedJ failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what()); + } +} + +Result AuboArm::stopJ(double acceleration) +{ + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return Result::success(); + } + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopJ", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + const double resolved_acceleration = acceleration > 0.0 ? acceleration : 31.0; + const int ret = robot_interface->getMotionControl()->stopJoint(resolved_acceleration); + busy_.store(false); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] stopJ failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopJ failed: ") + e.what()); + } +} + +Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame) +{ + (void)frame; + const auto ready = ensureConnected_("moveL"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + } + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + std::vector tcp_offset(6, 0.0); + robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); + std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; + robot_interface->getMotionControl()->moveLine( + pose, + options.acceleration > 0.0 ? options.acceleration : 0.5, + options.velocity > 0.0 ? options.velocity : 0.25, + options.blend_radius, + 0); + if (waitArrival(robot_interface) != 0) { + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL did not complete"); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what()); + } +} + +Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame) +{ + const auto ready = ensureConnected_("speedL"); + if (!ready.ok()) { + return ready; + } + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); + } + BusyGuard busy_guard{busy_}; + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + + robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + std::vector tcp_offset(6, 0.0); + robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); + + std::vector line_speed{velocity.vx, velocity.vy, velocity.vz, 0.0, 0.0, 0.0}; + std::vector angular_speed{velocity.wx, velocity.wy, velocity.wz, 0.0, 0.0, 0.0}; + if (frame == FrameType::Tool) { + auto tool_frame = robot_interface->getRobotState()->getTcpPose(); + if (tool_frame.size() < 6) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] speedL failed: tcp pose size is less than 6"); + } + tool_frame[0] = 0.0; + tool_frame[1] = 0.0; + tool_frame[2] = 0.0; + line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed); + angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed); + } else if (frame == FrameType::User) { + return Result::failure(ArmErrorCode::UnsupportedCommand, + "[AuboArm] speedL User frame requires a configured user coordinate frame"); + } + + std::vector speed{ + line_speed[0], + line_speed[1], + line_speed[2], + angular_speed[0], + angular_speed[1], + angular_speed[2], + }; + + const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2; + const double resolved_duration = duration > 0.0 ? duration : 100.0; + const int ret = robot_interface->getMotionControl()->speedLine( + speed, + resolved_acceleration, + resolved_duration); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] speedL failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what()); + } +} + +Result AuboArm::stopL(std::optional acceleration) +{ + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return Result::success(); + } + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopL", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + const double resolved_acceleration = + acceleration.has_value() && *acceleration > 0.0 ? *acceleration : 10.0; + const int ret = robot_interface->getMotionControl()->stopLine(resolved_acceleration, resolved_acceleration); + busy_.store(false); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] stopL failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopL failed: ") + e.what()); + } +} + +Result AuboArm::stopMotion() +{ + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return Result::success(); + } + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return Result::success(); + } + auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (robot_interface) { + robot_interface->getMotionControl()->stopMove(true, true); + } + busy_.store(false); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); + } +} + +Result AuboArm::startServoMode(const ServoOptions& options) +{ + const auto ready = ensureConnected_("startServoMode"); + if (!ready.ok()) { + return ready; + } + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "startServoMode", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + const int ret = robot_interface->getMotionControl()->setServoModeSelect(kAuboServoMode); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] startServoMode failed: ret=" + std::to_string(ret)); + } + if (!waitServoModeSelect(robot_interface, kAuboServoMode)) { + return Result::failure(ArmErrorCode::Timeout, + "[AuboArm] startServoMode failed: timeout waiting for servo mode"); + } + servo_options_ = options; + servo_mode_.store(true); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, + std::string("[AuboArm] startServoMode failed: ") + e.what()); + } +} + +Result AuboArm::servoJ(const JointPositionCommand& target) +{ + std::string error; + if (!validDof_(target.position.size(), error)) { + return Result::failure(ArmErrorCode::InvalidDof, error); + } + const auto ready = ensureConnected_("servoJ"); + if (!ready.ok()) { + return ready; + } + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "servoJ", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + if (!servo_mode_.load() && robot_interface->getMotionControl()->getServoModeSelect() == 0) { + const auto start_result = startServoMode(servo_options_); + if (!start_result.ok()) { + return start_result; + } + } + const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + const int ret = robot_interface->getMotionControl()->servoJoint( + target.position, + 0.0, + 0.0, + period, + servo_options_.lookahead_time, + servo_options_.gain); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] servoJ failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoJ failed: ") + e.what()); + } +} + +Result AuboArm::servoL(const CartesianPose& target, FrameType frame) +{ + const auto ready = ensureConnected_("servoL"); + if (!ready.ok()) { + return ready; + } + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "servoL", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + if (!servo_mode_.load() && robot_interface->getMotionControl()->getServoModeSelect() == 0) { + const auto start_result = startServoMode(servo_options_); + if (!start_result.ok()) { + return start_result; + } + } + + std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; + if (frame == FrameType::Tool) { + const auto current_pose = robot_interface->getRobotState()->getTcpPose(); + if (current_pose.size() < 6) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] servoL failed: tcp pose size is less than 6"); + } + pose = sdk_->rpc_client->getMath()->poseTrans(current_pose, pose); + } else if (frame == FrameType::User) { + return Result::failure(ArmErrorCode::UnsupportedCommand, + "[AuboArm] servoL User frame requires a configured user coordinate frame"); + } + + const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + const int ret = robot_interface->getMotionControl()->servoCartesian( + pose, + 0.0, + 0.0, + period, + servo_options_.lookahead_time, + servo_options_.gain); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] servoL failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoL failed: ") + e.what()); + } +} + +Result AuboArm::servoSpeedJ(const JointVelocityCommand& velocity) +{ + const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + return speedJ(velocity, 1.5, period); +} + +Result AuboArm::servoSpeedL(const CartesianVelocity& velocity, FrameType frame) +{ + const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + return speedL(velocity, 1.2, period, frame); +} + +Result AuboArm::stopServoMode() +{ + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + servo_mode_.store(false); + return Result::success(); + } + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopServoMode", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + const int ret = robot_interface->getMotionControl()->setServoModeSelect(0); + servo_mode_.store(false); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] stopServoMode failed: ret=" + std::to_string(ret)); + } + if (!waitServoModeSelect(robot_interface, 0)) { + return Result::failure(ArmErrorCode::Timeout, + "[AuboArm] stopServoMode failed: timeout waiting for servo mode disabled"); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, + std::string("[AuboArm] stopServoMode failed: ") + e.what()); + } +} + +Result AuboArm::connect(const std::string& ip, const int port) +{ + if (connected_.load()) { + return Result::success(); + } + if (ip.empty()) { + return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] ip is empty"); + } + + try { + const int resolved_port = port > 0 ? port : 30004; + auto sdk_state = std::make_unique(); + sdk_state->rpc_client = std::shared_ptr( + ::createRpcClient(), + [](arcs::aubo_sdk::RpcClient* client) { + if (client) { + ::destroyRpcClient(client); + } + }); + if (!sdk_state->rpc_client) { + return Result::failure(ArmErrorCode::ConnectionFailed, + "[AuboArm] connect failed: create RPC client failed"); + } + + sdk_state->rpc_client->setRequestTimeout(1000); + int ret = sdk_state->rpc_client->connect(ip, resolved_port); + if (ret != 0) { + return Result::failure(ArmErrorCode::ConnectionFailed, + "[AuboArm] connect failed: rpc connect ret=" + + std::to_string(ret) + ", ip=" + ip + + ", port=" + std::to_string(resolved_port)); + } + + ret = sdk_state->rpc_client->login(username_, password_); + if (ret != 0) { + if (sdk_state->rpc_client->hasConnected()) { + sdk_state->rpc_client->disconnect(); + } + return Result::failure(ArmErrorCode::ConnectionFailed, + "[AuboArm] connect failed: login ret=" + std::to_string(ret)); + } + + const auto robot_names = sdk_state->rpc_client->getRobotNames(); + if (robot_names.empty()) { + if (sdk_state->rpc_client->hasLogined()) { + sdk_state->rpc_client->logout(); + } + if (sdk_state->rpc_client->hasConnected()) { + sdk_state->rpc_client->disconnect(); + } + return Result::failure(ArmErrorCode::ConnectionFailed, + "[AuboArm] connect failed: robot name list is empty"); + } + + ip_ = ip; + port_ = resolved_port; + sdk_ = std::move(sdk_state); + connected_.store(true); + return Result::success(); + } catch (const std::exception& e) { + sdk_.reset(); + connected_.store(false); + return Result::failure(ArmErrorCode::ConnectionFailed, + std::string("[AuboArm] connect failed: ") + e.what()); + } +} + +Result AuboArm::disconnect() +{ + try { + if (sdk_ && sdk_->rpc_client) { + if (sdk_->rpc_client->hasLogined()) { + sdk_->rpc_client->logout(); + } + if (sdk_->rpc_client->hasConnected()) { + sdk_->rpc_client->disconnect(); + } + } + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what(); + } + sdk_.reset(); + connected_.store(false); + busy_.store(false); + servo_mode_.store(false); + return Result::success(); +} + +Result AuboArm::shutdown() +{ + (void)stopMotion(); + return disconnect(); +} + +Result AuboArm::loadProgram(const std::string& program_name) +{ + if (program_name.empty()) { + return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] loadProgram failed: program name is empty"); + } + const auto ready = ensureConnected_("loadProgram"); + if (!ready.ok()) { + return ready; + } + try { + const int ret = sdk_->rpc_client->getRuntimeMachine()->loadProgram(program_name); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] loadProgram failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, + std::string("[AuboArm] loadProgram failed: ") + e.what()); + } +} + +Result AuboArm::playProgram() +{ + const auto ready = ensureConnected_("playProgram"); + if (!ready.ok()) { + return ready; + } + try { + const int ret = sdk_->rpc_client->getRuntimeMachine()->runProgram(); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] playProgram failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, + std::string("[AuboArm] playProgram failed: ") + e.what()); + } +} + +Result AuboArm::pauseProgram() +{ + const auto ready = ensureConnected_("pauseProgram"); + if (!ready.ok()) { + return ready; + } + try { + const int ret = sdk_->rpc_client->getRuntimeMachine()->pause(); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] pauseProgram failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, + std::string("[AuboArm] pauseProgram failed: ") + e.what()); + } +} + +Result AuboArm::stopProgram() +{ + const auto ready = ensureConnected_("stopProgram"); + if (!ready.ok()) { + return ready; + } + try { + const int ret = sdk_->rpc_client->getRuntimeMachine()->abort(); + busy_.store(false); + if (ret != 0) { + return Result::failure(ArmErrorCode::CommandFailed, + "[AuboArm] stopProgram failed: ret=" + std::to_string(ret)); + } + return Result::success(); + } catch (const std::exception& e) { + return Result::failure(ArmErrorCode::CommandFailed, + std::string("[AuboArm] stopProgram failed: ") + e.what()); + } +} + +std::vector AuboArm::ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) +{ + (void)base_link; + (void)ee_link; + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + CMVR_LOG(ERROR) << "[AuboArm] ik failed: arm is not connected"; + return {}; + } + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "ik", interface_result); + if (!interface_result.ok()) { + CMVR_LOG(ERROR) << interface_result.message; + return {}; + } + const auto qnear = getJointState().position; + const std::vector target_pose{pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz}; + const auto result = robot_interface->getRobotAlgorithm()->inverseKinematics(qnear, target_pose); + const int ret = std::get<1>(result); + if (ret != 0) { + CMVR_LOG(ERROR) << "[AuboArm] ik failed: ret=" << ret; + return {}; + } + return std::get<0>(result); + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] ik failed: " << e.what(); + } + return {}; +} + +CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_link) +{ + (void)base_link; + (void)ee_link; + return getTcpPose(FrameType::Base); +} + +CartesianPose AuboArm::fk(bool is_tcp) +{ + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return {}; + } + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "fk", interface_result); + if (!interface_result.ok()) { + CMVR_LOG(ERROR) << interface_result.message; + return {}; + } + const auto q = getJointState().position; + if (q.size() != model_.dof) { + CMVR_LOG(ERROR) << "[AuboArm] fk failed: joint state dof mismatch"; + return {}; + } + const auto result = is_tcp + ? robot_interface->getRobotAlgorithm()->forwardKinematics(q) + : robot_interface->getRobotAlgorithm()->forwardToolKinematics(q); + const int ret = std::get<1>(result); + if (ret != 0) { + CMVR_LOG(ERROR) << "[AuboArm] fk failed: ret=" << ret; + return {}; + } + return poseFromVector(std::get<0>(result)); + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] fk failed: " << e.what(); + } + return {}; +} + +Result AuboArm::unsupported_(const std::string& name) const +{ + const std::string message = "[AuboArm] " + name + " is not implemented"; + CMVR_LOG(ERROR) << message; + return Result::failure(ArmErrorCode::UnsupportedCommand, message); +} + +bool AuboArm::validDof_(const std::size_t size, std::string& error) const +{ + if (size != model_.dof) { + error = "[AuboArm] command dof mismatch, expected=" + std::to_string(model_.dof) + + ", actual=" + std::to_string(size); + CMVR_LOG(ERROR) << error; + return false; + } + return true; +} + +Result AuboArm::ensureConnected_(const std::string& context) const +{ + if (!connected_.load()) { + return Result::failure(ArmErrorCode::NotConnected, + "[AuboArm] " + context + " failed: arm is not connected"); + } + if (!sdk_ || !sdk_->rpc_client) { + return Result::failure(ArmErrorCode::NotConnected, + "[AuboArm] " + context + " failed: SDK client is null"); + } + return Result::success(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h similarity index 90% rename from cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h rename to cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 390c7d5a..a4d4dd6f 100644 --- a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -29,13 +29,21 @@ public: CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override { return SafetyMode::Normal; } - ControlMode getControlMode() const override { return ControlMode::Position; } + ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } Result torqueOn() override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; Result protectiveStop() override { return emergencyStop(); } + Result recoverProtectiveStop( + const JointTrajectory&, + const MotionOptions&) override + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "protective recovery is not implemented for AuboArm"); + } Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } bool isProtectiveStopped() const override { return false; } @@ -98,8 +106,10 @@ private: std::string username_; std::string password_; double speed_scaling_{1.0}; + ServoOptions servo_options_; std::atomic connected_{false}; std::atomic busy_{false}; + std::atomic servo_mode_{false}; bool emergency_stopped_{false}; mutable std::mutex mutex_; diff --git a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp deleted file mode 100644 index 11c7312c..00000000 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ /dev/null @@ -1,575 +0,0 @@ -#include "devices/arm/aubo_arm/include/aubo_arm.h" - -#include -#include -#include -#include - -#include "common/base/logging/logger.h" - -#if defined(CMVR_HAS_AUBO_SDK) -#include "aubo_sdk/rpc.h" -#endif - -namespace cmvr::device { -namespace { - -struct BusyGuard { - std::atomic& busy; - ~BusyGuard() { busy.store(false); } -}; - -std::vector defaultJointNames(const std::size_t dof) -{ - std::vector names; - names.reserve(dof); - for (std::size_t i = 0; i < dof; ++i) { - names.push_back("joint_" + std::to_string(i + 1)); - } - return names; -} - -std::string vendorBrandName(const config::VendorRobotArmBrand brand) -{ - switch (brand) { - case config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM: - return "AuboARM"; - case config::VENDOR_ROBOT_ARM_BRAND_UNKNOWN: - default: - return "Unknown"; - } -} - -} // namespace - -#if defined(CMVR_HAS_AUBO_SDK) -struct AuboArm::SdkState { - std::shared_ptr rpc_client; -}; -#endif - -AuboArm::AuboArm(const config::RobotArmConfig& cfg) - : cfg_(cfg) -{ - id_ = cfg.id(); - if (cfg.has_vendor()) { - vendor_cfg_ = cfg.vendor(); - } - - ip_ = vendor_cfg_.ip(); - port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 30004; - username_ = vendor_cfg_.username().empty() ? "aubo" : vendor_cfg_.username(); - password_ = vendor_cfg_.password().empty() ? "123456" : vendor_cfg_.password(); - - const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; - model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model(); - model_.manufacturer = vendorBrandName(vendor_cfg_.brand()); - model_.dof = dof; - model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); - if (model_.joint_names.empty()) { - model_.joint_names = defaultJointNames(dof); - } - if (model_.joint_names.size() != dof) { - CMVR_LOG(ERROR) << "[AuboArm] joint_names size mismatch, id=" << id_; - model_.joint_names = defaultJointNames(dof); - } -} - -AuboArm::~AuboArm() -{ - (void)disconnect(); -} - -bool AuboArm::init() -{ - if (ip_.empty()) { - CMVR_LOG(ERROR) << "[AuboArm] ip is empty, id=" << id_; - return false; - } - const auto result = connect(ip_, port_); - if (!result.ok()) { - CMVR_LOG(ERROR) << "[AuboArm] init failed: " << result.message; - return false; - } - return true; -} - -bool AuboArm::stop() -{ - return stopMotion().ok(); -} - -ArmState AuboArm::getRobotState() const -{ - ArmState state; - state.connected = connected_.load(); - state.powered_on = state.connected; - state.brake_released = state.connected; - state.moving = busy_.load(); - state.robot_mode = getRobotMode(); - state.safety_mode = getSafetyMode(); - state.control_mode = getControlMode(); - state.emergency_stopped = emergency_stopped_; - state.speed_scaling = speed_scaling_; - state.actual_joint_state = getJointState(); - state.target_joint_state = state.actual_joint_state; - return state; -} - -JointGroupState AuboArm::getJointState() const -{ - JointGroupState state; - state.position.assign(model_.dof, 0.0); - state.velocity.assign(model_.dof, 0.0); - state.effort.assign(model_.dof, 0.0); - -#if defined(CMVR_HAS_AUBO_SDK) - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { - return state; - } - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot name list is empty"; - return state; - } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: robot interface is null"; - return state; - } - const auto robot_state = robot_interface->getRobotState(); - const auto positions = robot_state->getJointPositions(); - const auto velocities = robot_state->getJointSpeeds(); - const auto n = std::min(model_.dof, positions.size()); - for (std::size_t i = 0; i < n; ++i) { - state.position[i] = positions[i]; - } - const auto vn = std::min(model_.dof, velocities.size()); - for (std::size_t i = 0; i < vn; ++i) { - state.velocity[i] = velocities[i]; - } - } catch (const std::exception& e) { - CMVR_LOG(ERROR) << "[AuboArm] getJointState failed: " << e.what(); - } -#endif - return state; -} - -CartesianPose AuboArm::getTcpPose(FrameType frame) const -{ - (void)frame; - return {}; -} - -RobotMode AuboArm::getRobotMode() const -{ - if (!connected_.load()) { - return RobotMode::Disconnected; - } - if (emergency_stopped_) { - return RobotMode::Stopped; - } - return busy_.load() ? RobotMode::Running : RobotMode::Idle; -} - -Result AuboArm::torqueOn() -{ - const auto ready = ensureConnected_("torqueOn"); - if (!ready.ok()) { - return ready; - } - -#if defined(CMVR_HAS_AUBO_SDK) - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); - } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); - } - - double mass = 0.0; - std::vector cog(3, 0.0); - std::vector aom(3, 0.0); - std::vector inertia(6, 0.0); - robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); - - if (robot_interface->getRobotState()->getRobotModeType() != - arcs::common_interface::RobotModeType::Running) { - robot_interface->getRobotManage()->poweron(); - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - robot_interface->getRobotManage()->startup(); - } - emergency_stopped_ = false; - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); - } -#else - return unsupported_("torqueOn"); -#endif -} - -Result AuboArm::torqueOff() -{ - const auto ready = ensureConnected_("torqueOff"); - if (!ready.ok()) { - return ready; - } - -#if defined(CMVR_HAS_AUBO_SDK) - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); - } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); - } - robot_interface->getRobotManage()->poweroff(); - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOff failed: ") + e.what()); - } -#else - return unsupported_("torqueOff"); -#endif -} - -Result AuboArm::calibrateZeroQ(const std::string& joint_name) -{ - (void)joint_name; - return unsupported_("calibrateZeroQ"); -} - -Result AuboArm::emergencyStop() -{ - emergency_stopped_ = true; - return stopMotion(); -} - -Result AuboArm::setSpeedScaling(const double scaling) -{ - if (scaling < 0.0 || scaling > 1.0) { - return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]"); - } - speed_scaling_ = scaling; - return Result::success(); -} - -Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& options) -{ - std::string error; - if (!validDof_(target.position.size(), error)) { - return Result::failure(ArmErrorCode::InvalidDof, error); - } - const auto ready = ensureConnected_("moveJ"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - -#if defined(CMVR_HAS_AUBO_SDK) - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); - } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); - } - robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); - robot_interface->getMotionControl()->moveJoint( - target.position, - options.acceleration > 0.0 ? options.acceleration : 0.5, - options.velocity > 0.0 ? options.velocity : 0.5, - options.blend_radius, - 0); - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what()); - } -#else - return unsupported_("moveJ"); -#endif -} - -Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) -{ - (void)velocity; - (void)acceleration; - (void)duration; - return unsupported_("speedJ"); -} - -Result AuboArm::stopJ(double acceleration) -{ - (void)acceleration; - return stopMotion(); -} - -Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame) -{ - (void)frame; - const auto ready = ensureConnected_("moveL"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - -#if defined(CMVR_HAS_AUBO_SDK) - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); - } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); - } - robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); - std::vector tcp_offset(6, 0.0); - robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); - std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; - robot_interface->getMotionControl()->moveLine( - pose, - options.acceleration > 0.0 ? options.acceleration : 0.5, - options.velocity > 0.0 ? options.velocity : 0.25, - options.blend_radius, - 0); - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what()); - } -#else - return unsupported_("moveL"); -#endif -} - -Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame) -{ - (void)velocity; - (void)acceleration; - (void)duration; - (void)frame; - return unsupported_("speedL"); -} - -Result AuboArm::stopL(std::optional acceleration) -{ - (void)acceleration; - return stopMotion(); -} - -Result AuboArm::stopMotion() -{ -#if defined(CMVR_HAS_AUBO_SDK) - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { - return Result::success(); - } - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::success(); - } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (robot_interface) { - robot_interface->getMotionControl()->stopMove(); - } - busy_.store(false); - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); - } -#else - busy_.store(false); - return Result::success(); -#endif -} - -Result AuboArm::startServoMode(const ServoOptions& options) -{ - (void)options; - return unsupported_("startServoMode"); -} - -Result AuboArm::servoJ(const JointPositionCommand& target) -{ - (void)target; - return unsupported_("servoJ"); -} - -Result AuboArm::servoL(const CartesianPose& target, FrameType frame) -{ - (void)target; - (void)frame; - return unsupported_("servoL"); -} - -Result AuboArm::servoSpeedJ(const JointVelocityCommand& velocity) -{ - (void)velocity; - return unsupported_("servoSpeedJ"); -} - -Result AuboArm::servoSpeedL(const CartesianVelocity& velocity, FrameType frame) -{ - (void)velocity; - (void)frame; - return unsupported_("servoSpeedL"); -} - -Result AuboArm::stopServoMode() -{ - return Result::success(); -} - -Result AuboArm::connect(const std::string& ip, const int port) -{ - if (connected_.load()) { - return Result::success(); - } - if (ip.empty()) { - return Result::failure(ArmErrorCode::InvalidArgument, "[AuboArm] ip is empty"); - } - -#if defined(CMVR_HAS_AUBO_SDK) - try { - sdk_ = std::make_unique(); - sdk_->rpc_client = std::make_shared(); - sdk_->rpc_client->setRequestTimeout(1000); - sdk_->rpc_client->connect(ip, port > 0 ? port : 30004); - sdk_->rpc_client->login(username_, password_); - ip_ = ip; - port_ = port > 0 ? port : 30004; - connected_.store(true); - return Result::success(); - } catch (const std::exception& e) { - sdk_.reset(); - connected_.store(false); - return Result::failure(ArmErrorCode::ConnectionFailed, - std::string("[AuboArm] connect failed: ") + e.what()); - } -#else - (void)port; - return Result::failure(ArmErrorCode::UnsupportedCommand, - "[AuboArm] Aubo SDK is not available in this build"); -#endif -} - -Result AuboArm::disconnect() -{ -#if defined(CMVR_HAS_AUBO_SDK) - try { - if (sdk_ && sdk_->rpc_client) { - sdk_->rpc_client->logout(); - sdk_->rpc_client->disconnect(); - } - } catch (const std::exception& e) { - CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what(); - } - sdk_.reset(); -#endif - connected_.store(false); - busy_.store(false); - return Result::success(); -} - -Result AuboArm::shutdown() -{ - (void)stopMotion(); - return disconnect(); -} - -Result AuboArm::loadProgram(const std::string& program_name) -{ - (void)program_name; - return unsupported_("loadProgram"); -} - -Result AuboArm::playProgram() -{ - return unsupported_("playProgram"); -} - -Result AuboArm::pauseProgram() -{ - return unsupported_("pauseProgram"); -} - -Result AuboArm::stopProgram() -{ - return unsupported_("stopProgram"); -} - -std::vector AuboArm::ik(const std::string& base_link, - const std::string& ee_link, - const CartesianPose& pose) -{ - (void)base_link; - (void)ee_link; - (void)pose; - CMVR_LOG(ERROR) << "[AuboArm] ik is not implemented"; - return {}; -} - -CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_link) -{ - (void)base_link; - (void)ee_link; - CMVR_LOG(ERROR) << "[AuboArm] fk(base,ee) is not implemented"; - return {}; -} - -CartesianPose AuboArm::fk(bool is_tcp) -{ - (void)is_tcp; - CMVR_LOG(ERROR) << "[AuboArm] fk is not implemented"; - return {}; -} - -Result AuboArm::unsupported_(const std::string& name) const -{ - const std::string message = "[AuboArm] " + name + " is not implemented"; - CMVR_LOG(ERROR) << message; - return Result::failure(ArmErrorCode::UnsupportedCommand, message); -} - -bool AuboArm::validDof_(const std::size_t size, std::string& error) const -{ - if (size != model_.dof) { - error = "[AuboArm] command dof mismatch, expected=" + std::to_string(model_.dof) + - ", actual=" + std::to_string(size); - CMVR_LOG(ERROR) << error; - return false; - } - return true; -} - -Result AuboArm::ensureConnected_(const std::string& context) const -{ - if (!connected_.load()) { - return Result::failure(ArmErrorCode::NotConnected, - "[AuboArm] " + context + " failed: arm is not connected"); - } -#if defined(CMVR_HAS_AUBO_SDK) - if (!sdk_ || !sdk_->rpc_client) { - return Result::failure(ArmErrorCode::NotConnected, - "[AuboArm] " + context + " failed: SDK client is null"); - } -#endif - return Result::success(); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 1f6de9b7..74ef039f 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -117,6 +117,7 @@ bool HuayanRobot::init() CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message; return false; } + setSpeedScaling(1); return true; } diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 92a3d1a3..5db631fa 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -42,6 +42,14 @@ public: Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; Result protectiveStop() override { return emergencyStop(); } + Result recoverProtectiveStop( + const JointTrajectory&, + const MotionOptions&) override + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "protective recovery is not implemented for HuayanRobot"); + } Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } bool isProtectiveStopped() const override; @@ -140,4 +148,4 @@ private: #endif // CMVR_ES_HUAYAN_ROBOT_H -#endif //CMVR_ES_HUAYAN_ARM_H \ No newline at end of file +#endif //CMVR_ES_HUAYAN_ARM_H diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt index c0b7d99b..a194fa73 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -34,3 +34,21 @@ target_link_libraries(motor_robot_arm_mujoco_test gtest_main pthread ) + +add_executable(motor_robot_arm_gen2_mujoco_test + src/motor_robot_arm_gen2_mujoco_test.cpp +) + +target_link_libraries(motor_robot_arm_gen2_mujoco_test + PRIVATE + cmvr_es::device::motor_robot_arm + cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver + cmvr_es::device_manager + cmvr_es::mujoco_viewer + cmvr_es::proto + cmvr_es::task + gtest + gtest_main + pthread +) diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 7a0af759..73be9284 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -41,11 +41,14 @@ public: Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override { return emergencyStop(); } + Result protectiveStop() override; + Result recoverProtectiveStop( + const JointTrajectory& path, + const MotionOptions& options) override; Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } - bool isProtectiveStopped() const override { return false; } - bool isEmergencyStopped() const override { return emergency_stopped_; } + bool isProtectiveStopped() const override { return protective_stopped_.load(); } + bool isEmergencyStopped() const override { return emergency_stopped_.load(); } bool isFault() const override { return false; } Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; @@ -73,10 +76,10 @@ public: bool isConnected() const override { return motor_manager_ != nullptr; } Result powerOn() override { return torqueOn(); } Result powerOff() override { return torqueOff(); } - Result brakeRelease() override { return torqueOn(); } + Result brakeRelease() override; Result shutdown() override; Result clearFault() override { return Result::success(); } - Result unlockProtectiveStop() override { return Result::success(); } + Result unlockProtectiveStop() override; Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -94,6 +97,10 @@ public: private: bool containsJoint_(const std::string& joint_name) const; + bool safetyStopRequested_() const; + std::optional safetyStopResult_(const std::string& command, + bool interrupted = false) const; + Result quickStopMotors_(); bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const; bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const; std::shared_ptr getMotor_(const std::string& joint_name) const; @@ -126,7 +133,10 @@ private: mutable std::mutex mutex_; std::atomic busy_{false}; double speed_scaling_{1.0}; - bool emergency_stopped_{false}; + std::atomic protective_stopped_{false}; + std::atomic emergency_stopped_{false}; + std::atomic protective_recovery_active_{false}; + std::atomic protective_recovery_cancel_requested_{false}; ServoOptions servo_options_; }; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 77e8abbc..5acc3f39 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -1,6 +1,8 @@ #include "arm/motor_robot_arm/include/motor_robot_arm.h" +#include #include +#include #include #include #include @@ -28,6 +30,11 @@ struct BusyGuard { ~BusyGuard() { busy.store(false); } }; +struct AtomicFlagGuard { + std::atomic& flag; + ~AtomicFlagGuard() { flag.store(false); } +}; + } // namespace MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) @@ -132,12 +139,15 @@ bool MotorRobotArm::stop() ArmState MotorRobotArm::getRobotState() const { + const bool protective_stopped = protective_stopped_.load(); + const bool emergency_stopped = emergency_stopped_.load(); ArmState state; state.connected = motor_manager_ != nullptr; state.powered_on = true; - state.brake_released = !emergency_stopped_; + state.brake_released = !emergency_stopped; state.moving = busy(); - state.emergency_stopped = emergency_stopped_; + state.protective_stopped = protective_stopped; + state.emergency_stopped = emergency_stopped; state.speed_scaling = speed_scaling_; state.robot_mode = RobotMode::Idle; state.safety_mode = getSafetyMode(); @@ -186,7 +196,13 @@ CartesianPose MotorRobotArm::getTcpPose(const FrameType frame) const SafetyMode MotorRobotArm::getSafetyMode() const { - return emergency_stopped_ ? SafetyMode::EmergencyStop : SafetyMode::Normal; + if (emergency_stopped_.load()) { + return SafetyMode::EmergencyStop; + } + if (protective_stopped_.load()) { + return SafetyMode::ProtectiveStop; + } + return SafetyMode::Normal; } Result MotorRobotArm::torqueOn() @@ -196,9 +212,12 @@ Result MotorRobotArm::torqueOn() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->brake(); + if (!motor->torqueOn()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to torque on motor for joint: " + joint_name); + } } - emergency_stopped_ = false; + emergency_stopped_.store(false); return Result::success(); } @@ -209,7 +228,26 @@ Result MotorRobotArm::torqueOff() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->torqueOff(); + if (!motor->torqueOff() || !motor->brakeRelease()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to torque off motor for joint: " + joint_name); + } + } + return Result::success(); +} + +Result MotorRobotArm::brakeRelease() +{ + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_name); + } + if (!motor->brakeRelease()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to release brake for joint: " + joint_name); + } } return Result::success(); } @@ -233,17 +271,230 @@ Result MotorRobotArm::emergencyStop() if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } + protective_recovery_cancel_requested_.store(true); + protective_stopped_.store(false); + emergency_stopped_.store(true); + return quickStopMotors_(); +} + +Result MotorRobotArm::protectiveStop() +{ + if (emergency_stopped_.load()) { + return Result::success(); + } + if (cartesian_velocity_controller_) { + cartesian_velocity_controller_->shutdown(); + } + protective_recovery_cancel_requested_.store(true); + protective_stopped_.store(true); + return quickStopMotors_(); +} + +Result MotorRobotArm::recoverProtectiveStop( + const JointTrajectory& path, + const MotionOptions& options) +{ + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "protective recovery rejected: arm is in emergency stop"); + } + if (!protective_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery rejected: arm is not protective stopped"); + } + if (path.size() < 2 || + !std::isfinite(options.velocity) || options.velocity <= 0.0 || + !std::isfinite(options.acceleration) || options.acceleration <= 0.0) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "protective recovery path or options are invalid"); + } + + for (std::size_t i = 0; i < path.size(); ++i) { + const auto& sample = path[i]; + if (!std::isfinite(sample.time_s) || + sample.position.size() != joint_names_.size() || + sample.velocity.size() != joint_names_.size() || + (i > 0 && sample.time_s <= path[i - 1].time_s)) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "protective recovery sample shape or time is invalid"); + } + for (std::size_t joint = 0; joint < sample.position.size(); ++joint) { + if (!std::isfinite(sample.position[joint]) || + !std::isfinite(sample.velocity[joint])) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "protective recovery sample contains a non-finite value"); + } + } + } + + if (busy_.exchange(true)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery rejected: arm is busy"); + } + BusyGuard busy_guard{busy_}; + + bool expected = false; + if (!protective_recovery_active_.compare_exchange_strong(expected, true)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery is already active"); + } + AtomicFlagGuard recovery_guard{protective_recovery_active_}; + protective_recovery_cancel_requested_.store(false); + + if (!joint_planner_) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery planner is not initialized"); + } + + JointTrajectory recovery_trajectory; + const auto planning_start = std::chrono::steady_clock::now(); + if (!joint_planner_->planReplay( + readJointPosition_(), path, options, recovery_trajectory)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to plan protective recovery replay trajectory"); + } + const double planning_ms = std::chrono::duration( + std::chrono::steady_clock::now() - planning_start).count(); + CMVR_LOG(INFO) << "[MotorRobotArm] protective recovery planned" + << ", input_samples=" << path.size() + << ", command_samples=" << recovery_trajectory.size() + << ", planning_ms=" << planning_ms + << ", trajectory_duration_s=" + << recovery_trajectory.back().time_s; + + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "protective recovery interrupted by emergency stop during planning"); + } + if (protective_recovery_cancel_requested_.load()) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "protective recovery aborted by collision monitor during planning"); + } + + std::lock_guard lock(mutex_); + std::vector> motors; + motors.reserve(joint_names_.size()); + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_name); + } + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION && + !motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to set recovery position mode for joint: " + joint_name); + } + motors.push_back(std::move(motor)); + } + + const auto trajectory_start = std::chrono::steady_clock::now(); + for (std::size_t i = 0; i < recovery_trajectory.size(); ++i) { + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "protective recovery interrupted by emergency stop"); + } + if (protective_recovery_cancel_requested_.load()) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "protective recovery aborted by collision monitor"); + } + + const auto& sample = recovery_trajectory[i]; + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, sample.position, sample.velocity)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to submit protective recovery sample"); + } + + if (i + 1 < recovery_trajectory.size()) { + std::this_thread::sleep_until( + trajectory_start + + std::chrono::duration_cast( + std::chrono::duration( + recovery_trajectory[i + 1].time_s))); + } + } + + const std::vector zero_velocity(joint_names_.size(), 0.0); + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, path.front().position, zero_velocity)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to hold final protective recovery position"); + } + return Result::success(); +} + +Result MotorRobotArm::unlockProtectiveStop() +{ + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "cannot unlock protective stop while arm is emergency stopped"); + } + if (protective_recovery_active_.load()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "cannot unlock protective stop while recovery is active"); + } + protective_stopped_.store(false); + return Result::success(); +} + +Result MotorRobotArm::quickStopMotors_() +{ for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->brake(); + if (!motor->quickStop()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to quick stop motor for joint: " + joint_name); + } } - emergency_stopped_ = true; return Result::success(); } +bool MotorRobotArm::safetyStopRequested_() const +{ + return emergency_stopped_.load() || protective_stopped_.load(); +} + +std::optional MotorRobotArm::safetyStopResult_( + const std::string& command, + const bool interrupted) const +{ + const char* action = interrupted ? " interrupted by " : " rejected: arm is in "; + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + command + action + "emergency stop"); + } + if (protective_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + command + action + "protective stop"); + } + return std::nullopt; +} + Result MotorRobotArm::setSpeedScaling(const double scaling) { if (scaling < 0.0 || scaling > 1.0) { @@ -255,6 +506,9 @@ Result MotorRobotArm::setSpeedScaling(const double scaling) Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options) { + if (const auto stopped = safetyStopResult_("moveJ")) { + return *stopped; + } std::string error; if (!validatePositionCommand_(target, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); @@ -268,7 +522,7 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti BusyGuard busy_guard{busy_}; std::lock_guard lock(mutex_); - std::vector samples; + JointTrajectory samples; if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) { return Result::failure(ArmErrorCode::CommandFailed, "[MotorRobotArm] moveJ planner failed: " + id_); } @@ -284,24 +538,38 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to set cyclic position mode for joint: " + + joint_name); + } } motors.push_back(std::move(motor)); } const auto t0 = std::chrono::steady_clock::now(); constexpr double fallback_dt = 0.001; + std::vector command_velocity(motors.size(), 0.0); for (std::size_t k = 1; k < samples.size(); ++k) { + if (const auto stopped = safetyStopResult_("moveJ", true)) { + return *stopped; + } const auto& sample = samples[k]; if (sample.position.size() != motors.size()) { return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch"); } - for (std::size_t i = 0; i < motors.size(); ++i) { - const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0; - motors[i]->setTarget(sample.position[i], qd); + std::fill(command_velocity.begin(), command_velocity.end(), 0.0); + std::copy_n(sample.velocity.begin(), + std::min(sample.velocity.size(), command_velocity.size()), + command_velocity.begin()); + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, sample.position, command_velocity)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to submit atomic cyclic position command"); } if (k + 1 < samples.size()) { - const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t + const double next_t = samples[k + 1].time_s > 0.0 + ? samples[k + 1].time_s : static_cast(k + 1) * fallback_dt; std::this_thread::sleep_until(t0 + std::chrono::duration_cast( std::chrono::duration(next_t))); @@ -315,6 +583,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, const double duration) { (void)acceleration; + if (const auto stopped = safetyStopResult_("speedJ")) { + return *stopped; + } std::string error; if (!validateVelocityCommand_(velocity, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); @@ -329,9 +600,17 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, "motor not found for joint: " + joint_names_[i]); } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to set cyclic velocity mode for joint: " + + joint_names_[i]); + } + } + if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to command cyclic velocity for joint: " + + joint_names_[i]); } - motor->setTarget(velocity.velocity[i] * speed_scaling_); } } @@ -344,6 +623,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, Result MotorRobotArm::stopJ(const double acceleration) { + if (safetyStopRequested_()) { + return Result::success(); + } JointVelocityCommand zero; zero.velocity.assign(joint_names_.size(), 0.0); return speedJ(zero, acceleration, 0.0); @@ -353,6 +635,9 @@ Result MotorRobotArm::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame) { + if (const auto stopped = safetyStopResult_("moveL")) { + return *stopped; + } if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } @@ -394,8 +679,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target, << ", executable_path_m=" << trajectory.executable_path_length; } - return executeMoveLTrajectory_(trajectory) ? Result::success() - : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); + if (executeMoveLTrajectory_(trajectory)) { + return Result::success(); + } + if (const auto stopped = safetyStopResult_("moveL", true)) { + return *stopped; + } + return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); } Result MotorRobotArm::speedL(const CartesianVelocity& velocity, @@ -403,6 +693,9 @@ Result MotorRobotArm::speedL(const CartesianVelocity& velocity, const double duration, const FrameType frame) { + if (const auto stopped = safetyStopResult_("speedL")) { + return *stopped; + } if (busy_.load()) { return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); } @@ -442,12 +735,17 @@ Result MotorRobotArm::startServoMode(const ServoOptions& options) Result MotorRobotArm::servoJ(const JointPositionCommand& target) { + if (const auto stopped = safetyStopResult_("servoJ")) { + return *stopped; + } std::string error; if (!validatePositionCommand_(target, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); } std::lock_guard lock(mutex_); + std::vector> motors; + motors.reserve(joint_names_.size()); for (std::size_t i = 0; i < joint_names_.size(); ++i) { auto motor = getMotor_(joint_names_[i]); if (!motor) { @@ -455,9 +753,18 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target) "motor not found for joint: " + joint_names_[i]); } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to set cyclic position mode for joint: " + + joint_names_[i]); + } } - motor->setTarget(target.position[i], 0.0); + motors.push_back(std::move(motor)); + } + const std::vector velocities(motors.size(), 0.0); + if (!motor_manager_->commandCyclicPositionsAtomic(motors, target.position, velocities)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to submit atomic cyclic position command"); } return Result::success(); } @@ -725,7 +1032,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj return false; } if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return false; + } } motors.push_back(std::move(motor)); } @@ -733,14 +1042,17 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj auto next_deadline = std::chrono::steady_clock::now(); for (std::size_t i = 1; i < trajectory.position.size(); ++i) { + if (safetyStopRequested_()) { + return false; + } const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]); const auto& position = trajectory.position[i]; const auto& velocity = trajectory.velocity[i]; if (position.size() != motors.size() || velocity.size() != motors.size()) { return false; } - for (std::size_t j = 0; j < motors.size(); ++j) { - motors[j]->setTarget(position[j], velocity[j]); + if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) { + return false; } next_deadline += std::chrono::duration_cast( std::chrono::duration(dt_segment)); diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp new file mode 100644 index 00000000..ea3fd7aa --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp @@ -0,0 +1,881 @@ +#include "arm/motor_robot_arm/include/motor_robot_arm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "common/io/proto_file_io.h" +#include "common/math/transform_math.h" +#include "manager/device_manager/include/device_manager.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" +#include "task/self_collision_task/include/self_collision_task.h" + +namespace cmvr::device { +namespace { + +constexpr std::size_t kDof = 7; +constexpr std::array kJointNames = { + "right_arm_J1", "right_arm_J2", "right_arm_J3", "right_arm_J4", + "right_arm_J5", "right_arm_J6", "right_arm_J7" +}; + +const std::vector kSetupPose{ + 0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0 +}; + +const std::vector kTorsoCollisionPose{ + 1.57607137794121, + 2.06613762981425, + -1.76915077905899, + 0.959251437141443, + -0.725973209527894, + 1.79390262120717, + 0.2223354372144, +}; + +std::filesystem::path findProjectRoot() +{ + const std::filesystem::path marker = "model/gen2/gen2_fixed.xml"; + const auto search = [&](std::filesystem::path current) { + while (!current.empty()) { + if (std::filesystem::exists(current / marker)) { + return current; + } + const auto parent = current.parent_path(); + if (parent == current) { + break; + } + current = parent; + } + return std::filesystem::path{}; + }; + + auto root = search(std::filesystem::current_path()); + if (!root.empty()) { + return root; + } + return search(std::filesystem::path(__FILE__).parent_path()); +} + +double maxPositionError(const std::vector& actual, + const std::vector& expected) +{ + if (actual.size() != expected.size()) { + return std::numeric_limits::infinity(); + } + double error = 0.0; + for (std::size_t i = 0; i < actual.size(); ++i) { + error = std::max(error, std::abs(actual[i] - expected[i])); + } + return error; +} + +double translationError(const CartesianPose& lhs, const CartesianPose& rhs) +{ + return std::sqrt(std::pow(lhs.x - rhs.x, 2.0) + + std::pow(lhs.y - rhs.y, 2.0) + + std::pow(lhs.z - rhs.z, 2.0)); +} + +double rotationError(const CartesianPose& lhs, const CartesianPose& rhs) +{ + const Eigen::Matrix3d lhs_rotation = + common::math::poseToMatrix(lhs).block<3, 3>(0, 0); + const Eigen::Matrix3d rhs_rotation = + common::math::poseToMatrix(rhs).block<3, 3>(0, 0); + return std::abs(Eigen::AngleAxisd(lhs_rotation.transpose() * rhs_rotation).angle()); +} + +Eigen::Vector3d baseRotationDelta(const CartesianPose& start, const CartesianPose& end) +{ + const Eigen::Matrix3d start_rotation = + common::math::poseToMatrix(start).block<3, 3>(0, 0); + const Eigen::Matrix3d end_rotation = + common::math::poseToMatrix(end).block<3, 3>(0, 0); + const Eigen::AngleAxisd delta(end_rotation * start_rotation.transpose()); + return delta.axis() * delta.angle(); +} + +template +void waitFor(Predicate predicate, const std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (!predicate() && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +struct ScenarioOutcome { + Result move_j{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result move_l{Result::failure(ArmErrorCode::UnknownError, "not run")}; + double move_j_error{std::numeric_limits::infinity()}; + double move_l_error{std::numeric_limits::infinity()}; + double move_l_rotation_error{std::numeric_limits::infinity()}; + std::string worker_error; +}; + +class MotorRobotArmGen2MujocoTest : public ::testing::Test { +protected: + void SetUp() override + { + DeviceManager::destroyInstance(); + project_root_ = findProjectRoot(); + ASSERT_FALSE(project_root_.empty()); + + config::MujocoWorldRootConfig world_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(), + &world_root_config)); + ASSERT_GT(world_root_config.worlds_size(), 0); + + auto world_config = world_root_config.worlds(0); + world_config.set_model_path( + (project_root_ / "model/gen2/gen2_fixed.xml").string()); + world_device_ = std::make_shared(world_config); + ASSERT_TRUE(world_device_->init()); + ASSERT_TRUE(world_device_->start()); + + config::MotorRootConfig motor_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / + "cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt").string(), + &motor_root_config)); + + std::unordered_set right_arm_joints; + for (const auto* joint_name : kJointNames) { + right_arm_joints.insert(joint_name); + } + MotorManager::clearActiveJoints(); + MotorManager::setActiveJoints( + "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}}); + + motor_system_ = std::make_shared( + "mujoco_motors", motor_root_config.motor()); + ASSERT_TRUE(motor_system_->init()); + world_ = MotorManager::mujocoWorldFor("mujoco_motors"); + ASSERT_TRUE(world_); + ASSERT_TRUE(world_->isLoaded()); + + config::ArmRootConfig root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / + "cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt").string(), + &root_config)); + ASSERT_GT(root_config.arm().robot_arms_size(), 0); + + auto arm_config = root_config.arm().robot_arms(0); + arm_config.mutable_kinematics() + ->mutable_pinocchio_dls_ik_solver() + ->set_urdf_path((project_root_ / "model/gen2/robot.urdf").string()); + + arm_ = std::make_shared(arm_config); + ASSERT_TRUE(arm_->init()); + const Result torque_result = arm_->torqueOn(); + ASSERT_TRUE(torque_result.ok()) << torque_result.message; + + config::DeviceManagerConfig device_manager_config; + device_manager_config.set_name("gen2_collision_mujoco_test"); + DeviceManager::getInstance(device_manager_config).registerDevice(arm_); + } + + void TearDown() override + { + if (arm_) { + arm_->stop(); + } + if (motor_system_) { + motor_system_->stop(); + } + if (world_device_) { + world_device_->stop(); + } + DeviceManager::destroyInstance(); + MotorManager::clearActiveJoints(); + } + + std::filesystem::path project_root_; + std::shared_ptr world_device_; + std::shared_ptr motor_system_; + std::shared_ptr world_; + std::shared_ptr arm_; +}; + +TEST_F(MotorRobotArmGen2MujocoTest, HoldsInitialPosition) +{ + const auto start = arm_->getJointState().position; + ASSERT_EQ(start.size(), kDof); + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + const auto end = arm_->getJointState().position; + const double drift = maxPositionError(end, start); + + std::cout << "[MotorRobotArmGen2MujocoTest] hold max drift: " + << drift << std::endl; + EXPECT_LT(drift, 0.02); +} + +TEST_F(MotorRobotArmGen2MujocoTest, ProtectiveAndEmergencyStopAreDistinct) +{ + ASSERT_TRUE(arm_->protectiveStop().ok()); + EXPECT_TRUE(arm_->isProtectiveStopped()); + EXPECT_FALSE(arm_->isEmergencyStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::ProtectiveStop); + const auto protective_state = arm_->getRobotState(); + EXPECT_TRUE(protective_state.protective_stopped); + EXPECT_FALSE(protective_state.emergency_stopped); + + MotionOptions options; + options.velocity = 0.6; + options.acceleration = 2.0; + const Result protected_move = arm_->moveJ( + JointPositionCommand{kSetupPose}, options); + EXPECT_EQ(protected_move.code, ArmErrorCode::RobotInProtectiveStop); + + ASSERT_TRUE(arm_->unlockProtectiveStop().ok()); + EXPECT_FALSE(arm_->isProtectiveStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal); + + ASSERT_TRUE(arm_->emergencyStop().ok()); + EXPECT_FALSE(arm_->isProtectiveStopped()); + EXPECT_TRUE(arm_->isEmergencyStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::EmergencyStop); + const auto emergency_state = arm_->getRobotState(); + EXPECT_FALSE(emergency_state.protective_stopped); + EXPECT_TRUE(emergency_state.emergency_stopped); + + const Result rejected_unlock = arm_->unlockProtectiveStop(); + EXPECT_EQ(rejected_unlock.code, ArmErrorCode::RobotInEmergencyStop); + ASSERT_TRUE(arm_->torqueOn().ok()); + EXPECT_FALSE(arm_->isEmergencyStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal); +} + +TEST_F(MotorRobotArmGen2MujocoTest, MoveJ) +{ + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + ScenarioOutcome outcome; + + std::thread scenario([&] { + try { + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + MotionOptions options; + options.velocity = 0.6; + options.acceleration = 2.0; + + outcome.move_j = arm_->moveJ(JointPositionCommand{kSetupPose}, options); + waitFor([&] { + return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + outcome.move_j_error = maxPositionError( + arm_->getJointState().position, kSetupPose); + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(2)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + std::cout << "[MotorRobotArmGen2MujocoTest] moveJ max error: " + << outcome.move_j_error << std::endl; + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message; + EXPECT_LT(outcome.move_j_error, 0.08); +} + +TEST_F(MotorRobotArmGen2MujocoTest, MoveL) +{ + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + ScenarioOutcome outcome; + + std::thread scenario([&] { + try { + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + MotionOptions joint_options; + joint_options.velocity = 1.6; + joint_options.acceleration = 12.0; + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + waitFor([&] { + return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + outcome.move_j_error = maxPositionError( + arm_->getJointState().position, kSetupPose); + if (!outcome.move_j.ok()) { + throw std::runtime_error(outcome.move_j.message); + } + + MotionOptions cartesian_options; + cartesian_options.velocity = 0.08; + cartesian_options.acceleration = 0.4; + cartesian_options.jerk = 1.0; + + MotionOptions rotation_options; + rotation_options.velocity = 0.15; + rotation_options.acceleration = 0.5; + rotation_options.jerk = 2.0; + + const auto return_to_setup = [&](const char* step_name) { + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + if (!outcome.move_j.ok()) { + throw std::runtime_error( + std::string("moveJ before moveL ") + step_name + + ": " + outcome.move_j.message); + } + waitFor([&] { + return maxPositionError( + arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + }; + + struct CartesianStep { + const char* name; + double dx; + double dy; + double dz; + }; + const std::array translation_steps{{ + {"+X", 0.15, 0.0, 0.0}, + {"+Y", 0.0, 0.15, 0.0}, + {"+Z", 0.0, 0.0, 0.15}, + }}; + + struct RotationStep { + const char* name; + double drx; + double dry; + double drz; + }; + constexpr double kRotationStep = + 20.0 * 3.14159265358979323846 / 180.0; + const std::array rotation_steps{{ + {"+RX", kRotationStep, 0.0, 0.0}, + {"+RY", 0.0, kRotationStep, 0.0}, + {"+RZ", 0.0, 0.0, kRotationStep}, + }}; + + outcome.move_l_error = 0.0; + for (std::size_t i = 0; i < translation_steps.size(); ++i) { + const auto& step = translation_steps[i]; + if (i > 0) { + return_to_setup(step.name); + } + CartesianPose target = arm_->getTcpPose(); + target.x += step.dx; + target.y += step.dy; + target.z += step.dz; + + outcome.move_l = arm_->moveL( + target, cartesian_options, FrameType::Base); + if (!outcome.move_l.ok()) { + throw std::runtime_error( + std::string("moveL ") + step.name + ": " + + outcome.move_l.message); + } + waitFor([&] { + return translationError(arm_->getTcpPose(), target) < 0.005; + }, std::chrono::seconds(5)); + const double error = translationError(arm_->getTcpPose(), target); + outcome.move_l_error = std::max(outcome.move_l_error, error); + std::cout << "[MotorRobotArmGen2MujocoTest] moveL " + << step.name << " translation error: " << error + << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + outcome.move_l_rotation_error = 0.0; + for (const auto& step : rotation_steps) { + return_to_setup(step.name); + CartesianPose target = arm_->getTcpPose(); + target.rx += step.drx; + target.ry += step.dry; + target.rz += step.drz; + + outcome.move_l = arm_->moveL( + target, rotation_options, FrameType::Base); + if (!outcome.move_l.ok()) { + throw std::runtime_error( + std::string("moveL ") + step.name + ": " + + outcome.move_l.message); + } + waitFor([&] { + return rotationError(arm_->getTcpPose(), target) < 0.01; + }, std::chrono::seconds(7)); + const double error = rotationError(arm_->getTcpPose(), target); + outcome.move_l_rotation_error = std::max( + outcome.move_l_rotation_error, error); + std::cout << "[MotorRobotArmGen2MujocoTest] moveL " + << step.name << " rotation error: " << error + << " rad" << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(3)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + std::cout << "[MotorRobotArmGen2MujocoTest] moveJ setup max error: " + << outcome.move_j_error + << ", moveL translation error: " << outcome.move_l_error + << ", moveL rotation error: " + << outcome.move_l_rotation_error << " rad" << std::endl; + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message; + EXPECT_LT(outcome.move_j_error, 0.08); + EXPECT_TRUE(outcome.move_l.ok()) << outcome.move_l.message; + EXPECT_LT(outcome.move_l_error, 0.01); + EXPECT_LT(outcome.move_l_rotation_error, 0.02); +} + +TEST_F(MotorRobotArmGen2MujocoTest, SelfCollisionProtectiveStopMoveJMoveLSpeedL) +{ + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + + struct CollisionCaseOutcome { + std::string name; + Result setup_move{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result collision_move{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result recovery{Result::failure(ArmErrorCode::UnknownError, "not run")}; + task::SelfCollisionTaskStatus initial_status; + task::SelfCollisionTaskStatus stop_status; + task::SelfCollisionTaskStatus recovered_status; + CartesianPose start_tcp; + CartesianPose final_tcp; + std::vector final_position; + bool task_initialized{false}; + bool task_started{false}; + bool stop_seen{false}; + bool recovery_succeeded{false}; + bool monitor_ok{true}; + bool protective_stopped{false}; + bool emergency_stopped{false}; + SafetyMode safety_mode{SafetyMode::Unknown}; + }; + + CollisionCaseOutcome move_j_outcome; + CollisionCaseOutcome move_l_outcome; + CollisionCaseOutcome speed_l_outcome; + CartesianPose move_l_target; + CartesianPose collision_tcp_target; + std::string worker_error; + + std::thread scenario([&] { + try { + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + config::SelfCollisionTaskRootConfig root_config; + const auto config_path = project_root_ / + "cmvr-es/config/tasks/self_collision_task/" + "self_collision_task_gen2.pb.txt"; + if (!ProtoMessageIo::getProtoFromAsciiFile( + config_path.string(), &root_config)) { + throw std::runtime_error( + "failed to load self-collision config: " + config_path.string()); + } + auto collision_config = root_config.self_collision_task(); + collision_config.mutable_checker()->set_urdf_path( + (project_root_ / + "model/gen2/collision/robot_collision.urdf").string()); + + Eigen::Matrix4d collision_tcp_transform = Eigen::Matrix4d::Identity(); + const auto solver = arm_->kinematicsSolver(); + if (!solver || + !solver->fk(kTorsoCollisionPose, collision_tcp_transform, true)) { + throw std::runtime_error("failed to calculate collision TCP target"); + } + collision_tcp_target = + common::math::matrixToPose(collision_tcp_transform); + + const auto run_collision_case = [&]( + const std::string& name, + const std::function& start_motion, + const std::chrono::milliseconds stop_timeout) { + CollisionCaseOutcome outcome; + outcome.name = name; + + const Result torque_result = arm_->torqueOn(); + if (!torque_result.ok()) { + throw std::runtime_error( + name + " torqueOn: " + torque_result.message); + } + + MotionOptions setup_options; + setup_options.velocity = 0.6; + setup_options.acceleration = 2.0; + outcome.setup_move = arm_->moveJ( + JointPositionCommand{kSetupPose}, setup_options); + if (!outcome.setup_move.ok()) { + throw std::runtime_error( + name + " setup moveJ: " + outcome.setup_move.message); + } + + task::SelfCollisionTask collision_task(collision_config); + outcome.task_initialized = collision_task.init(); + if (!outcome.task_initialized) { + throw std::runtime_error( + name + " init: " + collision_task.detailStatusString()); + } + outcome.task_started = collision_task.start(); + if (!outcome.task_started || !collision_task.step(0.002)) { + throw std::runtime_error( + name + " start: " + collision_task.detailStatusString()); + } + outcome.initial_status = collision_task.latestStatus(); + if (outcome.initial_status.level != task::CollisionSafetyLevel::SAFE) { + throw std::runtime_error( + name + " setup pose is not SAFE: " + + collision_task.detailStatusString()); + } + outcome.start_tcp = arm_->getTcpPose(); + + std::atomic_bool monitor_running{true}; + std::atomic_bool monitor_ok{true}; + std::atomic_bool stop_seen{false}; + std::thread monitor([&] { + while (monitor_running.load()) { + if (!collision_task.step(0.002)) { + monitor_ok = false; + break; + } + const auto status = collision_task.latestStatus(); + if (status.stop_latched && !stop_seen.load()) { + outcome.stop_status = status; + stop_seen.store(true); + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + }); + + outcome.collision_move = start_motion(); + waitFor([&] { + return stop_seen.load() || !monitor_ok.load(); + }, stop_timeout); + if (!stop_seen.load()) { + arm_->stopMotion(); + monitor_running = false; + monitor.join(); + collision_task.stop(); + throw std::runtime_error(name + " did not trigger protective stop"); + } + + outcome.recovery = collision_task.requestRecovery( + outcome.stop_status.event_id); + outcome.recovered_status = collision_task.latestStatus(); + outcome.recovery_succeeded = outcome.recovery.ok(); + + monitor_running = false; + monitor.join(); + collision_task.stop(); + outcome.stop_seen = stop_seen.load(); + outcome.monitor_ok = monitor_ok.load(); + outcome.final_position = arm_->getJointState().position; + outcome.final_tcp = arm_->getTcpPose(); + outcome.protective_stopped = arm_->isProtectiveStopped(); + outcome.emergency_stopped = arm_->isEmergencyStopped(); + outcome.safety_mode = arm_->getSafetyMode(); + + std::cout << "[MotorRobotArmGen2MujocoTest] collision " + << name + << " stop_seen=" << outcome.stop_seen + << ", distance_m=" + << outcome.stop_status.result.minimum_distance_m + << ", pair=" << outcome.stop_status.result.first + << "/" << outcome.stop_status.result.second + << ", event_id=" << outcome.stop_status.event_id + << ", recovery=" << outcome.recovery.message + << ", recovered_distance_m=" + << outcome.recovered_status.result.minimum_distance_m + << std::endl; + return outcome; + }; + + move_j_outcome = run_collision_case( + "MoveJ", + [&] { + MotionOptions options; + options.velocity = 0.45; + options.acceleration = 1.0; + return arm_->moveJ( + JointPositionCommand{kTorsoCollisionPose}, options); + }, + std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + move_l_outcome = run_collision_case( + "MoveL", + [&] { + move_l_target = collision_tcp_target; + MotionOptions options; + options.velocity = 0.12; + options.acceleration = 0.5; + options.jerk = 2.0; + return arm_->moveL( + move_l_target, options, FrameType::Base); + }, + std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + speed_l_outcome = run_collision_case( + "SpeedL", + [&] { + const CartesianPose start = arm_->getTcpPose(); + Eigen::Vector3d linear_direction{ + collision_tcp_target.x - start.x, + collision_tcp_target.y - start.y, + collision_tcp_target.z - start.z, + }; + Eigen::Vector3d angular_direction = + baseRotationDelta(start, collision_tcp_target); + const double command_duration_s = std::max( + linear_direction.norm() / 0.05, + angular_direction.norm() / 0.20); + if (command_duration_s <= 0.0) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "SpeedL collision target has zero displacement"); + } + linear_direction /= command_duration_s; + angular_direction /= command_duration_s; + std::cout + << "[MotorRobotArmGen2MujocoTest] collision SpeedL target_time=" + << command_duration_s << " s" << std::endl; + return arm_->speedL( + CartesianVelocity{ + linear_direction.x(), + linear_direction.y(), + linear_direction.z(), + angular_direction.x(), + angular_direction.y(), + angular_direction.z(), + }, + 0.5, + 0.0, + FrameType::Base); + }, + std::chrono::seconds(15)); + } catch (const std::exception& error) { + worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(3)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + EXPECT_TRUE(worker_error.empty()) << worker_error; + const auto expect_protective_stop = [&](const CollisionCaseOutcome& outcome) { + EXPECT_TRUE(outcome.setup_move.ok()) + << outcome.name << ": " << outcome.setup_move.message; + EXPECT_TRUE(outcome.task_initialized) << outcome.name; + EXPECT_TRUE(outcome.task_started) << outcome.name; + EXPECT_EQ(outcome.initial_status.level, task::CollisionSafetyLevel::SAFE) + << outcome.name; + EXPECT_TRUE(outcome.monitor_ok) << outcome.name; + EXPECT_TRUE(outcome.stop_seen) << outcome.name; + EXPECT_EQ(outcome.stop_status.level, task::CollisionSafetyLevel::STOP) + << outcome.name; + EXPECT_TRUE(outcome.stop_status.stop_latched) << outcome.name; + EXPECT_NE(outcome.stop_status.event_id, 0U) << outcome.name; + EXPECT_EQ(outcome.stop_status.recovery_state, + task::ProtectiveRecoveryState::AVAILABLE) + << outcome.name; + EXPECT_GE(outcome.stop_status.recovery_sample_count, 2U) + << outcome.name; + EXPECT_LE(outcome.stop_status.result.minimum_distance_m, 0.005) + << outcome.name; + EXPECT_TRUE(outcome.stop_status.result.first == "body_link" || + outcome.stop_status.result.second == "body_link") + << outcome.name; + EXPECT_TRUE(outcome.recovery_succeeded) + << outcome.name << ": " << outcome.recovery.message; + EXPECT_FALSE(outcome.recovered_status.stop_latched) << outcome.name; + EXPECT_EQ(outcome.recovered_status.recovery_state, + task::ProtectiveRecoveryState::SUCCEEDED) + << outcome.name; + EXPECT_GE(outcome.recovered_status.result.minimum_distance_m, 0.025) + << outcome.name; + EXPECT_FALSE(outcome.protective_stopped) << outcome.name; + EXPECT_FALSE(outcome.emergency_stopped) << outcome.name; + EXPECT_EQ(outcome.safety_mode, SafetyMode::Normal) + << outcome.name; + }; + expect_protective_stop(move_j_outcome); + expect_protective_stop(move_l_outcome); + expect_protective_stop(speed_l_outcome); + + EXPECT_EQ(move_j_outcome.collision_move.code, + ArmErrorCode::RobotInProtectiveStop); + EXPECT_GT(maxPositionError(move_j_outcome.final_position, kTorsoCollisionPose), 0.02); + EXPECT_EQ(move_l_outcome.collision_move.code, + ArmErrorCode::RobotInProtectiveStop); + EXPECT_GT(translationError(move_l_outcome.final_tcp, move_l_target), 0.01); + EXPECT_TRUE(speed_l_outcome.collision_move.ok()) + << speed_l_outcome.collision_move.message; + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal); +} + +TEST_F(MotorRobotArmGen2MujocoTest, SpeedL) +{ + constexpr auto kCommandDuration = std::chrono::seconds(2); + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + ScenarioOutcome outcome; + std::array measured_deltas{}; + + std::thread scenario([&] { + try { + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + MotionOptions joint_options; + joint_options.velocity = 0.6; + joint_options.acceleration = 2.0; + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + waitFor([&] { + return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + outcome.move_j_error = maxPositionError( + arm_->getJointState().position, kSetupPose); + if (!outcome.move_j.ok()) { + throw std::runtime_error(outcome.move_j.message); + } + + struct SpeedStep { + const char* name; + CartesianVelocity command; + bool angular; + std::size_t axis; + }; + const std::array steps{{ + {"+X", CartesianVelocity{0.05, 0.0, 0.0, 0.0, 0.0, 0.0}, false, 0}, + {"+Y", CartesianVelocity{0.0, 0.05, 0.0, 0.0, 0.0, 0.0}, false, 1}, + {"+Z", CartesianVelocity{0.0, 0.0, 0.05, 0.0, 0.0, 0.0}, false, 2}, + {"+RX", CartesianVelocity{0.0, 0.0, 0.0, 0.30, 0.0, 0.0}, true, 0}, + {"+RY", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.30, 0.0}, true, 1}, + {"+RZ", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.0, 0.30}, true, 2}, + }}; + + for (std::size_t i = 0; i < steps.size(); ++i) { + const auto& step = steps[i]; + if (i > 0) { + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + if (!outcome.move_j.ok()) { + throw std::runtime_error( + std::string("moveJ before speedL ") + step.name + + ": " + outcome.move_j.message); + } + waitFor([&] { + return maxPositionError( + arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + } + + const CartesianPose start = arm_->getTcpPose(); + const Result speed_result = arm_->speedL( + step.command, 0.5, 0.0, FrameType::Base); + if (!speed_result.ok()) { + throw std::runtime_error( + std::string("speedL ") + step.name + ": " + + speed_result.message); + } + + std::this_thread::sleep_for(kCommandDuration); + const Result stop_result = arm_->stopL(0.5); + if (!stop_result.ok()) { + throw std::runtime_error( + std::string("stopL ") + step.name + ": " + + stop_result.message); + } + waitFor([&] { return !arm_->busy(); }, std::chrono::seconds(3)); + + const CartesianPose end = arm_->getTcpPose(); + if (step.angular) { + measured_deltas[i] = baseRotationDelta(start, end)[step.axis]; + std::cout << "[MotorRobotArmGen2MujocoTest] speedL " + << step.name << " rotation delta: " + << measured_deltas[i] << " rad" << std::endl; + } else { + const Eigen::Vector3d translation_delta{ + end.x - start.x, end.y - start.y, end.z - start.z}; + measured_deltas[i] = translation_delta[step.axis]; + std::cout << "[MotorRobotArmGen2MujocoTest] speedL " + << step.name << " translation delta: " + << measured_deltas[i] << " m" << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(3)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message; + EXPECT_LT(outcome.move_j_error, 0.08); + for (std::size_t i = 0; i < 3; ++i) { + EXPECT_GT(measured_deltas[i], 0.02); + } + for (std::size_t i = 3; i < measured_deltas.size(); ++i) { + EXPECT_GT(measured_deltas[i], 0.10); + } +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 6ab24556..f3b927d2 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -34,6 +34,9 @@ public: virtual Result emergencyStop() = 0; virtual Result protectiveStop() = 0; + virtual Result recoverProtectiveStop( + const JointTrajectory& path, + const MotionOptions& options) = 0; virtual Result setSpeedScaling(double scaling) = 0; virtual double getSpeedScaling() const = 0; virtual bool isProtectiveStopped() const = 0; diff --git a/cmvr-es/devices/arm/robot_arm_factory.h b/cmvr-es/devices/arm/robot_arm_factory.h index 5f504392..0a23742c 100644 --- a/cmvr-es/devices/arm/robot_arm_factory.h +++ b/cmvr-es/devices/arm/robot_arm_factory.h @@ -6,7 +6,7 @@ #include "cmvr/config/arm_config/arm_config.pb.h" #include "common/base/logging/logger.h" -#include "devices/arm/aubo_arm/include/aubo_arm.h" +#include "devices/arm/aubo_arm/aubo_arm.h" #include "devices/arm/huayan_arm/huayan_arm.h" #include "devices/arm/motor_robot_arm/include/motor_robot_arm.h" diff --git a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h index 69366ef4..6dfd8374 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h @@ -30,7 +30,7 @@ namespace cmvr { return BASE_ID + sdo_frame_.node_id(); } - void SetFrameData(msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data) { + void SetFrameData(msgs::CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data) { std::lock_guard lock(mutex_); sdo_frame_.set_cs(cs); sdo_frame_.set_index(index); diff --git a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h index 0a48af3e..ca78acd4 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h @@ -49,10 +49,10 @@ namespace cmvr { auto command = static_cast(bytes[0]); // 解析 index（字节1和字节2，低字节优先） - auto index = static_cast(bytes[1] + (bytes[2] << 8)); + const uint32_t index = bytes[1] + (bytes[2] << 8); // 解析 subindex（字节3） - auto subindex = static_cast(bytes[3]); + const uint32_t subindex = bytes[3]; // 根据 command 解析 data（字节4~7） uint32_t data = 0; @@ -94,4 +94,4 @@ namespace cmvr { ParseSdoData(sdo_response_, sensor_data); } } -} \ No newline at end of file +} diff --git a/cmvr-es/devices/device_types.h b/cmvr-es/devices/device_types.h index b79521be..ddf55a62 100644 --- a/cmvr-es/devices/device_types.h +++ b/cmvr-es/devices/device_types.h @@ -70,6 +70,87 @@ namespace cmvr::device { std::string type_name; }; + // DeviceManager lifecycle and device-reported health are deliberately + // separate. A device can, for example, be READY from the manager's point + // of view while its backend has not implemented health reporting yet. + enum class ManagedDeviceState { + Unknown, + Disabled, + Initializing, + Registered, + Ready, + Running, + Stopped, + Error, + }; + + enum class DeviceHealthState { + Unknown, + Healthy, + Degraded, + Fault, + }; + + inline std::string toString(ManagedDeviceState state) { + switch (state) { + case ManagedDeviceState::Disabled: + return "Disabled"; + case ManagedDeviceState::Initializing: + return "Initializing"; + case ManagedDeviceState::Registered: + return "Registered"; + case ManagedDeviceState::Ready: + return "Ready"; + case ManagedDeviceState::Running: + return "Running"; + case ManagedDeviceState::Stopped: + return "Stopped"; + case ManagedDeviceState::Error: + return "Error"; + case ManagedDeviceState::Unknown: + default: + return "Unknown"; + } + } + + inline std::string toString(DeviceHealthState state) { + switch (state) { + case DeviceHealthState::Healthy: + return "Healthy"; + case DeviceHealthState::Degraded: + return "Degraded"; + case DeviceHealthState::Fault: + return "Fault"; + case DeviceHealthState::Unknown: + default: + return "Unknown"; + } + } + + struct DeviceHealthSnapshot { + DeviceHealthState state = DeviceHealthState::Unknown; + std::string error_message; + }; + + struct ManagedDeviceSnapshot { + std::string id; + DeviceKind kind = DeviceKind::Unknown; + std::string type_name; + bool enabled = false; + ManagedDeviceState state = ManagedDeviceState::Unknown; + DeviceHealthSnapshot health; + bool abnormal = false; + std::string error_message; + std::uint64_t status_updated_at_unix_ms = 0; + }; + + struct DeviceManagerSnapshot { + std::string name; + std::string version; + std::string description; + std::vector devices; + }; + } // namespace cmvr::device #endif // CMVR_ES_DEVICE_TYPES_H diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 1cf8b9c7..9cdb53fb 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -130,6 +130,24 @@ namespace cmvr::device { virtual Status state() const = 0; virtual std::string lastError() const = 0; + DeviceHealthSnapshot healthSnapshot() override { + const auto lifecycle = state(); + const auto error = lastError(); + + DeviceHealthSnapshot health; + health.error_message = error; + if (lifecycle == Status::FAULT) { + health.state = DeviceHealthState::Fault; + } else if (!error.empty()) { + health.state = DeviceHealthState::Degraded; + } else if (lifecycle == Status::INITIALIZED || + lifecycle == Status::STREAMING || + lifecycle == Status::STOPPED) { + health.state = DeviceHealthState::Healthy; + } + return health; + } + virtual void getState(DexHandState& state) { state = DexHandState{}; const auto lifecycle = this->state(); diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 76689b2f..c3081330 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,6 +1,10 @@ add_library(motor_core INTERFACE) -target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices) +target_include_directories(motor_core + INTERFACE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/devices +) target_link_libraries(motor_core INTERFACE @@ -12,4 +16,5 @@ add_library(cmvr_es::device::motor_core ALIAS motor_core) add_subdirectory(drivers/ti5_canopen) add_subdirectory(drivers/mujoco) add_subdirectory(bus_runtime) +add_subdirectory(drivers/ethercat_motor) add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 983ccec7..28a7d288 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -48,13 +48,13 @@ namespace cmvr::device{ DeviceKind kind() const noexcept override { return DeviceKind::Motor; } - virtual void setMode(msgs::RunMode mode) { + virtual bool setMode(msgs::RunMode mode) { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->setMode(node_id_, mode); + return protocol_->setMode(node_id_, mode); } virtual msgs::RunMode getMode() { @@ -66,13 +66,22 @@ namespace cmvr::device{ return protocol_->getMode(node_id_); } - virtual void torqueOff() { + virtual bool torqueOn() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->torqueOff(node_id_); + return protocol_->torqueOn(node_id_); + } + + virtual bool torqueOff() { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->torqueOff(node_id_); } virtual void setLimitQ(double ub, double lb) { @@ -101,43 +110,69 @@ namespace cmvr::device{ } // virtual void setLimitTau(double tau) = 0; // virtual void setLimitCurrent(double tau) = 0; - virtual void brake() { + virtual bool brakeRelease() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->brake(node_id_); - } - /** - * - * @param q unit : rad - */ - virtual void setQ(double q) { - std::scoped_lock lock(mtx_); - if (!protocol_) { - CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; - } - protocol_->setQ(node_id_, q); + return protocol_->brakeRelease(node_id_); } - virtual void setTarget(double q,double qd) { + virtual bool quickStop() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->setTarget(node_id_,q, qd); + return protocol_->quickStop(node_id_); + } + virtual bool commandProfilePosition(double target_q, + double max_qd = 0.0, + double max_qdd = 0.0) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandProfilePosition(node_id_, target_q, max_qd, max_qdd); } - virtual void setTarget(double qd) { + virtual bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->setTarget(node_id_, qd); + return protocol_->commandProfileVelocity(node_id_, target_qd, max_qdd); + } + + virtual bool commandCyclicPosition(double target_q, + double target_qd = 0.0) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicPosition(node_id_, target_q, target_qd); + } + + virtual bool commandCyclicVelocity(double target_qd) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicVelocity(node_id_, target_qd); + } + + virtual bool commandCyclicTorque(double target_tau) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicTorque(node_id_, target_tau); } virtual bool calibrateZeroQ() { @@ -156,16 +191,6 @@ namespace cmvr::device{ } return protocol_->reachedTargetQ(node_id_); } - // rad /s - virtual void setQd(double qd) { - std::scoped_lock lock(mtx_); - if (!protocol_) { - CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; - } - return protocol_->setQd(node_id_,qd); - } - // virtual void setQdd(double qdd) = 0; // rad /s^2 // virtual void setTau(double tau) = 0; // N m // virtual void clear_err() = 0; // virtual void getStatus() = 0; diff --git a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt index 238d1841..ce09edab 100644 --- a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt +++ b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt @@ -4,7 +4,18 @@ add_library(motor_bus_runtime SHARED ethercat/src/ethercat_motor_bus_runtime.cpp ) -target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +set(IGH_ETHERCAT_ROOT + ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0 +) + +target_include_directories(motor_bus_runtime + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + PRIVATE + ${IGH_ETHERCAT_ROOT}/include +) + +target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib) target_link_libraries(motor_bus_runtime PUBLIC @@ -12,9 +23,28 @@ target_link_libraries(motor_bus_runtime cmvr_es::device::motor_core cmvr_es::mujoco_world PRIVATE + ethercat cmvr_es::device::canbus glog ) add_library(cmvr_es::device::motor_bus_runtime ALIAS motor_bus_runtime) install(TARGETS motor_bus_runtime LIBRARY DESTINATION lib) + +add_executable(ethercat_motor_bus_runtime_real_test + ethercat/src/ethercat_motor_bus_runtime_real_test.cpp +) + +target_include_directories(ethercat_motor_bus_runtime_real_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es +) + +target_link_libraries(ethercat_motor_bus_runtime_real_test + PRIVATE + cmvr_es::device::motor_bus_runtime + gtest + gtest_main + pthread + glog +) diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h index 5d7f8edf..02150507 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h @@ -1,15 +1,45 @@ #ifndef CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H #define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H +#include +#include +#include +#include +#include +#include #include +#include +#include #include +#include #include "../../abstract_motor_bus_runtime.h" +#include "motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h" + +typedef struct ec_domain ec_domain_t; +typedef struct ec_master ec_master_t; +typedef struct ec_slave_config ec_slave_config_t; namespace cmvr::device { class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime { public: + struct PdoWrite { + int motor_id{0}; + std::uint16_t index{0}; + std::uint8_t subindex{0}; + std::uint8_t bit_length{0}; + std::uint64_t raw_value{0}; + }; + + struct PdoRead { + int motor_id{0}; + std::uint16_t index{0}; + std::uint8_t subindex{0}; + std::uint8_t bit_length{0}; + std::uint64_t raw_value{0}; + }; + bool init(const config::MotorGroupConfig& group_cfg) override; bool start() override; void stop() override; @@ -17,12 +47,194 @@ public: const std::string& id() const { return id_; } const config::EtherCATConfig& config() const { return config_; } + void setPdoMapping(EthercatPdoMapping mapping); + const EthercatPdoMapping& pdoMapping() const { return pdo_mapping_; } const config::EthercatSlaveConfig* slaveForMotor(int motor_id) const; + bool hasMotor(int motor_id) const; + + bool hasPdoEntry(int motor_id, std::uint16_t index, std::uint8_t subindex) const; + + template + bool writePdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value) + { + return writePdoRaw_(motor_id, index, subindex, valueBitLength_(), + toRawValue_(value)); + } + + template + static PdoWrite makePdoWrite(int motor_id, + std::uint16_t index, + std::uint8_t subindex, + T value) + { + return PdoWrite{motor_id, index, subindex, valueBitLength_(), toRawValue_(value)}; + } + + bool writePdosAtomic(const PdoWrite* writes, std::size_t count); + + template + static PdoRead makePdoRead(int motor_id, + std::uint16_t index, + std::uint8_t subindex) + { + return PdoRead{motor_id, index, subindex, valueBitLength_(), 0}; + } + + template + static T pdoReadValue(const PdoRead& read) + { + return fromRawValue_(read.raw_value); + } + + bool readPdosAtomic(PdoRead* reads, std::size_t count) const; + + std::uint64_t commandGeneration() const { return command_generation_.load(); } + std::uint64_t sentCommandGeneration() const { return sent_command_generation_.load(); } + bool isHealthy() const { return healthy_.load(); } + + template + bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const + { + std::uint64_t raw = 0; + if (!readPdoRaw_(motor_id, index, subindex, valueBitLength_(), raw)) { + return false; + } + value = fromRawValue_(raw); + return true; + } + + template + bool writeSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value) + { + return writeSdoRaw_(motor_id, index, subindex, valueBitLength_(), + toRawValue_(value)); + } + + template + bool readSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) + { + std::uint64_t raw = 0; + if (!readSdoRaw_(motor_id, index, subindex, valueBitLength_(), raw)) { + return false; + } + value = fromRawValue_(raw); + return true; + } private: + struct PdoEntryRuntime { + EthercatPdoEntryConfig cfg; + unsigned int offset{0}; + bool rx{false}; + std::uint64_t value{0}; + }; + + struct SlaveRuntime { + config::EthercatSlaveConfig cfg; + ec_slave_config_t* slave_config{nullptr}; + std::unordered_map pdo_entries; + }; + + struct BusHealthState { + bool initialized{false}; + bool healthy{false}; + unsigned int slaves_responding{0}; + unsigned int master_al_states{0}; + bool link_up{false}; + int domain_result{0}; + unsigned int working_counter{0}; + unsigned int wc_state{0}; + }; + + struct SlaveHealthState { + bool initialized{false}; + bool healthy{false}; + int result{0}; + bool online{false}; + bool operational{false}; + unsigned int al_state{0}; + }; + + bool configureSlave_(SlaveRuntime& slave); + bool configureDc_(); + bool waitSlavesOperational_(); + void cyclicLoop_(); + void monitorBusHealth_(); + void readFeedbackLocked_(); + void writeCommandsLocked_(); + void releaseMaster_(); + bool hasValidPdoMapping_() const; + bool writePdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t value); + bool readPdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t& value) const; + bool writeSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t value); + bool readSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t& value); + + template + static constexpr std::uint8_t valueBitLength_() + { + using ValueType = std::remove_cv_t; + static_assert(std::is_integral_v, "EtherCAT object values must be integral"); + static_assert(!std::is_same_v, "bool is not a valid EtherCAT object value"); + static_assert(sizeof(ValueType) == 1 || sizeof(ValueType) == 2 || sizeof(ValueType) == 4, + "only 8/16/32-bit EtherCAT object values are supported"); + return static_cast(sizeof(ValueType) * 8); + } + + template + static std::uint64_t toRawValue_(T value) + { + using ValueType = std::remove_cv_t; + using UnsignedType = std::make_unsigned_t; + return static_cast(static_cast(value)); + } + + template + static T fromRawValue_(std::uint64_t raw) + { + using ValueType = std::remove_cv_t; + using UnsignedType = std::make_unsigned_t; + const auto unsigned_value = static_cast(raw); + ValueType value{}; + std::memcpy(&value, &unsigned_value, sizeof(ValueType)); + return value; + } + + static std::uint32_t pdoEntryKey_(std::uint16_t index, std::uint8_t subindex); + static std::string hexIndex_(std::uint32_t index); + static std::uint64_t maskValue_(std::uint64_t value, std::uint8_t bit_len); + static bool isSupportedBitLength_(std::uint8_t bit_len); + static std::uint64_t readEntryValue_(const std::uint8_t* domain_data, + const PdoEntryRuntime& entry); + static void writeEntryValue_(std::uint8_t* domain_data, + const PdoEntryRuntime& entry); + static std::uint64_t steadyTimeNs_(); + static std::uint64_t timePointNs_(std::chrono::steady_clock::time_point time_point); + static std::uint32_t usToNs_(std::uint32_t value_us); + static std::int32_t usToNs_(std::int32_t value_us); + std::string id_; config::EtherCATConfig config_; - std::unordered_map slaves_by_motor_id_; + EthercatPdoMapping pdo_mapping_; + std::unordered_map slaves_by_motor_id_; + + ec_master_t* master_{nullptr}; + ec_domain_t* domain_{nullptr}; + std::uint8_t* domain_data_{nullptr}; + + mutable std::mutex data_mutex_; + std::thread cyclic_thread_; + std::atomic running_{false}; + std::atomic healthy_{false}; + std::atomic health_monitor_enabled_{false}; + std::atomic command_generation_{0}; + std::atomic sent_command_generation_{0}; + BusHealthState last_bus_health_; + std::unordered_map last_slave_health_; + bool initialized_{false}; bool started_{false}; }; diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h new file mode 100644 index 00000000..30db0781 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h @@ -0,0 +1,35 @@ +#ifndef CMVR_ES_ETHERCAT_PDO_MAPPING_H +#define CMVR_ES_ETHERCAT_PDO_MAPPING_H + +#include +#include +#include + +namespace cmvr::device { + +struct EthercatPdoEntryConfig { + std::uint16_t index{0}; + std::uint8_t subindex{0}; + std::uint8_t bit_len{0}; + std::string name; + bool padding{false}; +}; + +struct EthercatPdoConfig { + std::uint16_t index{0}; + std::uint8_t sync_manager{0}; + bool rx{false}; + std::vector entries; +}; + +struct EthercatPdoMapping { + std::uint32_t vendor_id{0}; + std::uint32_t product_code{0}; + std::string name; + std::vector rx_pdos; + std::vector tx_pdos; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ETHERCAT_PDO_MAPPING_H diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp index 45c44243..d6148be4 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp @@ -1,7 +1,18 @@ #include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include +#include +#include +#include +#include +#include +#include +#include + #include "common/base/logging/logger.h" +#include + namespace cmvr::device { bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) @@ -21,14 +32,34 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) } config_ = group_cfg.ethercat(); - if (config_.master_id().empty()) { - CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] master_id is empty: " << id_; - return false; - } if (config_.cycle_us() <= 0) { CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] cycle_us must be positive: " << id_; return false; } + if (!config_.has_slave_op_timeout_ms()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_op_timeout_ms: " << id_; + return false; + } + if (config_.slave_op_timeout_ms() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_op_timeout_ms must be positive: " + << id_; + return false; + } + if (!config_.has_slave_state_poll_period_ms()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_state_poll_period_ms: " + << id_; + return false; + } + if (config_.slave_state_poll_period_ms() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_state_poll_period_ms must be positive: " + << id_; + return false; + } + if (!hasValidPdoMapping_()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] PDO mapping is not configured: " + << id_; + return false; + } slaves_by_motor_id_.clear(); for (const auto& slave : config_.slaves()) { @@ -36,34 +67,113 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid motor_id in slave config: " << id_; return false; } - if (slave.slave_index() < 0) { - CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid slave_index for motor " - << slave.motor_id() << " in group: " << id_; - return false; - } if (slaves_by_motor_id_.count(slave.motor_id()) > 0) { CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate slave motor_id: " << slave.motor_id() << " in group: " << id_; return false; } - slaves_by_motor_id_[slave.motor_id()] = &slave; + SlaveRuntime runtime; + runtime.cfg = slave; + slaves_by_motor_id_.emplace(slave.motor_id(), std::move(runtime)); } + if (slaves_by_motor_id_.empty()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] no EtherCAT slaves configured: " << id_; + return false; + } + + master_ = ecrt_request_master(config_.master_index()); + if (!master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to request EtherCAT master " + << config_.master_index() << ": " << id_; + return false; + } + + domain_ = ecrt_master_create_domain(master_); + if (!domain_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to create EtherCAT domain: " << id_; + releaseMaster_(); + return false; + } + + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + if (!configureSlave_(slave)) { + releaseMaster_(); + return false; + } + } + + if (!configureDc_()) { + releaseMaster_(); + return false; + } + + initialized_ = true; return true; } +void EthercatMotorBusRuntime::setPdoMapping(EthercatPdoMapping mapping) +{ + pdo_mapping_ = std::move(mapping); +} + bool EthercatMotorBusRuntime::start() { if (started_) { return true; } - CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] EtherCAT master is not implemented yet: " << id_; - return false; + if (!initialized_ || !master_ || !domain_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized: " << id_; + return false; + } + + if (ecrt_master_activate(master_) != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to activate EtherCAT master: " << id_; + return false; + } + + domain_data_ = ecrt_domain_data(domain_); + if (!domain_data_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to get EtherCAT domain data: " << id_; + return false; + } + + { + std::lock_guard lock(data_mutex_); + writeCommandsLocked_(); + } + + running_.store(true); + healthy_.store(false); + health_monitor_enabled_.store(false); + last_bus_health_ = {}; + last_slave_health_.clear(); + cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this); + started_ = true; + + if (!waitSlavesOperational_()) { + stop(); + return false; + } + health_monitor_enabled_.store(true); + + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_; + return true; } void EthercatMotorBusRuntime::stop() { + running_.store(false); + healthy_.store(false); + health_monitor_enabled_.store(false); + if (cyclic_thread_.joinable()) { + cyclic_thread_.join(); + } started_ = false; + initialized_ = false; + domain_data_ = nullptr; + releaseMaster_(); } const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const int motor_id) const @@ -72,7 +182,936 @@ const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const if (it == slaves_by_motor_id_.end()) { return nullptr; } - return it->second; + return &it->second.cfg; +} + +bool EthercatMotorBusRuntime::hasMotor(const int motor_id) const +{ + return slaves_by_motor_id_.count(motor_id) > 0; +} + +bool EthercatMotorBusRuntime::hasPdoEntry(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex) const +{ + std::lock_guard lock(data_mutex_); + const auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + return slave_it->second.pdo_entries.count(pdoEntryKey_(index, subindex)) > 0; +} + +bool EthercatMotorBusRuntime::configureSlave_(SlaveRuntime& slave) +{ + slave.slave_config = ecrt_master_slave_config(master_, + static_cast(slave.cfg.alias()), + static_cast(slave.cfg.position()), + pdo_mapping_.vendor_id, + pdo_mapping_.product_code); + if (!slave.slave_config) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to get slave config: group=" << id_ + << ", motor_id=" << slave.cfg.motor_id() + << ", alias=" << slave.cfg.alias() + << ", position=" << slave.cfg.position() + << ", vendor=" << hexIndex_(pdo_mapping_.vendor_id) + << ", product=" << hexIndex_(pdo_mapping_.product_code); + return false; + } + + struct SyncBuild { + bool rx{false}; + std::vector pdo_indices; + std::vector> entry_storage; + std::vector pdo_infos; + }; + + auto add_pdo_to_sync_build = [](std::map& builds, + const EthercatPdoConfig& pdo) { + auto& build = builds[pdo.sync_manager]; + build.rx = pdo.rx; + build.pdo_indices.push_back(pdo.index); + auto& entries = build.entry_storage.emplace_back(); + entries.reserve(pdo.entries.size()); + for (const auto& entry : pdo.entries) { + entries.push_back({entry.index, entry.subindex, entry.bit_len}); + } + }; + + std::map sync_builds; + for (const auto& pdo : pdo_mapping_.rx_pdos) { + add_pdo_to_sync_build(sync_builds, pdo); + } + for (const auto& pdo : pdo_mapping_.tx_pdos) { + add_pdo_to_sync_build(sync_builds, pdo); + } + + for (auto& [sync_manager, build] : sync_builds) { + (void)sync_manager; + build.pdo_infos.reserve(build.entry_storage.size()); + for (std::size_t i = 0; i < build.entry_storage.size(); ++i) { + auto& entries = build.entry_storage[i]; + build.pdo_infos.push_back({ + build.pdo_indices[i], + static_cast(entries.size()), + entries.data(), + }); + } + } + + std::vector sync_infos; + sync_infos.push_back({0, EC_DIR_OUTPUT, 0, nullptr, EC_WD_DISABLE}); + sync_infos.push_back({1, EC_DIR_INPUT, 0, nullptr, EC_WD_DISABLE}); + for (auto& [sync_manager, build] : sync_builds) { + const auto direction = build.rx ? EC_DIR_OUTPUT : EC_DIR_INPUT; + const auto watchdog = build.rx ? EC_WD_ENABLE : EC_WD_DISABLE; + sync_infos.push_back({ + sync_manager, + direction, + static_cast(build.pdo_infos.size()), + build.pdo_infos.data(), + watchdog, + }); + } + sync_infos.push_back({0xff}); + + if (ecrt_slave_config_pdos(slave.slave_config, EC_END, sync_infos.data()) != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure PDOs: group=" << id_ + << ", motor_id=" << slave.cfg.motor_id() + << ", position=" << slave.cfg.position(); + return false; + } + + auto register_entry = [&](const EthercatPdoEntryConfig& entry, const bool rx) -> bool { + if (entry.padding) { + return true; + } + if (!isSupportedBitLength_(entry.bit_len)) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported PDO entry bit length: " + << static_cast(entry.bit_len) + << ", entry=" << hexIndex_(entry.index) + << ":" << static_cast(entry.subindex) + << ", motor_id=" << slave.cfg.motor_id() + << ", group=" << id_; + return false; + } + + const auto key = pdoEntryKey_(entry.index, entry.subindex); + if (slave.pdo_entries.count(key) > 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate PDO entry: " + << hexIndex_(entry.index) + << ":" << static_cast(entry.subindex) + << ", motor_id=" << slave.cfg.motor_id() + << ", group=" << id_; + return false; + } + + const int result = ecrt_slave_config_reg_pdo_entry( + slave.slave_config, entry.index, entry.subindex, domain_, nullptr); + if (result < 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to register PDO entry " + << hexIndex_(entry.index) << ":" << static_cast(entry.subindex) + << ", motor_id=" << slave.cfg.motor_id() + << ", group=" << id_; + return false; + } + + PdoEntryRuntime runtime; + runtime.cfg = entry; + runtime.offset = static_cast(result); + runtime.rx = rx; + slave.pdo_entries.emplace(key, std::move(runtime)); + return true; + }; + + for (const auto& pdo : pdo_mapping_.rx_pdos) { + for (const auto& entry : pdo.entries) { + if (!register_entry(entry, true)) { + return false; + } + } + } + for (const auto& pdo : pdo_mapping_.tx_pdos) { + for (const auto& entry : pdo.entries) { + if (!register_entry(entry, false)) { + return false; + } + } + } + + return true; +} + +bool EthercatMotorBusRuntime::configureDc_() +{ + if (!config_.has_dc()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing explicit DC config: " + << id_ << ". Add dc { enable: false } or a complete enabled DC config."; + return false; + } + + const auto& dc = config_.dc(); + if (!dc.has_enable()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC enable: " << id_; + return false; + } + if (!dc.enable()) { + return true; + } + + if (!dc.has_reference_motor_id()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC reference_motor_id: " + << id_; + return false; + } + if (!dc.has_sync0_cycle_us()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_cycle_us: " + << id_; + return false; + } + if (!dc.has_sync0_shift_us()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_shift_us: " + << id_; + return false; + } + if (!dc.has_sync_reference_clock_period()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_reference_clock_period: " + << id_; + return false; + } + if (!dc.has_assign_activate()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC assign_activate: " + << id_; + return false; + } + if (!dc.has_sync_monitor_period_ms()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_monitor_period_ms: " + << id_; + return false; + } + if (dc.reference_motor_id() <= 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference_motor_id must be positive: " + << id_; + return false; + } + if (dc.sync0_cycle_us() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync0_cycle_us must be positive: " + << id_; + return false; + } + if (dc.sync_reference_clock_period() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_reference_clock_period must be positive: " + << id_; + return false; + } + if (dc.assign_activate() == 0 || dc.assign_activate() > 0xFFFFU) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC assign_activate must be in [1, 0xFFFF]: " + << id_ << ", value=" << hexIndex_(dc.assign_activate()); + return false; + } + if (dc.sync_monitor_period_ms() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_monitor_period_ms must be positive: " + << id_; + return false; + } + + const auto reference_it = slaves_by_motor_id_.find(dc.reference_motor_id()); + if (reference_it == slaves_by_motor_id_.end() || !reference_it->second.slave_config) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference motor is not configured: " + << id_ << ", reference_motor_id=" << dc.reference_motor_id(); + return false; + } + + const int select_result = ecrt_master_select_reference_clock( + master_, reference_it->second.slave_config); + if (select_result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to select DC reference clock: " + << id_ << ", reference_motor_id=" << dc.reference_motor_id() + << ", result=" << select_result; + return false; + } + + const auto assign_activate = static_cast(dc.assign_activate()); + const auto sync0_cycle_ns = usToNs_(dc.sync0_cycle_us()); + const auto sync0_shift_ns = usToNs_(dc.sync0_shift_us()); + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + const int result = ecrt_slave_config_dc(slave.slave_config, + assign_activate, + sync0_cycle_ns, + sync0_shift_ns, + 0, + 0); + if (result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure DC: " + << id_ << ", motor_id=" << motor_id + << ", position=" << slave.cfg.position() + << ", result=" << result; + return false; + } + } + + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] configured DC: " + << id_ + << ", reference_motor_id=" << dc.reference_motor_id() + << ", sync0_cycle_ns=" << sync0_cycle_ns + << ", sync0_shift_ns=" << sync0_shift_ns + << ", sync_reference_clock_period=" << dc.sync_reference_clock_period() + << ", assign_activate=" << hexIndex_(assign_activate) + << ", sync_monitor_period_ms=" << dc.sync_monitor_period_ms(); + return true; +} + +bool EthercatMotorBusRuntime::waitSlavesOperational_() +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.slave_op_timeout_ms()); + const auto poll_period = + std::chrono::milliseconds(config_.slave_state_poll_period_ms()); + + ec_domain_state_t last_domain_state{}; + int last_domain_result = 0; + do { + bool all_slaves_operational = true; + for (const auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + ec_slave_config_state_t state{}; + const int result = ecrt_slave_config_state(slave.slave_config, &state); + if (result != 0 || + !state.online || + !state.operational || + state.al_state != EC_AL_STATE_OP) { + all_slaves_operational = false; + break; + } + } + + last_domain_result = ecrt_domain_state(domain_, &last_domain_state); + const bool domain_complete = + last_domain_result == 0 && + last_domain_state.wc_state == EC_WC_COMPLETE; + + if (all_slaves_operational && domain_complete) { + healthy_.store(true); + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: " + << id_ + << ", working_counter=" << last_domain_state.working_counter; + return true; + } + + std::this_thread::sleep_for(poll_period); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] timeout waiting for EtherCAT slaves OP: " + << id_ + << ", timeout_ms=" << config_.slave_op_timeout_ms() + << ", domain_result=" << last_domain_result + << ", domain_wc_state=" << static_cast(last_domain_state.wc_state) + << ", working_counter=" << last_domain_state.working_counter; + + for (const auto& [motor_id, slave] : slaves_by_motor_id_) { + ec_slave_config_state_t state{}; + const int result = ecrt_slave_config_state(slave.slave_config, &state); + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave state: " + << id_ + << ", motor_id=" << motor_id + << ", position=" << slave.cfg.position() + << ", result=" << result + << ", online=" << state.online + << ", operational=" << state.operational + << ", al_state=" << static_cast(state.al_state); + } + return false; +} + +void EthercatMotorBusRuntime::cyclicLoop_() +{ + const auto period = std::chrono::microseconds(config_.cycle_us()); + const bool dc_enabled = config_.dc().enable(); + const auto dc_sync_period = dc_enabled ? config_.dc().sync_reference_clock_period() : 0U; + const auto dc_monitor_period = + dc_enabled ? std::chrono::milliseconds(config_.dc().sync_monitor_period_ms()) + : std::chrono::milliseconds(0); + std::uint32_t dc_sync_counter = 0; + bool dc_monitor_queued = false; + auto cycle_time = std::chrono::steady_clock::now(); + auto next_dc_monitor_time = cycle_time + dc_monitor_period; + const auto bus_monitor_period = std::chrono::milliseconds(100); + auto next_bus_monitor_time = cycle_time; + while (running_.load()) { + if (dc_enabled) { + ecrt_master_application_time(master_, timePointNs_(cycle_time)); + } + + ecrt_master_receive(master_); + ecrt_domain_process(domain_); + if (dc_enabled && dc_monitor_queued) { + const std::uint32_t dc_sync_diff_ns = + ecrt_master_sync_monitor_process(master_); + if (dc_sync_diff_ns == static_cast(-1)) { + CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] DC sync monitor failed: " + << id_; + } else { + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] dc_sync_diff_ns=" + << dc_sync_diff_ns + << ", group=" << id_; + } + dc_monitor_queued = false; + } + { + std::lock_guard lock(data_mutex_); + readFeedbackLocked_(); + writeCommandsLocked_(); + sent_command_generation_.store(command_generation_.load()); + } + ecrt_domain_queue(domain_); + if (dc_enabled) { + ++dc_sync_counter; + if (dc_sync_counter >= dc_sync_period) { + dc_sync_counter = 0; + ecrt_master_sync_reference_clock(master_); + } + ecrt_master_sync_slave_clocks(master_); + + if (cycle_time >= next_dc_monitor_time) { + const int monitor_result = ecrt_master_sync_monitor_queue(master_); + if (monitor_result == 0) { + dc_monitor_queued = true; + } else { + CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] failed to queue DC sync monitor: " + << id_ << ", result=" << monitor_result; + } + do { + next_dc_monitor_time += dc_monitor_period; + } while (cycle_time >= next_dc_monitor_time); + } + } + ecrt_master_send(master_); + + if (health_monitor_enabled_.load() && cycle_time >= next_bus_monitor_time) { + monitorBusHealth_(); + do { + next_bus_monitor_time += bus_monitor_period; + } while (cycle_time >= next_bus_monitor_time); + } + + cycle_time += period; + std::this_thread::sleep_until(cycle_time); + } +} + +void EthercatMotorBusRuntime::monitorBusHealth_() +{ + if (!master_ || !domain_) { + healthy_.store(false); + return; + } + + ec_master_state_t master_state{}; + ecrt_master_state(master_, &master_state); + ec_domain_state_t domain_state{}; + const int domain_result = ecrt_domain_state(domain_, &domain_state); + + bool all_slaves_healthy = true; + for (const auto& [motor_id, slave] : slaves_by_motor_id_) { + ec_slave_config_state_t state{}; + const int result = ecrt_slave_config_state(slave.slave_config, &state); + const bool slave_healthy = result == 0 && state.online && state.operational && + state.al_state == EC_AL_STATE_OP; + const SlaveHealthState current{ + true, + slave_healthy, + result, + state.online != 0, + state.operational != 0, + static_cast(state.al_state), + }; + auto& previous = last_slave_health_[motor_id]; + const bool changed = !previous.initialized || + previous.healthy != current.healthy || + previous.result != current.result || + previous.online != current.online || + previous.operational != current.operational || + previous.al_state != current.al_state; + if (changed && !current.healthy) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave communication unhealthy: " + << id_ + << ", motor_id=" << motor_id + << ", result=" << current.result + << ", online=" << current.online + << ", operational=" << current.operational + << ", al_state=" << current.al_state; + } else if (changed && previous.initialized && current.healthy) { + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] slave communication recovered: " + << id_ << ", motor_id=" << motor_id; + } + previous = current; + all_slaves_healthy = all_slaves_healthy && slave_healthy; + } + + const bool domain_healthy = domain_result == 0 && + domain_state.wc_state == EC_WC_COMPLETE; + const bool master_healthy = master_state.link_up && + master_state.slaves_responding >= slaves_by_motor_id_.size() && + (master_state.al_states & EC_AL_STATE_OP) != 0; + const bool bus_healthy = master_healthy && domain_healthy && all_slaves_healthy; + const BusHealthState current{ + true, + bus_healthy, + master_state.slaves_responding, + static_cast(master_state.al_states), + master_state.link_up != 0, + domain_result, + domain_state.working_counter, + static_cast(domain_state.wc_state), + }; + const bool changed = !last_bus_health_.initialized || + last_bus_health_.healthy != current.healthy || + last_bus_health_.slaves_responding != current.slaves_responding || + last_bus_health_.master_al_states != current.master_al_states || + last_bus_health_.link_up != current.link_up || + last_bus_health_.domain_result != current.domain_result || + last_bus_health_.working_counter != current.working_counter || + last_bus_health_.wc_state != current.wc_state; + if (changed && !current.healthy) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] bus communication unhealthy: " + << id_ + << ", link_up=" << current.link_up + << ", slaves_responding=" << current.slaves_responding + << ", master_al_states=" << current.master_al_states + << ", domain_result=" << current.domain_result + << ", wc_state=" << current.wc_state + << ", working_counter=" << current.working_counter; + } else if (changed && last_bus_health_.initialized && current.healthy) { + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] bus communication recovered: " + << id_ + << ", working_counter=" << current.working_counter; + } + last_bus_health_ = current; + healthy_.store(bus_healthy); +} + +void EthercatMotorBusRuntime::readFeedbackLocked_() +{ + if (!domain_data_) { + return; + } + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + for (auto& [key, entry] : slave.pdo_entries) { + (void)key; + if (!entry.rx) { + entry.value = readEntryValue_(domain_data_, entry); + } + } + } +} + +void EthercatMotorBusRuntime::writeCommandsLocked_() +{ + if (!domain_data_) { + return; + } + for (const auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + for (const auto& [key, entry] : slave.pdo_entries) { + (void)key; + if (entry.rx) { + writeEntryValue_(domain_data_, entry); + } + } + } +} + +void EthercatMotorBusRuntime::releaseMaster_() +{ + if (master_) { + ecrt_release_master(master_); + } + master_ = nullptr; + domain_ = nullptr; + domain_data_ = nullptr; + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + slave.slave_config = nullptr; + slave.pdo_entries.clear(); + } +} + +bool EthercatMotorBusRuntime::hasValidPdoMapping_() const +{ + return pdo_mapping_.vendor_id != 0 && + pdo_mapping_.product_code != 0 && + !pdo_mapping_.rx_pdos.empty() && + !pdo_mapping_.tx_pdos.empty(); +} + +bool EthercatMotorBusRuntime::writePdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + const std::uint64_t value) +{ + const PdoWrite write{motor_id, index, subindex, bit_len, value}; + return writePdosAtomic(&write, 1); +} + +bool EthercatMotorBusRuntime::writePdosAtomic(const PdoWrite* writes, + const std::size_t count) +{ + if (!writes || count == 0) { + return false; + } + + std::lock_guard lock(data_mutex_); + + for (std::size_t i = 0; i < count; ++i) { + const auto& write = writes[i]; + if (!isSupportedBitLength_(write.bit_length)) { + return false; + } + const auto slave_it = slaves_by_motor_id_.find(write.motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + const auto entry_it = slave_it->second.pdo_entries.find( + pdoEntryKey_(write.index, write.subindex)); + if (entry_it == slave_it->second.pdo_entries.end() || + !entry_it->second.rx || + entry_it->second.cfg.bit_len != write.bit_length) { + return false; + } + for (std::size_t previous = 0; previous < i; ++previous) { + if (writes[previous].motor_id == write.motor_id && + writes[previous].index == write.index && + writes[previous].subindex == write.subindex) { + return false; + } + } + } + + for (std::size_t i = 0; i < count; ++i) { + const auto& write = writes[i]; + auto& entry = slaves_by_motor_id_.at(write.motor_id) + .pdo_entries.at(pdoEntryKey_(write.index, write.subindex)); + entry.value = maskValue_(write.raw_value, entry.cfg.bit_len); + } + command_generation_.fetch_add(1); + return true; +} + +bool EthercatMotorBusRuntime::readPdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + std::uint64_t& value) const +{ + PdoRead read{motor_id, index, subindex, bit_len, 0}; + if (!readPdosAtomic(&read, 1)) { + return false; + } + value = read.raw_value; + return true; +} + +bool EthercatMotorBusRuntime::readPdosAtomic(PdoRead* reads, + const std::size_t count) const +{ + if (!reads || count == 0) { + return false; + } + + std::lock_guard lock(data_mutex_); + for (std::size_t i = 0; i < count; ++i) { + const auto& read = reads[i]; + if (!isSupportedBitLength_(read.bit_length)) { + return false; + } + const auto slave_it = slaves_by_motor_id_.find(read.motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + const auto entry_it = slave_it->second.pdo_entries.find( + pdoEntryKey_(read.index, read.subindex)); + if (entry_it == slave_it->second.pdo_entries.end() || + entry_it->second.rx || + entry_it->second.cfg.bit_len != read.bit_length) { + return false; + } + } + + for (std::size_t i = 0; i < count; ++i) { + auto& read = reads[i]; + const auto& entry = slaves_by_motor_id_.at(read.motor_id) + .pdo_entries.at(pdoEntryKey_(read.index, read.subindex)); + read.raw_value = maskValue_(entry.value, entry.cfg.bit_len); + } + return true; +} + +bool EthercatMotorBusRuntime::writeSdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + const std::uint64_t value) +{ + if (!isSupportedBitLength_(bit_len)) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported SDO write bit length: " + << static_cast(bit_len) + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", motor_id=" << motor_id; + return false; + } + + auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO write: " + << motor_id << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + auto& slave = slave_it->second; + if (!slave.slave_config || !master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO write: " + << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + + const auto raw_value = maskValue_(value, bit_len); + if (!started_) { + int result = -1; + switch (bit_len) { + case 8: + result = ecrt_slave_config_sdo8(slave.slave_config, index, subindex, + static_cast(raw_value)); + break; + case 16: + result = ecrt_slave_config_sdo16(slave.slave_config, index, subindex, + static_cast(raw_value)); + break; + case 32: + result = ecrt_slave_config_sdo32(slave.slave_config, index, subindex, + static_cast(raw_value)); + break; + default: + break; + } + if (result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure startup SDO: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", bit_len=" << static_cast(bit_len) + << ", value=" << raw_value + << ", result=" << result; + return false; + } + return true; + } + + std::array data{}; + switch (bit_len) { + case 8: + EC_WRITE_U8(data.data(), static_cast(raw_value)); + break; + case 16: + EC_WRITE_U16(data.data(), static_cast(raw_value)); + break; + case 32: + EC_WRITE_U32(data.data(), static_cast(raw_value)); + break; + default: + break; + } + + const auto data_size = static_cast(bit_len / 8); + std::uint32_t abort_code = 0; + const int result = ecrt_master_sdo_download( + master_, + static_cast(slave.cfg.position()), + index, + subindex, + data.data(), + data_size, + &abort_code); + if (result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to write SDO: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", slave_position=" << slave.cfg.position() + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", bit_len=" << static_cast(bit_len) + << ", value=" << raw_value + << ", result=" << result + << ", abort_code=" << hexIndex_(abort_code); + return false; + } + return true; +} + +bool EthercatMotorBusRuntime::readSdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + std::uint64_t& value) +{ + if (!isSupportedBitLength_(bit_len)) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported SDO read bit length: " + << static_cast(bit_len) + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", motor_id=" << motor_id; + return false; + } + + auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO read: " + << motor_id << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + const auto& slave = slave_it->second; + if (!master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO read: " + << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + + std::array data{}; + const auto data_size = static_cast(bit_len / 8); + std::size_t result_size = 0; + std::uint32_t abort_code = 0; + const int result = ecrt_master_sdo_upload( + master_, + static_cast(slave.cfg.position()), + index, + subindex, + data.data(), + data_size, + &result_size, + &abort_code); + if (result != 0 || result_size != data_size) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to read SDO: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", slave_position=" << slave.cfg.position() + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", bit_len=" << static_cast(bit_len) + << ", result=" << result + << ", result_size=" << result_size + << ", abort_code=" << hexIndex_(abort_code); + return false; + } + + switch (bit_len) { + case 8: + value = EC_READ_U8(data.data()); + break; + case 16: + value = EC_READ_U16(data.data()); + break; + case 32: + value = EC_READ_U32(data.data()); + break; + default: + return false; + } + value = maskValue_(value, bit_len); + return true; +} + +std::uint32_t EthercatMotorBusRuntime::pdoEntryKey_(const std::uint16_t index, + const std::uint8_t subindex) +{ + return (static_cast(index) << 8U) | subindex; +} + +std::string EthercatMotorBusRuntime::hexIndex_(const std::uint32_t index) +{ + std::ostringstream oss; + oss << "0x" << std::hex << std::uppercase << index; + return oss.str(); +} + +std::uint64_t EthercatMotorBusRuntime::maskValue_(const std::uint64_t value, + const std::uint8_t bit_len) +{ + switch (bit_len) { + case 8: + return value & 0xFFU; + case 16: + return value & 0xFFFFU; + case 32: + return value & 0xFFFFFFFFULL; + default: + return value; + } +} + +bool EthercatMotorBusRuntime::isSupportedBitLength_(const std::uint8_t bit_len) +{ + return bit_len == 8 || bit_len == 16 || bit_len == 32; +} + +std::uint64_t EthercatMotorBusRuntime::readEntryValue_(const std::uint8_t* domain_data, + const PdoEntryRuntime& entry) +{ + switch (entry.cfg.bit_len) { + case 8: + return EC_READ_U8(domain_data + entry.offset); + case 16: + return EC_READ_U16(domain_data + entry.offset); + case 32: + return EC_READ_U32(domain_data + entry.offset); + default: + return 0; + } +} + +void EthercatMotorBusRuntime::writeEntryValue_(std::uint8_t* domain_data, + const PdoEntryRuntime& entry) +{ + switch (entry.cfg.bit_len) { + case 8: + EC_WRITE_U8(domain_data + entry.offset, static_cast(entry.value)); + break; + case 16: + EC_WRITE_U16(domain_data + entry.offset, static_cast(entry.value)); + break; + case 32: + EC_WRITE_U32(domain_data + entry.offset, static_cast(entry.value)); + break; + default: + break; + } +} + +std::uint64_t EthercatMotorBusRuntime::steadyTimeNs_() +{ + return timePointNs_(std::chrono::steady_clock::now()); +} + +std::uint64_t EthercatMotorBusRuntime::timePointNs_( + const std::chrono::steady_clock::time_point time_point) +{ + const auto time_since_epoch = time_point.time_since_epoch(); + return static_cast( + std::chrono::duration_cast(time_since_epoch).count()); +} + +std::uint32_t EthercatMotorBusRuntime::usToNs_(const std::uint32_t value_us) +{ + return value_us * 1000U; +} + +std::int32_t EthercatMotorBusRuntime::usToNs_(const std::int32_t value_us) +{ + return value_us * 1000; } } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp new file mode 100644 index 00000000..ba470aae --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp @@ -0,0 +1,128 @@ +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" + +namespace cmvr::device { +namespace { + + + +config::MotorGroupConfig createSingleSlaveGroup() +{ + config::MotorGroupConfig group; + group.set_id("ethercat_real_test"); + group.set_bus_type(config::MOTOR_BUS_ETHERCAT); + group.set_vendor(config::MOTOR_VENDOR_EYOU); + group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402); + + auto* ethercat = group.mutable_ethercat(); + ethercat->set_master_index(0); + ethercat->set_cycle_us(1000); + ethercat->set_slave_op_timeout_ms(12000); + ethercat->set_slave_state_poll_period_ms(10); + + auto* dc = ethercat->mutable_dc(); + dc->set_enable(true); + dc->set_reference_motor_id(1); + dc->set_sync0_cycle_us(1000); + dc->set_sync0_shift_us(0); + dc->set_sync_reference_clock_period(1); + dc->set_assign_activate(768); + dc->set_sync_monitor_period_ms(1000); + + auto* slave = ethercat->add_slaves(); + slave->set_motor_id(1); + slave->set_alias(0); + slave->set_position(0); + + return group; +} + +} // namespace + +TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword) +{ + + + EthercatMotorBusRuntime runtime; + runtime.setPdoMapping(createEyouCia402PdoMapping()); + + ASSERT_TRUE(runtime.init(createSingleSlaveGroup())); + EXPECT_EQ(runtime.busType(), config::MOTOR_BUS_ETHERCAT); + EXPECT_TRUE(runtime.hasMotor(1)); + EXPECT_NE(runtime.slaveForMotor(1), nullptr); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_CONTROL_WORD_6040, 0x00)); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_STATUS_WORD_6041, 0x00)); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_OPERATION_MODE_6060, 0x00)); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00)); + + const bool started = runtime.start(); + EXPECT_TRUE(started); + if (!started) { + runtime.stop(); + return; + } + + const std::array command_writes{ + EthercatMotorBusRuntime::makePdoWrite( + 1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000), + EthercatMotorBusRuntime::makePdoWrite( + 1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0), + }; + auto invalid_writes = command_writes; + invalid_writes[1].bit_length = 16; + + const auto generation_before = runtime.commandGeneration(); + EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size())); + EXPECT_EQ(runtime.commandGeneration(), generation_before); + EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size())); + EXPECT_EQ(runtime.commandGeneration(), generation_before + 1); + + const int settle_ms = 1000; + std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms)); + EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1); + EXPECT_TRUE(runtime.isHealthy()); + + std::array feedback_reads{ + EthercatMotorBusRuntime::makePdoRead( + 1, msgs::CIA402_STATUS_WORD_6041, 0x00), + EthercatMotorBusRuntime::makePdoRead( + 1, msgs::CIA402_MODE_DISPLAY_6061, 0x00), + }; + auto invalid_feedback_reads = feedback_reads; + invalid_feedback_reads[1].bit_length = 16; + EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(), + invalid_feedback_reads.size())); + ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size())); + + const auto statusword = + EthercatMotorBusRuntime::pdoReadValue(feedback_reads[0]); + const auto mode_display = + EthercatMotorBusRuntime::pdoReadValue(feedback_reads[1]); + + std::cout << "CIA402 statusword: 0x" << std::hex << statusword + << ", mode display: " << std::dec << static_cast(mode_display) + << std::endl; + + const int hold_ms = 10000; + if (hold_ms > 0) { + std::cout << "Holding EtherCAT runtime for " << hold_ms + << " ms. Check slave state in another terminal." << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(hold_ms)); + } + + runtime.stop(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt new file mode 100644 index 00000000..36685aeb --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt @@ -0,0 +1,50 @@ +add_library(ethercat_motor_driver SHARED + src/cia402/cia402_protocol.cpp + src/cia402/cia402_status_monitor.cpp + src/vendor/eyou/eyou_motor.cpp + src/vendor/eyou/eyou_motor_adapter.cpp +) + +target_include_directories(ethercat_motor_driver + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +target_link_libraries(ethercat_motor_driver + PUBLIC + cmvr_es::device::motor_core + cmvr_es::device::motor_bus_runtime + PRIVATE + cmvr_es::proto + glog +) + +add_library(cmvr_es::device::ethercat_motor_driver ALIAS ethercat_motor_driver) +install(TARGETS ethercat_motor_driver LIBRARY DESTINATION lib) + +add_executable(eyou_motor_real_test + src/vendor/eyou/eyou_motor_real_test.cpp +) + +target_link_libraries(eyou_motor_real_test + PRIVATE + cmvr_es::device::ethercat_motor_driver + gtest + gtest_main + pthread + glog +) + +add_executable(eyou_motor_device_manager_real_test + src/vendor/eyou/eyou_motor_device_manager_real_test.cpp +) + +target_link_libraries(eyou_motor_device_manager_real_test + PRIVATE + cmvr_es::device_manager + cmvr_es::device::motor_manager + gtest + gtest_main + pthread + glog +) diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h new file mode 100644 index 00000000..62f869f4 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h @@ -0,0 +1,166 @@ +#ifndef CMVR_ES_CIA402_OBJECTS_H +#define CMVR_ES_CIA402_OBJECTS_H + +#include + +namespace cmvr::device::cia402 { + +union Controlword { + std::uint16_t value; + struct { + std::uint16_t switch_on : 1; + std::uint16_t enable_voltage : 1; + std::uint16_t quick_stop : 1; + std::uint16_t enable_operation : 1; + std::uint16_t new_set_point : 1; + std::uint16_t change_set_immediately : 1; + std::uint16_t relative : 1; + std::uint16_t fault_reset : 1; + std::uint16_t halt : 1; + std::uint16_t reserved : 2; + std::uint16_t manufacturer_specific : 5; + }; +}; + +union Statusword { + std::uint16_t value; + struct { + std::uint16_t ready_to_switch_on : 1; + std::uint16_t switched_on : 1; + std::uint16_t operation_enabled : 1; + std::uint16_t fault : 1; + std::uint16_t voltage_enabled : 1; + std::uint16_t quick_stop : 1; + std::uint16_t switch_on_disabled : 1; + std::uint16_t warning : 1; + std::uint16_t manufacturer_specific_8 : 1; + std::uint16_t remote : 1; + std::uint16_t target_reached : 1; + std::uint16_t internal_limit_active : 1; + std::uint16_t operation_mode_specific : 2; + std::uint16_t manufacturer_specific : 2; + }; +}; + +static_assert(sizeof(Controlword) == sizeof(std::uint16_t)); +static_assert(sizeof(Statusword) == sizeof(std::uint16_t)); + +enum class DeviceState { + SwitchOnDisabled, + ReadyToSwitchOn, + SwitchedOn, + OperationEnabled, +}; + +namespace detail { + +struct StateRule { + std::uint16_t relevant_bits; + std::uint16_t expected_bits; +}; + +inline StateRule stateRule(const DeviceState state) +{ + // CiA402 device states are matched by selected 0x6041 statusword bits. + switch (state) { + case DeviceState::SwitchOnDisabled: + return {0x004F, 0x0040}; + case DeviceState::ReadyToSwitchOn: + return {0x006F, 0x0021}; + case DeviceState::SwitchedOn: + return {0x006F, 0x0023}; + case DeviceState::OperationEnabled: + return {0x006F, 0x0027}; + } + return {0x006F, 0x0000}; +} + +} // namespace detail + +inline Controlword controlword(const std::uint16_t value) +{ + Controlword cw{}; + cw.value = value; + return cw; +} + +inline Statusword statusword(const std::uint16_t value) +{ + Statusword sw{}; + sw.value = value; + return sw; +} + +inline Controlword shutdownControlword() +{ + Controlword cw{}; + cw.quick_stop = 1; + cw.enable_voltage = 1; + return cw; +} + +inline Controlword switchOnControlword() +{ + auto cw = shutdownControlword(); + cw.switch_on = 1; + return cw; +} + +inline Controlword enableOperationControlword() +{ + auto cw = switchOnControlword(); + cw.enable_operation = 1; + return cw; +} + +inline Controlword quickStopControlword() +{ + auto cw = enableOperationControlword(); + cw.quick_stop = 0; + return cw; +} + +inline Controlword faultResetControlword() +{ + Controlword cw{}; + cw.fault_reset = 1; + return cw; +} + +inline Controlword profilePositionControlword(const bool new_set_point) +{ + auto cw = enableOperationControlword(); + cw.change_set_immediately = 1; + cw.new_set_point = new_set_point ? 1 : 0; + return cw; +} + +inline bool hasState(const Statusword status, const DeviceState state) +{ + const auto rule = detail::stateRule(state); + return (status.value & rule.relevant_bits) == rule.expected_bits; +} + +inline bool isSwitchOnDisabled(const Statusword status) +{ + return hasState(status, DeviceState::SwitchOnDisabled); +} + +inline bool isOperationEnabled(const Statusword status) +{ + return hasState(status, DeviceState::OperationEnabled); +} + +inline bool targetReached(const Statusword status) +{ + return status.target_reached != 0; +} + +inline bool setPointAcknowledged(const Statusword status) +{ + return (status.value & (1U << 12U)) != 0; +} + +} // namespace cmvr::device::cia402 + +#endif // CMVR_ES_CIA402_OBJECTS_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h new file mode 100644 index 00000000..f0903a91 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h @@ -0,0 +1,149 @@ +#ifndef CMVR_ES_CIA402_PROTOCOL_H +#define CMVR_ES_CIA402_PROTOCOL_H + +#include +#include +#include +#include +#include + +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" +#include "devices/motor/motor_protocol_interface.h" + +namespace cmvr::device { + +class Cia402StatusMonitor; + +class Cia402Protocol final : public MotorProtocolInterface { +public: + struct CyclicPositionCommand { + std::uint8_t node_id{0}; + double target_q{0.0}; + double target_qd{0.0}; + }; + + struct MotorFeedback { + std::uint8_t node_id{0}; + double q{0.0}; + double qd{0.0}; + }; + + explicit Cia402Protocol(std::shared_ptr bus_runtime, + const config::Cia402ProtocolConfig& config); + ~Cia402Protocol() override; + + bool initNode(std::uint8_t node_id) override; + + bool setMode(std::uint8_t node_id, msgs::RunMode mode) override; + msgs::RunMode getMode(std::uint8_t node_id) override; + void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override; + void setLimitQd(std::uint8_t node_id, double qd) override; + void setLimitQ(std::uint8_t node_id, double ub, double lb) override; + bool calibrateZeroQ(std::uint8_t node_id) override; + bool reachedTargetQ(std::uint8_t node_id) override; + bool commandProfilePosition(std::uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) override; + bool commandProfileVelocity(std::uint8_t node_id, + double target_qd, + double max_qdd) override; + bool commandCyclicPosition(std::uint8_t node_id, + double target_q, + double target_qd) override; + bool commandCyclicPositionsAtomic(const CyclicPositionCommand* commands, + std::size_t count); + bool readFeedbacksAtomic(MotorFeedback* feedbacks, std::size_t count) const; + bool commandCyclicVelocity(std::uint8_t node_id, + double target_qd) override; + bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override; + void setMotorConversion(std::uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) override; + bool torqueOn(std::uint8_t node_id) override; + bool torqueOff(std::uint8_t node_id) override; + bool brakeRelease(std::uint8_t node_id) override; + bool quickStop(std::uint8_t node_id) override; + + double getQ(std::uint8_t node_id) override; + double getQd(std::uint8_t node_id) override; + bool syncTargetToActualPosition(std::uint8_t node_id); + +private: + struct NodeState { + msgs::RunMode mode{msgs::RUN_MODE_CYCLIC_SYNC_POSITION}; + cia402::Controlword controlword{}; + std::int32_t target_position{0}; + std::int32_t target_velocity{0}; + std::int16_t target_torque{0}; + std::int32_t profile_velocity{0}; + std::int32_t profile_acceleration{0}; + std::int32_t profile_deceleration{0}; + double limit_q_lb{0.0}; + double limit_q_ub{0.0}; + double limit_qd{0.0}; + double limit_qdd{0.0}; + double encoder_counts_per_rev{0.0}; + double gear_ratio{0.0}; + }; + + static std::int8_t toCia402Mode_(msgs::RunMode mode); + static msgs::RunMode fromCia402Mode_(std::int8_t mode); + static cia402::Controlword nextControlword_(cia402::Statusword statusword); + static bool isOperationEnabled_(cia402::Statusword statusword); + static bool targetReached_(cia402::Statusword statusword); + + std::int32_t radToCounts_(double angle_rad, const NodeState& state) const; + double countsToRad_(std::int32_t counts, const NodeState& state) const; + std::int32_t radPerSecToCounts_(double velocity_rad_s, const NodeState& state) const; + std::int32_t radPerSec2ToCounts_(double acceleration_rad_s2, const NodeState& state) const; + double countsToRadPerSec_(std::int32_t velocity_counts_s, const NodeState& state) const; + NodeState& nodeState_(std::uint8_t node_id); + const NodeState* findNodeState_(std::uint8_t node_id) const; + bool hasValidConversion_(std::uint8_t node_id, const NodeState& state) const; + bool validateNodePdos_(std::uint8_t node_id) const; + bool readStatusword_(std::uint8_t node_id, std::uint16_t& statusword) const; + bool readActualPosition_(std::uint8_t node_id, std::int32_t& actual_position) const; + bool readActualVelocity_(std::uint8_t node_id, std::int32_t& actual_velocity) const; + bool readModeDisplay_(std::uint8_t node_id, std::int8_t& mode_display) const; + bool writeControlword_(std::uint8_t node_id, cia402::Controlword controlword); + bool writeControlwordAndWait_(std::uint8_t node_id, + cia402::Controlword controlword, + cia402::DeviceState target_state, + const char* state_name); + bool waitStatus_(std::uint8_t node_id, + cia402::DeviceState target_state, + const char* state_name) const; + bool waitMode_(std::uint8_t node_id, std::int8_t target_mode) const; + bool waitSetPointAcknowledged_(std::uint8_t node_id, bool acknowledged) const; + bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_name) const; + bool writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool writeVelocityLimitToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state); + bool writeTargetsForMode_(std::uint8_t node_id, + msgs::RunMode mode, + const NodeState& state) const; + bool appendTargetWritesForMode_( + std::uint8_t node_id, + msgs::RunMode mode, + const NodeState& state, + EthercatMotorBusRuntime::PdoWrite* writes, + std::size_t capacity, + std::size_t& count) const; + bool writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state); + bool writeNode_(std::uint8_t node_id, NodeState& state); + + std::shared_ptr bus_runtime_; + std::unique_ptr status_monitor_; + config::Cia402ProtocolConfig config_; + std::unordered_map nodes_; + std::mutex cyclic_position_mutex_; + std::vector cyclic_position_writes_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CIA402_PROTOCOL_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h new file mode 100644 index 00000000..3a92a7df --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h @@ -0,0 +1,78 @@ +#ifndef CMVR_ES_CIA402_STATUS_MONITOR_H +#define CMVR_ES_CIA402_STATUS_MONITOR_H + +#include +#include +#include +#include +#include +#include +#include + +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +namespace cmvr::device { + +class Cia402StatusMonitor final { +public: + Cia402StatusMonitor(std::shared_ptr bus_runtime, + std::chrono::milliseconds poll_period); + ~Cia402StatusMonitor(); + + Cia402StatusMonitor(const Cia402StatusMonitor&) = delete; + Cia402StatusMonitor& operator=(const Cia402StatusMonitor&) = delete; + + void addNode(std::uint8_t node_id); + void setExpectedOperationEnabled(std::uint8_t node_id, bool expected); + bool isNodeOperational(std::uint8_t node_id) const; + +private: + struct StatusSample { + bool read_ok{false}; + bool transport_healthy{false}; + bool expected_operation_enabled{false}; + bool operation_enabled{false}; + bool status_problem{false}; + bool command_blocked{false}; + std::uint16_t statusword{0}; + std::uint16_t error_code{0}; + std::int8_t mode_display{0}; + std::int32_t actual_position{0}; + std::int32_t actual_velocity{0}; + std::int16_t actual_torque{0}; + }; + + struct NodeMonitorState { + bool expected_operation_enabled{false}; + bool has_last_sample{false}; + StatusSample last_sample; + }; + + void monitorLoop_(); + void monitorNode_(std::uint8_t node_id, bool expected_operation_enabled); + bool readStatusSample_(std::uint8_t node_id, + bool expected_operation_enabled, + StatusSample& sample) const; + void reportErrorCodeTransition_(std::uint8_t node_id, + bool had_previous, + const StatusSample& previous, + const StatusSample& current) const; + void reportStatuswordTransition_(std::uint8_t node_id, + bool had_previous, + const StatusSample& previous, + const StatusSample& current) const; + static const char* deviceStateName_(std::uint16_t statusword); + static const char* errorCodeDescription_(std::uint16_t error_code); + + std::shared_ptr bus_runtime_; + std::chrono::milliseconds poll_period_; + std::mutex monitor_mutex_; + mutable std::mutex states_mutex_; + std::unordered_map states_; + std::atomic running_{true}; + std::thread monitor_thread_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CIA402_STATUS_MONITOR_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h new file mode 100644 index 00000000..0993dd57 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h @@ -0,0 +1,81 @@ +#ifndef CMVR_ES_EYOU_CIA402_PDO_MAPPING_H +#define CMVR_ES_EYOU_CIA402_PDO_MAPPING_H + +#include +#include +#include + +#include "cmvr/msgs/canopen.pb.h" +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h" + +namespace cmvr::device { + +namespace eyou_cia402_pdo_mapping_detail { + +inline constexpr std::uint32_t VENDOR_ID = 0x00001097; +inline constexpr std::uint32_t PRODUCT_CODE = 0x00002406; + +inline EthercatPdoEntryConfig entry(const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + std::string name) +{ + EthercatPdoEntryConfig cfg; + cfg.index = index; + cfg.subindex = subindex; + cfg.bit_len = bit_len; + cfg.name = std::move(name); + cfg.padding = index == 0 || bit_len == 0; + return cfg; +} + +} // namespace eyou_cia402_pdo_mapping_detail + +inline EthercatPdoMapping createEyouCia402PdoMapping() +{ + using namespace eyou_cia402_pdo_mapping_detail; + + EthercatPdoConfig rx_pdo; + rx_pdo.index = msgs::CANOPEN_RPDO2_MAP_1601; + rx_pdo.sync_manager = 2; + rx_pdo.rx = true; + rx_pdo.entries = { + entry(msgs::CIA402_CONTROL_WORD_6040, 0x00, 16, "Control Word"), + entry(msgs::CIA402_TARGET_POSITION_607A, 0x00, 32, "Target Position"), + entry(msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, 32, "Target Velocity"), + entry(msgs::CIA402_TARGET_TORQUE_6071, 0x00, 16, "Target Torque"), + entry(msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, 32, "Profile Acceleration"), + entry(msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, 32, "Profile Deceleration"), + entry(msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, 32, "Profile Velocity"), + entry(msgs::CIA402_TORQUE_SLOPE_6087, 0x00, 32, "Torque Slope"), + entry(msgs::CIA402_OPERATION_MODE_6060, 0x00, 8, "Mode Of Operation"), + entry(0x0000, 0x00, 8, "Padding"), + }; + + EthercatPdoConfig tx_pdo; + tx_pdo.index = msgs::CANOPEN_TPDO1_MAP_1A00; + tx_pdo.sync_manager = 3; + tx_pdo.rx = false; + tx_pdo.entries = { + entry(msgs::CIA402_STATUS_WORD_6041, 0x00, 16, "Status Word"), + entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"), + entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"), + entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"), + entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"), + entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"), + entry(0x0000, 0x00, 8, "Padding"), + }; + + EthercatPdoMapping mapping; + mapping.vendor_id = VENDOR_ID; + mapping.product_code = PRODUCT_CODE; + mapping.name = "EYOU ServoModule ECAT V145 CiA402"; + mapping.rx_pdos.push_back(std::move(rx_pdo)); + mapping.tx_pdos.push_back(std::move(tx_pdo)); + return mapping; +} + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_CIA402_PDO_MAPPING_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h new file mode 100644 index 00000000..4bdc2154 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h @@ -0,0 +1,53 @@ +#ifndef CMVR_ES_EYOU_MOTOR_H +#define CMVR_ES_EYOU_MOTOR_H + +#include +#include +#include + +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +namespace cmvr::device { + +class EyouMotor final : public AbstractMotor { +public: + EyouMotor(const config::MotorConfigItem& config, + std::shared_ptr cia402_protocol, + std::unique_ptr vendor_adapter); + + std::string typeName() const override { return "EyouMotor"; } + bool init() override; + void setLimitQ(double ub, double lb) override; + void setLimitQd(double qd) override; + bool calibrateZeroQ() override; + bool brakeRelease() override; + + static bool commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities); + static bool readFeedbacksAtomic( + const std::vector>& motors, + std::vector& positions, + std::vector& velocities); + +private: + bool hasDependencies_() const; + bool hasValidConversion_() const; + bool writeVendorPositionLimits_() const; + bool writeVendorVelocityLimit_() const; + std::int32_t radToCounts_(double angle_rad) const; + std::uint32_t radPerSecToCounts_(double velocity_rad_s) const; + + std::shared_ptr cia402_protocol_; + std::unique_ptr vendor_adapter_; + double encoder_counts_per_rev_{0.0}; + double gear_ratio_{0.0}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_MOTOR_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h new file mode 100644 index 00000000..c177a313 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h @@ -0,0 +1,34 @@ +#ifndef CMVR_ES_EYOU_MOTOR_ADAPTER_H +#define CMVR_ES_EYOU_MOTOR_ADAPTER_H + +#include +#include + +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h" + +namespace cmvr::device { + +class EyouMotorAdapter final : public MotorVendorAdapter { +public: + explicit EyouMotorAdapter(std::shared_ptr bus_runtime); + ~EyouMotorAdapter() override = default; + + bool initNode(std::uint8_t node_id) override; + bool writePositionLimits(std::uint8_t node_id, + std::int32_t lower_limit, + std::int32_t upper_limit) override; + bool writeVelocityLimit(std::uint8_t node_id, + std::uint32_t velocity_limit) override; + bool calibrateZero(std::uint8_t node_id, + std::int64_t counts_per_joint_revolution, + std::int32_t& zeroed_position) override; + bool brakeRelease(std::uint8_t node_id) override; + +private: + std::shared_ptr bus_runtime_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_MOTOR_ADAPTER_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h new file mode 100644 index 00000000..cee1369d --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h @@ -0,0 +1,20 @@ +#ifndef CMVR_ES_EYOU_OBJECTS_H +#define CMVR_ES_EYOU_OBJECTS_H + +#include + +namespace cmvr::device::eyou { + +inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003; + +inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014; + +inline constexpr std::uint16_t EYOU_OVER_SPEED_THRESHOLD_2024 = 0x2024; +inline constexpr std::uint16_t EYOU_FIRST_ENCODER_VALUE_202A = 0x202A; +inline constexpr std::uint16_t EYOU_SECOND_ENCODER_VALUE_202B = 0x202B; + +inline constexpr std::uint16_t EYOU_STORE_PARAMETERS_1010 = 0x1010; + +} // namespace cmvr::device::eyou + +#endif // CMVR_ES_EYOU_OBJECTS_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h new file mode 100644 index 00000000..7eb65f95 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h @@ -0,0 +1,26 @@ +#ifndef CMVR_ES_MOTOR_VENDOR_ADAPTER_H +#define CMVR_ES_MOTOR_VENDOR_ADAPTER_H + +#include + +namespace cmvr::device { + +class MotorVendorAdapter { +public: + virtual ~MotorVendorAdapter() = default; + + virtual bool initNode(std::uint8_t node_id) = 0; + virtual bool writePositionLimits(std::uint8_t node_id, + std::int32_t lower_limit, + std::int32_t upper_limit) = 0; + virtual bool writeVelocityLimit(std::uint8_t node_id, + std::uint32_t velocity_limit) = 0; + virtual bool calibrateZero(std::uint8_t node_id, + std::int64_t counts_per_joint_revolution, + std::int32_t& zeroed_position) = 0; + virtual bool brakeRelease(std::uint8_t node_id) = 0; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MOTOR_VENDOR_ADAPTER_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp new file mode 100644 index 00000000..a621ddda --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp @@ -0,0 +1,1123 @@ +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" + +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h" + +namespace cmvr::device { + +Cia402Protocol::Cia402Protocol(std::shared_ptr bus_runtime, + const config::Cia402ProtocolConfig& config) + : bus_runtime_(std::move(bus_runtime)), + config_(config) +{ + comm_proto = CommProto::ETHERCAT; + cyclic_position_writes_.reserve(512); + status_monitor_ = std::make_unique( + bus_runtime_, std::chrono::milliseconds(config_.status_poll_period_ms())); +} + +Cia402Protocol::~Cia402Protocol() = default; + +bool Cia402Protocol::initNode(const std::uint8_t node_id) +{ + if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] missing EtherCAT motor: " + << static_cast(node_id); + return false; + } + + if (!validateNodePdos_(node_id)) { + return false; + } + auto& state = nodeState_(node_id); + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + if (!writeNode_(node_id, state)) { + return false; + } + status_monitor_->addNode(node_id); + return true; +} + +bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id, + const double target_q, + const double max_qd, + const double max_qdd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state) || + !status_monitor_->isNodeOperational(node_id)) { + return false; + } + state.target_position = radToCounts_(target_q, state); + state.profile_velocity = std::abs(radPerSecToCounts_(max_qd, state)); + if (max_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + return writeProfilePositionTarget_(node_id, state); +} + +bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id, + const double target_qd, + const double max_qdd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state) || + !status_monitor_->isNodeOperational(node_id)) { + return false; + } + state.target_velocity = radPerSecToCounts_(target_qd, state); + if (max_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + return writeNode_(node_id, state); +} + +bool Cia402Protocol::commandCyclicPosition(const std::uint8_t node_id, + const double target_q, + const double target_qd) +{ + const CyclicPositionCommand command{node_id, target_q, target_qd}; + return commandCyclicPositionsAtomic(&command, 1); +} + +bool Cia402Protocol::commandCyclicPositionsAtomic( + const CyclicPositionCommand* commands, + const std::size_t count) +{ + if (!bus_runtime_ || !commands || count == 0 || count > 256) { + return false; + } + + std::lock_guard lock(cyclic_position_mutex_); + cyclic_position_writes_.clear(); + cyclic_position_writes_.reserve(count * 2); + + for (std::size_t i = 0; i < count; ++i) { + const auto& command = commands[i]; + const auto* state = findNodeState_(command.node_id); + if (!state || !hasValidConversion_(command.node_id, *state) || + state->mode != msgs::RUN_MODE_CYCLIC_SYNC_POSITION || + !status_monitor_->isNodeOperational(command.node_id)) { + return false; + } + for (std::size_t previous = 0; previous < i; ++previous) { + if (commands[previous].node_id == command.node_id) { + return false; + } + } + + cyclic_position_writes_.push_back( + EthercatMotorBusRuntime::makePdoWrite( + command.node_id, + msgs::CIA402_TARGET_POSITION_607A, + 0x00, + radToCounts_(command.target_q, *state))); + cyclic_position_writes_.push_back( + EthercatMotorBusRuntime::makePdoWrite( + command.node_id, + msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, + radPerSecToCounts_(command.target_qd, *state))); + } + + if (!bus_runtime_->writePdosAtomic(cyclic_position_writes_.data(), + cyclic_position_writes_.size())) { + return false; + } + + for (std::size_t i = 0; i < count; ++i) { + auto& state = nodeState_(commands[i].node_id); + state.target_position = radToCounts_(commands[i].target_q, state); + state.target_velocity = radPerSecToCounts_(commands[i].target_qd, state); + } + return true; +} + +bool Cia402Protocol::readFeedbacksAtomic(MotorFeedback* feedbacks, + const std::size_t count) const +{ + if (!bus_runtime_ || !feedbacks || count == 0 || count > 256) { + return false; + } + + std::array reads{}; + for (std::size_t i = 0; i < count; ++i) { + const auto* state = findNodeState_(feedbacks[i].node_id); + if (!state || !hasValidConversion_(feedbacks[i].node_id, *state)) { + return false; + } + for (std::size_t previous = 0; previous < i; ++previous) { + if (feedbacks[previous].node_id == feedbacks[i].node_id) { + return false; + } + } + reads[2 * i] = EthercatMotorBusRuntime::makePdoRead( + feedbacks[i].node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00); + reads[2 * i + 1] = EthercatMotorBusRuntime::makePdoRead( + feedbacks[i].node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00); + } + + if (!bus_runtime_->readPdosAtomic(reads.data(), count * 2)) { + return false; + } + + for (std::size_t i = 0; i < count; ++i) { + const auto* state = findNodeState_(feedbacks[i].node_id); + if (!state) { + return false; + } + const auto position = EthercatMotorBusRuntime::pdoReadValue(reads[2 * i]); + const auto velocity = + EthercatMotorBusRuntime::pdoReadValue(reads[2 * i + 1]); + feedbacks[i].q = countsToRad_(position, *state); + feedbacks[i].qd = countsToRadPerSec_(velocity, *state); + } + return true; +} + +bool Cia402Protocol::commandCyclicVelocity(const std::uint8_t node_id, + const double target_qd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state) || + !status_monitor_->isNodeOperational(node_id)) { + return false; + } + state.target_velocity = radPerSecToCounts_(target_qd, state); + return writeTargetsForMode_(node_id, state.mode, state); +} + +bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id, + const double target_tau) +{ + (void)target_tau; + CMVR_LOG(ERROR) << "[Cia402Protocol] cyclic torque command is not implemented, node=" + << static_cast(node_id); + return false; +} + +bool Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode) +{ + auto& state = nodeState_(node_id); + if (!prepareSafeTargetsForMode_(node_id, mode, state)) { + return false; + } + state.mode = mode; + if (!writeNode_(node_id, state)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write mode/controlword PDOs, node=" + << static_cast(node_id) + << ", mode=" << static_cast(mode); + return false; + } + return waitMode_(node_id, toCia402Mode_(mode)); +} + +msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id) +{ + std::int8_t mode_display = 0; + if (readModeDisplay_(node_id, mode_display)) { + return fromCia402Mode_(mode_display); + } + return nodeState_(node_id).mode; +} + +void Cia402Protocol::setLimitQdd(const std::uint8_t node_id, + const double u_qdd, + const double l_qdd) +{ + auto& state = nodeState_(node_id); + state.limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd)); + if (hasValidConversion_(node_id, state)) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + writeAccelerationLimitsToDictionary_(node_id, state); + } +} + +void Cia402Protocol::setLimitQd(const std::uint8_t node_id, const double qd) +{ + auto& state = nodeState_(node_id); + state.limit_qd = std::abs(qd); + if (hasValidConversion_(node_id, state)) { + state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state)); + writeVelocityLimitToDictionary_(node_id, state); + } +} + +void Cia402Protocol::setLimitQ(const std::uint8_t node_id, + const double ub, + const double lb) +{ + auto& state = nodeState_(node_id); + state.limit_q_ub = ub; + state.limit_q_lb = lb; + if (hasValidConversion_(node_id, state)) { + writePositionLimitsToDictionary_(node_id, state); + } +} + +bool Cia402Protocol::calibrateZeroQ(const std::uint8_t node_id) +{ + CMVR_LOG(ERROR) << "[Cia402Protocol] zero calibration is vendor-specific, node=" + << static_cast(node_id); + return false; +} + +bool Cia402Protocol::reachedTargetQ(const std::uint8_t node_id) +{ + std::uint16_t statusword = 0; + if (!readStatusword_(node_id, statusword)) { + return false; + } + return targetReached_(cia402::statusword(statusword)); +} + +void Cia402Protocol::setMotorConversion( + const std::uint8_t node_id, + const double encoder_counts_per_rev, + const double gear_ratio) +{ + auto& state = nodeState_(node_id); + state.encoder_counts_per_rev = encoder_counts_per_rev; + state.gear_ratio = gear_ratio; +} + +bool Cia402Protocol::torqueOn(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + status_monitor_->setExpectedOperationEnabled(node_id, false); + auto& state = nodeState_(node_id); + std::uint16_t statusword = 0; + if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) { + if (!writeControlword_(node_id, cia402::faultResetControlword())) { + return false; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } + + if (!prepareSafeTargetsForMode_(node_id, msgs::RUN_MODE_PROFILE_POSITION, state)) { + return false; + } + state.mode = msgs::RUN_MODE_PROFILE_POSITION; + state.target_velocity = 0; + state.target_torque = 0; + if (!writeTargetsForMode_(node_id, state.mode, state)) { + return false; + } + if (!bus_runtime_->writePdo(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, + toCia402Mode_(state.mode))) { + return false; + } + + if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), + cia402::DeviceState::SwitchedOn, + "Switched On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::enableOperationControlword(), + cia402::DeviceState::OperationEnabled, + "Operation Enabled")) { + return false; + } + if (!waitMode_(node_id, toCia402Mode_(state.mode))) { + return false; + } + + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + state.controlword = cia402::profilePositionControlword(false); + const bool enabled = writeTargetsForMode_(node_id, state.mode, state) && + writeControlword_(node_id, state.controlword) && + waitSetPointAcknowledged_(node_id, false); + if (enabled) { + status_monitor_->setExpectedOperationEnabled(node_id, true); + } + return enabled; +} + +bool Cia402Protocol::torqueOff(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + status_monitor_->setExpectedOperationEnabled(node_id, false); + auto& state = nodeState_(node_id); + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + state.target_velocity = 0; + state.target_torque = 0; + if (!writeTargetsForMode_(node_id, state.mode, state)) { + return false; + } + waitVelocityNearZero_(node_id, "torqueOff"); + + std::uint16_t statusword = 0; + if (!readStatusword_(node_id, statusword)) { + return false; + } + const auto status = cia402::statusword(statusword); + if (cia402::hasState(status, cia402::DeviceState::SwitchOnDisabled) || + cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) { + return true; + } + + if (cia402::hasState(status, cia402::DeviceState::OperationEnabled)) { + if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), + cia402::DeviceState::SwitchedOn, + "Switched On")) { + return false; + } + return writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On"); + } + + if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) { + return writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On"); + } + + return writeControlwordAndWait_(node_id, cia402::controlword(0), + cia402::DeviceState::SwitchOnDisabled, + "Switch On Disabled"); +} + +bool Cia402Protocol::brakeRelease(const std::uint8_t node_id) +{ + CMVR_LOG(ERROR) << "[Cia402Protocol] brake release is vendor-specific, node=" + << static_cast(node_id); + return false; +} + +bool Cia402Protocol::quickStop(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + status_monitor_->setExpectedOperationEnabled(node_id, false); + auto& state = nodeState_(node_id); + state.target_velocity = 0; + state.target_torque = 0; + if (!writeTargetsForMode_(node_id, state.mode, state)) { + return false; + } + + state.controlword = cia402::quickStopControlword(); + if (!writeControlword_(node_id, state.controlword)) { + return false; + } + return waitVelocityNearZero_(node_id, "quickStop"); +} + +double Cia402Protocol::getQ(const std::uint8_t node_id) +{ + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + return 0.0; + } + const auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return 0.0; + } + return countsToRad_(actual_position, state); +} + +double Cia402Protocol::getQd(const std::uint8_t node_id) +{ + std::int32_t actual_velocity = 0; + if (!readActualVelocity_(node_id, actual_velocity)) { + return 0.0; + } + const auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return 0.0; + } + return countsToRadPerSec_(actual_velocity, state); +} + +bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id) +{ + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual position, node=" + << static_cast(node_id); + return false; + } + + auto& state = nodeState_(node_id); + state.target_position = actual_position; + state.target_velocity = 0; + state.target_torque = 0; + return writeTargetsForMode_(node_id, state.mode, state); +} + +std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode) +{ + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + return 1; + case msgs::RUN_MODE_PROFILE_VELOCITY: + return 3; + case msgs::RUN_MODE_HOMING: + return 6; + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + return 8; + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + return 9; + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + return 10; + default: + return 8; + } +} + +msgs::RunMode Cia402Protocol::fromCia402Mode_(const std::int8_t mode) +{ + switch (mode) { + case 1: + return msgs::RUN_MODE_PROFILE_POSITION; + case 3: + return msgs::RUN_MODE_PROFILE_VELOCITY; + case 6: + return msgs::RUN_MODE_HOMING; + case 8: + return msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + case 9: + return msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + case 10: + return msgs::RUN_MODE_CYCLIC_SYNC_CURRENT; + default: + return msgs::RUN_MODE_UNSPECIFIED; + } +} + +cia402::Controlword Cia402Protocol::nextControlword_(const cia402::Statusword statusword) +{ + if (statusword.fault != 0) { + return cia402::faultResetControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::SwitchOnDisabled)) { + return cia402::shutdownControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::ReadyToSwitchOn)) { + return cia402::switchOnControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::SwitchedOn)) { + return cia402::enableOperationControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::OperationEnabled)) { + return cia402::enableOperationControlword(); + } + return cia402::shutdownControlword(); +} + +bool Cia402Protocol::isOperationEnabled_(const cia402::Statusword statusword) +{ + return cia402::isOperationEnabled(statusword); +} + +bool Cia402Protocol::targetReached_(const cia402::Statusword statusword) +{ + return cia402::targetReached(statusword); +} + +std::int32_t Cia402Protocol::radToCounts_(const double angle_rad, + const NodeState& state) const +{ + const double rev = angle_rad / (2.0 * M_PI); + return static_cast( + std::llround(rev * state.gear_ratio * state.encoder_counts_per_rev)); +} + +double Cia402Protocol::countsToRad_(const std::int32_t counts, + const NodeState& state) const +{ + return static_cast(counts) / + (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI; +} + +std::int32_t Cia402Protocol::radPerSecToCounts_( + const double velocity_rad_s, + const NodeState& state) const +{ + const double rev_per_sec = velocity_rad_s / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec * state.gear_ratio * state.encoder_counts_per_rev)); +} + +std::int32_t Cia402Protocol::radPerSec2ToCounts_( + const double acceleration_rad_s2, + const NodeState& state) const +{ + const double rev_per_sec2 = acceleration_rad_s2 / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec2 * state.gear_ratio * state.encoder_counts_per_rev)); +} + +double Cia402Protocol::countsToRadPerSec_( + const std::int32_t velocity_counts_s, + const NodeState& state) const +{ + return static_cast(velocity_counts_s) / + (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI; +} + +Cia402Protocol::NodeState& Cia402Protocol::nodeState_(const std::uint8_t node_id) +{ + return nodes_[node_id]; +} + +const Cia402Protocol::NodeState* Cia402Protocol::findNodeState_( + const std::uint8_t node_id) const +{ + const auto it = nodes_.find(node_id); + if (it == nodes_.end()) { + return nullptr; + } + return &it->second; +} + +bool Cia402Protocol::hasValidConversion_(const std::uint8_t node_id, + const NodeState& state) const +{ + if (state.encoder_counts_per_rev > 0.0 && state.gear_ratio > 0.0) { + return true; + } + CMVR_LOG(ERROR) << "[Cia402Protocol] missing conversion config for node " + << static_cast(node_id) + << ": encoder_counts_per_rev=" << state.encoder_counts_per_rev + << ", gear_ratio=" << state.gear_ratio; + return false; +} + +bool Cia402Protocol::validateNodePdos_(const std::uint8_t node_id) const +{ + if (!bus_runtime_) { + return false; + } + struct RequiredEntry { + std::uint16_t index; + std::uint8_t subindex; + const char* name; + }; + const RequiredEntry required[] = { + {msgs::CIA402_CONTROL_WORD_6040, 0x00, "controlword"}, + {msgs::CIA402_TARGET_POSITION_607A, 0x00, "target position"}, + {msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, "target velocity"}, + {msgs::CIA402_TARGET_TORQUE_6071, 0x00, "target torque"}, + {msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, "profile acceleration"}, + {msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, "profile deceleration"}, + {msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, "profile velocity"}, + {msgs::CIA402_OPERATION_MODE_6060, 0x00, "operation mode"}, + {msgs::CIA402_STATUS_WORD_6041, 0x00, "statusword"}, + {msgs::CIA402_ACTUAL_POSITION_6064, 0x00, "actual position"}, + {msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, "actual velocity"}, + {msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, "actual torque"}, + {msgs::CIA402_MODE_DISPLAY_6061, 0x00, "mode display"}, + {msgs::CIA402_ERROR_CODE_603F, 0x00, "error code"}, + }; + + for (const auto& entry : required) { + if (!bus_runtime_->hasPdoEntry(node_id, entry.index, entry.subindex)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] missing PDO entry for node " + << static_cast(node_id) + << ": " << entry.name + << " 0x" << std::hex << entry.index + << ":" << static_cast(entry.subindex) << std::dec; + return false; + } + } + return true; +} + +bool Cia402Protocol::readStatusword_(const std::uint8_t node_id, + std::uint16_t& statusword) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_STATUS_WORD_6041, 0x00, + statusword); +} + +bool Cia402Protocol::readActualPosition_(const std::uint8_t node_id, + std::int32_t& actual_position) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00, + actual_position); +} + +bool Cia402Protocol::readActualVelocity_(const std::uint8_t node_id, + std::int32_t& actual_velocity) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, + actual_velocity); +} + +bool Cia402Protocol::readModeDisplay_(const std::uint8_t node_id, + std::int8_t& mode_display) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00, + mode_display); +} + +bool Cia402Protocol::writeControlword_(const std::uint8_t node_id, + const cia402::Controlword controlword) +{ + if (!bus_runtime_) { + return false; + } + auto& state = nodeState_(node_id); + state.controlword = controlword; + return bus_runtime_->writePdo(node_id, msgs::CIA402_CONTROL_WORD_6040, + 0x00, controlword.value); +} + +bool Cia402Protocol::writeControlwordAndWait_( + const std::uint8_t node_id, + const cia402::Controlword controlword, + const cia402::DeviceState target_state, + const char* state_name) +{ + if (!writeControlword_(node_id, controlword)) { + return false; + } + return waitStatus_(node_id, target_state, state_name); +} + +bool Cia402Protocol::waitStatus_(const std::uint8_t node_id, + const cia402::DeviceState target_state, + const char* state_name) const +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.state_transition_timeout_ms()); + std::uint16_t last_statusword = 0; + do { + if (readStatusword_(node_id, last_statusword) && + cia402::hasState(cia402::statusword(last_statusword), target_state)) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for " + << state_name << ", node=" << static_cast(node_id) + << ", last_statusword=0x" << std::hex << last_statusword << std::dec; + return false; +} + +bool Cia402Protocol::waitMode_(const std::uint8_t node_id, + const std::int8_t target_mode) const +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.state_transition_timeout_ms()); + std::int8_t last_mode = 0; + do { + if (readModeDisplay_(node_id, last_mode) && last_mode == target_mode) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for operation mode, node=" + << static_cast(node_id) + << ", target_mode=" << static_cast(target_mode) + << ", last_mode=" << static_cast(last_mode); + return false; +} + +bool Cia402Protocol::waitSetPointAcknowledged_( + const std::uint8_t node_id, + const bool acknowledged) const +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.state_transition_timeout_ms()); + std::uint16_t last_statusword = 0; + do { + if (readStatusword_(node_id, last_statusword) && + cia402::setPointAcknowledged(cia402::statusword(last_statusword)) == acknowledged) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for set-point acknowledge=" + << acknowledged + << ", node=" << static_cast(node_id) + << ", last_statusword=0x" << std::hex << last_statusword << std::dec; + return false; +} + +bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id, + const char* action_name) const +{ + const auto* state = findNodeState_(node_id); + if (state == nullptr || !hasValidConversion_(node_id, *state)) { + return false; + } + + const auto tolerance_counts = std::max( + 1, std::abs(radPerSecToCounts_(config_.stopped_velocity_tolerance_rad_s(), *state))); + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.velocity_stop_timeout_ms()); + std::int32_t last_velocity = 0; + do { + if (readActualVelocity_(node_id, last_velocity) && + std::abs(last_velocity) <= tolerance_counts) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting velocity near zero " + << "during " << action_name + << ", node=" << static_cast(node_id) + << ", last_velocity=" << last_velocity + << ", tolerance=" << tolerance_counts; + return false; +} + +bool Cia402Protocol::writePositionLimitsToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_q_lb) || !std::isfinite(state.limit_q_ub) || + state.limit_q_ub <= state.limit_q_lb) { + return true; + } + + const auto lower_limit = radToCounts_(state.limit_q_lb, state); + const auto upper_limit = radToCounts_(state.limit_q_ub, state); + + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, lower_limit) && + bus_runtime_->writeSdo(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, upper_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write software position " + << "limits to dictionary, node=" << static_cast(node_id) + << ", lower=" << lower_limit + << ", upper=" << upper_limit; + } + return ok; +} + +bool Cia402Protocol::writeVelocityLimitToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_qd) || state.limit_qd <= 0.0) { + return true; + } + + const auto velocity_limit = + static_cast(std::abs(radPerSecToCounts_(state.limit_qd, state))); + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_MAX_PROFILE_VELOCITY_607F, + 0x00, velocity_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write velocity limit " + << "to dictionary, node=" << static_cast(node_id) + << ", velocity_limit=" << velocity_limit; + } + return ok; +} + +bool Cia402Protocol::writeAccelerationLimitsToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_qdd) || state.limit_qdd <= 0.0) { + return true; + } + + const auto acceleration_limit = + static_cast(std::abs(radPerSec2ToCounts_(state.limit_qdd, state))); + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, acceleration_limit) && + bus_runtime_->writeSdo(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, acceleration_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write acceleration limits " + << "to dictionary, node=" << static_cast(node_id) + << ", acceleration_limit=" << acceleration_limit; + } + return ok; +} + +bool Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id, + NodeState& state) +{ + if (!bus_runtime_) { + return false; + } + + if (state.profile_velocity <= 0 && state.limit_qd > 0.0) { + state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state)); + } + if (state.profile_acceleration <= 0 && state.limit_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + + if (!writeTargetsForMode_(node_id, state.mode, state)) { + return false; + } + + state.controlword = cia402::profilePositionControlword(false); + if (!writeControlword_(node_id, state.controlword) || + !waitSetPointAcknowledged_(node_id, false)) { + return false; + } + + state.controlword = cia402::profilePositionControlword(true); + if (!writeControlword_(node_id, state.controlword) || + !waitSetPointAcknowledged_(node_id, true)) { + state.controlword = cia402::profilePositionControlword(false); + writeControlword_(node_id, state.controlword); + return false; + } + + state.controlword = cia402::profilePositionControlword(false); + return writeControlword_(node_id, state.controlword) && + waitSetPointAcknowledged_(node_id, false); +} + +bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id, + const msgs::RunMode mode, + NodeState& state) +{ + state.target_velocity = 0; + state.target_torque = 0; + + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: { + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual " + << "position before switching mode, node=" + << static_cast(node_id) + << ", mode=" << static_cast(mode); + return false; + } + state.target_position = actual_position; + break; + } + + case msgs::RUN_MODE_PROFILE_VELOCITY: + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + case msgs::RUN_MODE_HOMING: + case msgs::RUN_MODE_UNSPECIFIED: + default: + break; + } + + return true; +} + +bool Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id, + const msgs::RunMode mode, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + + std::array writes{}; + std::size_t count = 0; + if (!appendTargetWritesForMode_(node_id, mode, state, + writes.data(), writes.size(), count)) { + return false; + } + return count == 0 || bus_runtime_->writePdosAtomic(writes.data(), count); +} + +bool Cia402Protocol::appendTargetWritesForMode_( + const std::uint8_t node_id, + const msgs::RunMode mode, + const NodeState& state, + EthercatMotorBusRuntime::PdoWrite* writes, + const std::size_t capacity, + std::size_t& count) const +{ + if (!bus_runtime_ || !writes) { + return false; + } + + const auto append = [&](const auto write) { + if (count >= capacity) { + return false; + } + writes[count++] = write; + return true; + }; + + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_POSITION_607A, + 0x00, state.target_position))) { + return false; + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, 0x00)) { + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_PROFILE_VELOCITY_6081, + 0x00, state.profile_velocity))) { + return false; + } + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, state.profile_acceleration))) { + return false; + } + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration))) { + return false; + } + } + break; + + case msgs::RUN_MODE_PROFILE_VELOCITY: + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity))) { + return false; + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, state.profile_acceleration))) { + return false; + } + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration))) { + return false; + } + } + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_POSITION_607A, + 0x00, state.target_position)) || + !append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity))) { + return false; + } + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity))) { + return false; + } + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + if (!append(EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_TARGET_TORQUE_6071, + 0x00, state.target_torque))) { + return false; + } + break; + + default: + break; + } + return true; +} + +bool Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state) +{ + if (!bus_runtime_) { + return false; + } + + std::uint16_t statusword = 0; + if (readStatusword_(node_id, statusword)) { + const auto status = cia402::statusword(statusword); + state.controlword = nextControlword_(status); + std::int32_t actual_position = 0; + if (!isOperationEnabled_(status) && + readActualPosition_(node_id, actual_position) && + actual_position != 0) { + state.target_position = actual_position; + } + } + + std::array writes{}; + std::size_t count = 0; + writes[count++] = EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, state.controlword.value); + writes[count++] = EthercatMotorBusRuntime::makePdoWrite( + node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, toCia402Mode_(state.mode)); + if (!appendTargetWritesForMode_(node_id, state.mode, state, + writes.data(), writes.size(), count)) { + return false; + } + return bus_runtime_->writePdosAtomic(writes.data(), count); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp new file mode 100644 index 00000000..1e0eeb5f --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp @@ -0,0 +1,284 @@ +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h" + +#include +#include +#include +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "common/base/logging/logger.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" + +namespace cmvr::device { + +Cia402StatusMonitor::Cia402StatusMonitor( + std::shared_ptr bus_runtime, + const std::chrono::milliseconds poll_period) + : bus_runtime_(std::move(bus_runtime)), + poll_period_(std::max(poll_period, std::chrono::milliseconds{1})), + monitor_thread_(&Cia402StatusMonitor::monitorLoop_, this) +{ +} + +Cia402StatusMonitor::~Cia402StatusMonitor() +{ + running_.store(false); + if (monitor_thread_.joinable()) { + monitor_thread_.join(); + } +} + +void Cia402StatusMonitor::addNode(const std::uint8_t node_id) +{ + { + std::lock_guard lock(states_mutex_); + states_.try_emplace(node_id); + } + monitorNode_(node_id, false); +} + +void Cia402StatusMonitor::setExpectedOperationEnabled( + const std::uint8_t node_id, + const bool expected) +{ + { + std::lock_guard lock(states_mutex_); + states_[node_id].expected_operation_enabled = expected; + } + monitorNode_(node_id, expected); +} + +bool Cia402StatusMonitor::isNodeOperational(const std::uint8_t node_id) const +{ + if (!bus_runtime_ || !bus_runtime_->isHealthy()) { + return false; + } + std::lock_guard lock(states_mutex_); + const auto it = states_.find(node_id); + return it != states_.end() && it->second.has_last_sample && + it->second.last_sample.read_ok && + it->second.last_sample.transport_healthy && + it->second.last_sample.operation_enabled && + !it->second.last_sample.command_blocked; +} + +void Cia402StatusMonitor::monitorLoop_() +{ + while (running_.load()) { + std::array, 256> nodes{}; + std::size_t node_count = 0; + { + std::lock_guard lock(states_mutex_); + for (const auto& [node_id, state] : states_) { + if (node_count >= nodes.size()) { + break; + } + nodes[node_count++] = {node_id, state.expected_operation_enabled}; + } + } + for (std::size_t i = 0; i < node_count; ++i) { + monitorNode_(nodes[i].first, nodes[i].second); + } + std::this_thread::sleep_for(poll_period_); + } +} + +void Cia402StatusMonitor::monitorNode_( + const std::uint8_t node_id, + const bool expected_operation_enabled) +{ + std::lock_guard monitor_lock(monitor_mutex_); + StatusSample current; + readStatusSample_(node_id, expected_operation_enabled, current); + + StatusSample previous; + bool had_previous = false; + { + std::lock_guard lock(states_mutex_); + auto& state = states_[node_id]; + had_previous = state.has_last_sample; + previous = state.last_sample; + state.last_sample = current; + state.has_last_sample = true; + } + + if (!current.transport_healthy) { + return; + } + if (!current.read_ok) { + if (!had_previous || previous.read_ok) { + CMVR_LOG(ERROR) << "[Cia402StatusMonitor] failed to read node status snapshot" + << ", node=" << static_cast(node_id); + } + return; + } + if (had_previous && !previous.read_ok) { + CMVR_LOG(INFO) << "[Cia402StatusMonitor] node status snapshot recovered" + << ", node=" << static_cast(node_id); + } + + reportErrorCodeTransition_(node_id, had_previous, previous, current); + reportStatuswordTransition_(node_id, had_previous, previous, current); +} + +void Cia402StatusMonitor::reportErrorCodeTransition_( + const std::uint8_t node_id, + const bool had_previous, + const StatusSample& previous, + const StatusSample& current) const +{ + const bool error_code_changed = + !had_previous || !previous.read_ok || + previous.error_code != current.error_code; + if (current.error_code != 0 && error_code_changed) { + CMVR_LOG(ERROR) << "[Cia402StatusMonitor] [error code] 0x" + << std::hex << std::uppercase << std::setw(4) + << std::setfill('0') << current.error_code + << std::dec << std::nouppercase << std::setfill(' ') + << ' ' << errorCodeDescription_(current.error_code) + << ", node=" << static_cast(node_id); + } else if (current.error_code == 0 && had_previous && previous.read_ok && + previous.error_code != 0) { + CMVR_LOG(INFO) << "[Cia402StatusMonitor] [error code] recovered" + << ", node=" << static_cast(node_id) + << ", previous_code=0x" << std::hex << std::uppercase + << std::setw(4) << std::setfill('0') << previous.error_code + << std::dec << std::nouppercase << std::setfill(' '); + } +} + +void Cia402StatusMonitor::reportStatuswordTransition_( + const std::uint8_t node_id, + const bool had_previous, + const StatusSample& previous, + const StatusSample& current) const +{ + const bool status_changed = + !had_previous || !previous.read_ok || + previous.status_problem != current.status_problem || + (previous.statusword & 0x0888) != (current.statusword & 0x0888); + if (current.status_problem && status_changed) { + const auto status = cia402::statusword(current.statusword); + CMVR_LOG(WARNING) << "[Cia402StatusMonitor] [statusword] 0x" + << std::hex << std::uppercase << std::setw(4) + << std::setfill('0') << current.statusword + << std::dec << std::nouppercase << std::setfill(' ') + << ' ' << deviceStateName_(current.statusword) + << ", node=" << static_cast(node_id) + << (status.fault != 0 ? ", fault" : "") + << (status.warning != 0 ? ", warning" : "") + << (status.internal_limit_active != 0 + ? ", internal limit active" + : "") + << (current.expected_operation_enabled && + !current.operation_enabled + ? ", operation not enabled" + : ""); + } else if (!current.status_problem && had_previous && previous.read_ok && + previous.status_problem) { + CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] recovered" + << ", node=" << static_cast(node_id) + << ", previous_statusword=0x" << std::hex << std::uppercase + << std::setw(4) << std::setfill('0') << previous.statusword + << std::dec << std::nouppercase << std::setfill(' '); + } +} + +bool Cia402StatusMonitor::readStatusSample_( + const std::uint8_t node_id, + const bool expected_operation_enabled, + StatusSample& sample) const +{ + sample.expected_operation_enabled = expected_operation_enabled; + sample.transport_healthy = bus_runtime_ && bus_runtime_->isHealthy(); + if (!sample.transport_healthy) { + return false; + } + + std::array reads{ + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_STATUS_WORD_6041, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ERROR_CODE_603F, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00), + EthercatMotorBusRuntime::makePdoRead( + node_id, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00), + }; + if (!bus_runtime_->readPdosAtomic(reads.data(), reads.size())) { + return false; + } + + sample.statusword = EthercatMotorBusRuntime::pdoReadValue(reads[0]); + sample.error_code = EthercatMotorBusRuntime::pdoReadValue(reads[1]); + sample.mode_display = EthercatMotorBusRuntime::pdoReadValue(reads[2]); + sample.actual_position = EthercatMotorBusRuntime::pdoReadValue(reads[3]); + sample.actual_velocity = EthercatMotorBusRuntime::pdoReadValue(reads[4]); + sample.actual_torque = EthercatMotorBusRuntime::pdoReadValue(reads[5]); + sample.read_ok = true; + + const auto status = cia402::statusword(sample.statusword); + sample.operation_enabled = cia402::isOperationEnabled(status); + sample.status_problem = status.fault != 0 || status.warning != 0 || + status.internal_limit_active != 0 || + (expected_operation_enabled && !sample.operation_enabled); + sample.command_blocked = status.fault != 0 || status.warning != 0 || + sample.error_code != 0 || + (expected_operation_enabled && !sample.operation_enabled); + return true; +} + +const char* Cia402StatusMonitor::deviceStateName_(const std::uint16_t statusword) +{ + if ((statusword & 0x004F) == 0x000F) { + return "FaultReactionActive"; + } + if ((statusword & 0x004F) == 0x0008) { + return "Fault"; + } + if ((statusword & 0x006F) == 0x0007) { + return "QuickStopActive"; + } + const auto status = cia402::statusword(statusword); + if (cia402::isOperationEnabled(status)) { + return "OperationEnabled"; + } + if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) { + return "SwitchedOn"; + } + if (cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) { + return "ReadyToSwitchOn"; + } + if (cia402::isSwitchOnDisabled(status)) { + return "SwitchOnDisabled"; + } + return "NotReadyToSwitchOn"; +} + +const char* Cia402StatusMonitor::errorCodeDescription_(const std::uint16_t error_code) +{ + switch (error_code) { + case 0x0000: + return "no error"; + case 0x2310: + return "continuous over-current"; + case 0x3210: + return "DC bus over-voltage"; + case 0x3220: + return "DC bus under-voltage"; + case 0x4210: + return "device over-temperature"; + case 0x4310: + return "drive over-temperature"; + case 0x8611: + return "position following error"; + default: + return "unknown or vendor-specific error"; + } +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp new file mode 100644 index 00000000..20fcbfa2 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp @@ -0,0 +1,277 @@ +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +EyouMotor::EyouMotor(const config::MotorConfigItem& config, + std::shared_ptr cia402_protocol, + std::unique_ptr vendor_adapter) + : cia402_protocol_(std::move(cia402_protocol)), + vendor_adapter_(std::move(vendor_adapter)) +{ + info_.id = config.id(); + info_.joint_name = config.joint_name(); + info_.limit_q_lb = config.limit_q_lb(); + info_.limit_q_ub = config.limit_q_ub(); + info_.limit_qd = config.limit_qd(); + info_.limit_qdd = config.limit_qdd(); + encoder_counts_per_rev_ = config.encoder_counts_per_rev(); + gear_ratio_ = config.gear_ratio(); + node_id_ = static_cast(info_.id); + id_ = info_.joint_name; + protocol_ = cia402_protocol_; +} + +bool EyouMotor::init() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_() || !hasValidConversion_()) { + return false; + } + + if (!cia402_protocol_->initNode(node_id_)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to init CiA402 node: " << info_.joint_name; + return false; + } + if (!vendor_adapter_->initNode(node_id_)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name; + return false; + } + cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); + + cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); + if (!writeVendorVelocityLimit_()) { + return false; + } + if (info_.limit_qdd > 0.0) { + cia402_protocol_->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd); + } + if (!writeVendorPositionLimits_()) { + return false; + } + return true; +} + +void EyouMotor::setLimitQ(const double ub, const double lb) +{ + std::scoped_lock lock(mtx_); + info_.limit_q_ub = ub; + info_.limit_q_lb = lb; + if (!hasDependencies_() || !hasValidConversion_()) { + return; + } + writeVendorPositionLimits_(); +} + +void EyouMotor::setLimitQd(const double qd) +{ + std::scoped_lock lock(mtx_); + info_.limit_qd = qd; + if (!hasDependencies_() || !hasValidConversion_()) { + return; + } + cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); + writeVendorVelocityLimit_(); +} + +bool EyouMotor::calibrateZeroQ() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_() || !hasValidConversion_()) { + return false; + } + + const auto counts_per_joint_revolution = static_cast( + std::llround(encoder_counts_per_rev_ * gear_ratio_)); + std::int32_t zeroed_position = 0; + if (!vendor_adapter_->calibrateZero(node_id_, counts_per_joint_revolution, + zeroed_position)) { + CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name; + return false; + } + if (!cia402_protocol_->syncTargetToActualPosition(node_id_)) { + return false; + } + if (!writeVendorPositionLimits_()) { + return false; + } + return true; +} + +bool EyouMotor::commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) +{ + if (motors.empty() || motors.size() != positions.size() || + motors.size() != velocities.size() || motors.size() > 256) { + return false; + } + + std::array lock_order{}; + std::array commands{}; + std::shared_ptr protocol; + for (std::size_t i = 0; i < motors.size(); ++i) { + const auto motor = std::dynamic_pointer_cast(motors[i]); + if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) { + return false; + } + if (!protocol) { + protocol = motor->cia402_protocol_; + } else if (protocol.get() != motor->cia402_protocol_.get()) { + CMVR_LOG(ERROR) << "[EyouMotor] batch target motors belong to different " + << "CiA402 protocols"; + return false; + } + for (std::size_t previous = 0; previous < i; ++previous) { + if (lock_order[previous] == motor.get()) { + return false; + } + } + lock_order[i] = motor.get(); + commands[i] = Cia402Protocol::CyclicPositionCommand{ + motor->node_id_, positions[i], velocities[i]}; + } + + std::sort(lock_order.begin(), lock_order.begin() + motors.size(), + std::less{}); + std::array, 256> locks{}; + for (std::size_t i = 0; i < motors.size(); ++i) { + locks[i] = std::unique_lock(lock_order[i]->mtx_); + } + return protocol && protocol->commandCyclicPositionsAtomic(commands.data(), motors.size()); +} + +bool EyouMotor::readFeedbacksAtomic( + const std::vector>& motors, + std::vector& positions, + std::vector& velocities) +{ + if (motors.empty() || motors.size() > 256) { + return false; + } + + std::array lock_order{}; + std::array feedbacks{}; + std::shared_ptr protocol; + for (std::size_t i = 0; i < motors.size(); ++i) { + const auto motor = std::dynamic_pointer_cast(motors[i]); + if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) { + return false; + } + if (!protocol) { + protocol = motor->cia402_protocol_; + } else if (protocol.get() != motor->cia402_protocol_.get()) { + CMVR_LOG(ERROR) << "[EyouMotor] batch feedback motors belong to different " + << "CiA402 protocols"; + return false; + } + for (std::size_t previous = 0; previous < i; ++previous) { + if (lock_order[previous] == motor.get()) { + return false; + } + } + lock_order[i] = motor.get(); + feedbacks[i].node_id = motor->node_id_; + } + + std::sort(lock_order.begin(), lock_order.begin() + motors.size(), + std::less{}); + std::array, 256> locks{}; + for (std::size_t i = 0; i < motors.size(); ++i) { + locks[i] = std::unique_lock(lock_order[i]->mtx_); + } + if (!protocol || !protocol->readFeedbacksAtomic(feedbacks.data(), motors.size())) { + return false; + } + + positions.resize(motors.size()); + velocities.resize(motors.size()); + for (std::size_t i = 0; i < motors.size(); ++i) { + positions[i] = feedbacks[i].q; + velocities[i] = feedbacks[i].qd; + } + return true; +} + +bool EyouMotor::brakeRelease() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_()) { + return false; + } + return vendor_adapter_->brakeRelease(node_id_); +} + +bool EyouMotor::hasDependencies_() const +{ + if (!cia402_protocol_ || !vendor_adapter_) { + CMVR_LOG(ERROR) << "[EyouMotor] missing protocol or vendor adapter: " + << info_.joint_name; + return false; + } + if (cia402_protocol_->comm_proto != MotorProtocolInterface::CommProto::ETHERCAT) { + CMVR_LOG(ERROR) << "[EyouMotor] invalid protocol for motor: " << info_.joint_name; + return false; + } + return true; +} + +bool EyouMotor::hasValidConversion_() const +{ + if (encoder_counts_per_rev_ > 0.0 && gear_ratio_ > 0.0) { + return true; + } + CMVR_LOG(ERROR) << "[EyouMotor] missing encoder conversion config: " + << info_.joint_name + << ", encoder_counts_per_rev=" << encoder_counts_per_rev_ + << ", gear_ratio=" << gear_ratio_; + return false; +} + +bool EyouMotor::writeVendorPositionLimits_() const +{ + if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) || + info_.limit_q_ub <= info_.limit_q_lb) { + return true; + } + + cia402_protocol_->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); + return vendor_adapter_->writePositionLimits(node_id_, + radToCounts_(info_.limit_q_lb), + radToCounts_(info_.limit_q_ub)); +} + +bool EyouMotor::writeVendorVelocityLimit_() const +{ + if (!std::isfinite(info_.limit_qd) || info_.limit_qd <= 0.0) { + return true; + } + + return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd)); +} + +std::int32_t EyouMotor::radToCounts_(const double angle_rad) const +{ + const double rev = angle_rad / (2.0 * M_PI); + return static_cast( + std::llround(rev * gear_ratio_ * encoder_counts_per_rev_)); +} + +std::uint32_t EyouMotor::radPerSecToCounts_(const double velocity_rad_s) const +{ + const double rev_per_sec = std::abs(velocity_rad_s) / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec * gear_ratio_ * encoder_counts_per_rev_)); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp new file mode 100644 index 00000000..04e0fcfa --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp @@ -0,0 +1,376 @@ +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +#include +#include +#include +#include +#include +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "common/base/logging/logger.h" +#include "common/math/support_functions.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h" + +namespace cmvr::device { + +EyouMotorAdapter::EyouMotorAdapter( + std::shared_ptr bus_runtime) + : bus_runtime_(std::move(bus_runtime)) +{ +} + +bool EyouMotorAdapter::initNode(const std::uint8_t node_id) +{ + return bus_runtime_ && bus_runtime_->hasMotor(node_id); +} + +bool EyouMotorAdapter::writePositionLimits(const std::uint8_t node_id, + const std::int32_t lower_limit, + const std::int32_t upper_limit) +{ + if (!bus_runtime_) { + return false; + } + + const auto write_limits = [&]() { + return bus_runtime_->writeSdo(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0) && + bus_runtime_->writeSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, upper_limit) && + bus_runtime_->writeSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, lower_limit) && + bus_runtime_->writeSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0x4C494D54); + }; + const auto readback_matches = [&]() { + std::int32_t actual_lower = 0; + std::int32_t actual_upper = 0; + return bus_runtime_->readSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, actual_lower) && + bus_runtime_->readSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, actual_upper) && + actual_lower == lower_limit && + actual_upper == upper_limit; + }; + + const bool ok = write_limits() && readback_matches(); + if (!ok) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write software position " + << "limits, node=" << static_cast(node_id) + << ", soft_limit_state=" << 0x4C494D54 + << ", lower=" << lower_limit + << ", upper=" << upper_limit; + } + return ok; +} + +bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id, + const std::uint32_t velocity_limit) +{ + if (!bus_runtime_) { + return false; + } + + std::uint32_t actual_velocity_limit = 0; + const bool ok = + bus_runtime_->writeSdo(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024, + 0x00, velocity_limit) && + bus_runtime_->readSdo(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024, + 0x00, actual_velocity_limit) && + actual_velocity_limit == velocity_limit; + if (!ok) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write over speed " + << "threshold, node=" << static_cast(node_id) + << ", expected=" << velocity_limit + << ", actual=" << actual_velocity_limit; + } + return ok; +} + +bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id, + const std::int64_t counts_per_joint_revolution, + std::int32_t& zeroed_position) +{ + if (!bus_runtime_) { + return false; + } + + const auto& zero_config = bus_runtime_->config().zero_calibration(); + const auto home_offset_timeout = + std::chrono::milliseconds{zero_config.timeout_ms()}; + const auto home_offset_poll_period = + std::chrono::milliseconds{zero_config.poll_period_ms()}; + const auto home_offset_stable_samples = zero_config.stable_sample_count(); + const auto home_offset_position_tolerance_counts = + zero_config.position_tolerance_counts(); + const auto home_offset_stable_delta_counts = + zero_config.stable_delta_counts(); + + std::uint32_t original_soft_limit_state = 0; + std::int32_t original_home_offset = 0; + std::int32_t original_position = 0; + if (!bus_runtime_->readSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, original_soft_limit_state) || + !bus_runtime_->readSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, original_home_offset) || + !bus_runtime_->readSdo( + node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, original_position)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to snapshot calibration state, node=" + << static_cast(node_id); + return false; + } + + // EYOU applies HomeOffset additively, so clearing it exposes this raw position. + const auto expected_cleared_position_wide = + static_cast(original_position) - + static_cast(original_home_offset); + if (expected_cleared_position_wide < std::numeric_limits::min() || + expected_cleared_position_wide > std::numeric_limits::max()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] cleared position would overflow, node=" + << static_cast(node_id) + << ", original_position=" << original_position + << ", original_home_offset=" << original_home_offset; + return false; + } + const auto expected_cleared_position = + static_cast(expected_cleared_position_wide); + + const auto write_home_offset = [&](const std::int32_t value) { + return bus_runtime_->writeSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, 0x00, value); + }; + const auto save_parameters = [&]() { + return bus_runtime_->writeSdo( + node_id, eyou::EYOU_STORE_PARAMETERS_1010, + 0x01, 0x65766173); + }; + const auto wait_for_soft_limit = [&](const std::uint32_t expected_state) { + const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout; + do { + std::uint32_t actual_soft_limit_state = 0; + if (bus_runtime_->readSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, actual_soft_limit_state) && + actual_soft_limit_state == expected_state) { + return true; + } + std::this_thread::sleep_for(home_offset_poll_period); + } while (std::chrono::steady_clock::now() < deadline); + return false; + }; + const auto restore_soft_limit = [&]() { + return bus_runtime_->writeSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, original_soft_limit_state) && + wait_for_soft_limit(original_soft_limit_state); + }; + const auto wait_for_position = [&](const char* phase, + const std::int32_t expected_offset, + const std::int32_t expected_position, + std::int32_t& observed_position) { + const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout; + std::uint32_t stable_samples = 0; + bool has_previous_position = false; + std::int32_t previous_position = 0; + std::int32_t observed_offset = 0; + + do { + const bool read_ok = + bus_runtime_->readSdo( + node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, observed_offset) && + bus_runtime_->readSdo( + node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, observed_position); + const bool position_stable = + !has_previous_position || + SupportFunctions::cyclicAbsoluteDifference( + observed_position, previous_position, + counts_per_joint_revolution) <= home_offset_stable_delta_counts; + const bool sample_matches = + read_ok && observed_offset == expected_offset && + SupportFunctions::cyclicAbsoluteDifference( + observed_position, expected_position, + counts_per_joint_revolution) <= home_offset_position_tolerance_counts && + position_stable; + + stable_samples = sample_matches ? stable_samples + 1 : 0; + if (stable_samples >= home_offset_stable_samples) { + return true; + } + + if (read_ok) { + previous_position = observed_position; + has_previous_position = true; + } + std::this_thread::sleep_for(home_offset_poll_period); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[EyouMotorAdapter] timed out waiting for home offset state, node=" + << static_cast(node_id) + << ", phase=" << phase + << ", expected_offset=" << expected_offset + << ", actual_offset=" << observed_offset + << ", expected_position=" << expected_position + << ", actual_position=" << observed_position + << ", cyclic_position_distance=" + << SupportFunctions::cyclicAbsoluteDifference( + observed_position, expected_position, + counts_per_joint_revolution) + << ", counts_per_joint_revolution=" + << counts_per_joint_revolution + << ", stable_samples=" << stable_samples; + return false; + }; + // Follow EYOU's required clear -> set -> save sequence during rollback too. + const auto rollback = [&](const char* failed_phase) { + const bool clear_written = write_home_offset(0); + std::int32_t cleared_position = 0; + const bool clear_applied = + clear_written && + wait_for_position("rollback_clear_home_offset", 0, + expected_cleared_position, cleared_position); + const bool offset_written = write_home_offset(original_home_offset); + std::int32_t restored_position = 0; + const bool offset_applied = + offset_written && + wait_for_position("rollback_apply_home_offset", original_home_offset, + original_position, restored_position); + const bool parameters_saved = offset_written && save_parameters(); + const bool saved_state_confirmed = + parameters_saved && + wait_for_position("rollback_save_home_offset", original_home_offset, + original_position, restored_position); + const bool soft_limit_restored = restore_soft_limit(); + const bool rollback_ok = clear_applied && offset_applied && + saved_state_confirmed && soft_limit_restored; + CMVR_LOG(ERROR) << "[EyouMotorAdapter] calibration failed and original state was " + << (rollback_ok ? "restored" : "not fully restored") + << ", node=" << static_cast(node_id) + << ", phase=" << failed_phase + << ", original_home_offset=" << original_home_offset + << ", original_soft_limit_state=" << original_soft_limit_state + << ", clear_applied=" << clear_applied + << ", offset_applied=" << offset_applied + << ", saved_state_confirmed=" << saved_state_confirmed + << ", soft_limit_restored=" << soft_limit_restored; + return false; + }; + + if (!bus_runtime_->writeSdo(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0)) { + const bool soft_limit_restored = restore_soft_limit(); + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position " + << "limit before home offset calibration, node=" + << static_cast(node_id) + << ", soft_limit_restored=" << soft_limit_restored; + return false; + } + if (!wait_for_soft_limit(0)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] software position limit did not disable, node=" + << static_cast(node_id); + return rollback("disable_soft_limit"); + } + + if (!write_home_offset(0)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node=" + << static_cast(node_id); + return rollback("clear_home_offset"); + } + + std::int32_t actual_position = 0; + if (!wait_for_position("clear_home_offset", 0, + expected_cleared_position, actual_position)) { + return rollback("wait_for_cleared_position"); + } + if (actual_position == std::numeric_limits::min()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home " + << "offset calibration, node=" << static_cast(node_id) + << ", actual_position=" << actual_position; + return rollback("negate_actual_position"); + } + + const auto home_offset = static_cast(-actual_position); + if (!write_home_offset(home_offset)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node=" + << static_cast(node_id) + << ", home_offset=" << home_offset; + return rollback("write_home_offset"); + } + + if (!wait_for_position("apply_home_offset", home_offset, 0, + zeroed_position)) { + return rollback("wait_for_zero_before_save"); + } + + if (!save_parameters()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node=" + << static_cast(node_id); + return rollback("save_parameters"); + } + + if (!wait_for_position("save_home_offset", home_offset, 0, + zeroed_position)) { + return rollback("wait_for_zero_after_save"); + } + + if (!restore_soft_limit()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position " + << "limit after home offset calibration, node=" + << static_cast(node_id) + << ", original_soft_limit_state=" << original_soft_limit_state; + return rollback("restore_soft_limit"); + } + + CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node=" + << static_cast(node_id) + << ", original_home_offset=" << original_home_offset + << ", cleared_position=" << actual_position + << ", home_offset=" << home_offset + << ", zeroed_position=" << zeroed_position + << ", soft_limit_state=" << original_soft_limit_state; + return true; +} + +bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + if (!bus_runtime_->writeSdo( + node_id, eyou::EYOU_BRAKE_CONTROL_2014, + 0x01, + 1)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to release brake, node=" + << static_cast(node_id); + return false; + } + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds{1000}; + do { + std::uint8_t brake_state = 0; + if (bus_runtime_->readSdo( + node_id, eyou::EYOU_BRAKE_CONTROL_2014, + 0x02, brake_state) && + (brake_state == 1 || brake_state == 2)) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds{10}); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[EyouMotorAdapter] brake release timeout, node=" + << static_cast(node_id); + return false; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp new file mode 100644 index 00000000..3b877f00 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp @@ -0,0 +1,382 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "common/config/config_files.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::device { +namespace { + +constexpr const char* kMotorManagerId = "ethercat_motors"; +constexpr const char* kMotorConfigFile = + "devices/motor/ethercat_motors_four_real_test.pb.txt"; +constexpr std::array kFourMotorIds{1, 2, 3, 4}; +constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; +constexpr std::chrono::milliseconds kStatsSamplePeriod{10}; +constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500}; +constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000}; +constexpr double kPi = 3.14159265358979323846; +constexpr double kRaisedCosineCoefficientRad = 0.2; +constexpr double kFourMotorPeriodS = 2.0; +constexpr std::array kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0}; +constexpr double kMinimumPositionExcursionRad = 0.2; +constexpr double kMaximumAbsoluteTrackingErrorRad = 0.15; +constexpr double kMaximumRmsTrackingErrorRad = 0.08; +constexpr double kMaximumErrorSpreadRad = 0.10; +constexpr double kFinalPositionToleranceRad = 0.05; + +class DeviceManagerDestroyGuard { +public: + ~DeviceManagerDestroyGuard() + { + DeviceManager::destroyInstance(); + } +}; + +class MotorManagerStopGuard { +public: + explicit MotorManagerStopGuard(std::shared_ptr motor_manager) + : motor_manager_(std::move(motor_manager)) + { + } + + ~MotorManagerStopGuard() + { + if (motor_manager_ && !motor_manager_->stop()) { + std::cerr << "failed to stop motor manager during test cleanup" << std::endl; + } + } + +private: + std::shared_ptr motor_manager_; +}; + +class MultiMotorSafetyGuard { +public: + explicit MultiMotorSafetyGuard( + const std::vector>& motors) + : motors_(motors) + { + } + + ~MultiMotorSafetyGuard() + { + if (armed_) { + stop(); + } + } + + bool stop() + { + bool all_ok = true; + for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) { + if (*it && !(*it)->quickStop()) { + all_ok = false; + } + } + for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) { + if (*it && !(*it)->torqueOff()) { + all_ok = false; + } + } + armed_ = !all_ok; + return all_ok; + } + +private: + const std::vector>& motors_; + bool armed_{true}; +}; + +struct TrackingErrorStats { + std::int64_t sample_count{0}; + double sum_error{0.0}; + double sum_error_sq{0.0}; + double max_abs_error{0.0}; + double sin_projection{0.0}; + double cos_projection{0.0}; + + void add(const double error, const double theta) + { + ++sample_count; + sum_error += error; + sum_error_sq += error * error; + max_abs_error = std::max(max_abs_error, std::fabs(error)); + sin_projection += error * std::sin(theta); + cos_projection += error * std::cos(theta); + } + + double mean() const + { + return sample_count > 0 ? sum_error / static_cast(sample_count) : 0.0; + } + + double rms() const + { + return sample_count > 0 + ? std::sqrt(sum_error_sq / static_cast(sample_count)) + : 0.0; + } + + double fundamentalAmplitude() const + { + if (sample_count == 0) { + return 0.0; + } + const double scale = 2.0 / static_cast(sample_count); + return scale * std::sqrt(sin_projection * sin_projection + + cos_projection * cos_projection); + } + + double phaseRad() const + { + return std::atan2(cos_projection, sin_projection); + } +}; + +double normalizePhaseRad(double phase) +{ + while (phase > kPi) { + phase -= 2.0 * kPi; + } + while (phase < -kPi) { + phase += 2.0 * kPi; + } + return phase; +} + +double radToDeg(const double rad) +{ + return rad * 180.0 / kPi; +} + +config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig() +{ + config::DeviceManagerConfig config; + config.set_name("eyou_motor_device_manager_real_test"); + config.set_version("test"); + config.set_init_all_motors_when_no_active_joints(true); + + auto* motor_entry = config.add_devices(); + motor_entry->set_id(kMotorManagerId); + motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); + motor_entry->set_config_file(kMotorConfigFile); + motor_entry->set_enable(true); + + return config; +} + +void printMotorState(const int motor_id, const std::shared_ptr& motor) +{ + ASSERT_NE(motor, nullptr); + std::cout << "motor_id=" << motor_id + << ", joint_name=" << motor->jointName() + << ", q=" << motor->getQ() << " rad" + << ", qd=" << motor->getQd() << " rad/s" + << std::endl; +} + +} // namespace + +TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState) +{ + ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); + DeviceManagerDestroyGuard guard; + + auto& device_manager = + DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig()); + auto motor_manager = device_manager.getDevice(kMotorManagerId); + ASSERT_NE(motor_manager, nullptr); + MotorManagerStopGuard motor_manager_stop_guard(motor_manager); + + for (const int motor_id : kFourMotorIds) { + auto motor = motor_manager->getMotor(static_cast(motor_id)); + ASSERT_NE(motor, nullptr); + printMotorState(motor_id, motor); + } +} + +TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory) +{ + ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); + DeviceManagerDestroyGuard guard; + + auto& device_manager = + DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig()); + auto motor_manager = device_manager.getDevice(kMotorManagerId); + ASSERT_NE(motor_manager, nullptr); + MotorManagerStopGuard motor_manager_stop_guard(motor_manager); + + std::vector> motors(kFourMotorIds.size()); + for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) { + const int motor_id = kFourMotorIds[i]; + motors[i] = motor_manager->getMotor(static_cast(motor_id)); + ASSERT_NE(motors[i], nullptr); + printMotorState(motor_id, motors[i]); + } + MultiMotorSafetyGuard safety_guard(motors); + + std::cout << "calibrate zero for four EtherCAT motors" << std::endl; + for (const auto& motor : motors) { + ASSERT_TRUE(motor->torqueOff()); + } + for (std::size_t i = 0; i < motors.size(); ++i) { + const int motor_id = kFourMotorIds[i]; + std::cout << "before calibrateZeroQ: "; + printMotorState(motor_id, motors[i]); + ASSERT_TRUE(motors[i]->calibrateZeroQ()); + std::cout << "after calibrateZeroQ: "; + printMotorState(motor_id, motors[i]); + } + + for (const auto& motor : motors) { + ASSERT_TRUE(motor->torqueOn()); + } + for (const auto& motor : motors) { + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)); + } + + std::vector center_q; + std::vector actual_qd; + ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd)); + + const double omega = 2.0 * kPi / kFourMotorPeriodS; + std::cout << "command four motors in CSP, duration=" + << kFourMotorTrajectoryDuration.count() + << " ms, command_period=" << kCyclicCommandPeriod.count() + << " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad + << " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad + << " rad, period=" << kFourMotorPeriodS + << " s" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto end_time = start_time + kFourMotorTrajectoryDuration; + auto next_command_time = start_time; + auto next_stats_time = start_time; + std::uint64_t missed_command_deadlines = 0; + std::array error_stats; + std::vector target_q(motors.size(), 0.0); + std::vector target_qd(motors.size(), 0.0); + std::vector actual_q; + std::array minimum_actual_q{}; + std::array maximum_actual_q{}; + std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin()); + std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin()); + double max_error_spread_rad = 0.0; + double sum_error_spread_sq = 0.0; + std::int64_t error_spread_sample_count = 0; + while (true) { + std::this_thread::sleep_until(next_command_time); + const auto now = std::chrono::steady_clock::now(); + if (now > end_time) { + break; + } + const double t_s = std::chrono::duration(now - start_time).count(); + + for (std::size_t i = 0; i < motors.size(); ++i) { + const double theta = omega * t_s + kFourMotorPhaseRad[i]; + target_q[i] = + center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta)); + target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta); + } + ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd)); + + if (now >= next_stats_time) { + ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd)); + double min_error = std::numeric_limits::max(); + double max_error = std::numeric_limits::lowest(); + for (std::size_t i = 0; i < motors.size(); ++i) { + const double theta = omega * t_s + kFourMotorPhaseRad[i]; + const double error = actual_q[i] - target_q[i]; + error_stats[i].add(error, theta); + minimum_actual_q[i] = std::min(minimum_actual_q[i], actual_q[i]); + maximum_actual_q[i] = std::max(maximum_actual_q[i], actual_q[i]); + min_error = std::min(min_error, error); + max_error = std::max(max_error, error); + } + const double error_spread = max_error - min_error; + max_error_spread_rad = std::max(max_error_spread_rad, error_spread); + sum_error_spread_sq += error_spread * error_spread; + ++error_spread_sample_count; + next_stats_time = now + kStatsSamplePeriod; + } + + next_command_time += kCyclicCommandPeriod; + const auto command_complete_time = std::chrono::steady_clock::now(); + if (next_command_time <= command_complete_time) { + const auto skipped_periods = + (command_complete_time - next_command_time) / kCyclicCommandPeriod + 1; + missed_command_deadlines += static_cast(skipped_periods); + next_command_time += skipped_periods * kCyclicCommandPeriod; + } + } + + std::copy(center_q.begin(), center_q.end(), target_q.begin()); + std::fill(target_qd.begin(), target_qd.end(), 0.0); + ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd)); + std::this_thread::sleep_for(kHoldAfterTrajectoryDuration); + ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd)); + + std::cout << "after four motor CSP trajectory" << std::endl; + for (std::size_t i = 0; i < motors.size(); ++i) { + printMotorState(kFourMotorIds[i], motors[i]); + } + + const double reference_phase = error_stats.front().phaseRad(); + const double rms_error_spread = + error_spread_sample_count > 0 + ? std::sqrt(sum_error_spread_sq / static_cast(error_spread_sample_count)) + : 0.0; + std::cout << "four motor CSP tracking error statistics, sample_period=" + << kStatsSamplePeriod.count() + << " ms, samples=" << error_stats.front().sample_count + << ", missed_command_deadlines=" << missed_command_deadlines + << ", max_error_spread=" << max_error_spread_rad + << " rad, rms_error_spread=" << rms_error_spread + << " rad" << std::endl; + for (std::size_t i = 0; i < motors.size(); ++i) { + const double phase = error_stats[i].phaseRad(); + const double relative_phase = normalizePhaseRad(phase - reference_phase); + const double relative_phase_ms = relative_phase / omega * 1000.0; + std::cout << " motor_id=" << kFourMotorIds[i] + << ", mean_error=" << error_stats[i].mean() + << " rad, rms_error=" << error_stats[i].rms() + << " rad, max_abs_error=" << error_stats[i].max_abs_error + << " rad, error_fundamental_amp=" + << error_stats[i].fundamentalAmplitude() + << " rad, error_phase=" << phase + << " rad (" << radToDeg(phase) + << " deg), relative_phase_to_motor1=" << relative_phase + << " rad (" << radToDeg(relative_phase) + << " deg, " << relative_phase_ms + << " ms)" << std::endl; + EXPECT_GE(maximum_actual_q[i] - minimum_actual_q[i], + kMinimumPositionExcursionRad) + << "motor_id=" << kFourMotorIds[i] << " did not complete enough motion"; + EXPECT_LE(error_stats[i].max_abs_error, + kMaximumAbsoluteTrackingErrorRad) + << "motor_id=" << kFourMotorIds[i] << " exceeded maximum tracking error"; + EXPECT_LE(error_stats[i].rms(), kMaximumRmsTrackingErrorRad) + << "motor_id=" << kFourMotorIds[i] << " exceeded RMS tracking error"; + EXPECT_NEAR(actual_q[i], center_q[i], kFinalPositionToleranceRad) + << "motor_id=" << kFourMotorIds[i] << " did not return to its start position"; + } + EXPECT_LE(max_error_spread_rad, kMaximumErrorSpreadRad); + EXPECT_TRUE(safety_guard.stop()); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp new file mode 100644 index 00000000..ecc9c007 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp @@ -0,0 +1,569 @@ +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +namespace cmvr::device { +namespace { + +constexpr int kMotorId = 1; +constexpr std::chrono::milliseconds kCommandSamplePeriod{100}; +constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; +constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000}; +constexpr double kDefaultGearRatio = 101.0; +constexpr double kEncoderCountsPerMotorRev = 65536.0; +constexpr double kPi = 3.14159265358979323846; + +config::MotorGroupConfig createSingleSlaveGroup() +{ + config::MotorGroupConfig group; + group.set_id("eyou_motor_real_test"); + group.set_bus_type(config::MOTOR_BUS_ETHERCAT); + group.set_vendor(config::MOTOR_VENDOR_EYOU); + group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402); + + auto* ethercat = group.mutable_ethercat(); + ethercat->set_master_index(0); + ethercat->set_cycle_us(1000); + ethercat->set_slave_op_timeout_ms(12000); + ethercat->set_slave_state_poll_period_ms(10); + + auto* cia402 = ethercat->mutable_cia402(); + cia402->set_state_transition_timeout_ms(1200); + cia402->set_velocity_stop_timeout_ms(2000); + cia402->set_status_poll_period_ms(10); + cia402->set_stopped_velocity_tolerance_rad_s(0.001); + + auto* zero_calibration = ethercat->mutable_zero_calibration(); + zero_calibration->set_timeout_ms(2000); + zero_calibration->set_poll_period_ms(10); + zero_calibration->set_stable_sample_count(5); + zero_calibration->set_position_tolerance_counts(10000); + zero_calibration->set_stable_delta_counts(1000); + + auto* dc = ethercat->mutable_dc(); + dc->set_enable(false); + dc->set_reference_motor_id(kMotorId); + dc->set_sync0_cycle_us(1000); + dc->set_sync0_shift_us(0); + dc->set_sync_reference_clock_period(1); + dc->set_assign_activate(768); + dc->set_sync_monitor_period_ms(1000); + + auto* slave = ethercat->add_slaves(); + slave->set_motor_id(kMotorId); + slave->set_alias(0); + slave->set_position(0); + + return group; +} + +class RuntimeStopGuard { +public: + explicit RuntimeStopGuard(std::shared_ptr runtime) + : runtime_(std::move(runtime)) + { + } + + ~RuntimeStopGuard() + { + if (runtime_) { + runtime_->stop(); + } + } + +private: + std::shared_ptr runtime_; +}; + +std::shared_ptr startRuntime() +{ + auto runtime = std::make_shared(); + runtime->setPdoMapping(createEyouCia402PdoMapping()); + if (!runtime->init(createSingleSlaveGroup())) { + return nullptr; + } + if (!runtime->start()) { + runtime->stop(); + return nullptr; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + return runtime; +} + +std::shared_ptr createProtocol( + const std::shared_ptr& runtime) +{ + return std::make_shared(runtime, runtime->config().cia402()); +} + +config::MotorConfigItem createMotorConfig() +{ + config::MotorConfigItem config; + config.set_id(kMotorId); + config.set_joint_name("ethercat_test_joint"); + config.set_limit_q_lb(-36.14); + config.set_limit_q_ub(36.14); + config.set_limit_qd(10.0); + config.set_limit_qdd(100.0); + config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev); + config.set_gear_ratio(kDefaultGearRatio); + return config; +} + +std::unique_ptr createMotor( + const std::shared_ptr& runtime) +{ + auto motor = std::make_unique( + createMotorConfig(), + createProtocol(runtime), + std::make_unique(runtime)); + if (!motor->init()) { + return nullptr; + } + return motor; +} + +void printMotorState(const char* label, AbstractMotor& motor) +{ + std::cout << label + << ": motor_q=" << motor.getQ() << " rad" + << ", motor_qd=" << motor.getQd() << " rad/s" + << std::endl; +} + +std::string hex16(const std::uint16_t value) +{ + std::ostringstream oss; + oss << "0x" << std::uppercase << std::hex << std::setw(4) << std::setfill('0') + << value; + return oss.str(); +} + +void printRawEthercatFeedback(const char* label, + const std::shared_ptr& runtime) +{ + std::uint16_t statusword = 0; + std::int8_t mode_display = 0; + std::int32_t actual_position = 0; + std::int32_t actual_velocity = 0; + std::int16_t actual_torque = 0; + std::uint16_t error_code = 0; + + runtime->readPdo(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword); + runtime->readPdo(kMotorId, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_POSITION_6064, 0x00, + actual_position); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, + actual_velocity); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, + actual_torque); + runtime->readPdo(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code); + + std::cout << label + << ": statusword=" << hex16(statusword) + << ", mode_display=" << static_cast(mode_display) + << ", actual_position=" << actual_position + << ", actual_velocity=" << actual_velocity + << ", actual_torque=" << actual_torque + << ", error_code=" << hex16(error_code) + << std::endl; +} + +void sampleMotorState(AbstractMotor& motor, + const std::chrono::milliseconds duration) +{ + for (auto elapsed = std::chrono::milliseconds{0}; + elapsed < duration; + elapsed += kCommandSamplePeriod) { + std::this_thread::sleep_for(kCommandSamplePeriod); + std::cout << "t=" << (elapsed + kCommandSamplePeriod).count() << " ms"; + printMotorState("", motor); + } +} + +double nearbySafeTarget(const double current_q, const double delta_rad) +{ + return current_q + (current_q > 0.0 ? -std::abs(delta_rad) : std::abs(delta_rad)); +} + +} // namespace + +TEST(EyouMotorRealTest, ReadMotorStateOnly) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + printMotorState("motor state", *motor); + printRawEthercatFeedback("raw feedback", runtime); + for (int i = 1; i <= 10; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + printMotorState("motor state", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } +} + +TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + printMotorState("before calibrateZeroQ", *motor); + ASSERT_TRUE(motor->calibrateZeroQ()); + printMotorState("after calibrateZeroQ", *motor); + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION)); + ASSERT_TRUE(motor->commandProfilePosition(1.5,0.8,3.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); +} + +TEST(EyouMotorRealTest, CommandProfilePosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION)); + + ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); +} + +TEST(EyouMotorRealTest, CommandProfileVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + + + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY)); + + std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); + + std::cout << "motor.commandProfileVelocity(0 rad/s, 1.0 rad/s^2)" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(0.0, 1.0)); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); +} + +TEST(EyouMotorRealTest, CommandCyclicPosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)); + + const std::chrono::milliseconds trajectory_duration{12000}; + const double period_s = 6.0; + const double excursion_rad = 3.0; + const double center_q = motor->getQ(); + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q + << " rad, period=" << period_s + << " s, excursion=" << excursion_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = trajectory_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s; + const double target_q = + center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta)); + const double target_qd = + 0.5 * excursion_rad * omega * std::sin(theta); + + ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_q=" << target_q + << " rad, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } +} + +TEST(EyouMotorRealTest, CommandCyclicVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)); + + const std::chrono::milliseconds trajectory_duration{12000}; + const double period_s = 5.0; + const double excursion_rad = 5.0; + const double phase_rad = 0.0; + const double omega = 2.0 * kPi / period_s; + const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega; + + std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s + << " s, velocity_amplitude=" << velocity_amplitude_rad_s + << " rad/s, excursion=" << excursion_rad + << " rad, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = trajectory_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_qd = velocity_amplitude_rad_s * std::sin(theta); + + ASSERT_TRUE(motor->commandCyclicVelocity(target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl; + ASSERT_TRUE(motor->commandCyclicVelocity(0.0)); + sampleMotorState(*motor, std::chrono::milliseconds{500}); + printRawEthercatFeedback("raw feedback after stop", runtime); +} + +TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)); + + const std::chrono::milliseconds run_duration{2000}; + const double period_s = 6.0; + const double velocity_amplitude_rad_s = 4.5; + const double phase_rad = 0.0; + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicVelocity(sin), then quickStop at " + << run_duration.count() + << " ms, period=" << period_s + << " s, velocity_amplitude=" << velocity_amplitude_rad_s + << " rad/s, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = run_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_qd = velocity_amplitude_rad_s * std::sin(theta); + + ASSERT_TRUE(motor->commandCyclicVelocity(target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInProfilePosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION)); + + const std::chrono::milliseconds quick_stop_time{1000}; + const double start_q = motor->getQ(); + const double target_q = nearbySafeTarget(start_q, 4.0); + const double max_qd = 2.0; + const double max_qdd = 10.0; + + std::cout << "motor.commandProfilePosition(" << target_q + << " rad, " << max_qd + << " rad/s, " << max_qdd + << " rad/s^2), then quickStop at " + << quick_stop_time.count() << " ms" << std::endl; + ASSERT_TRUE(motor->commandProfilePosition(target_q, max_qd, max_qdd)); + std::cout << "wait " << quick_stop_time.count() + << " ms before quickStop" << std::endl; + sampleMotorState(*motor, quick_stop_time); + printRawEthercatFeedback("raw feedback before quickStop", runtime); + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInProfileVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY)); + + const std::chrono::milliseconds quick_stop_time{2000}; + const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0; + const double max_qdd = 10.0; + + std::cout << "motor.commandProfileVelocity(" << target_qd + << " rad/s, " << max_qdd + << " rad/s^2), then quickStop at " + << quick_stop_time.count() << " ms" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(target_qd, max_qdd)); + std::cout << "wait " << quick_stop_time.count() + << " ms before quickStop" << std::endl; + sampleMotorState(*motor, quick_stop_time); + printRawEthercatFeedback("raw feedback before quickStop", runtime); + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInCyclicPosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)); + + const std::chrono::milliseconds run_duration{2000}; + const double start_q = motor->getQ(); + const double target_qd = start_q > 0.0 ? -2.0 : 2.0; + + std::cout << "motor.commandCyclicPosition(linear), start_q=" << start_q + << " rad, target_qd=" << target_qd + << " rad/s, then quickStop at " + << run_duration.count() + << " ms, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = run_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double target_q = start_q + target_qd * t_s; + + ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_q=" << target_q + << " rad, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h index b0293806..57c97387 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h +++ b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h @@ -21,29 +21,37 @@ public: std::string typeName() const override { return "MujocoMotor"; } bool init() override; - void setMode(msgs::RunMode mode) override; + bool setMode(msgs::RunMode mode) override; msgs::RunMode getMode() override; - void torqueOff() override; + bool torqueOn() override; + bool torqueOff() override; + bool brakeRelease() override; + bool quickStop() override; void setLimitQ(double ub, double lb) override; void setLimitQd(double qd) override; void setLimitQdd(double u_qdd, double l_qdd) override; - void brake() override; - void setQ(double q) override; - void setTarget(double q, double qd) override; - void setTarget(double qd) override; bool calibrateZeroQ() override; bool reachedTargetQ() override; - void setQd(double qd) override; + bool commandProfilePosition(double target_q, + double max_qd = 0.0, + double max_qdd = 0.0) override; + bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) override; + bool commandCyclicPosition(double target_q, + double target_qd = 0.0) override; + bool commandCyclicVelocity(double target_qd) override; + bool commandCyclicTorque(double target_tau) override; double getQ() override; double getQd() override; - static bool setTargetsAtomic(const std::vector>& motors, - const std::vector& positions, - const std::vector& velocities); + static bool commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities); private: + bool holdPosition_(); double clampQ_(double q) const; double clampQd_(double qd) const; std::shared_ptr worldLocked_() const; diff --git a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp index 160c1005..447e8d18 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp +++ b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp @@ -42,10 +42,11 @@ bool MujocoMotor::init() return true; } -void MujocoMotor::setMode(const msgs::RunMode mode) +bool MujocoMotor::setMode(const msgs::RunMode mode) { std::scoped_lock lock(mtx_); mode_ = mode; + return true; } msgs::RunMode MujocoMotor::getMode() @@ -54,11 +55,27 @@ msgs::RunMode MujocoMotor::getMode() return mode_; } -void MujocoMotor::torqueOff() +bool MujocoMotor::torqueOn() { - brake(); + return holdPosition_(); +} + +bool MujocoMotor::torqueOff() +{ + const bool ok = holdPosition_(); std::scoped_lock lock(mtx_); mode_ = msgs::RUN_MODE_UNSPECIFIED; + return ok; +} + +bool MujocoMotor::brakeRelease() +{ + return true; +} + +bool MujocoMotor::quickStop() +{ + return holdPosition_(); } void MujocoMotor::setLimitQ(const double ub, const double lb) @@ -82,38 +99,78 @@ void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd) info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_)); } -void MujocoMotor::brake() +bool MujocoMotor::holdPosition_() { const auto world = worldLocked_(); double q = 0.0; if (!world || !world->getJointPosition(info_.joint_name, q)) { - return; + return false; } std::scoped_lock lock(mtx_); target_q_ = q; mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; world->setJointTargetState(info_.joint_name, q, 0.0); + return true; } -void MujocoMotor::setQ(const double q) +bool MujocoMotor::commandProfilePosition(const double target_q, + const double max_qd, + const double max_qdd) { - setTarget(q, 0.0); + (void)max_qdd; + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_PROFILE_POSITION; + target_q_ = clampQ_(target_q); + const double profile_qd = max_qd > 0.0 ? max_qd : info_.limit_qd; + return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(profile_qd)); } -void MujocoMotor::setTarget(const double q, const double qd) +bool MujocoMotor::commandProfileVelocity(const double target_qd, const double max_qdd) +{ + (void)max_qdd; + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_PROFILE_VELOCITY; + return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd)); +} + +bool MujocoMotor::commandCyclicPosition(const double target_q, + const double target_qd) { std::scoped_lock lock(mtx_); const auto world = worldLocked_(); if (!world) { - return; + return false; } - target_q_ = clampQ_(q); - world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd)); + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + target_q_ = clampQ_(target_q); + return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(target_qd)); } -void MujocoMotor::setTarget(const double qd) +bool MujocoMotor::commandCyclicVelocity(const double target_qd) { - setQd(qd); + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd)); +} + +bool MujocoMotor::commandCyclicTorque(const double target_tau) +{ + (void)target_tau; + CMVR_LOG(ERROR) << "[MujocoMotor] cyclic torque command is not implemented: " + << info_.joint_name; + return false; } bool MujocoMotor::calibrateZeroQ() @@ -140,16 +197,6 @@ bool MujocoMotor::reachedTargetQ() } } -void MujocoMotor::setQd(const double qd) -{ - std::scoped_lock lock(mtx_); - const auto world = worldLocked_(); - if (!world) { - return; - } - world->setJointTargetVelocity(info_.joint_name, clampQd_(qd)); -} - double MujocoMotor::getQ() { const auto world = worldLocked_(); @@ -170,9 +217,10 @@ double MujocoMotor::getQd() return qd; } -bool MujocoMotor::setTargetsAtomic(const std::vector>& motors, - const std::vector& positions, - const std::vector& velocities) +bool MujocoMotor::commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) { if (motors.size() != positions.size() || motors.size() != velocities.size() || motors.empty()) { return false; @@ -187,7 +235,7 @@ bool MujocoMotor::setTargetsAtomic(const std::vector(motors[i]); if (!motor) { return false; } @@ -217,9 +265,13 @@ bool MujocoMotor::setTargetsAtomic(const std::vectormtx_); - motors[i]->target_q_ = clamped_positions[i]; - motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + const auto motor = std::dynamic_pointer_cast(motors[i]); + if (!motor) { + return false; + } + std::scoped_lock lock(motor->mtx_); + motor->target_q_ = clamped_positions[i]; + motor->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; } return true; } diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h index 64db8ea2..d1fc84ae 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h @@ -22,6 +22,8 @@ namespace cmvr { info_.limit_q_ub = config.limit_q_ub(); info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5; info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0; + encoder_counts_per_rev_ = config.encoder_counts_per_rev(); + gear_ratio_ = config.gear_ratio(); node_id_ = info_.id; } @@ -37,6 +39,19 @@ namespace cmvr { } if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) { auto canopen_protocol = std::dynamic_pointer_cast(protocol_); + if (!canopen_protocol) { + CMVR_LOG(ERROR) << "[Ti5Motor] invalid CANopen protocol for motor: " + << info_.joint_name; + return false; + } + if (encoder_counts_per_rev_ <= 0.0 || gear_ratio_ <= 0.0) { + CMVR_LOG(ERROR) << "[Ti5Motor] missing encoder conversion config: " + << info_.joint_name + << ", encoder_counts_per_rev=" << encoder_counts_per_rev_ + << ", gear_ratio=" << gear_ratio_; + return false; + } + protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); // torqueOff(node_id_); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION); // canopen_protocol->torqueOff(node_id_); @@ -45,8 +60,8 @@ namespace cmvr { canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE); canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); - // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); canopen_protocol->setLimitQd(node_id_, info_.limit_qd); canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd); @@ -55,6 +70,10 @@ namespace cmvr { } return true; } + + private: + double encoder_counts_per_rev_{0.0}; + double gear_ratio_{0.0}; }; diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h index e837cd89..3b64a284 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h @@ -18,6 +18,7 @@ #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h" #include +#include namespace cmvr { namespace device { @@ -29,28 +30,40 @@ namespace cmvr { bool initNode(uint8_t node_id) override; - void setMode(uint8_t node_id, msgs::RunMode mode); - void setTarget(uint8_t node_id, double angle_rad, double vel) override; - void setTarget(uint8_t node_id, double vel) override; - void setQ(uint8_t node_id, double angle_rad) override; + bool setMode(uint8_t node_id, msgs::RunMode mode) override; void setLimitQ(uint8_t node_id, double ub, double lb) override; void setLimitQd(uint8_t node_id, double qd) override; void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override; bool calibrateZeroQ(uint8_t node_id) override; - void brake(uint8_t node_id) override; + bool torqueOn(uint8_t node_id) override; + bool torqueOff(uint8_t node_id) override; + bool brakeRelease(uint8_t node_id) override; + bool quickStop(uint8_t node_id) override; bool reachedTargetQ(uint8_t node_id) override; double getQ(uint8_t node_id) override; double getQd(uint8_t node_id) override; - void setQd(uint8_t node_id, double qd) override; - void setQdd(uint8_t node_id, double qdd) override; - - void torqueOff(uint8_t node_id) override; + bool commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) override; + bool commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) override; + bool commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) override; + bool commandCyclicVelocity(uint8_t node_id, + double target_qd) override; + bool commandCyclicTorque(uint8_t node_id, double target_tau) override; + void setMotorConversion(uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) override; void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10); - void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, - msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10); + void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, uint32_t index, + uint32_t sub_index, uint32_t data, uint32_t delay_ms = 10); void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel); void configPdo(uint8_t node_id); @@ -70,14 +83,20 @@ namespace cmvr { private: - static constexpr double GearRatio = 101.0; // 电机减速比 static constexpr double RADTODEG = 180.0 / M_PI; + static constexpr double Ti5VelocityUnitScale = 100.0; + static constexpr double Ti5AccelerationTimeScale = 1000.0; + + struct MotorConversion { + double encoder_counts_per_rev{0.0}; + double gear_ratio{0.0}; + }; + std::shared_ptr can_client_{nullptr}; // key node_id // std::unordered_map cur_mode_{}; - std::unordered_map last_Qd_{}; - std::unordered_map last_Qdd_{}; + std::unordered_map motor_conversions_{}; std::shared_ptr > can_sender_{nullptr}; std::shared_ptr > message_manager_{nullptr}; @@ -94,11 +113,7 @@ namespace cmvr { std::map rpdo1_commands_{}; std::map rpdo2_commands_{}; - void setPPTargetPosBySdo(uint8_t node_id, int32_t pos); - - void setPPTargetPosByPdo(uint8_t node_id, int32_t pos); - - void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos); + void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos); void configTPDO1(uint8_t node_id); @@ -107,6 +122,14 @@ namespace cmvr { void configRPDO1(uint8_t node_id, bool enable); void configRPDO2(uint8_t node_id, bool enable); + const MotorConversion* conversionForNode(uint8_t node_id) const; + double radToCounts(double angle_rad, const MotorConversion& conversion) const; + double countsToRad(int32_t counts, const MotorConversion& conversion) const; + double radPerSecToVelocityRaw(double velocity_rad_s, const MotorConversion& conversion) const; + uint32_t radPerSec2ToAccelerationRaw(double acceleration_rad_s2, + const MotorConversion& conversion) const; + double velocityRawToRadPerSec(int32_t velocity_raw, const MotorConversion& conversion) const; + bool waitUntil(std::function condition, int timeout_ms) { auto start = std::chrono::steady_clock::now(); diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp index b5486dec..4410332d 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp @@ -3,6 +3,7 @@ // Created by lgv on 2025/7/24. // +#include "cmvr/msgs/cia402.pb.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h" using namespace cmvr::device::motor; @@ -19,17 +20,17 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, switch (sdo_response.index()) { - case msgs::CONTROL_WORD_6040: + case msgs::CIA402_CONTROL_WORD_6040: motor_status->set_ctrl_word(sdo_response.data()); break; - case msgs::STATUS_WORD_6041: + case msgs::CIA402_STATUS_WORD_6041: motor_status->set_status_word(sdo_response.data()); break; - case msgs::ACTUAL_POSITION_6064: + case msgs::CIA402_ACTUAL_POSITION_6064: motor_status->set_position(static_cast(sdo_response.data())); CMVR_LOG(INFO) << "pos = " << motor_status->position(); break; - case msgs::POSITION_OFFSET_2008: + case msgs::CANOPEN_POSITION_OFFSET_2008: motor_status->set_position_offset(sdo_response.data()); } // diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp index e4c98b36..d1f67403 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp @@ -19,16 +19,4 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]); motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]); - - - double gearRatio = 101.0; - double radToDeg = 180.0 / M_PI; - auto speed = (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0); - - auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg); - - // CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s"; - - - } diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp index c5546b2a..14937d69 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp @@ -3,6 +3,7 @@ // Created by lgv on 2025/8/1. // +#include "cmvr/msgs/cia402.pb.h" #include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" #include "canbus/canopen/register.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h" @@ -94,45 +95,153 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { return ErrorCode::OK; } +void Ti5MotorCanopenProtocol::setMotorConversion( + const uint8_t node_id, + const double encoder_counts_per_rev, + const double gear_ratio) { + motor_conversions_[node_id] = {encoder_counts_per_rev, gear_ratio}; +} -void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index, +const Ti5MotorCanopenProtocol::MotorConversion* +Ti5MotorCanopenProtocol::conversionForNode(const uint8_t node_id) const { + const auto it = motor_conversions_.find(node_id); + if (it != motor_conversions_.end() && + it->second.encoder_counts_per_rev > 0.0 && + it->second.gear_ratio > 0.0) { + return &it->second; + } + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] missing conversion config for node " + << static_cast(node_id); + return nullptr; +} + +double Ti5MotorCanopenProtocol::radToCounts( + const double angle_rad, + const MotorConversion& conversion) const { + return (angle_rad * RADTODEG) / 360.0 * + conversion.gear_ratio * conversion.encoder_counts_per_rev; +} + +double Ti5MotorCanopenProtocol::countsToRad( + const int32_t counts, + const MotorConversion& conversion) const { + return (counts * 360.0) / + (conversion.gear_ratio * conversion.encoder_counts_per_rev * RADTODEG); +} + +double Ti5MotorCanopenProtocol::radPerSecToVelocityRaw( + const double velocity_rad_s, + const MotorConversion& conversion) const { + return ((velocity_rad_s * RADTODEG) * conversion.gear_ratio * Ti5VelocityUnitScale) / + 360.0; +} + +uint32_t Ti5MotorCanopenProtocol::radPerSec2ToAccelerationRaw( + const double acceleration_rad_s2, + const MotorConversion& conversion) const { + const auto raw = ((std::abs(acceleration_rad_s2) * RADTODEG) * + conversion.gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + return static_cast(std::abs(raw)); +} + +double Ti5MotorCanopenProtocol::velocityRawToRadPerSec( + const int32_t velocity_raw, + const MotorConversion& conversion) const { + return (velocity_raw * 360.0) / + (conversion.gear_ratio * Ti5VelocityUnitScale * RADTODEG); +} + + +void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data, uint32_t delay_ms) { sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data); can_sender_->Update(sdo_commands_[node_id]->ID()); std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); } -void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) { - auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - - switch (getMode(node_id)) { - // case RUN_MODE_CYCLIC_SYNC_POSITION: - // setCSPTargetPosByPdo(node_id, static_cast(cmd)); - // break; - case RUN_MODE_PROFILE_POSITION: - // setPPTargetPosByPdo(node_id, static_cast(cmd)); - setPPTargetPosBySdo(node_id, static_cast(cmd)); - break; +bool Ti5MotorCanopenProtocol::commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; } + if (max_qd > 0.0) { + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, + SUB_INDEX_0, + static_cast(std::abs(radPerSecToVelocityRaw(max_qd, *conversion))), + 0); + } + if (max_qdd > 0.0) { + const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, + SUB_INDEX_0, accel, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, + SUB_INDEX_0, accel, 0); + } + writeProfilePositionTargetBySdo(node_id, static_cast(radToCounts(target_q, *conversion))); + return true; } -void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) { - auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; +bool Ti5MotorCanopenProtocol::commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + if (max_qdd > 0.0) { + const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, + SUB_INDEX_0, accel, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, + SUB_INDEX_0, accel, 0); + } + const auto speed = radPerSecToVelocityRaw(target_qd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_VELOCITY_60FF, + SUB_INDEX_0, + static_cast(static_cast(std::llround(speed))), + 0); + return true; +} + +bool Ti5MotorCanopenProtocol::commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + auto pos_cmd = radToCounts(target_q, *conversion); + auto speed = radPerSecToVelocityRaw(target_qd, *conversion); rpdo1_commands_[node_id]->SetTargetPos(pos_cmd); rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed))); can_sender_->Update(rpdo1_commands_[node_id]->ID()); + return true; } - -void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) { - auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; +bool Ti5MotorCanopenProtocol::commandCyclicVelocity(uint8_t node_id, + double target_qd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + auto speed = radPerSecToVelocityRaw(target_qd, *conversion); rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed)); can_sender_->Update(rpdo2_commands_[node_id]->ID()); + return true; } +bool Ti5MotorCanopenProtocol::commandCyclicTorque(uint8_t node_id, double target_tau) { + (void)target_tau; + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] cyclic torque command is not implemented, node=" + << static_cast(node_id); + return false; +} -void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) { +void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos) { controlword_t cw = {}; cw.switch_on = 1; cw.enable_voltage = 1; @@ -141,45 +250,20 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) cw.change_set_immediately = 1; // 1. 设置目标位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, pos); // 2. 设置触发位（bit4 = 1） cw.new_set_point = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); // 3. 清除触发位（bit4 = 0），准备下一次触发 cw.new_set_point = 0; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); } -void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) { - // 触发目标位置运动 - controlword_t cw; - cw.value = 0x0F; - cw.new_set_point = 1; - cw.change_set_immediately = 1; - - rpdo1_commands_[node_id]->SetTargetPos(pos); - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - cw.new_set_point = 0; - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); -} - -void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) { - rpdo1_commands_[node_id]->SetTargetPos(pos); - rpdo1_commands_[node_id]->SetCtrlWord(0x0F); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); -} - - -void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { +bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // cur_mode_[node_id] = mode; @@ -188,36 +272,36 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { controlword_t cw = {}; cw.quick_stop = 1; cw.enable_voltage = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); // configRPDO1(node_id, false); // configRPDO2(node_id, false); // 1 : 先设置模式 auto data = static_cast(mode); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CIA402_OPERATION_MODE_6060, SUB_INDEX_0, data); // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F） cw.switch_on = 1; cw.enable_operation = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); switch (mode) { case RUN_MODE_PROFILE_POSITION: { // 4 : 设置目标位置（为当前位置） auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); // 5 : 触发位置运动（new_set_point 翻转） cw.new_set_point = 1; cw.change_set_immediately = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标） cw.new_set_point = 0; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } @@ -225,19 +309,19 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // configRPDO1(node_id, true); // 设置目标位置为当前位置 auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); //3 : 使能 15 cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } case RUN_MODE_PROFILE_VELOCITY: { cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } @@ -245,13 +329,21 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // configRPDO2(node_id, true); cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } default: // TODO: Handle unspecified or unknown mode - break; + return false; } + + if (!waitUntil([&]() { return getMode(node_id) == mode; }, 500)) { + CMVR_LOG(ERROR) << "motor " << static_cast(node_id) + << ": operation mode switch failed, target_mode=" + << static_cast(mode); + return false; + } + return true; } void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) { @@ -262,9 +354,9 @@ void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand c void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, SUB_INDEX_0, speed); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); } @@ -272,128 +364,128 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) { //TDPO1 配置 状态字 和 控制字 // 1: 失能 pdo uint32_t cob_id = TPDO1_BASE_ID_180 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 0); // 2: 配置为异步 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 3：配置约束时间 unit:0.1ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_3, 10); // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_5, 0); // 5 ：映射控制字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1, - CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_1, + CIA402_CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); //6 : 映射状态字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2, - STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_2, + CIA402_STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); //7 : 映射模式 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3, - MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_3, + CIA402_MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); //8 映射错误码 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4, - ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_4, + CIA402_ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); //9 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 4); //10 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31)); } void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) { // 1: 失能 pdo uint32_t cob_id = TPDO2_BASE_ID_280 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 0); // 2: 配置为异步 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 3：配置约束时间 unit:0.1ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_3, 100); // 4 : 配置周期发送时间 unit : ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_5, 0); // 5 ：映射当前位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1, - ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_1, + CIA402_ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); //6 : 映射当前速度 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2, - ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_2, + CIA402_ACTUAL_VELOCITY_606C << 16 | SUB_INDEX_0 << 8 | 32); //9 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 2); //10 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); } void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool enable) { // 1: 失能 pdo uint32_t cob_id = RPDO1_BASE_ID_200 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 0); // 2: 配置为 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // // 3：配置约束时间 unit:0.1ms - // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10); + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_3,10); // // // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0); + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_5,0); // 5 ：映射位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1, - TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_1, + CIA402_TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); //6 : 映射控制字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2, - PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_2, + CIA402_PROFILE_VELOCITY_6081 << 16 | SUB_INDEX_0 << 8 | 32); //7 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 2); //8 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); } void Ti5MotorCanopenProtocol::configRPDO2(uint8_t node_id, bool enable) { // 1: 失能 pdo uint32_t cob_id = RPDO2_BASE_ID_300 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 0); if (!enable) return; // 2: 配置为 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 5 ：映射位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_MAP_1601, SUB_INDEX_1, - TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_1, + CIA402_TARGET_VELOCITY_60FF << 16 | SUB_INDEX_0 << 8 | 32); //7 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 1); //8 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31)); } @@ -405,36 +497,49 @@ void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) { } void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) { - auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; - auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel)); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + auto accel = ((u_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + auto decel = ((l_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); } void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) { - auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; - // seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + auto speed = radPerSecToVelocityRaw(qd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_MAX_PROFILE_VELOCITY_607F, SUB_INDEX_0, speed); } void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) { - ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0; - lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0; + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + ub = radToCounts(ub, *conversion); + lb = radToCounts(lb, *conversion); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub); } bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { // 0: 设置控制字为 0x06，确保停机状态 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000); // 1: 清除偏置值 0x2008 ← 0 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0); // 2: 等待确认清除成功 - seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0); if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == 0; }, 1000)) { @@ -443,18 +548,18 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { } // 3: 读取当前位置 0x6064 - seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); + seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); // 4: 将当前位置写入偏置寄存器 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); // 5: 保存参数到永久区（0x2000 ← 1） - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100); // 6: 确认写入成功 - seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); + seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos; }, 500)) { @@ -465,16 +570,27 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { return true; } -void Ti5MotorCanopenProtocol::brake(uint8_t node_id) { +bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) { + return setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); +} + +bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) { + (void)node_id; + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented"; + return false; +} + +bool Ti5MotorCanopenProtocol::quickStop(uint8_t node_id) { // // 开机未使能电机时调用 - // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); // 6 抱闸 0 ： 立即停机 自由 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0); - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6); - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_QUICK_STOP_DECELERATION_6085, SUB_INDEX_0, 0XFFFFFFF0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100); // 必须要发送 0xf 才能按照6085中设定的减速度减速 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + return true; } bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { @@ -483,62 +599,37 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { return st.target_reached == 1; } -void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) { - auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; - switch (getMode(node_id)) { - case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: - case msgs::RUN_MODE_PROFILE_POSITION: { - auto it = last_Qd_.find(node_id); - if (it == last_Qd_.end() || it->second != speed) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)), - 0); - last_Qd_[node_id] = speed; - } - break; - } - case msgs::RUN_MODE_PROFILE_VELOCITY: - case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: { - // 在速度模式下，直接设置目标速度 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0); - break; - } - default: - break; - } -} - -void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) { - uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0); - auto it = last_Qdd_.find(node_id); - if (it == last_Qdd_.end() || it->second != accel) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel); - last_Qdd_[node_id] = accel; - } -} - -void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) { +bool Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) { // 0 ： 立即停机 自由 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0); - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20); // 必须要发送 0xf 才能按照6085中设定的减速度减速 - // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); // 停机之后，要重新使能？ // cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED; + return true; } double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } auto data_ptr = std::make_unique(); message_manager_->GetSensorData(data_ptr.get()); auto cnt = data_ptr->motors().at(node_id).position(); - return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); + return countsToRad(cnt, *conversion); } double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } auto data_ptr = std::make_unique(); message_manager_->GetSensorData(data_ptr.get()); auto cnt = data_ptr->motors().at(node_id).speed(); - return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG); + return velocityRawToRadPerSec(cnt, *conversion); } diff --git a/cmvr-es/devices/motor/manager/CMakeLists.txt b/cmvr-es/devices/motor/manager/CMakeLists.txt index 8eea8209..8ea5f495 100644 --- a/cmvr-es/devices/motor/manager/CMakeLists.txt +++ b/cmvr-es/devices/motor/manager/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(motor_manager PRIVATE cmvr_es::device::ti5_canopen_motor_driver cmvr_es::device::mujoco_motor_driver + cmvr_es::device::ethercat_motor_driver cmvr_es::ik_solver glog ) diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index 3bbd2cab..3673355a 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -45,6 +45,14 @@ public: std::shared_ptr getMotor(std::uint8_t node_id) const; std::shared_ptr getMotor(const std::string& joint_name) const; const std::unordered_map>& motorsMap() const; + bool commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) const; + bool readFeedbacksAtomic( + const std::vector>& motors, + std::vector& positions, + std::vector& velocities) const; static std::shared_ptr managerFor(const std::string& id); static std::shared_ptr mujocoWorldFor(const std::string& id); diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index d1e6affe..3a6b5118 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -1,21 +1,27 @@ -#include "motor/manager/include/motor_manager.h" +#include "devices/motor/manager/include/motor_manager.h" +#include #include #include #include +#include #include #include #include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" #include "common/base/logging/logger.h" #include "common/config/config_files.h" -#include "../../bus_runtime/abstract_motor_bus_runtime.h" -#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h" -#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" -#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" -#include "motor/drivers/mujoco/include/mujoco_motor.h" -#include "motor/drivers/ti5_canopen/include/ti5_motor.h" -#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" +#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" +#include "devices/motor/drivers/mujoco/include/mujoco_motor.h" +#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h" +#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" namespace cmvr::device { @@ -91,11 +97,17 @@ bool MotorManager::init() all_ok = false; continue; } - if (!bus_runtime->start()) { + + const bool start_before_motor_init = + motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT; + if (start_before_motor_init && !bus_runtime->start()) { bus_runtime->stop(); all_ok = false; continue; } + if (start_before_motor_init) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime); if (motors.empty()) { @@ -116,6 +128,11 @@ bool MotorManager::init() all_ok = false; continue; } + if (!start_before_motor_init && !bus_runtime->start()) { + bus_runtime->stop(); + all_ok = false; + continue; + } bus_runtimes_.push_back(std::move(bus_runtime)); } @@ -222,6 +239,46 @@ const std::unordered_map>& MotorMana return motors_by_joint_; } +bool MotorManager::commandCyclicPositionsAtomic( + const std::vector>& motors, + const std::vector& positions, + const std::vector& velocities) const +{ + if (motors.empty() || motors.size() != positions.size() || + motors.size() != velocities.size()) { + return false; + } + + if (std::dynamic_pointer_cast(motors.front())) { + return EyouMotor::commandCyclicPositionsAtomic(motors, positions, velocities); + } + if (std::dynamic_pointer_cast(motors.front())) { + return MujocoMotor::commandCyclicPositionsAtomic(motors, positions, velocities); + } + + CMVR_LOG(ERROR) << "[MotorManager] atomic cyclic position is unsupported for motor type: " + << motors.front()->typeName(); + return false; +} + +bool MotorManager::readFeedbacksAtomic( + const std::vector>& motors, + std::vector& positions, + std::vector& velocities) const +{ + if (motors.empty()) { + return false; + } + + if (std::dynamic_pointer_cast(motors.front())) { + return EyouMotor::readFeedbacksAtomic(motors, positions, velocities); + } + + CMVR_LOG(ERROR) << "[MotorManager] atomic feedback is unsupported for motor type: " + << motors.front()->typeName(); + return false; +} + std::shared_ptr MotorManager::managerFor(const std::string& id) { std::lock_guard lock(registry_mutex_); @@ -398,8 +455,19 @@ std::shared_ptr MotorManager::createBusRuntime_( return std::make_shared(); case config::MOTOR_BUS_MUJOCO: return std::make_shared(); - case config::MOTOR_BUS_ETHERCAT: - return std::make_shared(); + case config::MOTOR_BUS_ETHERCAT: { + auto runtime = std::make_shared(); + if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU && + group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { + runtime->setPdoMapping(createEyouCia402PdoMapping()); + return runtime; + } + CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return nullptr; + } default: CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " << config::MotorBusType_Name(group_cfg.bus_type()) @@ -534,6 +602,14 @@ std::vector> MotorManager::createEthercatMotors_( CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << group_cfg.id(); return {}; } + if (group_cfg.vendor() != config::MOTOR_VENDOR_EYOU || + group_cfg.protocol() != config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { + CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return {}; + } for (const auto& motor_cfg : motor_cfgs) { if (!ethercat_bus_runtime->slaveForMotor(motor_cfg.id())) { CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT slave config for motor id " @@ -542,11 +618,22 @@ std::vector> MotorManager::createEthercatMotors_( } } - CMVR_LOG(ERROR) << "[MotorManager] EtherCAT motor creation is not implemented: vendor=" - << config::MotorVendor_Name(group_cfg.vendor()) - << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) - << ", group=" << group_cfg.id(); - return {}; + auto protocol = std::make_shared( + ethercat_bus_runtime, group_cfg.ethercat().cia402()); + + std::vector> motors; + motors.reserve(motor_cfgs.size()); + for (const auto& cfg : motor_cfgs) { + auto motor = std::make_shared( + cfg, protocol, std::make_unique(ethercat_bus_runtime)); + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorManager] failed to init EYOU EtherCAT motor: " + << cfg.joint_name(); + return {}; + } + motors.push_back(std::move(motor)); + } + return motors; } } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 634a715c..6afbcb64 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -15,7 +15,8 @@ namespace cmvr { public: enum class CommProto : uint8_t { CANOPEN = 1, - CUSTOM = 2 + ETHERCAT = 2, + CUSTOM = 3 }; virtual ~MotorProtocolInterface() = default; @@ -26,22 +27,42 @@ namespace cmvr { */ virtual bool initNode(uint8_t node_id) = 0; - virtual void setQ(uint8_t node_id, double angle_rad) = 0; - virtual void setTarget(uint8_t node_id, double angle_rad,double vel) = 0; - virtual void setTarget(uint8_t node_id,double vel) = 0; - virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0; + virtual bool setMode(uint8_t node_id,msgs::RunMode mode ) = 0; virtual msgs::RunMode getMode(uint8_t node_id) = 0; virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; virtual void setLimitQd(uint8_t node_id,double qd) = 0; virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0; virtual bool calibrateZeroQ(uint8_t node_id) = 0; virtual bool reachedTargetQ(uint8_t node_id) = 0; - virtual void setQd(uint8_t node_id, double qd) = 0; - virtual void setQdd(uint8_t node_id,double qdd) = 0; - // virtual void setVelocity(uint8_t node_id, double velocity) = 0; - // virtual void clearError(uint8_t node_id) = 0; - virtual void brake(uint8_t node_id) = 0; - virtual void torqueOff(uint8_t node_id) = 0; + // target_q: rad, max_qd: rad/s, max_qdd: rad/s^2. + // Profile Position 写入目标位置和轮廓速度/加速度，并触发一次新目标。 + virtual bool commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) = 0; + // target_qd: rad/s, max_qdd: rad/s^2. + // Profile Velocity 写入目标速度和轮廓加速度。 + virtual bool commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) = 0; + // target_q: rad, target_qd: rad/s. + // Cyclic Position 周期写入目标位置和目标速度。 + virtual bool commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) = 0; + // target_qd: rad/s. + // Cyclic Velocity 周期写入目标速度。 + virtual bool commandCyclicVelocity(uint8_t node_id, + double target_qd) = 0; + // target_tau: N*m. + virtual bool commandCyclicTorque(uint8_t node_id, double target_tau) = 0; + virtual void setMotorConversion(uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) = 0; + virtual bool torqueOn(uint8_t node_id) = 0; + virtual bool torqueOff(uint8_t node_id) = 0; + virtual bool brakeRelease(uint8_t node_id) = 0; + virtual bool quickStop(uint8_t node_id) = 0; virtual double getQ(uint8_t node_id) = 0; virtual double getQd(uint8_t node_id) = 0; diff --git a/cmvr-es/devices/state_define.h b/cmvr-es/devices/state_define.h index daa3fb0c..8906347d 100644 --- a/cmvr-es/devices/state_define.h +++ b/cmvr-es/devices/state_define.h @@ -12,6 +12,8 @@ #include #include #include + +#include "common/types/agv/agv_types.h" #include "common/types/geometry_types.h" @@ -175,10 +177,6 @@ namespace cmvr::device{ int volume; // 0~100, 支持软音量调节 } SpeakerState; - typedef struct { - - }AGVState; - } diff --git a/cmvr-es/manager/device_manager/CMakeLists.txt b/cmvr-es/manager/device_manager/CMakeLists.txt index c43db8d8..a9ed557a 100644 --- a/cmvr-es/manager/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -22,3 +22,29 @@ target_link_libraries(device_manager PRIVATE add_library(cmvr_es::device_manager ALIAS device_manager) install(TARGETS device_manager LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(device_manager_snapshot_test + tests/device_manager_snapshot_test.cpp + ) + target_link_libraries(device_manager_snapshot_test PRIVATE + cmvr_es::device_manager + ) + add_test( + NAME device_manager_snapshot_test + COMMAND device_manager_snapshot_test + ) + set_tests_properties(device_manager_snapshot_test PROPERTIES TIMEOUT 20) + if(UNIX AND NOT APPLE) + get_property(_device_manager_test_library_dirs + DIRECTORY PROPERTY LINK_DIRECTORIES) + list(PREPEND _device_manager_test_library_dirs + "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime") + list(JOIN _device_manager_test_library_dirs ":" + _device_manager_test_library_path) + set_tests_properties(device_manager_snapshot_test PROPERTIES + ENVIRONMENT + "LD_LIBRARY_PATH=${_device_manager_test_library_path}" + ) + endif() +endif() diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index d680dded..b6726f20 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -7,9 +7,12 @@ #include #include +#include #include -#include #include +#include +#include + #include "device_factory.h" #include "cmvr/config/device_manager_config/device_manager_config.pb.h" @@ -31,6 +34,8 @@ namespace cmvr::device { void getDeviceList(std::list> &device_list); void registerDevice(const std::shared_ptr& device); void registerDevice(const std::string& device_id, const std::shared_ptr& device); + std::shared_ptr getDeviceBase(const std::string& device_id); + DeviceManagerSnapshot snapshot() const; std::string version() const; std::string name() const; @@ -44,7 +49,10 @@ namespace cmvr::device { static std::shared_ptr instance_; config::DeviceManagerConfig cfg_; + mutable std::shared_mutex devices_mutex_; + std::mutex lifecycle_mutex_; std::unordered_map devices_; + std::unordered_map device_statuses_; std::unique_ptr dev_factory_; explicit DeviceManager(const config::DeviceManagerConfig &cfg); @@ -52,6 +60,8 @@ namespace cmvr::device { void pre_scan_robot_arm_dependencies_() const; void init_devices_(); void configure_mujoco_viewer_pip_(); + void start_devices_(); + void stop_devices_(); }; } // cmvr diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index d957afb4..db84586f 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -5,6 +5,9 @@ #include "../include/device_manager.h" +#include +#include + #include "devices/agv/abstract_agv.h" #include "devices/arm/robot_arm.h" #include "devices/battery/abstract_battery.h" @@ -26,6 +29,9 @@ using namespace cmvr::device; namespace { +using GroupJointSelection = std::unordered_map>; +using MotorJointSelections = std::unordered_map; + void logSection(const char* title) { CMVR_LOG(INFO) << "---------------- " << title << " ----------------"; @@ -71,6 +77,32 @@ bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group, return false; } +void addAllMotorJoints(const std::string& motor_system_id, + const cmvr::config::MotorRootConfig& root_cfg, + MotorJointSelections& selections) +{ + auto& group_selection = selections[motor_system_id]; + for (const auto& motor_group : root_cfg.motor().motor_groups()) { + if (motor_group.id().empty()) { + continue; + } + + auto& selected_joints = group_selection[motor_group.id()]; + for (const auto& motor : motor_group.motors().motors()) { + if (!motor.joint_name().empty()) { + selected_joints.insert(motor.joint_name()); + } + } + if (selected_joints.empty()) { + group_selection.erase(motor_group.id()); + } + } + + if (group_selection.empty()) { + selections.erase(motor_system_id); + } +} + } // namespace template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -174,6 +206,17 @@ std::shared_ptr DeviceManager::getDevice(const std::string& device_i return ptr; } +std::shared_ptr DeviceManager::getDeviceBase(const std::string& device_id) +{ + std::shared_lock lock(devices_mutex_); + const auto it = devices_.find(device_id); + if (it == devices_.end() || !it->second.device) { + CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found."; + return nullptr; + } + return it->second.device; +} + void DeviceManager::getDeviceList(std::list>& device_list){ device_list.clear(); for (const auto& [device_id, record] : devices_) { @@ -217,6 +260,63 @@ void DeviceManager::registerDevice(const std::string& device_id, << ", kind=" << toString(device->kind()); } +DeviceManagerSnapshot DeviceManager::snapshot() const +{ + struct SnapshotSource { + ManagedDeviceSnapshot status; + std::shared_ptr device; + }; + + std::vector sources; + { + std::shared_lock lock(devices_mutex_); + sources.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + SnapshotSource source; + source.status.id = id; + source.status.kind = record.kind; + source.status.type_name = record.type_name; + source.status.enabled = true; + source.status.state = ManagedDeviceState::Ready; + source.device = record.device; + sources.push_back(std::move(source)); + } + } + + DeviceManagerSnapshot result; + result.name = name(); + result.version = version(); + result.description = description(); + result.devices.reserve(sources.size()); + + for (auto& source : sources) { + if (source.device) { + try { + source.status.health = source.device->healthSnapshot(); + } catch (const std::exception& error) { + source.status.health.state = DeviceHealthState::Fault; + source.status.health.error_message = error.what(); + } catch (...) { + source.status.health.state = DeviceHealthState::Fault; + source.status.health.error_message = + "device health snapshot threw an unknown exception"; + } + } + source.status.abnormal = + source.status.health.state == DeviceHealthState::Degraded || + source.status.health.state == DeviceHealthState::Fault; + source.status.error_message = source.status.health.error_message; + result.devices.push_back(std::move(source.status)); + } + + std::sort(result.devices.begin(), result.devices.end(), + [](const ManagedDeviceSnapshot& lhs, + const ManagedDeviceSnapshot& rhs) { + return lhs.id < rhs.id; + }); + return result; +} + std::string DeviceManager::version() const { return cfg_.version().empty() ? "1.0" : cfg_.version(); } @@ -256,8 +356,7 @@ void DeviceManager::log_device_plan_() const void DeviceManager::pre_scan_robot_arm_dependencies_() const { - using GroupJointSelection = std::unordered_map>; - std::unordered_map selections; + MotorJointSelections selections; std::unordered_map motor_roots; for (const auto& entry : cfg_.devices()) { @@ -387,6 +486,15 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } } + if (selections.empty() && cfg_.init_all_motors_when_no_active_joints()) { + CMVR_LOG(INFO) << "[DeviceManager]: No active motor joints from RobotArm; " + << "initialize all configured motors because " + << "init_all_motors_when_no_active_joints=true"; + for (const auto& [motor_system_id, root_cfg] : motor_roots) { + addAllMotorJoints(motor_system_id, root_cfg, selections); + } + } + MotorManager::clearActiveJoints(); for (auto& [motor_system_id, group_selection] : selections) { MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); diff --git a/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp new file mode 100644 index 00000000..05d5bf8c --- /dev/null +++ b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp @@ -0,0 +1,379 @@ +#include "manager/device_manager/include/device_manager.h" + +#include "devices/camera/abstract_camera.h" +#include "devices/dexhand/abstract_dexhand.h" +#include "devices/microphone/abstract_microphone.h" + +#include +#include +#include +#include +#include +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + return false; \ + } \ + } while (false) + +using cmvr::device::AbstractDevice; +using cmvr::device::DeviceHealthSnapshot; +using cmvr::device::DeviceHealthState; +using cmvr::device::DeviceKind; +using cmvr::device::DeviceManager; +using cmvr::device::DeviceManagerSnapshot; +using cmvr::device::ManagedDeviceSnapshot; +using cmvr::device::ManagedDeviceState; + +class MemoryCamera final : public cmvr::device::AbstractCamera { +public: + std::string typeName() const override { return "MemoryCamera"; } + void getState(cmvr::device::CameraState& output) override + { + output = state; + } + + cmvr::device::CameraState state{}; +}; + +class MemoryMicrophone final : public cmvr::device::AbstractMicrophone { +public: + std::string typeName() const override { return "MemoryMicrophone"; } + void getState(cmvr::device::MicrophoneState& output) override + { + output = state; + } + + cmvr::device::MicrophoneState state{}; +}; + +class MemoryDexHand final : public cmvr::device::AbstractDexHand { +public: + std::string typeName() const override { return "MemoryDexHand"; } + Status state() const override { return lifecycle; } + std::string lastError() const override { return error; } + void setAngles(const std::vector&) override {} + void setTactilePollingRegions( + const std::vector&) override {} + std::vector getSensorData() override { return {}; } + TactileRegionData getSensorData(FingerType, TactileRegion) override + { + return {}; + } + ResultantForce getResultantForce(FingerType, TactileRegion) override + { + return {}; + } + + Status lifecycle{Status::CREATED}; + std::string error; +}; + +class FakeDevice final : public AbstractDevice { +public: + explicit FakeDevice(std::string id, + DeviceKind kind = DeviceKind::Camera) + : AbstractDevice(std::move(id)), kind_(kind) + { + } + + DeviceKind kind() const noexcept override { return kind_; } + std::string typeName() const override { return "FakeDevice"; } + + bool start() override + { + ++start_calls; + if (throw_on_start) { + throw std::runtime_error(std::string(700, 's')); + } + return start_result; + } + + bool stop() override + { + ++stop_calls; + if (throw_on_stop) { + throw std::runtime_error(std::string(700, 't')); + } + return stop_result; + } + + DeviceHealthSnapshot healthSnapshot() override + { + ++health_calls; + if (throw_on_health) { + throw std::runtime_error(std::string(700, 'h')); + } + return health; + } + + DeviceKind kind_; + bool start_result{true}; + bool stop_result{true}; + bool throw_on_start{false}; + bool throw_on_stop{false}; + bool throw_on_health{false}; + DeviceHealthSnapshot health{DeviceHealthState::Healthy, {}}; + std::atomic start_calls{0}; + std::atomic stop_calls{0}; + std::atomic health_calls{0}; +}; + +const ManagedDeviceSnapshot* findDevice(const DeviceManagerSnapshot& snapshot, + const std::string& id) +{ + for (const auto& device : snapshot.devices) { + if (device.id == id) { + return &device; + } + } + return nullptr; +} + +bool isSorted(const DeviceManagerSnapshot& snapshot) +{ + for (std::size_t i = 1; i < snapshot.devices.size(); ++i) { + if (snapshot.devices[i].id < snapshot.devices[i - 1].id) { + return false; + } + } + return true; +} + +bool testCategoryHealthAdapters() +{ + MemoryCamera camera; + CHECK_TRUE(camera.healthSnapshot().state == + DeviceHealthState::Unknown); + camera.state.is_initialized = true; + CHECK_TRUE(camera.healthSnapshot().state == + DeviceHealthState::Healthy); + camera.state.error_message = "camera warning"; + CHECK_TRUE(camera.healthSnapshot().state == + DeviceHealthState::Degraded); + camera.state.is_error = true; + CHECK_TRUE(camera.healthSnapshot().state == + DeviceHealthState::Fault); + + MemoryMicrophone microphone; + microphone.state.is_initialized = true; + CHECK_TRUE(microphone.healthSnapshot().state == + DeviceHealthState::Healthy); + microphone.state.is_error = true; + microphone.state.error_message = "microphone fault"; + const auto microphone_health = microphone.healthSnapshot(); + CHECK_TRUE(microphone_health.state == DeviceHealthState::Fault); + CHECK_TRUE(microphone_health.error_message == "microphone fault"); + + MemoryDexHand dexhand; + CHECK_TRUE(dexhand.healthSnapshot().state == + DeviceHealthState::Unknown); + dexhand.lifecycle = MemoryDexHand::Status::INITIALIZED; + CHECK_TRUE(dexhand.healthSnapshot().state == + DeviceHealthState::Healthy); + dexhand.error = "temporary warning"; + CHECK_TRUE(dexhand.healthSnapshot().state == + DeviceHealthState::Degraded); + dexhand.lifecycle = MemoryDexHand::Status::FAULT; + CHECK_TRUE(dexhand.healthSnapshot().state == + DeviceHealthState::Fault); + return true; +} + +bool testConfiguredAndDynamicSnapshots() +{ + cmvr::config::DeviceManagerConfig config; + config.set_name("snapshot-test"); + config.set_version("9.1"); + config.set_description("device manager snapshot test"); + + auto* disabled = config.add_devices(); + disabled->set_id("disabled_camera"); + disabled->set_type( + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA); + disabled->set_enable(false); + + auto* broken = config.add_devices(); + broken->set_id("broken_device"); + broken->set_type( + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); + broken->set_enable(true); + + auto* duplicate_disabled = config.add_devices(); + duplicate_disabled->set_id("duplicate_device"); + duplicate_disabled->set_type( + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA); + duplicate_disabled->set_enable(false); + + auto* duplicate_enabled = config.add_devices(); + duplicate_enabled->set_id("duplicate_device"); + duplicate_enabled->set_type( + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA); + duplicate_enabled->set_enable(true); + + auto& manager = DeviceManager::getInstance(config); + auto configured = manager.snapshot(); + CHECK_TRUE(configured.name == "snapshot-test"); + CHECK_TRUE(configured.version == "9.1"); + CHECK_TRUE(configured.description == "device manager snapshot test"); + CHECK_TRUE(configured.devices.size() == 3); + CHECK_TRUE(isSorted(configured)); + + const auto* disabled_status = + findDevice(configured, "disabled_camera"); + CHECK_TRUE(disabled_status != nullptr); + CHECK_TRUE(!disabled_status->enabled); + CHECK_TRUE(disabled_status->kind == DeviceKind::Camera); + CHECK_TRUE(disabled_status->type_name == "Camera"); + CHECK_TRUE(disabled_status->state == ManagedDeviceState::Disabled); + CHECK_TRUE(disabled_status->health.state == + DeviceHealthState::Unknown); + CHECK_TRUE(!disabled_status->abnormal); + CHECK_TRUE(disabled_status->status_updated_at_unix_ms != 0); + + const auto* broken_status = findDevice(configured, "broken_device"); + CHECK_TRUE(broken_status != nullptr); + CHECK_TRUE(broken_status->enabled); + CHECK_TRUE(broken_status->state == ManagedDeviceState::Error); + CHECK_TRUE(broken_status->health.state == DeviceHealthState::Unknown); + CHECK_TRUE(broken_status->abnormal); + CHECK_TRUE(!broken_status->error_message.empty()); + CHECK_TRUE(broken_status->error_message.size() <= 512); + + const auto* duplicate_status = + findDevice(configured, "duplicate_device"); + CHECK_TRUE(duplicate_status != nullptr); + CHECK_TRUE(duplicate_status->enabled); + CHECK_TRUE(duplicate_status->state == ManagedDeviceState::Error); + CHECK_TRUE(duplicate_status->abnormal); + CHECK_TRUE(duplicate_status->error_message == + "duplicate configured device id: duplicate_device"); + + auto healthy = std::make_shared("z_healthy"); + auto degraded = std::make_shared("a_degraded"); + degraded->health = { + DeviceHealthState::Degraded, std::string(700, 'd')}; + auto start_fail = std::make_shared("m_start_fail"); + start_fail->start_result = false; + auto stop_fail = std::make_shared("n_stop_fail"); + stop_fail->stop_result = false; + auto health_throw = std::make_shared("b_health_throw"); + health_throw->throw_on_health = true; + + manager.registerDevice(healthy); + manager.registerDevice(degraded); + manager.registerDevice(start_fail); + manager.registerDevice(stop_fail); + manager.registerDevice(health_throw); + + // Duplicate registration must retain the original object and status. + manager.registerDevice( + std::make_shared("z_healthy", DeviceKind::Speaker)); + CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy); + + const auto registered = manager.snapshot(); + CHECK_TRUE(isSorted(registered)); + const auto* healthy_registered = + findDevice(registered, "z_healthy"); + CHECK_TRUE(healthy_registered != nullptr); + CHECK_TRUE(healthy_registered->state == + ManagedDeviceState::Registered); + CHECK_TRUE(healthy_registered->health.state == + DeviceHealthState::Healthy); + CHECK_TRUE(!healthy_registered->abnormal); + + const auto* degraded_registered = + findDevice(registered, "a_degraded"); + CHECK_TRUE(degraded_registered != nullptr); + CHECK_TRUE(degraded_registered->abnormal); + CHECK_TRUE(degraded_registered->health.state == + DeviceHealthState::Degraded); + CHECK_TRUE(degraded_registered->health.error_message.size() == 512); + CHECK_TRUE(degraded_registered->error_message.size() == 512); + + const auto* thrown_health = + findDevice(registered, "b_health_throw"); + CHECK_TRUE(thrown_health != nullptr); + CHECK_TRUE(thrown_health->abnormal); + CHECK_TRUE(thrown_health->health.state == + DeviceHealthState::Fault); + CHECK_TRUE(thrown_health->health.error_message.size() <= 512); + CHECK_TRUE(thrown_health->error_message.size() <= 512); + + manager.start(); + const auto running = manager.snapshot(); + CHECK_TRUE(findDevice(running, "z_healthy")->state == + ManagedDeviceState::Running); + CHECK_TRUE(findDevice(running, "m_start_fail")->state == + ManagedDeviceState::Error); + CHECK_TRUE(findDevice(running, "m_start_fail")->abnormal); + CHECK_TRUE(findDevice(running, "m_start_fail")->health.state == + DeviceHealthState::Healthy); + CHECK_TRUE(healthy->start_calls.load() == 1); + + // The earlier value snapshot remains independent from manager mutations. + CHECK_TRUE(healthy_registered->state == + ManagedDeviceState::Registered); + + manager.stop(); + const auto stopped = manager.snapshot(); + CHECK_TRUE(findDevice(stopped, "z_healthy")->state == + ManagedDeviceState::Stopped); + CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state == + ManagedDeviceState::Error); + CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal); + CHECK_TRUE(healthy->stop_calls.load() == 1); + return true; +} + +bool testConcurrentSnapshotAndRegistration() +{ + auto& manager = DeviceManager::getInstance(); + std::atomic done{false}; + std::atomic reader_ok{true}; + + std::thread reader([&] { + while (!done.load(std::memory_order_acquire)) { + const auto current = manager.snapshot(); + if (!isSorted(current)) { + reader_ok.store(false, std::memory_order_release); + return; + } + } + }); + + for (int i = 0; i < 32; ++i) { + manager.registerDevice( + std::make_shared( + "concurrent_" + std::to_string(i))); + } + done.store(true, std::memory_order_release); + reader.join(); + + CHECK_TRUE(reader_ok.load(std::memory_order_acquire)); + const auto final_snapshot = manager.snapshot(); + CHECK_TRUE(isSorted(final_snapshot)); + for (int i = 0; i < 32; ++i) { + CHECK_TRUE( + findDevice(final_snapshot, + "concurrent_" + std::to_string(i)) != nullptr); + } + return true; +} + +} // namespace + +int main() +{ + DeviceManager::destroyInstance(); + const bool success = + testCategoryHealthAdapters() && + testConfiguredAndDynamicSnapshots() && + testConcurrentSnapshotAndRegistration(); + DeviceManager::destroyInstance(); + return success ? 0 : 1; +} diff --git a/cmvr-es/manager/media_source_hub/include/media_source_hub.h b/cmvr-es/manager/media_source_hub/include/media_source_hub.h index 39daa7bd..c7dcb553 100644 --- a/cmvr-es/manager/media_source_hub/include/media_source_hub.h +++ b/cmvr-es/manager/media_source_hub/include/media_source_hub.h @@ -123,10 +123,6 @@ private: std::shared_ptr impl_; }; -// Compatibility alias for older protocol tests and integrations. New code should -// use MediaSourceHub directly. -using MediaSourceManager = MediaSourceHub; - } // namespace cmvr::media #endif // CMVR_ES_MANAGER_MEDIA_SOURCE_HUB_H diff --git a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp index de6f1338..36b21653 100644 --- a/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp +++ b/cmvr-es/manager/media_source_hub/src/device_media_source_adapter.cpp @@ -32,12 +32,10 @@ std::string normalizedCodec(std::string codec) { Codec videoCodec(const std::string& value) { const std::string codec = normalizedCodec(value); - if (codec == "h264" || codec == "avc" || codec == "avc1" || - codec == "libx264" || codec == "h264qsv") { + if (codec == "h264" || codec == "avc" || codec == "avc1" || codec == "libx264") { return Codec::H264; } - if (codec == "h265" || codec == "hevc" || codec == "hvc1" || - codec == "libx265" || codec == "h265qsv" || codec == "hevcqsv") { + if (codec == "h265" || codec == "hevc" || codec == "hvc1" || codec == "libx265") { return Codec::H265; } return Codec::UNKNOWN; diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index 05410231..a8af771c 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -36,6 +36,10 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type) return "TASK_TYPE_TOUCH_SCREEN"; case config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER: return "TASK_TYPE_GRPC_SERVER"; + case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION: + return "TASK_TYPE_SELF_COLLISION"; + case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE: + return "TASK_TYPE_QUIC_EDGE"; case config::TaskConfigEntry::TASK_TYPE_UNKNOWN: default: return "TASK_TYPE_UNKNOWN"; diff --git a/cmvr-es/runtime/CMakeLists.txt b/cmvr-es/runtime/CMakeLists.txt index e03b852a..f8a9a628 100644 --- a/cmvr-es/runtime/CMakeLists.txt +++ b/cmvr-es/runtime/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(cmvr_runtime PUBLIC cmvr_es::device_manager cmvr_es::task_manager cmvr_es::service + cmvr_es::quic_edge_task cmvr_es::mujoco_viewer ) diff --git a/cmvr-es/runtime/src/cmvr_runtime.cpp b/cmvr-es/runtime/src/cmvr_runtime.cpp index 24cf7e2e..6f0378ae 100644 --- a/cmvr-es/runtime/src/cmvr_runtime.cpp +++ b/cmvr-es/runtime/src/cmvr_runtime.cpp @@ -11,6 +11,7 @@ #include "common/config/config_files.h" #include "common/io/proto_file_io.h" #include "task/grpc_server_task/include/grpc_server_task.h" +#include "task/quic_edge_task/include/quic_edge_task.h" namespace cmvr { namespace { @@ -102,6 +103,7 @@ bool Runtime::init_(const std::string& config_path, device::DeviceManager::getInstance(device_manager_root.device_manager()); task::registerGrpcServerTaskFactory(); + task::registerQuicEdgeTaskFactory(); if (app_config.task_manager_config_file().empty()) { CMVR_LOG(ERROR) << "TaskManager config file is empty"; diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 63adb882..24fad685 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(service grpc/src/grpc_head_service.cpp grpc/src/grpc_dexhand_service.cpp grpc/src/grpc_arm_service.cpp + grpc/src/grpc_agv_service.cpp grpc/src/grpc_hlc_service.cpp ../task/grpc_server_task/src/grpc_server_task.cpp ) @@ -28,6 +29,21 @@ target_link_libraries(service PRIVATE add_library(cmvr_es::service ALIAS service) install(TARGETS service LIBRARY DESTINATION lib) +if(BUILD_TESTING) + add_executable(grpc_camera_stream_policy_test + grpc/tests/grpc_camera_stream_policy_test.cpp + ) + target_include_directories(grpc_camera_stream_policy_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME grpc_camera_stream_policy_test + COMMAND grpc_camera_stream_policy_test + ) + set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) +endif() + # -------------------------------------------------------- # Unit test # -------------------------------------------------------- diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md new file mode 100644 index 00000000..0ed71894 --- /dev/null +++ b/cmvr-es/service/README.md @@ -0,0 +1,233 @@ +# Service 模块开发指南 + +`service/` 实现边缘端对外协议和设备抽象之间的适配。Service 负责解析请求、查找设备、转换 DTO 和返回结果，不负责创建具体设备后端。 + +返回[项目总览](../../README.md)。 + +## 当前结构 + +| 目录 | 职责 | +| --- | --- | +| `grpc/` | 入站设备控制、状态查询和兼容流式接口 | +| `quic_edge/` | 边缘端主动连接平台的 QUIC client、控制状态机和媒体 packetizer | +| `quic_edge/tests/` | 已登记到 CTest 的 QUIC 协议测试 | + +两个遗留 gRPC client test 位于 `grpc/src/*_client_test.cpp`，当前没有通过 `add_test()` 登记。 + +gRPC 和 QUIC 的职责边界： + +- 机械臂、AGV 等可靠控制继续使用 gRPC； +- 节点注册、心跳和 IP 上报使用 QUIC reliable stream； +- 实时音视频使用 QUIC DATAGRAM； +- `quic_edge/` 不是平台 Gateway，也不是浏览器服务器。 + +## 新增 gRPC Service + +当前没有动态 service registry，必须完成以下全部步骤。 + +### 1. 定义 Proto + +在 [`../../protos/cmvr/api/`](../../protos/cmvr/api/) 增加或扩展： + +- `_command.proto` +- `_service.proto` + +import 路径必须相对于 `protos/`。兼容规则见 [`../../protos/README.md`](../../protos/README.md)。 + +### 2. 实现 Service + +目录约定： + +```text +service/grpc/ +├── include/grpc_example_service.h +└── src/grpc_example_service.cpp +``` + +实现类继承生成的： + +```cpp +cmvr::api::ExampleService::Service +``` + +通过已初始化的 `DeviceManager` 获取抽象设备。不要在 service 中创建厂商 SDK 对象，不要绕过设备 factory。 + +### 3. 加入 service target + +将实现 `.cpp` 加入 [`CMakeLists.txt`](CMakeLists.txt) 的 `service` library，并声明最小依赖。 + +### 4. 注册到 GrpcServerTask + +还必须修改： + +- [`../task/grpc_server_task/include/grpc_server_task.h`](../task/grpc_server_task/include/grpc_server_task.h) +- [`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp) + +完成： + +1. 增加 service owner； +2. 在 start 中构造； +3. 调用 `builder.RegisterService(...)`； +4. 在 `clearServices()` 中 reset。 + +漏掉该步骤时项目可能编译成功，但服务不会出现在 reflection 或运行时。 + +### 5. 测试 + +- 直接测试 service handler 或启动临时 gRPC server； +- 覆盖设备不存在、类型不匹配、设备错误和取消； +- 使用 grpcurl/reflection 验证服务全名； +- 流式 RPC 覆盖客户端断开和慢消费者； +- 在 CMake 中使用 `if(BUILD_TESTING)` 包裹测试目标，并通过 `add_test()` 登记。 + +`grpc_arm_client_test` 和 `grpc_hlc_client_test` 是未登记到 CTest 的历史可执行文件，不能代表默认自动覆盖。 + +## gRPC 实现约束 + +### 错误语义 + +当前历史服务存在两种风格： + +- gRPC status 返回 OK，业务失败写入 Feedback header； +- 使用非 OK gRPC status 表达 transport/API 失败。 + +扩展已有服务时保持其兼容语义。新增服务必须在设计时明确： + +- 哪些错误使用 gRPC status； +- 哪些错误使用业务 Feedback； +- 是否允许部分成功； +- deadline/cancellation 如何映射； +- 不得同时返回互相矛盾的 transport 和业务状态。 + +### 流式 RPC + +- 检查 `context->IsCancelled()`； +- 检查 `Read()` / `Write()` 返回； +- 使用 RAII 或 MediaSourceHub Subscription 释放 producer lease； +- 不持有设备状态锁进行网络写； +- 为 wait/read 使用有限 timeout； +- 慢客户端不能阻塞设备生产线程； +- H.264/H.265 丢帧后等待关键帧恢复； +- gRPC RGB 流在积压超过 `camera_stream_max_pending_frames` 或帧龄超过 + `camera_stream_max_frame_age_ms` 时主动丢弃旧帧，请求 IDR，并从下一个关键帧恢复。 + +当前仅 gRPC RGB 和麦克风流使用 MediaSourceHub；Depth/RGBD 仍直接读取设备帧。 + +gRPC 相机实时流默认最多保留 2 帧积压、最大允许 250 ms 帧龄。两个配置项填 0 +时使用上述默认值。该策略以低延迟为目标，不保证每个视频帧都到达客户端；控制命令 +仍由普通 gRPC RPC 承担。 + +`FrameData` 附带 `capture_utc_ns`、`source_sequence`、`pts`、`dts`、 +`source_fps`、`source_timestamp` 和 `source_frame_number`。平台端可用采集时间 +与接收时间的差值区分设备、网络、服务端写阻塞和客户端解码/渲染队列延迟。新增字段 +保持 protobuf wire compatibility，旧客户端可以继续连接，但需要重新生成代码后才能 +读取这些诊断字段。 + +### 当前安全状态 + +GrpcServerTask 使用同步 `grpc::ServerBuilder` 和 `grpc::InsecureServerCredentials()`。reflection 由配置控制。当前没有 gRPC TLS、认证、授权或标准 health service。 + +QUIC 配置中的 `grpc_endpoint_tls` 只是上报字段，不会启用 gRPC TLS。 + +## 扩展 QUIC Edge + +关键层次： + +| 层 | 主要文件 | +| --- | --- | +| 控制流 framing | `quic_edge/src/control_framing.cpp` | +| DATAGRAM 固定头和分片 | `quic_edge/src/datagram_packetizer.cpp` | +| 会话和状态机 | `quic_edge/src/quic_edge_service.cpp` | +| 传输抽象 | `quic_edge/include/quic_transport.h` | +| MsQuic 后端 | `quic_edge/src/msquic_transport.cpp` | +| 设备媒体适配 | `quic_edge/src/quic_edge_device_adapter.cpp` | +| DeviceManager 心跳适配 | `quic_edge/src/quic_edge_device_adapter.cpp` | + +线协议见 [`../../protos/cmvr/quic_edge/v1/README.md`](../../protos/cmvr/quic_edge/v1/README.md)。 + +### 新增 Transport 后端 + +1. 实现 `QuicTransport` 完整接口； +2. 明确 callback 所在线程； +3. stop/close 后不得再访问已销毁 service； +4. `QUEUED` 表示 transport 接管待发送数据； +5. `WOULD_BLOCK` 或 `ERROR` 不得接管任何字节； +6. DATAGRAM batch 本地准入必须原子； +7. native send 部分失败时关闭连接并清理 session； +8. 添加 fake transport 故障注入测试； +9. 在默认 transport factory 中显式选择后端。 + +### 新增控制消息 + +不能只修改 Proto，还要同步： + +- envelope 构造与发送； +- 入站 dispatch； +- 合法状态和消息时序； +- message sequence 校验； +- session ID 和 heartbeat sequence 校验； +- reconnect 后状态清理； +- Java Gateway 对端； +- framing、状态机和 fake transport 测试。 + +当前 Edge 入站只接受： + +- `NodeRegisterResponse` +- `NodeHeartbeatAck` +- `ProtocolError` + +虽然 Proto 定义了 `MediaSessionClose`，本版本 Edge 收到它仍会判为 unexpected，不应将其描述为已实现的双向控制能力。 + +### DeviceManager 心跳快照 + +`QuicEdgeService` 通过可注入的 `DeviceSnapshotProvider` 获取协议无关的纯值 +快照。生产构造绑定已经初始化的 `DeviceManager`，fake transport 测试则注入 +合成快照，因此协议状态机不需要创建硬件对象或依赖 DeviceManager 单例。 +DeviceManager 的本地快照继续保留禁用设备；QUIC wire 映射层仅序列化 +`enabled=true` 的设备。已启用但创建、初始化或启动失败的设备不会被过滤。 + +心跳线程只读取 Manager 维护的内存状态，不能在这里同步访问厂商 SDK、网络或 +设备总线。新增设备健康探针必须实现 `AbstractDevice::healthSnapshot()` 的 +线程安全、无阻塞 I/O 契约；未实现时上报 `UNSPECIFIED`，不得伪造为健康。 +设备异常字符串会限长，整条消息仍受 `maximum_control_frame_bytes` 约束。 + +### 跨 QUIC 通道顺序 + +Edge 会先调用可靠流发送 session/descriptor，再调用 DATAGRAM 发送媒体，但 QUIC stream 与 DATAGRAM 没有跨通道到达顺序保证。 + +Gateway 必须容忍 DATAGRAM 先到，对未知 session epoch 或 codec generation 的数据有界暂存或丢弃。 + +## QUIC 测试要求 + +参考 [`quic_edge/CMakeLists.txt`](quic_edge/CMakeLists.txt) 和 `quic_edge_protocol_test`，至少覆盖： + +- 控制消息拆包、粘包和超限； +- 重复或倒退 sequence； +- 注册、ACK 超时和重连； +- DeviceManager 已启用设备过滤、类型/状态映射和 provider 失败隔离； +- Gateway 返回零心跳周期时采用本地 `heartbeat_interval_ms`； +- session epoch 清理； +- DATAGRAM header 字节序； +- 分片边界和超大帧； +- 原子队列准入和背压； +- 丢帧、generation 和关键帧恢复； +- stop 与 callback 并发。 + +```bash +cmake --build build --target quic_edge_protocol_test +ctest \ + --test-dir build \ + -R '^quic_edge_protocol_test$' \ + --output-on-failure +``` + +## 提交检查 + +- [ ] service 不创建具体硬件后端 +- [ ] Proto、实现、CMake 和 GrpcServerTask 注册均已更新 +- [ ] 错误语义与已有服务兼容 +- [ ] deadline、cancel、Read/Write 失败均处理 +- [ ] 流式资源通过 RAII 释放 +- [ ] gRPC 安全能力没有被配置字段误描述 +- [ ] QUIC 状态机与 Gateway 同步更新 +- [ ] 协议测试已登记到 CTest diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h new file mode 100644 index 00000000..6ec4e65d --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -0,0 +1,82 @@ +#ifndef CMVR_ES_GRPC_AGV_SERVICE_H +#define CMVR_ES_GRPC_AGV_SERVICE_H + +#include "cmvr/api/agv_service.grpc.pb.h" +#include "devices/agv/abstract_agv.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { + +class gRPCAgvServiceImpl final : public api::AgvService::Service { +public: + gRPCAgvServiceImpl(); + ~gRPCAgvServiceImpl() override = default; + + grpc::Status getRuntimeState(grpc::ServerContext* context, + const api::AgvRuntimeStateCommand_Request* request, + api::AgvRuntimeStateCommand_Feedback* response) override; + grpc::Status getNavigationStatus(grpc::ServerContext* context, + const api::AgvNavigationStatusCommand_Request* request, + api::AgvNavigationStatusCommand_Feedback* response) override; + grpc::Status emergencyStop(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status clearFault(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status navigateToPose(grpc::ServerContext* context, + const api::AgvNavigateToPoseCommand_Request* request, + api::AgvNavigateToPoseCommand_Feedback* response) override; + grpc::Status navigateToStation(grpc::ServerContext* context, + const api::AgvNavigateToStationCommand_Request* request, + api::AgvNavigateToStationCommand_Feedback* response) override; + grpc::Status followPath(grpc::ServerContext* context, + const api::AgvFollowPathCommand_Request* request, + api::AgvFollowPathCommand_Feedback* response) override; + grpc::Status pauseNavigation(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status resumeNavigation(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status cancelNavigation(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status setVelocity(grpc::ServerContext* context, + const api::AgvSetVelocityCommand_Request* request, + api::AgvSetVelocityCommand_Feedback* response) override; + grpc::Status stopVelocityControl(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + grpc::Status listMaps(grpc::ServerContext* context, + const api::AgvListMapsCommand_Request* request, + api::AgvListMapsCommand_Feedback* response) override; + grpc::Status listStations(grpc::ServerContext* context, + const api::AgvListStationsCommand_Request* request, + api::AgvListStationsCommand_Feedback* response) override; + grpc::Status switchMap(grpc::ServerContext* context, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) override; + grpc::Status uploadMap(grpc::ServerContext* context, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) override; + grpc::Status downloadMap(grpc::ServerContext* context, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) override; + grpc::Status startMapping(grpc::ServerContext* context, + const api::AgvStartMappingCommand_Request* request, + api::AgvStartMappingCommand_Feedback* response) override; + grpc::Status streamMap(grpc::ServerContext* context, + const api::AgvMapStreamCommand_Request* request, + grpc::ServerWriter* writer) override; + grpc::Status stopMapping(grpc::ServerContext* context, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) override; + +private: + device::DeviceManager& dmgr_; +}; + +} // namespace cmvr::service + +#endif // CMVR_ES_GRPC_AGV_SERVICE_H diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h index 9a39ab3a..07a2dc30 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ b/cmvr-es/service/grpc/include/grpc_arm_service.h @@ -50,6 +50,9 @@ public: grpc::Status computeForwardKinematics(grpc::ServerContext* context, const api::ComputeForwardKinematics_Request* request, api::ComputeForwardKinematics_Response* response) override; + grpc::Status clearFault(grpc::ServerContext *context, + const cmvr::api::CommandHeader_Request *request, + cmvr::api::CommandHeader_Feedback *response) override; private: device::DeviceManager& dmgr_; diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index 17028a92..41fb2646 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -18,6 +18,7 @@ namespace cmvr::service grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override; grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override; grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override; + grpc::Status ExecuteJsonCommand(grpc::ServerContext* context, const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) override; grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp new file mode 100644 index 00000000..1f8d160f --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -0,0 +1,752 @@ +#include "service/grpc/include/grpc_agv_service.h" + +#include +#include +#include +#include + +#include + +using google::protobuf::util::TimeUtil; + +namespace cmvr::service { + +namespace { + +void fillFeedback(api::CommandHeader_Feedback* feedback, + const bool success, + const std::string& message = {}) +{ + feedback->set_success(success); + feedback->set_error_message(message); + *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime(); +} + +grpc::Status resultToStatus(const device::AgvResult& result) +{ + if (result.ok()) { + return grpc::Status::OK; + } + return grpc::Status(grpc::StatusCode::INTERNAL, result.message); +} + +template +grpc::Status setResponseResult(Response* response, const device::AgvResult& result) +{ + fillFeedback(response->mutable_header(), result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); +} + +grpc::Status setResponseResult(api::CommandHeader_Feedback* response, const device::AgvResult& result) +{ + fillFeedback(response, result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); +} + +template +grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) +{ + const std::string message = "AGV device not found: " + device_id; + fillFeedback(response->mutable_header(), false, message); + return grpc::Status(grpc::StatusCode::NOT_FOUND, message); +} + +grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std::string& device_id) +{ + const std::string message = "AGV device not found: " + device_id; + fillFeedback(response, false, message); + return grpc::Status(grpc::StatusCode::NOT_FOUND, message); +} + +device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src) +{ + device::AgvAdapterParams dst; + for (const auto& [key, value] : src.values()) { + dst.values.emplace(key, value); + } + return dst; +} + +device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src) +{ + device::AgvMotionOptions dst; + dst.max_speed = src.max_speed(); + dst.max_angular_speed = src.max_angular_speed(); + dst.max_acceleration = src.max_acceleration(); + dst.max_angular_acceleration = src.max_angular_acceleration(); + dst.reach_distance = src.reach_distance(); + dst.reach_angle = src.reach_angle(); + dst.speed_ratio = src.speed_ratio() > 0.0 ? src.speed_ratio() : 1.0; + dst.asynchronous = src.asynchronous(); + return dst; +} + +device::AgvVelocity toVelocity(const msgs::AgvVelocity& src) +{ + return {src.vx(), src.vy(), src.wz()}; +} + +device::AgvPathSegment toPathSegment(const msgs::AgvPathSegment& src) +{ + device::AgvPathSegment dst; + dst.source_station = src.source_station(); + dst.target_station = src.target_station(); + return dst; +} + +math::Pose2d toPose2d(const msgs::AgvPose2d& src) +{ + return {src.x(), src.y(), src.theta()}; +} + +device::AgvMapDimension toMapDimension(const msgs::AgvMapDimension src) +{ + switch (src) { + case msgs::AGV_MAP_2D: + return device::AgvMapDimension::Map2D; + case msgs::AGV_MAP_3D: + return device::AgvMapDimension::Map3D; + case msgs::AGV_MAP_2D_AND_3D: + return device::AgvMapDimension::Map2DAnd3D; + case msgs::AGV_MAP_DIMENSION_UNSPECIFIED: + default: + return device::AgvMapDimension::Unspecified; + } +} + +msgs::AgvMapDimension toProtoMapDimension(const device::AgvMapDimension src) +{ + switch (src) { + case device::AgvMapDimension::Map2D: + return msgs::AGV_MAP_2D; + case device::AgvMapDimension::Map3D: + return msgs::AGV_MAP_3D; + case device::AgvMapDimension::Map2DAnd3D: + return msgs::AGV_MAP_2D_AND_3D; + case device::AgvMapDimension::Unspecified: + default: + return msgs::AGV_MAP_DIMENSION_UNSPECIFIED; + } +} + +msgs::AgvMapUpdateType toProtoMapUpdateType(const device::AgvMapUpdateType src) +{ + switch (src) { + case device::AgvMapUpdateType::Snapshot: + return msgs::AGV_MAP_UPDATE_SNAPSHOT; + case device::AgvMapUpdateType::Incremental: + return msgs::AGV_MAP_UPDATE_INCREMENTAL; + case device::AgvMapUpdateType::Reset: + return msgs::AGV_MAP_UPDATE_RESET; + case device::AgvMapUpdateType::Unspecified: + default: + return msgs::AGV_MAP_UPDATE_UNSPECIFIED; + } +} + +msgs::AgvMapObjectType toProtoMapObjectType(const device::AgvMapObjectType src) +{ + switch (src) { + case device::AgvMapObjectType::Station: + return msgs::AGV_MAP_OBJECT_STATION; + case device::AgvMapObjectType::Line: + return msgs::AGV_MAP_OBJECT_LINE; + case device::AgvMapObjectType::Area: + return msgs::AGV_MAP_OBJECT_AREA; + case device::AgvMapObjectType::QrTag: + return msgs::AGV_MAP_OBJECT_QR_TAG; + case device::AgvMapObjectType::Reflector: + return msgs::AGV_MAP_OBJECT_REFLECTOR; + case device::AgvMapObjectType::BinLocation: + return msgs::AGV_MAP_OBJECT_BIN_LOCATION; + case device::AgvMapObjectType::ExternalDevice: + return msgs::AGV_MAP_OBJECT_EXTERNAL_DEVICE; + case device::AgvMapObjectType::Unspecified: + default: + return msgs::AGV_MAP_OBJECT_UNSPECIFIED; + } +} + +void fillPose2d(msgs::AgvPose2d* dst, const math::Pose2d& src) +{ + dst->set_x(src.x); + dst->set_y(src.y); + dst->set_theta(src.theta); +} + +void fillVelocity(msgs::AgvVelocity* dst, const device::AgvVelocity& src) +{ + dst->set_vx(src.vx); + dst->set_vy(src.vy); + dst->set_wz(src.wz); +} + +void fillBattery(msgs::AgvBatteryState* dst, const device::AgvBatteryState& src) +{ + dst->set_percentage(src.percentage); + dst->set_voltage(src.voltage); + dst->set_current(src.current); + dst->set_temperature(src.temperature); + dst->set_charging(src.charging); +} + +void fillRuntimeState(msgs::AgvRuntimeState* dst, const device::AgvRuntimeState& src) +{ + dst->set_timestamp(src.timestamp); + dst->set_mode(static_cast(src.mode)); + dst->set_connected(src.connected); + dst->set_localized(src.localized); + dst->set_moving(src.moving); + dst->set_fault(src.fault); + dst->set_emergency_stopped(src.emergency_stopped); + fillPose2d(dst->mutable_pose(), src.pose); + fillVelocity(dst->mutable_velocity(), src.velocity); + fillBattery(dst->mutable_battery(), src.battery); + dst->set_current_map(src.current_map); + dst->set_current_station(src.current_station); + dst->set_last_error(src.last_error); +} + +void fillNavigationStatus(msgs::AgvNavigationStatus* dst, const device::AgvNavigationStatus& src) +{ + dst->set_state(static_cast(src.state)); + dst->set_type(static_cast(src.type)); + dst->set_progress(src.progress); + dst->set_message(src.message); +} + +void fillStation(msgs::AgvStation* dst, const device::AgvStation& src) +{ + dst->set_id(src.id); + dst->set_type(src.type); + fillPose2d(dst->mutable_pose(), src.pose); + dst->set_description(src.description); +} + +void fillMapPoint3D(msgs::AgvMapPoint3D* dst, const device::AgvMapPoint3D& src) +{ + dst->set_x(src.x); + dst->set_y(src.y); + dst->set_z(src.z); +} + +void fillMapObject(msgs::AgvMapObject* dst, const device::AgvMapObject& src) +{ + dst->set_id(src.id); + dst->set_type(toProtoMapObjectType(src.type)); + for (const auto& point : src.points) { + fillMapPoint3D(dst->add_points(), point); + } + dst->set_heading(src.heading); + auto* properties = dst->mutable_properties(); + for (const auto& [key, value] : src.properties) { + (*properties)[key] = value; + } +} + +void fillUnifiedMap2D(msgs::AgvUnifiedMap2D* dst, const device::AgvUnifiedMap2D& src) +{ + dst->set_frame_id(src.frame_id); + dst->set_timestamp(src.timestamp); + dst->set_resolution(src.resolution); + dst->set_width(src.width); + dst->set_height(src.height); + fillPose2d(dst->mutable_origin(), src.origin); + for (const auto value : src.data) { + dst->add_data(value); + } + for (const auto& object : src.objects) { + fillMapObject(dst->add_objects(), object); + } +} + +void fillUnifiedMap3D(msgs::AgvUnifiedMap3D* dst, const device::AgvUnifiedMap3D& src) +{ + dst->set_frame_id(src.frame_id); + dst->set_timestamp(src.timestamp); + dst->set_voxel_resolution(src.voxel_resolution); + for (const auto& point : src.points) { + auto* dst_point = dst->add_points(); + dst_point->set_x(point.x); + dst_point->set_y(point.y); + dst_point->set_z(point.z); + dst_point->set_intensity(point.intensity); + dst_point->set_ring(point.ring); + dst_point->set_time_offset(point.time_offset); + } + for (const auto& voxel : src.voxels) { + auto* dst_voxel = dst->add_voxels(); + dst_voxel->set_x(voxel.x); + dst_voxel->set_y(voxel.y); + dst_voxel->set_z(voxel.z); + dst_voxel->set_probability(voxel.probability); + } + for (const auto& plane : src.planes) { + auto* dst_plane = dst->add_planes(); + fillMapPoint3D(dst_plane->mutable_center(), plane.center); + fillMapPoint3D(dst_plane->mutable_normal(), plane.normal); + dst_plane->set_d(plane.d); + dst_plane->set_radius(plane.radius); + } + for (const auto& object : src.objects) { + fillMapObject(dst->add_objects(), object); + } +} + +void fillUnifiedMapUpdate(msgs::AgvUnifiedMapUpdate* dst, const device::AgvUnifiedMapUpdate& src) +{ + dst->set_map_id(src.map_id); + dst->set_session_id(src.session_id); + dst->set_sequence(src.sequence); + dst->set_resume_token(src.resume_token); + dst->set_dimension(toProtoMapDimension(src.dimension)); + dst->set_update_type(toProtoMapUpdateType(src.update_type)); + dst->set_frame_id(src.frame_id); + dst->set_timestamp(src.timestamp); + dst->set_snapshot_begin(src.snapshot_begin); + dst->set_snapshot_end(src.snapshot_end); + dst->set_chunk_index(src.chunk_index); + dst->set_chunk_count(src.chunk_count); + if (src.map_2d) { + fillUnifiedMap2D(dst->mutable_map_2d(), *src.map_2d); + } else if (src.map_3d) { + fillUnifiedMap3D(dst->mutable_map_3d(), *src.map_3d); + } +} + +} // namespace + +gRPCAgvServiceImpl::gRPCAgvServiceImpl() + : dmgr_(device::DeviceManager::getInstance()) +{ +} + +grpc::Status gRPCAgvServiceImpl::getRuntimeState(grpc::ServerContext*, + const api::AgvRuntimeStateCommand_Request* request, + api::AgvRuntimeStateCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + fillRuntimeState(response->mutable_state(), agv->runtimeState()); + fillFeedback(response->mutable_header(), true); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::getNavigationStatus(grpc::ServerContext*, + const api::AgvNavigationStatusCommand_Request* request, + api::AgvNavigationStatusCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + fillNavigationStatus(response->mutable_status(), agv->navigationStatus()); + fillFeedback(response->mutable_header(), true); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->emergencyStop()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->clearFault()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*, + const api::AgvNavigateToPoseCommand_Request* request, + api::AgvNavigateToPoseCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, agv->navigateToPose( + toPose2d(request->pose()), + toMotionOptions(request->options()), + toAdapterParams(request->adapter_params()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*, + const api::AgvNavigateToStationCommand_Request* request, + api::AgvNavigateToStationCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, agv->navigateToStation( + request->station_id(), + toMotionOptions(request->options()), + toAdapterParams(request->adapter_params()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*, + const api::AgvFollowPathCommand_Request* request, + api::AgvFollowPathCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::vector path; + path.reserve(static_cast(request->path_size())); + for (const auto& segment : request->path()) { + path.push_back(toPathSegment(segment)); + } + return setResponseResult(response, agv->followPath(path)); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->pauseNavigation()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->resumeNavigation()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->cancelNavigation()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, + const api::AgvSetVelocityCommand_Request* request, + api::AgvSetVelocityCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->stopVelocityControl()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::listMaps(grpc::ServerContext*, + const api::AgvListMapsCommand_Request* request, + api::AgvListMapsCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::vector maps; + const auto result = agv->listMaps(maps); + if (result.ok()) { + for (const auto& map : maps) { + response->add_maps(map); + } + } + return setResponseResult(response, result); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::listStations(grpc::ServerContext*, + const api::AgvListStationsCommand_Request* request, + api::AgvListStationsCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::vector stations; + const auto result = agv->listStations(stations); + if (result.ok()) { + for (const auto& station : stations) { + fillStation(response->add_stations(), station); + } + } + return setResponseResult(response, result); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, agv->switchMap(request->map_name())); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult(response, agv->uploadMap(request->map_name(), request->content())); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::downloadMap(grpc::ServerContext*, + const api::AgvMapCommand_Request* request, + api::AgvMapCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + std::string content; + const auto result = agv->downloadMap(request->map_name(), content); + if (result.ok()) { + response->set_content(content); + } + return setResponseResult(response, result); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*, + const api::AgvStartMappingCommand_Request* request, + api::AgvStartMappingCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + if (!agv) { + return setDeviceNotFound(response, device_id); + } + device::AgvMappingOptions options; + options.dimension = toMapDimension(request->dimension()); + options.map_name = request->map_name(); + options.real_time = request->real_time(); + const auto result = agv->startMapping(options); + if (result.ok()) { + response->set_session_id(device_id + "_mapping"); + } + return setResponseResult(response, result); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::streamMap(grpc::ServerContext* context, + const api::AgvMapStreamCommand_Request* request, + grpc::ServerWriter* writer) +{ + try { + const std::string device_id = request->header().device_id(); + auto agv = dmgr_.getDevice(device_id); + + api::AgvMapStreamCommand_Feedback feedback; + if (!agv) { + const std::string message = "AGV device not found: " + device_id; + fillFeedback(feedback.mutable_header(), false, message); + writer->Write(feedback); + return grpc::Status(grpc::StatusCode::NOT_FOUND, message); + } + + device::AgvMapStreamOptions options; + options.dimension = toMapDimension(request->dimension()); + options.map_name = request->map_name(); + options.resume_token = request->resume_token(); + options.snapshot = request->snapshot(); + options.incremental = request->incremental(); + options.max_chunk_bytes = request->max_chunk_bytes(); + + std::uint64_t after_sequence = 0; + if (!request->resume_token().empty()) { + try { + after_sequence = static_cast(std::stoull(request->resume_token())); + } catch (...) { + after_sequence = 0; + } + } + + bool wrote_any = false; + while (!context->IsCancelled()) { + options.wait_timeout_ms = (!options.incremental && wrote_any) ? 20 : 1000; + device::AgvUnifiedMapUpdate update; + const auto result = agv->getUnifiedMapUpdate(after_sequence, options, update); + if (!result.ok()) { + if (result.code == device::AgvErrorCode::Timeout && wrote_any && !options.incremental) { + return grpc::Status::OK; + } + if (result.code == device::AgvErrorCode::Timeout && wrote_any && options.incremental) { + continue; + } + fillFeedback(feedback.mutable_header(), false, result.message); + writer->Write(feedback); + return resultToStatus(result); + } + + api::AgvMapStreamCommand_Feedback update_feedback; + fillFeedback(update_feedback.mutable_header(), true); + fillUnifiedMapUpdate(update_feedback.mutable_update(), update); + if (!writer->Write(update_feedback)) { + return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream writer closed"); + } + wrote_any = true; + after_sequence = update.sequence; + options.resume_token.clear(); + } + + return grpc::Status(grpc::StatusCode::CANCELLED, "AGV map stream cancelled"); + } catch (const std::exception& e) { + api::AgvMapStreamCommand_Feedback feedback; + fillFeedback(feedback.mutable_header(), false, e.what()); + writer->Write(feedback); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*, + const api::CommandHeader_Request* request, + api::CommandHeader_Feedback* response) +{ + try { + auto agv = dmgr_.getDevice(request->device_id()); + if (!agv) { + return setDeviceNotFound(response, request->device_id()); + } + return setResponseResult(response, agv->stopMapping()); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 28cad7ea..1ae88019 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -407,4 +407,23 @@ grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext*, return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented"); } +grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, + const cmvr::api::CommandHeader_Request *request, + cmvr::api::CommandHeader_Feedback *response) +{ + try { + const std::string device_id = request->device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + const auto result = arm->clearFault(); + fillFeedback(response, result.ok(), result.ok() ? "" : result.message); + return resultToStatus(result); + } catch (const std::exception& e) { + fillFeedback(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} } // namespace cmvr::service + diff --git a/cmvr-es/service/grpc/src/grpc_camera_service.cpp b/cmvr-es/service/grpc/src/grpc_camera_service.cpp index 449249ae..5ca371ef 100644 --- a/cmvr-es/service/grpc/src/grpc_camera_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_camera_service.cpp @@ -7,11 +7,9 @@ #include "../include/grpc_camera_service.h" #include -#include #include #include #include -#include using namespace std; using namespace cmvr::service; @@ -205,9 +203,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context, } Rs2Intrinsics intrinsics = {0}; dev->getRGBImage(image,intrinsics); - if (image.empty()) { - return failResponse(response, "Camera returned an empty RGB image: " + dev_id); - } + response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); response->mutable_intrinsics()->set_fy(intrinsics.fy); @@ -260,9 +256,6 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context, } Rs2Intrinsics intrinsics = {0}; dev->getDepthImage(image,intrinsics); - if (image.empty()) { - return failResponse(response, "Camera returned an empty depth image: " + dev_id); - } response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); @@ -319,12 +312,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, } Rs2Intrinsics intrinsics = {0}; dev->getRGBDImages(color_image,depth_image, intrinsics); - if (color_image.empty()) { - return failResponse(response, "Camera returned an empty RGB image: " + dev_id); - } - if (depth_image.empty()) { - return failResponse(response, "Camera returned an empty depth image: " + dev_id); - } response->mutable_header()->set_success(true); response->mutable_intrinsics()->set_fx(intrinsics.fx); @@ -337,8 +324,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - // color image is serialized in the existing OpenCV BGR byte order; - // clients convert it once when constructing an RGB image. + // color image if (color_image.type() == CV_8UC3) { response->mutable_color_frame()->set_type(api::FrameData::U8C3); } @@ -367,9 +353,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context, else if (depth_image.type() == CV_32FC1) { response->mutable_depth_frame()->set_type(api::FrameData::F32C1); } - else { - return failResponse(response, "unsupported depth image type"); - } response->mutable_depth_frame()->set_data( reinterpret_cast(depth_image.data), depth_image.total() * depth_image.elemSize()); response->mutable_depth_frame()->set_height(depth_image.rows); @@ -521,12 +504,11 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con !frame_data.depthFrame.empty()) { response.mutable_header()->set_success(true); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); - response.mutable_depth_frame()->set_type(api::FrameData::U16C1); response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - response.mutable_depth_frame()->set_codec("none"); - response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); + response.mutable_depth_frame()->set_codec(frame_data.codec); + response.mutable_depth_frame()->set_width(frame_data.width); + response.mutable_depth_frame()->set_height(frame_data.height); response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); @@ -608,12 +590,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con response.mutable_color_frame()->set_width(frame_data.width); response.mutable_color_frame()->set_height(frame_data.height); - response.mutable_depth_frame()->set_type(api::FrameData::U16C1); response.mutable_depth_frame()->set_data(frame_data.depthFrame.data(), frame_data.depthFrame.size()); response.mutable_depth_frame()->set_is_key_frame(frame_data.depthKey); - response.mutable_depth_frame()->set_codec("none"); - response.mutable_depth_frame()->set_width(frame_data.depth_width > 0 ? frame_data.depth_width : frame_data.width); - response.mutable_depth_frame()->set_height(frame_data.depth_height > 0 ? frame_data.depth_height : frame_data.height); + response.mutable_depth_frame()->set_codec(frame_data.codec); + response.mutable_depth_frame()->set_width(frame_data.width); + response.mutable_depth_frame()->set_height(frame_data.height); response.mutable_intrinsics()->set_fx(frame_data.intrinsics.fx); response.mutable_intrinsics()->set_fy(frame_data.intrinsics.fy); @@ -686,40 +667,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte return grpc::Status::OK; } - std::atomic client_eof_requested{false}; - std::atomic request_stream_closed{false}; - std::atomic control_requests_read{1}; - std::thread request_reader([&] { - api::GetRGBImageStreamCommand_Request control_request; - while (stream->Read(&control_request)) { - ++control_requests_read; - if (control_request.eof()) { - client_eof_requested.store(true, std::memory_order_release); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] client requested RGB stream EOF" - << ", id=" << dev_id - << ", peer=" << context->peer() - << ", control_requests=" << control_requests_read.load(); - break; - } - } - request_stream_closed.store(true, std::memory_order_release); - }); - struct RequestReaderJoiner { - std::thread& thread; - ~RequestReaderJoiner() { - if (thread.joinable()) { - thread.join(); - } - } - } request_reader_joiner{request_reader}; - const auto join_request_reader = [&] { - if (request_reader.joinable()) { - request_reader.join(); - } - }; - bool waiting_for_key_frame = true; - const char* exit_reason = "unknown"; auto last_key_frame_request = std::chrono::steady_clock::now(); auto last_latency_log = std::chrono::steady_clock::time_point{}; uint64_t discarded_since_log = 0; @@ -765,25 +713,15 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte }; while (true) { - if (client_eof_requested.load(std::memory_order_acquire)) { - exit_reason = "client_eof"; - break; - } if (context->IsCancelled()) { - exit_reason = "context_cancelled"; - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled" - << ", id=" << dev_id - << ", peer=" << context->peer() - << ", client_eof=" << client_eof_requested.load() - << ", request_stream_closed=" << request_stream_closed.load(); + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl](GetRGBImageStream) context is cancelled,id=" << dev_id; break; } const auto read = subscription.waitRead(std::chrono::milliseconds(100)); if (!read || !read->value || read->value->empty()) { if (!subscription.valid()) { - exit_reason = "subscription_invalid"; break; } if (waiting_for_key_frame) { @@ -827,7 +765,6 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte "Unsupported camera stream codec or payload format: " + dev_id); setCurrentTimestamp(response.mutable_header()->mutable_timestamp()); stream->Write(response); - exit_reason = "unsupported_stream"; break; } if (read->dropped_since_last_read > 0 || read->generation_changed || frame.discontinuity) { @@ -871,17 +808,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte const auto write_started = std::chrono::steady_clock::now(); if (!stream->Write(response)) { - exit_reason = "write_failed"; - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed" - << ", id=" << dev_id - << ", peer=" << context->peer() - << ", context_cancelled=" << context->IsCancelled() - << ", client_eof=" << client_eof_requested.load() - << ", request_stream_closed=" << request_stream_closed.load() - << ", control_requests=" << control_requests_read.load() - << ", write_ms=" - << std::chrono::duration_cast( - std::chrono::steady_clock::now() - write_started).count() / 1000.0; + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (stream->Write) failed,id=" << dev_id; break; } last_write_duration = std::chrono::duration_cast( @@ -902,15 +829,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte last_write_duration); } } - join_request_reader(); - CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end" - << ", id=" << dev_id - << ", reason=" << exit_reason - << ", peer=" << context->peer() - << ", context_cancelled=" << context->IsCancelled() - << ", client_eof=" << client_eof_requested.load() - << ", request_stream_closed=" << request_stream_closed.load() - << ", control_requests=" << control_requests_read.load(); + CMVR_LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImageStream): end,id=" << dev_id; return grpc::Status::OK; } catch (const exception &e) { diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index cbeeed60..a9ac6a31 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -91,6 +91,37 @@ grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, c return grpc::Status::OK; } +grpc::Status gRPCSystemServiceImpl::ExecuteJsonCommand(grpc::ServerContext* context, + const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) +{ + try { + const std::string& dev_id = request->header().device_id(); + auto dev = dmgr_.getDeviceBase(dev_id); + if (!dev) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message("Device not found: " + dev_id); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } + + std::string response_json; + const bool success = dev->executeJsonCommand(request->request_json(), response_json); + response->mutable_header()->set_success(success); + if (!success) { + response->mutable_header()->set_error_message(response_json); + } + response->set_response_json(response_json); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } + catch (std::exception& e) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +} + grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { diff --git a/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp b/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp new file mode 100644 index 00000000..1d84b942 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_camera_stream_policy_test.cpp @@ -0,0 +1,54 @@ +#include "service/grpc/include/grpc_camera_stream_policy.h" + +#include +#include +#include + +namespace { + +bool check(const bool condition, const char* expression, const int line) { + if (condition) { + return true; + } + std::cerr << "CHECK failed at line " << line << ": " << expression << '\n'; + return false; +} + +#define CHECK_TRUE(expression) \ + do { \ + if (!check(static_cast(expression), #expression, __LINE__)) { \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() { + using namespace std::chrono_literals; + using cmvr::service::cameraFrameAgeNs; + using cmvr::service::cameraFrameExceedsAgeLimit; + using cmvr::service::makeCameraStreamLowLatencyConfig; + + const auto defaults = makeCameraStreamLowLatencyConfig(0, 0); + CHECK_TRUE(defaults.max_pending_frames == 2); + CHECK_TRUE(defaults.max_frame_age == 250ms); + + const auto configured = makeCameraStreamLowLatencyConfig(7, 900); + CHECK_TRUE(configured.max_pending_frames == 7); + CHECK_TRUE(configured.max_frame_age == 900ms); + + CHECK_TRUE(!cameraFrameAgeNs(0, 1'000'000'000ULL).has_value()); + CHECK_TRUE(!cameraFrameAgeNs(2'000'000'000ULL, 1'000'000'000ULL).has_value()); + CHECK_TRUE(cameraFrameAgeNs(1'000'000'000ULL, 1'250'000'000ULL).value() == + 250'000'000ULL); + + CHECK_TRUE(!cameraFrameExceedsAgeLimit( + 1'000'000'000ULL, 1'250'000'000ULL, 250ms)); + CHECK_TRUE(cameraFrameExceedsAgeLimit( + 1'000'000'000ULL, 1'250'000'001ULL, 250ms)); + CHECK_TRUE(!cameraFrameExceedsAgeLimit( + 1'000'000'000ULL, 2'000'000'000ULL, 0ms)); + + std::cout << "grpc_camera_stream_policy_test: PASS\n"; + return 0; +} diff --git a/cmvr-es/service/quic_edge/CMakeLists.txt b/cmvr-es/service/quic_edge/CMakeLists.txt new file mode 100644 index 00000000..6dbe62bd --- /dev/null +++ b/cmvr-es/service/quic_edge/CMakeLists.txt @@ -0,0 +1,28 @@ +find_package(Threads REQUIRED) + +add_library(quic_edge_service STATIC + src/control_framing.cpp + src/datagram_packetizer.cpp + src/quic_edge_types.cpp + src/quic_transport.cpp + src/msquic_transport.cpp + src/quic_edge_service.cpp + src/quic_edge_device_adapter.cpp +) +target_compile_features(quic_edge_service PUBLIC cxx_std_17) +target_include_directories(quic_edge_service PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es) +target_link_libraries(quic_edge_service + PUBLIC + cmvr_es::proto + cmvr_es::media_source_hub + PRIVATE + cmvr_es::device_media_source_adapter + cmvr_es::device_manager + Threads::Threads + msquic +) + +add_library(cmvr_es::quic_edge_service ALIAS quic_edge_service) +install(TARGETS quic_edge_service + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/cmvr-es/service/quic_edge/include/control_framing.h b/cmvr-es/service/quic_edge/include/control_framing.h new file mode 100644 index 00000000..def77614 --- /dev/null +++ b/cmvr-es/service/quic_edge/include/control_framing.h @@ -0,0 +1,54 @@ +#ifndef CMVR_ES_QUIC_EDGE_CONTROL_FRAMING_H +#define CMVR_ES_QUIC_EDGE_CONTROL_FRAMING_H + +#include +#include +#include +#include + +namespace cmvr::quic_edge { + +class ControlFrameEncoder { +public: + static bool encode(const std::uint8_t* payload, + std::size_t payload_size, + std::size_t maximum_payload_size, + std::vector* framed, + std::string* error); + + static bool encode(const std::vector& payload, + std::size_t maximum_payload_size, + std::vector* framed, + std::string* error) + { + return encode(payload.data(), payload.size(), maximum_payload_size, framed, error); + } +}; + +class ControlFrameDecoder { +public: + explicit ControlFrameDecoder(std::size_t maximum_payload_size); + + bool push(const std::uint8_t* data, + std::size_t size, + std::vector>* decoded_frames, + std::string* error); + + bool push(const std::vector& data, + std::vector>* decoded_frames, + std::string* error) + { + return push(data.data(), data.size(), decoded_frames, error); + } + + void reset(); + std::size_t bufferedBytes() const { return buffer_.size(); } + +private: + std::size_t maximum_payload_size_; + std::vector buffer_; +}; + +} // namespace cmvr::quic_edge + +#endif // CMVR_ES_QUIC_EDGE_CONTROL_FRAMING_H diff --git a/cmvr-es/service/quic_edge/include/datagram_packetizer.h b/cmvr-es/service/quic_edge/include/datagram_packetizer.h new file mode 100644 index 00000000..6f0d5be2 --- /dev/null +++ b/cmvr-es/service/quic_edge/include/datagram_packetizer.h @@ -0,0 +1,47 @@ +#ifndef CMVR_ES_QUIC_EDGE_DATAGRAM_PACKETIZER_H +#define CMVR_ES_QUIC_EDGE_DATAGRAM_PACKETIZER_H + +#include +#include +#include +#include + +#include "service/quic_edge/include/quic_edge_types.h" + +namespace cmvr::quic_edge { + +class DatagramPacketizer { +public: + explicit DatagramPacketizer(std::uint64_t initial_packet_sequence = 0); + + bool packetize(const media::MediaFrame& frame, + std::uint32_t wire_track_id, + std::uint32_t codec_generation_token, + std::uint64_t session_epoch, + std::uint64_t wire_frame_sequence, + bool force_discontinuity, + std::size_t maximum_datagram_bytes, + std::vector* packets, + std::string* error); + + static bool decodeHeader(const std::uint8_t* data, + std::size_t size, + DatagramHeader* header, + std::string* error); + + static bool decodeHeader(const std::vector& packet, + DatagramHeader* header, + std::string* error) + { + return decodeHeader(packet.data(), packet.size(), header, error); + } + + std::uint64_t nextPacketSequence() const { return next_packet_sequence_; } + +private: + std::uint64_t next_packet_sequence_; +}; + +} // namespace cmvr::quic_edge + +#endif // CMVR_ES_QUIC_EDGE_DATAGRAM_PACKETIZER_H diff --git a/cmvr-es/service/quic_edge/include/quic_edge_service.h b/cmvr-es/service/quic_edge/include/quic_edge_service.h new file mode 100644 index 00000000..e4618499 --- /dev/null +++ b/cmvr-es/service/quic_edge/include/quic_edge_service.h @@ -0,0 +1,200 @@ +#ifndef CMVR_ES_QUIC_EDGE_SERVICE_H +#define CMVR_ES_QUIC_EDGE_SERVICE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cmvr/config/quic_edge_config/quic_edge_config.pb.h" +#include "devices/device_types.h" +#include "manager/media_source_hub/include/media_source_hub.h" +#include "service/quic_edge/include/control_framing.h" +#include "service/quic_edge/include/datagram_packetizer.h" +#include "service/quic_edge/include/quic_transport.h" + +namespace cmvr::quic_edge { + +enum class QuicEdgeServiceState { + UNINITIALIZED, + STOPPED, + CONNECTING, + REGISTERING, + ONLINE, + BACKOFF, + FAILED, +}; + +const char* toString(QuicEdgeServiceState state); + +struct QuicEdgeStats { + std::uint64_t connection_attempts{0}; + std::uint64_t successful_connections{0}; + std::uint64_t reconnects{0}; + std::uint64_t registrations_sent{0}; + std::uint64_t registrations_accepted{0}; + std::uint64_t registrations_rejected{0}; + std::uint64_t heartbeats_sent{0}; + std::uint64_t heartbeats_acknowledged{0}; + std::uint64_t heartbeat_timeouts{0}; + std::uint64_t protocol_errors{0}; + std::uint64_t media_sessions_opened{0}; + std::uint64_t frames_queued{0}; + std::uint64_t datagrams_queued{0}; + std::uint64_t frames_dropped_source{0}; + std::uint64_t frames_dropped_backpressure{0}; + std::uint64_t frames_dropped_oversize{0}; + std::uint64_t frames_dropped_no_datagram{0}; + std::uint64_t frames_skipped_waiting_keyframe{0}; + std::uint64_t source_errors{0}; +}; + +struct QuicEdgeStatus { + bool registered{false}; + std::string node_id; + std::string boot_id; + std::string session_id; + std::string observed_source_ip; + std::string last_media_error; + std::uint64_t heartbeat_sequence{0}; + std::uint64_t last_heartbeat_ack_unix_ms{0}; + std::size_t active_media_tracks{0}; +}; + +class QuicEdgeService { +public: + using DeviceSnapshotProvider = + std::function; + + explicit QuicEdgeService(config::QuicEdgeConfig config); + QuicEdgeService(config::QuicEdgeConfig config, + std::unique_ptr transport, + media::MediaSourceHub& media_hub, + DeviceSnapshotProvider device_snapshot_provider = {}); + ~QuicEdgeService(); + + QuicEdgeService(const QuicEdgeService&) = delete; + QuicEdgeService& operator=(const QuicEdgeService&) = delete; + + static bool validateConfig(const config::QuicEdgeConfig& config, + std::string* error); + + bool initialize(std::string* error); + bool start(std::string* error); + void stop(); + + QuicEdgeServiceState state() const; + std::string lastError() const; + QuicEdgeStats stats() const; + QuicEdgeStatus status() const; + +private: + using SourceRegistrar = std::function; + + struct ActiveTrack { + config::QuicEdgeTrackConfig config; + std::string source_track_id; + media::MediaSourceHub::Subscription subscription; + std::optional last_description; + bool waiting_for_keyframe{false}; + bool keyframe_requested{false}; + bool next_frame_discontinuous{false}; + }; + + void run(); + void runMedia(); + bool startMediaWorker(std::string* error); + void stopMediaWorker(); + void recordMediaConnectionFailure(const std::string& error); + bool performRegistration(ControlFrameDecoder* decoder, std::string* error); + bool sendNodeRegistration(std::string* error); + bool sendHeartbeat(std::uint64_t sequence, std::string* error); + bool receiveAndDispatchControl(ControlFrameDecoder* decoder, + std::chrono::milliseconds timeout, + bool* received, + std::string* error); + bool dispatchControlFrame(const std::vector& frame, + std::string* error); + bool openMediaSession(std::vector* tracks, + std::string* error); + void refreshMediaTracks(std::vector* tracks); + bool hasEnabledMediaTracks() const; + bool ensureSourceRegistered(const config::QuicEdgeTrackConfig& track, + const std::string& source_track_id, + std::string* error); + bool sendSessionOpen(std::string* error); + bool sendTrackDescription(const MediaTrackDescription& description, + std::string* error); + bool sendControlEnvelope(const std::string& serialized, + std::string* error); + bool processTrack(ActiveTrack* track, bool* sent_anything, + std::string* error); + std::uint64_t shedBufferedFrames(ActiveTrack* track); + void requestKeyFrame(ActiveTrack* track); + std::uint32_t maximumFrameBytes(const ActiveTrack& track) const; + void initializeIdentity(); + void resetConnectionStatus(); + void recordMediaError(const std::string& error); + void setState(QuicEdgeServiceState state, const std::string& error = {}); + bool waitForStop(std::chrono::milliseconds duration); + std::chrono::milliseconds nextBackoff(std::chrono::milliseconds current); + std::chrono::milliseconds jittered(std::chrono::milliseconds base); + std::uint64_t nextSessionEpoch(); + + config::QuicEdgeConfig config_; + std::unique_ptr transport_; + media::MediaSourceHub* media_hub_{nullptr}; + bool using_global_media_hub_{false}; + SourceRegistrar source_registrar_; + DeviceSnapshotProvider device_snapshot_provider_; + + std::mutex lifecycle_mutex_; + std::mutex control_send_mutex_; + mutable std::mutex mutex_; + std::condition_variable stop_cv_; + std::condition_variable media_stop_cv_; + QuicEdgeServiceState state_{QuicEdgeServiceState::UNINITIALIZED}; + std::string last_error_; + std::string last_media_error_; + QuicEdgeStats stats_; + bool registered_{false}; + std::string session_id_; + std::string observed_source_ip_; + std::uint64_t heartbeat_sequence_{0}; + std::uint64_t outstanding_heartbeat_sequence_{0}; + std::uint64_t last_heartbeat_ack_unix_ms_{0}; + std::size_t active_media_tracks_{0}; + bool stop_requested_{false}; + bool media_stop_requested_{false}; + bool media_connection_failed_{false}; + std::string media_connection_error_; + std::thread worker_; + std::thread media_worker_; + + std::string node_id_; + std::string boot_id_; + std::string software_version_; + DatagramPacketizer packetizer_; + std::uint64_t session_epoch_{0}; + std::unordered_map next_wire_frame_sequence_; + std::uint64_t control_message_sequence_{0}; + std::uint64_t inbound_message_sequence_{0}; + bool has_inbound_message_sequence_{false}; + std::uint32_t effective_heartbeat_interval_ms_{0}; + std::chrono::steady_clock::time_point heartbeat_deadline_{}; + std::chrono::steady_clock::time_point next_heartbeat_{}; + std::chrono::steady_clock::time_point next_media_source_retry_{}; + bool media_session_announced_{false}; +}; + +} // namespace cmvr::quic_edge + +#endif // CMVR_ES_QUIC_EDGE_SERVICE_H diff --git a/cmvr-es/service/quic_edge/include/quic_edge_types.h b/cmvr-es/service/quic_edge/include/quic_edge_types.h new file mode 100644 index 00000000..36fda822 --- /dev/null +++ b/cmvr-es/service/quic_edge/include/quic_edge_types.h @@ -0,0 +1,118 @@ +#ifndef CMVR_ES_QUIC_EDGE_TYPES_H +#define CMVR_ES_QUIC_EDGE_TYPES_H + +#include +#include +#include +#include + +#include "common/media/media_frame.h" + +namespace cmvr::quic_edge { + +constexpr std::uint32_t kDatagramMagic = 0x434d5144U; // "CMQD" +constexpr std::uint8_t kProtocolVersion = 1U; +constexpr std::size_t kDatagramHeaderBytes = 64U; + +enum DatagramFlag : std::uint16_t { + DATAGRAM_FLAG_NONE = 0, + DATAGRAM_FLAG_KEY_FRAME = 1U << 0U, + DATAGRAM_FLAG_RESERVED_1 = 1U << 1U, + DATAGRAM_FLAG_DISCONTINUITY = 1U << 2U, +}; + +// All integer fields are serialized in network byte order. codec_generation +// is a compact token for the full 64-bit MediaSourceHub descriptor generation +// announced on the reliable control stream. +struct DatagramHeader { + std::uint8_t protocol_version{kProtocolVersion}; + media::MediaKind kind{media::MediaKind::UNKNOWN}; + std::uint16_t flags{DATAGRAM_FLAG_NONE}; + std::uint16_t fragment_index{0}; + std::uint16_t fragment_count{0}; + std::uint16_t payload_size{0}; + std::uint32_t track_id{0}; + std::uint32_t codec_generation{0}; + std::uint64_t session_epoch{0}; + std::uint64_t packet_sequence{0}; + std::uint64_t frame_sequence{0}; + std::uint64_t capture_timestamp_us{0}; + std::uint32_t frame_size{0}; + std::uint32_t fragment_offset{0}; +}; + +struct DatagramPacket { + DatagramHeader header; + std::vector bytes; +}; + +struct MediaTrackDescription { + std::uint32_t track_id{0}; + std::uint32_t codec_generation_token{0}; + std::string source_track_id; + std::string source_id; + media::MediaKind kind{media::MediaKind::UNKNOWN}; + media::Codec codec{media::Codec::UNKNOWN}; + media::PayloadFormat payload_format{media::PayloadFormat::UNKNOWN}; + std::uint64_t codec_generation{0}; + std::uint32_t width{0}; + std::uint32_t height{0}; + std::uint32_t nominal_rate{0}; + std::uint32_t sample_rate{0}; + std::uint32_t channels{0}; + std::vector codec_config; + + bool operator==(const MediaTrackDescription& other) const + { + return track_id == other.track_id && + codec_generation_token == other.codec_generation_token && + source_track_id == other.source_track_id && + source_id == other.source_id && + kind == other.kind && + codec == other.codec && + payload_format == other.payload_format && + codec_generation == other.codec_generation && + width == other.width && + height == other.height && + nominal_rate == other.nominal_rate && + sample_rate == other.sample_rate && + channels == other.channels && + codec_config == other.codec_config; + } + + bool operator!=(const MediaTrackDescription& other) const + { + return !(*this == other); + } +}; + +std::uint32_t descriptorGenerationToken(std::uint64_t generation); +const char* codecName(media::Codec codec); +const char* payloadFormatName(media::PayloadFormat format); + +inline MediaTrackDescription describeTrack( + const std::uint32_t wire_track_id, + const media::TrackDescriptor& descriptor) +{ + MediaTrackDescription description; + description.track_id = wire_track_id; + description.codec_generation_token = + descriptorGenerationToken(descriptor.generation); + description.source_track_id = descriptor.id; + description.source_id = descriptor.source_id; + description.kind = descriptor.kind; + description.codec = descriptor.codec; + description.payload_format = descriptor.payload_format; + description.codec_generation = descriptor.generation; + description.width = descriptor.width; + description.height = descriptor.height; + description.nominal_rate = descriptor.nominal_rate; + description.sample_rate = descriptor.sample_rate; + description.channels = descriptor.channels; + description.codec_config = descriptor.codec_config; + return description; +} + +} // namespace cmvr::quic_edge + +#endif // CMVR_ES_QUIC_EDGE_TYPES_H diff --git a/cmvr-es/service/quic_edge/include/quic_transport.h b/cmvr-es/service/quic_edge/include/quic_transport.h new file mode 100644 index 00000000..a5301a87 --- /dev/null +++ b/cmvr-es/service/quic_edge/include/quic_transport.h @@ -0,0 +1,73 @@ +#ifndef CMVR_ES_QUIC_EDGE_QUIC_TRANSPORT_H +#define CMVR_ES_QUIC_EDGE_QUIC_TRANSPORT_H + +#include +#include +#include +#include +#include +#include + +#include "cmvr/config/quic_edge_config/quic_edge_config.pb.h" +#include "service/quic_edge/include/quic_edge_types.h" + +namespace cmvr::quic_edge { + +enum class TransportSendResult { + QUEUED, + WOULD_BLOCK, + DISCONNECTED, + ERROR, +}; + +enum class TransportReceiveResult { + DATA, + TIMEOUT, + DISCONNECTED, + ERROR, +}; + +class QuicTransport { +public: + virtual ~QuicTransport() = default; + + virtual bool connect(const config::QuicEdgeConfig& config, + std::chrono::milliseconds timeout, + std::string* error) = 0; + virtual void disconnect() = 0; + virtual bool isConnected() const = 0; + virtual std::size_t maximumDatagramBytes() const = 0; + virtual std::size_t maximumDatagramBatchPackets() const = 0; + + // QUEUED means ownership was accepted. WOULD_BLOCK and synchronous ERROR + // mean no bytes were accepted, so the caller may safely retry the complete + // framed message. DISCONNECTED means the connection is no longer usable. + virtual TransportSendResult sendControl( + std::vector framed_message, + std::string* error) = 0; + + // Returns one raw byte chunk from the reliable control stream. QUIC stream + // callback boundaries are not message boundaries; callers must feed all + // DATA chunks into their framing decoder in order. + // + // The default implementation reports DISCONNECTED so existing transports + // that only support sending remain source-compatible. + virtual TransportReceiveResult receiveControl( + std::vector* chunk, + std::chrono::milliseconds timeout, + std::string* error); + + // Local admission is atomic for the complete batch. A native QUIC API may + // still fail after accepting earlier fragments; in that case the transport + // must reset the connection so the receiver discards the partial session. + virtual TransportSendResult sendDatagramBatch( + std::vector packets, + std::string* error) = 0; +}; + +std::unique_ptr createDefaultQuicTransport( + std::size_t send_queue_depth); + +} // namespace cmvr::quic_edge + +#endif // CMVR_ES_QUIC_EDGE_QUIC_TRANSPORT_H diff --git a/cmvr-es/service/quic_edge/src/control_framing.cpp b/cmvr-es/service/quic_edge/src/control_framing.cpp new file mode 100644 index 00000000..a47f49c0 --- /dev/null +++ b/cmvr-es/service/quic_edge/src/control_framing.cpp @@ -0,0 +1,115 @@ +#include "service/quic_edge/include/control_framing.h" + +#include +#include + +namespace cmvr::quic_edge { +namespace { + +void setError(std::string* error, const std::string& message) +{ + if (error) { + *error = message; + } +} + +std::uint32_t readLength(const std::vector& buffer) +{ + return (static_cast(buffer[0]) << 24U) | + (static_cast(buffer[1]) << 16U) | + (static_cast(buffer[2]) << 8U) | + static_cast(buffer[3]); +} + +} // namespace + +bool ControlFrameEncoder::encode(const std::uint8_t* payload, + const std::size_t payload_size, + const std::size_t maximum_payload_size, + std::vector* framed, + std::string* error) +{ + if (!framed) { + setError(error, "control-frame output is null"); + return false; + } + framed->clear(); + if (!payload || payload_size == 0U) { + setError(error, "control-frame payload is empty"); + return false; + } + if (payload_size > maximum_payload_size || + payload_size > std::numeric_limits::max()) { + setError(error, "control-frame payload exceeds configured limit"); + return false; + } + + const auto length = static_cast(payload_size); + framed->resize(4U + payload_size); + (*framed)[0] = static_cast(length >> 24U); + (*framed)[1] = static_cast(length >> 16U); + (*framed)[2] = static_cast(length >> 8U); + (*framed)[3] = static_cast(length); + std::copy_n(payload, payload_size, framed->data() + 4U); + return true; +} + +ControlFrameDecoder::ControlFrameDecoder(const std::size_t maximum_payload_size) + : maximum_payload_size_(maximum_payload_size) +{ + buffer_.reserve(std::min(maximum_payload_size + 4U, 4096U)); +} + +bool ControlFrameDecoder::push(const std::uint8_t* data, + const std::size_t size, + std::vector>* decoded_frames, + std::string* error) +{ + if (!decoded_frames) { + setError(error, "decoded control-frame output is null"); + return false; + } + if (size != 0U && !data) { + setError(error, "control-frame input is null"); + return false; + } + + std::size_t consumed = 0U; + while (consumed < size) { + if (buffer_.size() < 4U) { + const std::size_t prefix_bytes = + std::min(4U - buffer_.size(), size - consumed); + buffer_.insert(buffer_.end(), data + consumed, + data + consumed + prefix_bytes); + consumed += prefix_bytes; + if (buffer_.size() < 4U) { + break; + } + const std::uint32_t declared = readLength(buffer_); + if (declared == 0U || declared > maximum_payload_size_) { + reset(); + setError(error, "control-frame declared length is invalid"); + return false; + } + } + + const std::size_t total_size = 4U + readLength(buffer_); + const std::size_t bytes_needed = total_size - buffer_.size(); + const std::size_t copied = + std::min(bytes_needed, size - consumed); + buffer_.insert(buffer_.end(), data + consumed, data + consumed + copied); + consumed += copied; + if (buffer_.size() == total_size) { + decoded_frames->emplace_back(buffer_.begin() + 4, buffer_.end()); + buffer_.clear(); + } + } + return true; +} + +void ControlFrameDecoder::reset() +{ + buffer_.clear(); +} + +} // namespace cmvr::quic_edge diff --git a/cmvr-es/service/quic_edge/src/datagram_packetizer.cpp b/cmvr-es/service/quic_edge/src/datagram_packetizer.cpp new file mode 100644 index 00000000..6e8854d8 --- /dev/null +++ b/cmvr-es/service/quic_edge/src/datagram_packetizer.cpp @@ -0,0 +1,270 @@ +#include "service/quic_edge/include/datagram_packetizer.h" + +#include +#include + +namespace cmvr::quic_edge { +namespace { + +void setError(std::string* error, const std::string& message) +{ + if (error) { + *error = message; + } +} + +void writeU16(std::vector* out, const std::size_t offset, + const std::uint16_t value) +{ + (*out)[offset] = static_cast(value >> 8U); + (*out)[offset + 1U] = static_cast(value); +} + +void writeU32(std::vector* out, const std::size_t offset, + const std::uint32_t value) +{ + (*out)[offset] = static_cast(value >> 24U); + (*out)[offset + 1U] = static_cast(value >> 16U); + (*out)[offset + 2U] = static_cast(value >> 8U); + (*out)[offset + 3U] = static_cast(value); +} + +void writeU64(std::vector* out, const std::size_t offset, + const std::uint64_t value) +{ + for (std::size_t i = 0; i < 8U; ++i) { + (*out)[offset + i] = static_cast(value >> (56U - i * 8U)); + } +} + +std::uint16_t readU16(const std::uint8_t* data, const std::size_t offset) +{ + return static_cast( + (static_cast(data[offset]) << 8U) | + static_cast(data[offset + 1U])); +} + +std::uint32_t readU32(const std::uint8_t* data, const std::size_t offset) +{ + return (static_cast(data[offset]) << 24U) | + (static_cast(data[offset + 1U]) << 16U) | + (static_cast(data[offset + 2U]) << 8U) | + static_cast(data[offset + 3U]); +} + +std::uint64_t readU64(const std::uint8_t* data, const std::size_t offset) +{ + std::uint64_t value = 0; + for (std::size_t i = 0; i < 8U; ++i) { + value = (value << 8U) | static_cast(data[offset + i]); + } + return value; +} + +bool validKind(const media::MediaKind kind) +{ + return kind == media::MediaKind::VIDEO || kind == media::MediaKind::AUDIO; +} + +} // namespace + +DatagramPacketizer::DatagramPacketizer(const std::uint64_t initial_packet_sequence) + : next_packet_sequence_(initial_packet_sequence) +{ +} + +bool DatagramPacketizer::packetize(const media::MediaFrame& frame, + const std::uint32_t wire_track_id, + const std::uint32_t codec_generation_token, + const std::uint64_t session_epoch, + const std::uint64_t wire_frame_sequence, + const bool force_discontinuity, + const std::size_t maximum_datagram_bytes, + std::vector* packets, + std::string* error) +{ + if (!packets) { + setError(error, "packet output is null"); + return false; + } + packets->clear(); + if (wire_track_id == 0U) { + setError(error, "track_id must be non-zero"); + return false; + } + if (!frame.descriptor || !validKind(frame.descriptor->kind)) { + setError(error, "unsupported media kind"); + return false; + } + if (codec_generation_token == 0U) { + setError(error, "codec generation token must be non-zero"); + return false; + } + if (session_epoch == 0U) { + setError(error, "session_epoch must be non-zero"); + return false; + } + if (wire_frame_sequence == 0U) { + setError(error, "wire_frame_sequence must be non-zero"); + return false; + } + if (frame.empty()) { + setError(error, "media frame payload is empty"); + return false; + } + if (frame.size() > std::numeric_limits::max()) { + setError(error, "media frame is larger than the v1 frame_size field"); + return false; + } + if (maximum_datagram_bytes <= kDatagramHeaderBytes) { + setError(error, "maximum DATAGRAM size does not leave room for payload"); + return false; + } + + const std::size_t payload_capacity = std::min( + maximum_datagram_bytes - kDatagramHeaderBytes, + std::numeric_limits::max()); + const std::size_t fragment_count = + (frame.size() + payload_capacity - 1U) / payload_capacity; + if (fragment_count == 0U || + fragment_count > std::numeric_limits::max()) { + setError(error, "media frame requires too many DATAGRAM fragments"); + return false; + } + if (next_packet_sequence_ > + std::numeric_limits::max() - fragment_count) { + setError(error, "DATAGRAM packet sequence exhausted"); + return false; + } + + std::uint16_t flags = DATAGRAM_FLAG_NONE; + if (frame.key_frame) { + flags |= DATAGRAM_FLAG_KEY_FRAME; + } + if (frame.discontinuity || force_discontinuity) { + flags |= DATAGRAM_FLAG_DISCONTINUITY; + } + + std::vector encoded_packets; + encoded_packets.reserve(fragment_count); + for (std::size_t i = 0; i < fragment_count; ++i) { + const std::size_t offset = i * payload_capacity; + const std::size_t payload_size = + std::min(payload_capacity, frame.size() - offset); + + DatagramPacket packet; + packet.header.protocol_version = kProtocolVersion; + packet.header.kind = frame.descriptor->kind; + packet.header.flags = flags; + packet.header.fragment_index = static_cast(i); + packet.header.fragment_count = static_cast(fragment_count); + packet.header.payload_size = static_cast(payload_size); + packet.header.track_id = wire_track_id; + packet.header.codec_generation = codec_generation_token; + packet.header.session_epoch = session_epoch; + packet.header.packet_sequence = next_packet_sequence_ + i; + packet.header.frame_sequence = wire_frame_sequence; + packet.header.capture_timestamp_us = frame.capture_time_ns / 1000U; + packet.header.frame_size = static_cast(frame.size()); + packet.header.fragment_offset = static_cast(offset); + + packet.bytes.resize(kDatagramHeaderBytes + payload_size); + writeU32(&packet.bytes, 0U, kDatagramMagic); + packet.bytes[4U] = packet.header.protocol_version; + packet.bytes[5U] = static_cast(packet.header.kind); + writeU16(&packet.bytes, 6U, packet.header.flags); + writeU16(&packet.bytes, 8U, static_cast(kDatagramHeaderBytes)); + writeU16(&packet.bytes, 10U, packet.header.fragment_index); + writeU16(&packet.bytes, 12U, packet.header.fragment_count); + writeU16(&packet.bytes, 14U, packet.header.payload_size); + writeU32(&packet.bytes, 16U, packet.header.track_id); + writeU32(&packet.bytes, 20U, packet.header.codec_generation); + writeU64(&packet.bytes, 24U, packet.header.session_epoch); + writeU64(&packet.bytes, 32U, packet.header.packet_sequence); + writeU64(&packet.bytes, 40U, packet.header.frame_sequence); + writeU64(&packet.bytes, 48U, packet.header.capture_timestamp_us); + writeU32(&packet.bytes, 56U, packet.header.frame_size); + writeU32(&packet.bytes, 60U, packet.header.fragment_offset); + std::copy_n(frame.data() + offset, payload_size, + packet.bytes.data() + kDatagramHeaderBytes); + encoded_packets.push_back(std::move(packet)); + } + + next_packet_sequence_ += fragment_count; + *packets = std::move(encoded_packets); + return true; +} + +bool DatagramPacketizer::decodeHeader(const std::uint8_t* data, + const std::size_t size, + DatagramHeader* header, + std::string* error) +{ + if (!data || !header) { + setError(error, "DATAGRAM input or header output is null"); + return false; + } + if (size < kDatagramHeaderBytes) { + setError(error, "DATAGRAM is shorter than the v1 header"); + return false; + } + if (readU32(data, 0U) != kDatagramMagic) { + setError(error, "DATAGRAM magic mismatch"); + return false; + } + if (data[4U] != kProtocolVersion) { + setError(error, "unsupported DATAGRAM protocol version"); + return false; + } + const auto kind = static_cast(data[5U]); + if (!validKind(kind)) { + setError(error, "unsupported DATAGRAM media kind"); + return false; + } + if (readU16(data, 8U) != kDatagramHeaderBytes) { + setError(error, "unsupported DATAGRAM header size"); + return false; + } + + DatagramHeader decoded; + decoded.protocol_version = data[4U]; + decoded.kind = kind; + decoded.flags = readU16(data, 6U); + decoded.fragment_index = readU16(data, 10U); + decoded.fragment_count = readU16(data, 12U); + decoded.payload_size = readU16(data, 14U); + decoded.track_id = readU32(data, 16U); + decoded.codec_generation = readU32(data, 20U); + decoded.session_epoch = readU64(data, 24U); + decoded.packet_sequence = readU64(data, 32U); + decoded.frame_sequence = readU64(data, 40U); + decoded.capture_timestamp_us = readU64(data, 48U); + decoded.frame_size = readU32(data, 56U); + decoded.fragment_offset = readU32(data, 60U); + + if (decoded.track_id == 0U || decoded.codec_generation == 0U || + decoded.session_epoch == 0U) { + setError(error, "DATAGRAM has a zero track, generation, or session identifier"); + return false; + } + if (decoded.fragment_count == 0U || + decoded.fragment_index >= decoded.fragment_count) { + setError(error, "DATAGRAM fragment index/count is invalid"); + return false; + } + if (decoded.payload_size != size - kDatagramHeaderBytes) { + setError(error, "DATAGRAM payload size does not match packet length"); + return false; + } + if (decoded.frame_size == 0U || decoded.payload_size == 0U || + decoded.fragment_offset > decoded.frame_size || + decoded.payload_size > decoded.frame_size - decoded.fragment_offset) { + setError(error, "DATAGRAM fragment is outside the declared frame"); + return false; + } + + *header = decoded; + return true; +} + +} // namespace cmvr::quic_edge diff --git a/cmvr-es/service/quic_edge/src/msquic_transport.cpp b/cmvr-es/service/quic_edge/src/msquic_transport.cpp new file mode 100644 index 00000000..e45639f0 --- /dev/null +++ b/cmvr-es/service/quic_edge/src/msquic_transport.cpp @@ -0,0 +1,856 @@ +#include "service/quic_edge/include/quic_transport.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::quic_edge { +namespace { + +std::string statusText(const char* operation, const QUIC_STATUS status) +{ + std::ostringstream output; + output << operation << " failed with QUIC_STATUS 0x" << std::hex + << static_cast(status); + return output.str(); +} + +constexpr std::size_t kControlFramePrefixBytes = sizeof(std::uint32_t); +constexpr std::size_t kMinimumControlReceiveQueueBytes = 64U * 1024U; +constexpr std::size_t kMaximumControlReceiveQueueBytes = + 32U * 1024U * 1024U + 2U * kControlFramePrefixBytes; +constexpr std::size_t kMaximumControlReceiveQueueChunks = 4096U; +constexpr std::size_t kMaximumReservedControlSends = 8U; +constexpr std::uint64_t kMaximumHeartbeatIntervalMs = 60ULL * 60ULL * 1000ULL; + +std::size_t reservedControlSends(const std::size_t queue_depth) +{ + if (queue_depth <= 1U) return 0U; + return std::min( + kMaximumReservedControlSends, + std::max(1U, queue_depth / 16U)); +} + +std::size_t controlReceiveQueueBytes(const config::QuicEdgeConfig& config) +{ + // Keep enough room for two maximum-sized framed control messages while + // enforcing a transport-level hard bound independent of peer behavior. + const std::uint64_t desired = + 2ULL * (static_cast( + config.maximum_control_frame_bytes()) + + kControlFramePrefixBytes); + return static_cast(std::clamp( + desired, kMinimumControlReceiveQueueBytes, + kMaximumControlReceiveQueueBytes)); +} + +class MsQuicTransport final : public QuicTransport { +public: + explicit MsQuicTransport(const std::size_t send_queue_depth) + : send_queue_depth_(std::max(1U, send_queue_depth)), + reserved_control_sends_(reservedControlSends(send_queue_depth_)) + { + } + + ~MsQuicTransport() override + { + disconnect(); + if (registration_ && api_) api_->RegistrationClose(registration_); + if (api_) MsQuicClose(api_); + } + + bool connect(const config::QuicEdgeConfig& config, + const std::chrono::milliseconds timeout, + std::string* error) override + { + disconnect(); + QUIC_STATUS status = QUIC_STATUS_SUCCESS; + { + // Configuration creation and connection publication are one API + // operation. A concurrent disconnect can therefore never close a + // just-created configuration before ConnectionStart consumes it. + std::lock_guard operation_lock(api_operation_mutex_); + if (!openApi(error) || !openConfiguration(config, error)) { + return false; + } + { + std::lock_guard lock(mutex_); + shutdown_complete_ = false; + connected_ = false; + datagram_send_enabled_ = false; + maximum_datagram_bytes_ = 0U; + control_stream_start_completed_ = false; + control_stream_started_ = false; + control_stream_shutdown_complete_ = false; + control_stream_receive_closed_ = true; + control_receive_queue_.clear(); + control_receive_queue_bytes_ = 0U; + maximum_control_receive_queue_bytes_ = + controlReceiveQueueBytes(config); + control_receive_error_.clear(); + connect_error_.clear(); + } + HQUIC opened_connection = nullptr; + status = api_->ConnectionOpen( + registration_, &MsQuicTransport::connectionCallback, this, + &opened_connection); + if (QUIC_SUCCEEDED(status)) { + { + std::lock_guard state_lock(mutex_); + connection_ = opened_connection; + } + const std::string& server_name = + config.tls().allow_insecure() || config.tls().server_name().empty() + ? config.server_host() : config.tls().server_name(); + CMVR_LOG(INFO) << "[MsQuicTransport] ConnectionStart" + << ", connect_host=" << server_name + << ", configured_server_host=" << config.server_host() + << ", port=" << config.server_port() + << ", ca_file=" << config.tls().ca_file() + << ", allow_insecure=" << config.tls().allow_insecure(); + status = api_->ConnectionStart( + opened_connection, configuration_, QUIC_ADDRESS_FAMILY_UNSPEC, + server_name.c_str(), + static_cast(config.server_port())); + } + } + if (QUIC_FAILED(status)) { + setError(error, statusText("ConnectionOpen/Start", status)); + closeConnectionHandles(); + return false; + } + + { + std::unique_lock lock(mutex_); + if (!condition_.wait_for(lock, timeout, [this]() { + return connected_ || shutdown_complete_; + }) || !connected_) { + const std::string message = connect_error_.empty() + ? "MsQuic connect timed out" : connect_error_; + lock.unlock(); + disconnect(); + setError(error, message); + return false; + } + } + + bool connection_available = false; + { + std::lock_guard operation_lock(api_operation_mutex_); + HQUIC connection = nullptr; + HQUIC opened_stream = nullptr; + { + std::lock_guard state_lock(mutex_); + connection_available = connected_ && connection_; + connection = connection_; + } + if (connection_available) { + status = api_->StreamOpen( + connection, QUIC_STREAM_OPEN_FLAG_NONE, + &MsQuicTransport::streamCallback, this, &opened_stream); + if (QUIC_SUCCEEDED(status)) { + { + std::lock_guard state_lock(mutex_); + control_stream_ = opened_stream; + control_stream_start_completed_ = false; + control_stream_started_ = false; + control_stream_shutdown_complete_ = false; + control_stream_receive_closed_ = false; + } + status = api_->StreamStart( + opened_stream, QUIC_STREAM_START_FLAG_IMMEDIATE); + } + } + } + if (!connection_available) { + setError(error, "QUIC connection closed before control stream setup"); + disconnect(); + return false; + } + if (QUIC_FAILED(status)) { + { + std::lock_guard lock(mutex_); + control_stream_start_completed_ = true; + control_stream_started_ = false; + } + setError(error, statusText("control StreamOpen/Start", status)); + disconnect(); + return false; + } + + { + std::unique_lock lock(mutex_); + const bool completed = condition_.wait_for(lock, timeout, [this]() { + return control_stream_start_completed_ || !connected_; + }); + if (!completed || !connected_ || !control_stream_started_) { + const std::string message = connect_error_.empty() + ? "QUIC control stream start did not complete successfully" + : connect_error_; + lock.unlock(); + disconnect(); + setError(error, message); + return false; + } + } + return true; + } + + void disconnect() override + { + HQUIC connection = nullptr; + HQUIC stream = nullptr; + bool stream_started = false; + { + // Resolve and use the handle while holding the API-operation lock. + // This prevents a concurrent closeConnectionHandles() call from + // closing the handle between the state read and ConnectionShutdown. + std::lock_guard operation_lock(api_operation_mutex_); + { + std::lock_guard lock(mutex_); + connection = connection_; + stream = control_stream_; + stream_started = control_stream_started_; + connected_ = false; + control_stream_receive_closed_ = true; + ++control_receive_epoch_; + condition_.notify_all(); + } + if (stream && stream_started && api_) { + const auto flags = static_cast( + QUIC_STREAM_SHUTDOWN_FLAG_ABORT_SEND | + QUIC_STREAM_SHUTDOWN_FLAG_ABORT_RECEIVE | + QUIC_STREAM_SHUTDOWN_FLAG_IMMEDIATE); + (void)api_->StreamShutdown(stream, flags, 0); + } + if (connection && api_) { + api_->ConnectionShutdown( + connection, QUIC_CONNECTION_SHUTDOWN_FLAG_NONE, 0); + } + } + if (stream && stream_started && api_) { + std::unique_lock lock(mutex_); + // A started stream handle may only be closed after MsQuic delivers + // SHUTDOWN_COMPLETE. IMMEDIATE schedules that completion without a + // graceful network wait, but it is not guaranteed to run inline. + condition_.wait(lock, [this, stream]() { + return control_stream_ != stream || + control_stream_shutdown_complete_; + }); + } + closeConnectionHandles(); + } + + bool isConnected() const override + { + std::lock_guard lock(mutex_); + return connected_; + } + + std::size_t maximumDatagramBytes() const override + { + std::lock_guard lock(mutex_); + return maximum_datagram_bytes_; + } + + std::size_t maximumDatagramBatchPackets() const override + { + // Static empty-queue limit. sendDatagramBatch performs the real-time + // admission check under mutex and may still return WOULD_BLOCK. + return send_queue_depth_ - reserved_control_sends_; + } + + TransportSendResult sendControl(std::vector message, + std::string* error) override + { + std::lock_guard operation_lock(api_operation_mutex_); + HQUIC stream = nullptr; + { + std::lock_guard lock(mutex_); + if (!connected_ || !control_stream_) return TransportSendResult::DISCONNECTED; + if (pending_send_count_ >= send_queue_depth_) { + return TransportSendResult::WOULD_BLOCK; + } + ++pending_send_count_; + stream = control_stream_; + } + auto* context = new SendContext(this, std::move(message)); + registerSend(context); + const QUIC_STATUS status = api_->StreamSend( + stream, &context->buffer, 1, QUIC_SEND_FLAG_NONE, context); + if (QUIC_FAILED(status)) { + finishSend(context); + setError(error, statusText("StreamSend", status)); + return TransportSendResult::ERROR; + } + return TransportSendResult::QUEUED; + } + + TransportReceiveResult receiveControl( + std::vector* chunk, + const std::chrono::milliseconds timeout, + std::string* error) override + { + if (!chunk) { + setError(error, "control receive output must not be null"); + return TransportReceiveResult::ERROR; + } + chunk->clear(); + if (error) error->clear(); + if (timeout.count() < 0) { + setError(error, "control receive timeout must not be negative"); + return TransportReceiveResult::ERROR; + } + + std::unique_lock lock(mutex_); + const std::uint64_t receive_epoch = control_receive_epoch_; + const auto ready = [this, receive_epoch]() { + return !control_receive_error_.empty() || + !control_receive_queue_.empty() || + control_stream_receive_closed_ || !connected_ || + control_receive_epoch_ != receive_epoch; + }; + if (!ready()) { + bool signaled = false; + if (timeout == std::chrono::milliseconds::max()) { + condition_.wait(lock, ready); + signaled = true; + } else { + signaled = condition_.wait_for(lock, timeout, ready); + } + if (!signaled) return TransportReceiveResult::TIMEOUT; + } + + // A disconnect/reconnect boundary invalidates the old stream decoder + // even if a new connection became ready before this waiter ran. + if (control_receive_epoch_ != receive_epoch) { + setError(error, "QUIC control stream session changed"); + return TransportReceiveResult::DISCONNECTED; + } + if (!control_receive_error_.empty()) { + setError(error, control_receive_error_); + return TransportReceiveResult::ERROR; + } + // Preserve bytes already delivered by MsQuic even when shutdown raced + // with the consumer; report DISCONNECTED only after draining them. + if (!control_receive_queue_.empty()) { + *chunk = std::move(control_receive_queue_.front()); + control_receive_queue_.pop_front(); + control_receive_queue_bytes_ -= chunk->size(); + return TransportReceiveResult::DATA; + } + setError(error, connect_error_.empty() + ? "QUIC control stream is disconnected" : connect_error_); + return TransportReceiveResult::DISCONNECTED; + } + + TransportSendResult sendDatagramBatch( + std::vector packets, + std::string* error) override + { + std::lock_guard operation_lock(api_operation_mutex_); + if (packets.empty()) { + setError(error, "empty MsQuic DATAGRAM batch"); + return TransportSendResult::ERROR; + } + HQUIC connection = nullptr; + { + std::lock_guard lock(mutex_); + if (!connected_ || !connection_) return TransportSendResult::DISCONNECTED; + if (!datagram_send_enabled_) { + setError(error, "peer/path disabled QUIC DATAGRAM sending"); + return TransportSendResult::ERROR; + } + for (const auto& packet : packets) { + if (packet.bytes.empty() || + packet.bytes.size() > maximum_datagram_bytes_) { + setError(error, + "QUIC DATAGRAM exceeds the current path limit"); + return TransportSendResult::ERROR; + } + } + const std::size_t datagram_capacity = + send_queue_depth_ - reserved_control_sends_; + if (pending_send_count_ >= datagram_capacity || + packets.size() > datagram_capacity - pending_send_count_) { + return TransportSendResult::WOULD_BLOCK; + } + pending_send_count_ += packets.size(); + connection = connection_; + } + + std::size_t submitted = 0; + for (auto& packet : packets) { + auto* context = new SendContext(this, std::move(packet.bytes)); + registerSend(context); + const QUIC_STATUS status = api_->DatagramSend( + connection, &context->buffer, 1, QUIC_SEND_FLAG_NONE, context); + if (QUIC_FAILED(status)) { + finishSend(context); + releaseReservedSends(packets.size() - submitted - 1U); + setError(error, statusText("DatagramSend", status)); + // A submission error after earlier fragments is terminal. The + // reconnect/session epoch prevents receivers from combining a + // partial old frame with new traffic. + api_->ConnectionShutdown(connection, + QUIC_CONNECTION_SHUTDOWN_FLAG_SILENT, 1); + return TransportSendResult::ERROR; + } + ++submitted; + } + return TransportSendResult::QUEUED; + } + +private: + struct SendContext { + SendContext(MsQuicTransport* value_owner, std::vector value) + : owner(value_owner), bytes(std::move(value)) + { + buffer.Buffer = bytes.data(); + buffer.Length = static_cast(bytes.size()); + } + MsQuicTransport* owner; + std::vector bytes; + QUIC_BUFFER buffer{}; + }; + + static QUIC_STATUS QUIC_API connectionCallback( + HQUIC connection, void* context, QUIC_CONNECTION_EVENT* event) + { + auto* self = static_cast(context); + switch (event->Type) { + case QUIC_CONNECTION_EVENT_CONNECTED: { + std::lock_guard lock(self->mutex_); + if (connection != self->connection_) break; + self->connected_ = true; + self->condition_.notify_all(); + break; + } + case QUIC_CONNECTION_EVENT_SHUTDOWN_INITIATED_BY_TRANSPORT: { + std::lock_guard lock(self->mutex_); + if (connection != self->connection_) break; + self->connected_ = false; + self->control_stream_receive_closed_ = true; + self->connect_error_ = statusText( + "peer/transport shutdown", + event->SHUTDOWN_INITIATED_BY_TRANSPORT.Status); + self->condition_.notify_all(); + break; + } + case QUIC_CONNECTION_EVENT_SHUTDOWN_INITIATED_BY_PEER: { + std::lock_guard lock(self->mutex_); + if (connection != self->connection_) break; + self->connected_ = false; + self->control_stream_receive_closed_ = true; + self->connect_error_ = "peer closed the QUIC connection"; + self->condition_.notify_all(); + break; + } + case QUIC_CONNECTION_EVENT_SHUTDOWN_COMPLETE: { + std::lock_guard lock(self->mutex_); + if (connection != self->connection_) break; + self->connected_ = false; + self->control_stream_receive_closed_ = true; + self->shutdown_complete_ = true; + self->condition_.notify_all(); + break; + } + case QUIC_CONNECTION_EVENT_DATAGRAM_SEND_STATE_CHANGED: + switch (event->DATAGRAM_SEND_STATE_CHANGED.State) { + case QUIC_DATAGRAM_SEND_LOST_DISCARDED: + case QUIC_DATAGRAM_SEND_ACKNOWLEDGED: + case QUIC_DATAGRAM_SEND_ACKNOWLEDGED_SPURIOUS: + case QUIC_DATAGRAM_SEND_CANCELED: + self->finishSend(static_cast( + event->DATAGRAM_SEND_STATE_CHANGED.ClientContext)); + break; + default: + break; + } + break; + case QUIC_CONNECTION_EVENT_DATAGRAM_STATE_CHANGED: { + std::lock_guard lock(self->mutex_); + if (connection != self->connection_) break; + self->datagram_send_enabled_ = + event->DATAGRAM_STATE_CHANGED.SendEnabled != FALSE; + self->maximum_datagram_bytes_ = self->datagram_send_enabled_ + ? event->DATAGRAM_STATE_CHANGED.MaxSendLength : 0U; + self->condition_.notify_all(); + break; + } + default: + break; + } + return QUIC_STATUS_SUCCESS; + } + + static QUIC_STATUS QUIC_API streamCallback( + HQUIC stream, void* context, QUIC_STREAM_EVENT* event) + { + auto* self = static_cast(context); + switch (event->Type) { + case QUIC_STREAM_EVENT_START_COMPLETE: { + std::lock_guard lock(self->mutex_); + if (stream != self->control_stream_) break; + self->control_stream_start_completed_ = true; + self->control_stream_started_ = + QUIC_SUCCEEDED(event->START_COMPLETE.Status); + if (!self->control_stream_started_) { + self->control_stream_receive_closed_ = true; + self->connect_error_ = statusText( + "control stream start", event->START_COMPLETE.Status); + } + self->condition_.notify_all(); + break; + } + case QUIC_STREAM_EVENT_RECEIVE: + return self->handleControlReceive(stream, event); + case QUIC_STREAM_EVENT_SEND_COMPLETE: + if (event->SEND_COMPLETE.Canceled != FALSE) { + std::lock_guard lock(self->mutex_); + if (stream == self->control_stream_ && self->connected_) { + if (self->connect_error_.empty()) { + self->connect_error_ = + "QUIC control stream send was canceled"; + } + self->control_stream_receive_closed_ = true; + self->connected_ = false; + self->condition_.notify_all(); + } + } + self->finishSend(static_cast( + event->SEND_COMPLETE.ClientContext)); + break; + case QUIC_STREAM_EVENT_PEER_RECEIVE_ABORTED: { + std::lock_guard lock(self->mutex_); + if (stream != self->control_stream_ || !self->connected_) break; + if (self->connect_error_.empty()) { + self->connect_error_ = + "peer aborted the QUIC control stream receive direction"; + } + self->control_stream_receive_closed_ = true; + self->connected_ = false; + self->condition_.notify_all(); + break; + } + case QUIC_STREAM_EVENT_PEER_SEND_SHUTDOWN: + case QUIC_STREAM_EVENT_PEER_SEND_ABORTED: { + std::lock_guard lock(self->mutex_); + if (stream != self->control_stream_) break; + self->control_stream_receive_closed_ = true; + self->condition_.notify_all(); + break; + } + case QUIC_STREAM_EVENT_SHUTDOWN_COMPLETE: { + std::lock_guard lock(self->mutex_); + if (stream != self->control_stream_) break; + self->control_stream_shutdown_complete_ = true; + self->control_stream_receive_closed_ = true; + self->condition_.notify_all(); + break; + } + default: + break; + } + return QUIC_STATUS_SUCCESS; + } + + QUIC_STATUS handleControlReceive( + HQUIC stream, const QUIC_STREAM_EVENT* event) + { + if (event->RECEIVE.BufferCount != 0U && !event->RECEIVE.Buffers) { + return failControlReceive( + stream, "MsQuic supplied a null control receive buffer", + QUIC_STATUS_INVALID_PARAMETER); + } + + std::size_t received_bytes = 0U; + for (std::uint32_t index = 0U; + index < event->RECEIVE.BufferCount; ++index) { + const QUIC_BUFFER& buffer = event->RECEIVE.Buffers[index]; + if (buffer.Length != 0U && !buffer.Buffer) { + return failControlReceive( + stream, "MsQuic supplied a null control receive payload", + QUIC_STATUS_INVALID_PARAMETER); + } + if (buffer.Length > + std::numeric_limits::max() - received_bytes) { + return failControlReceive( + stream, "QUIC control receive byte count overflow", + QUIC_STATUS_BUFFER_TOO_SMALL); + } + received_bytes += buffer.Length; + } + if (received_bytes == 0U) return QUIC_STATUS_SUCCESS; + + std::size_t queue_limit = 0U; + { + std::lock_guard lock(mutex_); + if (stream != control_stream_) return QUIC_STATUS_SUCCESS; + queue_limit = maximum_control_receive_queue_bytes_; + } + if (received_bytes > queue_limit) { + return failControlReceive( + stream, "QUIC control receive chunk exceeds the bounded queue", + QUIC_STATUS_BUFFER_TOO_SMALL); + } + + std::vector chunk; + try { + chunk.resize(received_bytes); + std::size_t offset = 0U; + for (std::uint32_t index = 0U; + index < event->RECEIVE.BufferCount; ++index) { + const QUIC_BUFFER& buffer = event->RECEIVE.Buffers[index]; + std::copy_n(buffer.Buffer, buffer.Length, + chunk.begin() + static_cast(offset)); + offset += buffer.Length; + } + } catch (...) { + return failControlReceive( + stream, "unable to allocate QUIC control receive storage", + QUIC_STATUS_OUT_OF_MEMORY); + } + + { + std::lock_guard lock(mutex_); + if (stream != control_stream_ || + control_stream_receive_closed_ || !connected_) { + return QUIC_STATUS_SUCCESS; + } + if (control_receive_queue_.size() >= + kMaximumControlReceiveQueueChunks || + control_receive_queue_bytes_ > + maximum_control_receive_queue_bytes_ - received_bytes) { + control_receive_error_ = + "QUIC control receive queue capacity exceeded"; + control_stream_receive_closed_ = true; + connected_ = false; + condition_.notify_all(); + return QUIC_STATUS_BUFFER_TOO_SMALL; + } + try { + control_receive_queue_.push_back(std::move(chunk)); + } catch (...) { + control_receive_error_ = + "unable to enqueue QUIC control receive bytes"; + control_stream_receive_closed_ = true; + connected_ = false; + condition_.notify_all(); + return QUIC_STATUS_OUT_OF_MEMORY; + } + control_receive_queue_bytes_ += received_bytes; + condition_.notify_all(); + } + // Returning success consumes every buffer synchronously. Their bytes + // are now owned by control_receive_queue_. + return QUIC_STATUS_SUCCESS; + } + + QUIC_STATUS failControlReceive( + HQUIC stream, const char* message, const QUIC_STATUS status) + { + std::lock_guard lock(mutex_); + if (stream != control_stream_) return QUIC_STATUS_SUCCESS; + control_receive_error_ = message; + control_stream_receive_closed_ = true; + connected_ = false; + condition_.notify_all(); + return status; + } + + bool openApi(std::string* error) + { + if (api_) return true; + QUIC_STATUS status = MsQuicOpen2(&api_); + if (QUIC_FAILED(status)) { + setError(error, statusText("MsQuicOpen2", status)); + return false; + } + const QUIC_REGISTRATION_CONFIG registration_config = { + "cmvr-es-quic-edge", QUIC_EXECUTION_PROFILE_LOW_LATENCY}; + status = api_->RegistrationOpen(®istration_config, ®istration_); + if (QUIC_FAILED(status)) { + setError(error, statusText("RegistrationOpen", status)); + MsQuicClose(api_); + api_ = nullptr; + return false; + } + return true; + } + + bool openConfiguration(const config::QuicEdgeConfig& config, + std::string* error) + { + QUIC_BUFFER alpn{}; + alpn.Buffer = reinterpret_cast( + const_cast(config.alpn().data())); + alpn.Length = static_cast(config.alpn().size()); + QUIC_SETTINGS settings{}; + settings.IsSet.DatagramReceiveEnabled = TRUE; + settings.DatagramReceiveEnabled = TRUE; + settings.IsSet.IdleTimeoutMs = TRUE; + const std::uint64_t heartbeat_safe_idle_timeout = + kMaximumHeartbeatIntervalMs + + 2ULL * config.control_response_timeout_ms() + 60000ULL; + settings.IdleTimeoutMs = std::max( + heartbeat_safe_idle_timeout, + std::max( + 30000ULL, config.reconnect().connect_timeout_ms() * 3ULL)); + QUIC_STATUS status = api_->ConfigurationOpen( + registration_, &alpn, 1, &settings, sizeof(settings), + nullptr, &configuration_); + if (QUIC_FAILED(status)) { + setError(error, statusText("ConfigurationOpen", status)); + return false; + } + + QUIC_CREDENTIAL_CONFIG credential{}; + credential.Flags = QUIC_CREDENTIAL_FLAG_CLIENT; + if (config.tls().allow_insecure()) { + credential.Flags = static_cast( + credential.Flags | QUIC_CREDENTIAL_FLAG_NO_CERTIFICATE_VALIDATION); + } else { + credential.Flags = static_cast( + credential.Flags | QUIC_CREDENTIAL_FLAG_SET_CA_CERTIFICATE_FILE); + credential.CaCertificateFile = config.tls().ca_file().c_str(); + } + QUIC_CERTIFICATE_FILE certificate{}; + if (!config.tls().certificate_file().empty()) { + certificate.CertificateFile = config.tls().certificate_file().c_str(); + certificate.PrivateKeyFile = config.tls().private_key_file().c_str(); + credential.Type = QUIC_CREDENTIAL_TYPE_CERTIFICATE_FILE; + credential.CertificateFile = &certificate; + } else { + credential.Type = QUIC_CREDENTIAL_TYPE_NONE; + } + status = api_->ConfigurationLoadCredential(configuration_, &credential); + if (QUIC_FAILED(status)) { + setError(error, statusText("ConfigurationLoadCredential", status)); + api_->ConfigurationClose(configuration_); + configuration_ = nullptr; + return false; + } + return true; + } + + void closeConnectionHandles() + { + std::lock_guard operation_lock(api_operation_mutex_); + HQUIC stream = nullptr; + HQUIC connection = nullptr; + HQUIC configuration = nullptr; + { + std::lock_guard lock(mutex_); + stream = std::exchange(control_stream_, nullptr); + connection = std::exchange(connection_, nullptr); + configuration = std::exchange(configuration_, nullptr); + connected_ = false; + datagram_send_enabled_ = false; + maximum_datagram_bytes_ = 0U; + control_stream_start_completed_ = false; + control_stream_started_ = false; + control_stream_shutdown_complete_ = false; + control_stream_receive_closed_ = true; + ++control_receive_epoch_; + condition_.notify_all(); + } + if (!api_) return; + if (stream) api_->StreamClose(stream); + if (connection) api_->ConnectionClose(connection); + if (configuration) api_->ConfigurationClose(configuration); + std::vector abandoned_sends; + { + // ConnectionClose/StreamClose are the final handle calls. Once + // they return, no send callback may still reference these + // contexts, so any missing completion can be reclaimed safely. + std::lock_guard lock(mutex_); + abandoned_sends.assign(outstanding_sends_.begin(), + outstanding_sends_.end()); + outstanding_sends_.clear(); + pending_send_count_ = 0U; + } + for (auto* context : abandoned_sends) delete context; + } + + void finishSend(SendContext* context) + { + if (!context) return; + bool owned = false; + { + std::lock_guard lock(mutex_); + owned = outstanding_sends_.erase(context) != 0U; + if (owned && pending_send_count_ != 0U) --pending_send_count_; + condition_.notify_all(); + } + if (owned) delete context; + } + + void registerSend(SendContext* context) + { + std::lock_guard lock(mutex_); + outstanding_sends_.insert(context); + } + + void releaseReservedSends(const std::size_t count) + { + std::lock_guard lock(mutex_); + pending_send_count_ = count > pending_send_count_ + ? 0U : pending_send_count_ - count; + condition_.notify_all(); + } + + static void setError(std::string* error, const std::string& message) + { + if (error) *error = message; + } + + const std::size_t send_queue_depth_; + const std::size_t reserved_control_sends_; + const QUIC_API_TABLE* api_{nullptr}; + HQUIC registration_{nullptr}; + HQUIC configuration_{nullptr}; + HQUIC connection_{nullptr}; + HQUIC control_stream_{nullptr}; + + mutable std::mutex api_operation_mutex_; + mutable std::mutex mutex_; + std::condition_variable condition_; + bool connected_{false}; + bool shutdown_complete_{false}; + bool datagram_send_enabled_{false}; + bool control_stream_start_completed_{false}; + bool control_stream_started_{false}; + bool control_stream_shutdown_complete_{false}; + bool control_stream_receive_closed_{true}; + std::size_t maximum_datagram_bytes_{0}; + std::size_t maximum_control_receive_queue_bytes_{ + kMinimumControlReceiveQueueBytes}; + std::size_t control_receive_queue_bytes_{0}; + std::uint64_t control_receive_epoch_{0}; + std::size_t pending_send_count_{0}; + std::deque> control_receive_queue_; + std::unordered_set outstanding_sends_; + std::string control_receive_error_; + std::string connect_error_; +}; + +} // namespace + +std::unique_ptr createMsQuicTransport( + const std::size_t send_queue_depth) +{ + return std::make_unique(send_queue_depth); +} + +} // namespace cmvr::quic_edge diff --git a/cmvr-es/service/quic_edge/src/quic_edge_device_adapter.cpp b/cmvr-es/service/quic_edge/src/quic_edge_device_adapter.cpp new file mode 100644 index 00000000..856535dc --- /dev/null +++ b/cmvr-es/service/quic_edge/src/quic_edge_device_adapter.cpp @@ -0,0 +1,69 @@ +#include "service/quic_edge/include/quic_edge_service.h" + +#include + +#include "manager/device_manager/include/device_manager.h" +#include "manager/media_source_hub/include/device_media_source_adapter.h" + +namespace cmvr::quic_edge { + +QuicEdgeService::QuicEdgeService(config::QuicEdgeConfig config) + : config_(std::move(config)), + transport_(createDefaultQuicTransport(config_.datagram_send_queue_depth())), + media_hub_(&media::globalMediaSourceHub()), + using_global_media_hub_(true), + device_snapshot_provider_([] { + return device::DeviceManager::getInstance().snapshot(); + }) +{ + initializeIdentity(); + source_registrar_ = [this]( + const config::QuicEdgeTrackConfig& track, + const std::string& source_track_id, + std::string* error) { + auto& manager = device::DeviceManager::getInstance(); + bool registered = false; + switch (track.source_kind()) { + case config::QuicEdgeTrackConfig::SOURCE_KIND_CAMERA: { + auto camera = + manager.getDevice(track.device_id()); + if (!camera) { + if (error) { + *error = "camera unavailable for QUIC edge: " + + track.device_id(); + } + return false; + } + registered = media::ensureCameraMediaSource(*media_hub_, camera); + break; + } + case config::QuicEdgeTrackConfig::SOURCE_KIND_MICROPHONE: { + auto microphone = manager.getDevice( + track.device_id()); + if (!microphone) { + if (error) { + *error = "microphone unavailable for QUIC edge: " + + track.device_id(); + } + return false; + } + registered = media::ensureMicrophoneMediaSource( + *media_hub_, microphone); + break; + } + case config::QuicEdgeTrackConfig::SOURCE_KIND_UNSPECIFIED: + if (error) *error = "unsupported QUIC edge source kind"; + return false; + } + if (!registered && !media_hub_->hasSource(source_track_id)) { + if (error) { + *error = "failed to register MediaSourceHub source: " + + source_track_id; + } + return false; + } + return true; + }; +} + +} // namespace cmvr::quic_edge diff --git a/cmvr-es/service/quic_edge/src/quic_edge_service.cpp b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp new file mode 100644 index 00000000..bd513457 --- /dev/null +++ b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp @@ -0,0 +1,1871 @@ +#include "service/quic_edge/include/quic_edge_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "cmvr/quic_edge/v1/quic_edge.pb.h" +#include "common/base/logging/logger.h" + +namespace cmvr::quic_edge { +namespace { + +constexpr std::uint32_t kMinimumDatagramBytes = + static_cast(kDatagramHeaderBytes + 1U); +constexpr std::uint32_t kMaximumDatagramBytes = 65527U; +constexpr std::uint32_t kMaximumControlBytes = 16U * 1024U * 1024U; +constexpr std::uint32_t kMaximumConfiguredFrameBytes = 256U * 1024U * 1024U; +constexpr std::size_t kMaximumDrainPerPoll = 4096U; +constexpr std::size_t kMaximumDeviceErrorBytes = 512U; +constexpr std::uint32_t kMinimumHeartbeatIntervalMs = 250U; +constexpr std::uint32_t kMaximumHeartbeatIntervalMs = 60U * 60U * 1000U; +constexpr auto kControlPollInterval = std::chrono::milliseconds(50); +constexpr auto kMediaSourceRetryInterval = std::chrono::seconds(1); +constexpr std::size_t kMaximumReservedControlSends = 8U; + +void setError(std::string* error, const std::string& message) +{ + if (error) { + *error = message; + } +} + +std::string boundedDeviceError(std::string message) +{ + if (message.size() <= kMaximumDeviceErrorBytes) return message; + std::size_t end = kMaximumDeviceErrorBytes; + while (end > 0U && + (static_cast(message[end]) & 0xc0U) == 0x80U) { + --end; + } + message.resize(end); + return message; +} + +std::string trim(std::string value) +{ + const auto first = value.find_first_not_of(" \t\r\n"); + if (first == std::string::npos) return {}; + const auto last = value.find_last_not_of(" \t\r\n"); + return value.substr(first, last - first + 1U); +} + +std::string readTextFile(const std::string& path) +{ + std::ifstream input(path, std::ios::in | std::ios::binary); + if (!input) return {}; + std::ostringstream contents; + contents << input.rdbuf(); + return input.good() || input.eof() ? contents.str() : std::string{}; +} + +std::string randomBootId() +{ + std::random_device random_device; + std::mt19937_64 generator(random_device()); + std::uniform_int_distribution distribution; + const std::uint64_t high = distribution(generator); + const std::uint64_t low = distribution(generator); + std::ostringstream stream; + stream << std::hex << std::setfill('0') + << std::setw(8) << static_cast(high >> 32U) << '-' + << std::setw(4) << static_cast(high >> 16U) << '-' + << std::setw(4) << static_cast(high) << '-' + << std::setw(4) << static_cast(low >> 48U) << '-' + << std::setw(12) << (low & 0x0000ffffffffffffULL); + return stream.str(); +} + +std::string resolveNodeId(const std::string& configured_node_id) +{ + if (!configured_node_id.empty() && configured_node_id != "auto") { + return configured_node_id; + } + std::array hostname{}; + if (::gethostname(hostname.data(), hostname.size() - 1U) == 0) { + hostname.back() = '\0'; + const std::string resolved = trim(hostname.data()); + if (!resolved.empty()) return resolved; + } + return "cmvr-es"; +} + +std::string resolveBootId() +{ + const std::string kernel_boot_id = + trim(readTextFile("/proc/sys/kernel/random/boot_id")); + return kernel_boot_id.empty() ? randomBootId() : kernel_boot_id; +} + +std::uint64_t unixTimeMs() +{ + const auto now = std::chrono::system_clock::now().time_since_epoch(); + return static_cast( + std::chrono::duration_cast(now).count()); +} + +std::vector collectInterfaceAddresses( + const bool include_loopback) +{ + std::vector addresses; + ifaddrs* interface_list = nullptr; + if (::getifaddrs(&interface_list) != 0 || interface_list == nullptr) { + return addresses; + } + + std::set seen; + for (const ifaddrs* current = interface_list; + current != nullptr; current = current->ifa_next) { + if (!current->ifa_addr || !current->ifa_name || + (current->ifa_flags & IFF_UP) == 0) { + continue; + } + const bool loopback = (current->ifa_flags & IFF_LOOPBACK) != 0; + if (loopback && !include_loopback) continue; + + const int family = current->ifa_addr->sa_family; + std::array buffer{}; + const void* source = nullptr; + v1::NetworkInterfaceAddress::AddressFamily proto_family = + v1::NetworkInterfaceAddress::ADDRESS_FAMILY_UNSPECIFIED; + if (family == AF_INET) { + source = &reinterpret_cast( + current->ifa_addr)->sin_addr; + proto_family = v1::NetworkInterfaceAddress::ADDRESS_FAMILY_IPV4; + } else if (family == AF_INET6) { + source = &reinterpret_cast( + current->ifa_addr)->sin6_addr; + proto_family = v1::NetworkInterfaceAddress::ADDRESS_FAMILY_IPV6; + } else { + continue; + } + if (::inet_ntop(family, source, buffer.data(), buffer.size()) == nullptr) { + continue; + } + const std::string ip_address(buffer.data()); + if (ip_address.empty() || ip_address == "0.0.0.0" || ip_address == "::") { + continue; + } + const std::string key = std::string(current->ifa_name) + '\n' + ip_address; + if (!seen.insert(key).second) continue; + + v1::NetworkInterfaceAddress address; + address.set_interface_name(current->ifa_name); + address.set_ip_address(ip_address); + address.set_family(proto_family); + address.set_loopback(loopback); + addresses.push_back(std::move(address)); + } + ::freeifaddrs(interface_list); + + std::sort(addresses.begin(), addresses.end(), + [](const v1::NetworkInterfaceAddress& lhs, + const v1::NetworkInterfaceAddress& rhs) { + if (lhs.loopback() != rhs.loopback()) return !lhs.loopback(); + if (lhs.interface_name() != rhs.interface_name()) { + return lhs.interface_name() < rhs.interface_name(); + } + if (lhs.family() != rhs.family()) return lhs.family() < rhs.family(); + return lhs.ip_address() < rhs.ip_address(); + }); + return addresses; +} + +std::string advertisedGrpcHost( + const config::QuicEdgeConfig& config, + const std::vector& interfaces) +{ + if (!config.grpc_endpoint_host().empty() && + config.grpc_endpoint_host() != "auto") { + return config.grpc_endpoint_host(); + } + for (const auto& address : interfaces) { + if (!address.loopback() && + address.family() == v1::NetworkInterfaceAddress::ADDRESS_FAMILY_IPV4) { + return address.ip_address(); + } + } + for (const auto& address : interfaces) { + if (!address.loopback()) return address.ip_address(); + } + return interfaces.empty() ? "127.0.0.1" : interfaces.front().ip_address(); +} + +void populateDescriptor(const config::QuicEdgeConfig& config, + const std::string& node_id, + const std::string& boot_id, + const std::string& software_version, + v1::NodeDescriptor* descriptor) +{ + if (!descriptor) return; + descriptor->set_node_id(node_id); + descriptor->set_boot_id(boot_id); + descriptor->set_software_version(software_version); + const auto interfaces = + collectInterfaceAddresses(config.include_loopback_interfaces()); + for (const auto& address : interfaces) { + *descriptor->add_local_interfaces() = address; + } + auto* endpoint = descriptor->mutable_grpc_endpoint(); + endpoint->set_host(advertisedGrpcHost(config, interfaces)); + endpoint->set_port(config.grpc_endpoint_port()); + endpoint->set_tls(config.grpc_endpoint_tls()); +} + +void populateHeartbeatNetwork(const config::QuicEdgeConfig& config, + v1::NodeHeartbeat* heartbeat) +{ + if (!heartbeat) return; + const auto interfaces = + collectInterfaceAddresses(config.include_loopback_interfaces()); + for (const auto& address : interfaces) { + *heartbeat->add_local_interfaces() = address; + } + auto* endpoint = heartbeat->mutable_grpc_endpoint(); + endpoint->set_host(advertisedGrpcHost(config, interfaces)); + endpoint->set_port(config.grpc_endpoint_port()); + endpoint->set_tls(config.grpc_endpoint_tls()); +} + +v1::DeviceKind toProtoDeviceKind(const device::DeviceKind kind) +{ + switch (kind) { + case device::DeviceKind::AGV: return v1::DEVICE_KIND_AGV; + case device::DeviceKind::Arm: return v1::DEVICE_KIND_ARM; + case device::DeviceKind::Battery: return v1::DEVICE_KIND_BATTERY; + case device::DeviceKind::BioHead: return v1::DEVICE_KIND_BIO_HEAD; + case device::DeviceKind::Camera: return v1::DEVICE_KIND_CAMERA; + case device::DeviceKind::CanBus: return v1::DEVICE_KIND_CAN_BUS; + case device::DeviceKind::DexHand: return v1::DEVICE_KIND_DEX_HAND; + case device::DeviceKind::Gripper: return v1::DEVICE_KIND_GRIPPER; + case device::DeviceKind::Microphone: return v1::DEVICE_KIND_MICROPHONE; + case device::DeviceKind::Motor: return v1::DEVICE_KIND_MOTOR; + case device::DeviceKind::MotorSystem: + return v1::DEVICE_KIND_MOTOR_SYSTEM; + case device::DeviceKind::Robot: return v1::DEVICE_KIND_ROBOT; + case device::DeviceKind::Speaker: return v1::DEVICE_KIND_SPEAKER; + case device::DeviceKind::Unknown: break; + } + return v1::DEVICE_KIND_UNSPECIFIED; +} + +v1::ManagedDeviceState toProtoManagedDeviceState( + const device::ManagedDeviceState state) +{ + switch (state) { + case device::ManagedDeviceState::Disabled: + return v1::MANAGED_DEVICE_STATE_DISABLED; + case device::ManagedDeviceState::Initializing: + return v1::MANAGED_DEVICE_STATE_INITIALIZING; + case device::ManagedDeviceState::Registered: + return v1::MANAGED_DEVICE_STATE_REGISTERED; + case device::ManagedDeviceState::Ready: + return v1::MANAGED_DEVICE_STATE_READY; + case device::ManagedDeviceState::Running: + return v1::MANAGED_DEVICE_STATE_RUNNING; + case device::ManagedDeviceState::Stopped: + return v1::MANAGED_DEVICE_STATE_STOPPED; + case device::ManagedDeviceState::Error: + return v1::MANAGED_DEVICE_STATE_ERROR; + case device::ManagedDeviceState::Unknown: + break; + } + return v1::MANAGED_DEVICE_STATE_UNSPECIFIED; +} + +v1::DeviceHealthStatus toProtoDeviceHealthStatus( + const device::DeviceHealthState state) +{ + switch (state) { + case device::DeviceHealthState::Healthy: + return v1::DEVICE_HEALTH_STATUS_HEALTHY; + case device::DeviceHealthState::Degraded: + return v1::DEVICE_HEALTH_STATUS_DEGRADED; + case device::DeviceHealthState::Fault: + return v1::DEVICE_HEALTH_STATUS_FAULT; + case device::DeviceHealthState::Unknown: + break; + } + return v1::DEVICE_HEALTH_STATUS_UNSPECIFIED; +} + +void populateDeviceManagerSnapshot( + const device::DeviceManagerSnapshot& source, + const std::uint64_t sampled_at_unix_ms, + v1::DeviceManagerSnapshot* destination) +{ + if (!destination) return; + destination->set_manager_name(source.name); + destination->set_manager_version(source.version); + destination->set_manager_description(source.description); + destination->set_sampled_at_unix_ms(sampled_at_unix_ms); + std::vector ordered_devices; + ordered_devices.reserve(source.devices.size()); + for (const auto& source_device : source.devices) { + // DeviceManager keeps disabled entries for local configuration and + // diagnostics, but the platform heartbeat only advertises devices + // that are enabled on this edge node. Enabled entries remain visible + // even when creation, initialization or start has failed. + if (!source_device.enabled) continue; + ordered_devices.push_back(&source_device); + } + std::sort( + ordered_devices.begin(), ordered_devices.end(), + [](const auto* lhs, const auto* rhs) { + if (lhs->id != rhs->id) return lhs->id < rhs->id; + if (lhs->kind != rhs->kind) return lhs->kind < rhs->kind; + return lhs->type_name < rhs->type_name; + }); + + for (const auto* source_device : ordered_devices) { + auto* destination_device = destination->add_devices(); + destination_device->set_device_id(source_device->id); + destination_device->set_kind(toProtoDeviceKind(source_device->kind)); + destination_device->set_type_name(source_device->type_name); + destination_device->set_enabled(source_device->enabled); + destination_device->set_manager_state( + toProtoManagedDeviceState(source_device->state)); + destination_device->set_health( + toProtoDeviceHealthStatus(source_device->health.state)); + destination_device->set_has_error(source_device->abnormal); + destination_device->set_error_message(boundedDeviceError( + source_device->error_message.empty() + ? source_device->health.error_message + : source_device->error_message)); + destination_device->set_status_updated_at_unix_ms( + source_device->status_updated_at_unix_ms); + } +} + +v1::MediaKind toProtoKind(const media::MediaKind kind) +{ + switch (kind) { + case media::MediaKind::VIDEO: return v1::MEDIA_KIND_VIDEO; + case media::MediaKind::AUDIO: return v1::MEDIA_KIND_AUDIO; + case media::MediaKind::UNKNOWN: break; + } + return v1::MEDIA_KIND_UNSPECIFIED; +} + +media::MediaKind expectedKind(const config::QuicEdgeTrackConfig& track) +{ + switch (track.source_kind()) { + case config::QuicEdgeTrackConfig::SOURCE_KIND_CAMERA: + return media::MediaKind::VIDEO; + case config::QuicEdgeTrackConfig::SOURCE_KIND_MICROPHONE: + return media::MediaKind::AUDIO; + case config::QuicEdgeTrackConfig::SOURCE_KIND_UNSPECIFIED: + break; + } + return media::MediaKind::UNKNOWN; +} + +std::string sourceTrackId(const config::QuicEdgeTrackConfig& track) +{ + if (!track.source_track_id().empty()) { + return track.source_track_id(); + } + switch (track.source_kind()) { + case config::QuicEdgeTrackConfig::SOURCE_KIND_CAMERA: + return track.device_id() + "/video/color"; + case config::QuicEdgeTrackConfig::SOURCE_KIND_MICROPHONE: + return track.device_id() + "/audio/main"; + case config::QuicEdgeTrackConfig::SOURCE_KIND_UNSPECIFIED: + break; + } + return {}; +} + +std::size_t reservedControlSendSlots(const std::size_t queue_depth) +{ + if (queue_depth <= 1U) return 0U; + return std::min( + kMaximumReservedControlSends, + std::max(1U, queue_depth / 16U)); +} + +} // namespace + +const char* toString(const QuicEdgeServiceState state) +{ + switch (state) { + case QuicEdgeServiceState::UNINITIALIZED: return "UNINITIALIZED"; + case QuicEdgeServiceState::STOPPED: return "STOPPED"; + case QuicEdgeServiceState::CONNECTING: return "CONNECTING"; + case QuicEdgeServiceState::REGISTERING: return "REGISTERING"; + case QuicEdgeServiceState::ONLINE: return "ONLINE"; + case QuicEdgeServiceState::BACKOFF: return "BACKOFF"; + case QuicEdgeServiceState::FAILED: return "FAILED"; + } + return "UNKNOWN"; +} + +QuicEdgeService::QuicEdgeService(config::QuicEdgeConfig config, + std::unique_ptr transport, + media::MediaSourceHub& media_hub, + DeviceSnapshotProvider device_snapshot_provider) + : config_(std::move(config)), + transport_(std::move(transport)), + media_hub_(&media_hub), + device_snapshot_provider_(std::move(device_snapshot_provider)) +{ + initializeIdentity(); +} + +QuicEdgeService::~QuicEdgeService() +{ + stop(); +} + +bool QuicEdgeService::validateConfig(const config::QuicEdgeConfig& config, + std::string* error) +{ + if (config.id().empty()) { + setError(error, "QUIC edge task id is empty"); + return false; + } + if (config.server_host().empty()) { + setError(error, "QUIC edge server_host is empty"); + return false; + } + if (config.server_port() == 0U || config.server_port() > 65535U) { + setError(error, "QUIC edge server_port must be in [1, 65535]"); + return false; + } + if (config.alpn().empty() || config.alpn().size() > 255U) { + setError(error, "QUIC edge ALPN must contain 1 to 255 bytes"); + return false; + } + if (!config.has_tls()) { + setError(error, "QUIC edge TLS configuration is missing"); + return false; + } + if (!config.tls().allow_insecure() && + (config.tls().ca_file().empty() || config.tls().server_name().empty())) { + setError(error, "secure QUIC edge TLS requires ca_file and server_name"); + return false; + } + if (!config.tls().allow_insecure() && + config.tls().server_name() != config.server_host()) { + setError(error, + "QUIC edge v1 requires tls.server_name to match server_host"); + return false; + } + const bool has_certificate = !config.tls().certificate_file().empty(); + const bool has_private_key = !config.tls().private_key_file().empty(); + if (has_certificate != has_private_key) { + setError(error, "QUIC edge client certificate and private key must be configured together"); + return false; + } + if (!config.has_reconnect() || config.reconnect().initial_delay_ms() == 0U || + config.reconnect().maximum_delay_ms() < config.reconnect().initial_delay_ms() || + config.reconnect().connect_timeout_ms() == 0U) { + setError(error, "QUIC edge reconnect configuration is invalid"); + return false; + } + if (!std::isfinite(config.reconnect().multiplier()) || + config.reconnect().multiplier() < 1.0 || + config.reconnect().jitter_percent() > 100U) { + setError(error, "QUIC edge reconnect multiplier or jitter is invalid"); + return false; + } + if (config.maximum_datagram_bytes() < kMinimumDatagramBytes || + config.maximum_datagram_bytes() > kMaximumDatagramBytes) { + setError(error, "QUIC edge maximum_datagram_bytes is outside the v1 range"); + return false; + } + if (config.maximum_control_frame_bytes() == 0U || + config.maximum_control_frame_bytes() > kMaximumControlBytes) { + setError(error, "QUIC edge maximum_control_frame_bytes is invalid"); + return false; + } + if (config.maximum_frame_bytes() == 0U || + config.maximum_frame_bytes() > kMaximumConfiguredFrameBytes) { + setError(error, "QUIC edge maximum_frame_bytes is invalid"); + return false; + } + const std::uint64_t fragment_payload = + config.maximum_datagram_bytes() - kDatagramHeaderBytes; + if (config.maximum_frame_bytes() > fragment_payload * 65535U) { + setError(error, "QUIC edge maximum_frame_bytes exceeds v1 fragment capacity"); + return false; + } + if (config.datagram_send_queue_depth() < 2U) { + setError(error, "QUIC edge datagram_send_queue_depth must be at least 2"); + return false; + } + const bool has_enabled_media = std::any_of( + config.tracks().begin(), config.tracks().end(), + [](const config::QuicEdgeTrackConfig& track) { return track.enable(); }); + const std::uint64_t media_send_slots = + config.datagram_send_queue_depth() - + reservedControlSendSlots(config.datagram_send_queue_depth()); + if (has_enabled_media && + config.maximum_frame_bytes() > fragment_payload * media_send_slots) { + setError(error, + "maximum_frame_bytes exceeds the atomic DATAGRAM send-queue capacity"); + return false; + } + if (config.media_poll_interval_ms() == 0U || + config.media_poll_interval_ms() > 1000U) { + setError(error, "QUIC edge media_poll_interval_ms must be in [1, 1000]"); + return false; + } + if (config.grpc_endpoint_port() == 0U || + config.grpc_endpoint_port() > 65535U) { + setError(error, "advertised gRPC endpoint port must be in [1, 65535]"); + return false; + } + if (config.heartbeat_interval_ms() < kMinimumHeartbeatIntervalMs || + config.heartbeat_interval_ms() > kMaximumHeartbeatIntervalMs) { + setError(error, "heartbeat_interval_ms must be in [250, 3600000]"); + return false; + } + if (config.control_response_timeout_ms() == 0U || + config.control_response_timeout_ms() > kMaximumHeartbeatIntervalMs) { + setError(error, "control_response_timeout_ms must be in [1, 3600000]"); + return false; + } + + std::unordered_set wire_track_ids; + std::unordered_set source_track_ids; + for (const auto& track : config.tracks()) { + if (!track.enable()) { + continue; + } + if (track.track_id() == 0U || + !wire_track_ids.insert(track.track_id()).second) { + setError(error, "enabled QUIC edge track IDs must be unique and non-zero"); + return false; + } + if (expectedKind(track) == media::MediaKind::UNKNOWN) { + setError(error, "enabled QUIC edge track has unsupported source_kind"); + return false; + } + if (track.device_id().empty()) { + setError(error, "enabled QUIC edge track has empty device_id"); + return false; + } + const std::string source_id = sourceTrackId(track); + if (source_id.empty() || !source_track_ids.insert(source_id).second) { + setError(error, "enabled QUIC edge source track IDs must be unique and non-empty"); + return false; + } + if (track.max_frame_bytes() > config.maximum_frame_bytes()) { + setError(error, "track max_frame_bytes exceeds the global frame limit"); + return false; + } + } + return true; +} + +bool QuicEdgeService::initialize(std::string* error) +{ + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (worker_.joinable()) { + setError(error, "cannot initialize QUIC edge service while it is running"); + return false; + } + std::string validation_error; + if (!validateConfig(config_, &validation_error)) { + setState(QuicEdgeServiceState::FAILED, validation_error); + setError(error, validation_error); + return false; + } + if (!transport_ || !media_hub_) { + const std::string message = "QUIC edge transport or MediaSourceHub is null"; + setState(QuicEdgeServiceState::FAILED, message); + setError(error, message); + return false; + } + setState(QuicEdgeServiceState::STOPPED); + return true; +} + +bool QuicEdgeService::start(std::string* error) +{ + std::lock_guard lifecycle_lock(lifecycle_mutex_); + { + std::lock_guard lock(mutex_); + if (state_ == QuicEdgeServiceState::UNINITIALIZED) { + setError(error, "QUIC edge service is not initialized"); + return false; + } + if (state_ == QuicEdgeServiceState::FAILED) { + setError(error, last_error_); + return false; + } + if (worker_.joinable()) { + return true; + } + stop_requested_ = false; + } + try { + worker_ = std::thread(&QuicEdgeService::run, this); + } catch (const std::exception& exception) { + const std::string message = + std::string("failed to start QUIC edge worker: ") + exception.what(); + setState(QuicEdgeServiceState::FAILED, message); + setError(error, message); + return false; + } + return true; +} + +void QuicEdgeService::stop() +{ + std::lock_guard lifecycle_lock(lifecycle_mutex_); + { + std::lock_guard lock(mutex_); + stop_requested_ = true; + media_stop_requested_ = true; + } + stop_cv_.notify_all(); + media_stop_cv_.notify_all(); + if (transport_) { + transport_->disconnect(); + } + if (worker_.joinable()) { + worker_.join(); + } + resetConnectionStatus(); + std::lock_guard lock(mutex_); + if (state_ != QuicEdgeServiceState::UNINITIALIZED && + state_ != QuicEdgeServiceState::FAILED) { + state_ = QuicEdgeServiceState::STOPPED; + } +} + +QuicEdgeServiceState QuicEdgeService::state() const +{ + std::lock_guard lock(mutex_); + return state_; +} + +std::string QuicEdgeService::lastError() const +{ + std::lock_guard lock(mutex_); + return last_error_; +} + +QuicEdgeStats QuicEdgeService::stats() const +{ + std::lock_guard lock(mutex_); + return stats_; +} + +QuicEdgeStatus QuicEdgeService::status() const +{ + std::lock_guard lock(mutex_); + QuicEdgeStatus result; + result.registered = registered_; + result.node_id = node_id_; + result.boot_id = boot_id_; + result.session_id = session_id_; + result.observed_source_ip = observed_source_ip_; + result.last_media_error = last_media_error_; + result.heartbeat_sequence = heartbeat_sequence_; + result.last_heartbeat_ack_unix_ms = last_heartbeat_ack_unix_ms_; + result.active_media_tracks = active_media_tracks_; + return result; +} + +void QuicEdgeService::run() +{ + try { + auto backoff = std::chrono::milliseconds(config_.reconnect().initial_delay_ms()); + while (true) { + { + std::lock_guard lock(mutex_); + if (stop_requested_) break; + ++stats_.connection_attempts; + } + resetConnectionStatus(); + setState(QuicEdgeServiceState::CONNECTING); + CMVR_LOG(INFO) << "[QuicEdgeService] connecting" + << ", target=" << config_.server_host() << ':' + << config_.server_port() + << ", tls_server_name=" << config_.tls().server_name() + << ", alpn=" << config_.alpn() + << ", node_id=" << node_id_; + + std::string error; + if (!transport_->connect( + config_, + std::chrono::milliseconds(config_.reconnect().connect_timeout_ms()), + &error)) { + CMVR_LOG(ERROR) << "[QuicEdgeService] connect failed" + << ", target=" << config_.server_host() << ':' + << config_.server_port() + << ", error=" + << (error.empty() ? "QUIC connect failed" : error) + << ", backoff_ms=" << backoff.count(); + setState(QuicEdgeServiceState::BACKOFF, + error.empty() ? "QUIC connect failed" : error); + if (waitForStop(jittered(backoff))) break; + backoff = nextBackoff(backoff); + continue; + } + + std::uint64_t acknowledged_heartbeats_at_connect = 0U; + { + std::lock_guard lock(mutex_); + ++stats_.successful_connections; + if (stats_.successful_connections > 1U) ++stats_.reconnects; + acknowledged_heartbeats_at_connect = + stats_.heartbeats_acknowledged; + } + session_epoch_ = nextSessionEpoch(); + control_message_sequence_ = 0U; + inbound_message_sequence_ = 0U; + has_inbound_message_sequence_ = false; + ControlFrameDecoder decoder(config_.maximum_control_frame_bytes()); + setState(QuicEdgeServiceState::REGISTERING); + CMVR_LOG(INFO) << "[QuicEdgeService] connected, registering node" + << ", target=" << config_.server_host() << ':' + << config_.server_port() + << ", node_id=" << node_id_; + + if (!performRegistration(&decoder, &error)) { + transport_->disconnect(); + { + std::lock_guard lock(mutex_); + if (stop_requested_) break; + } + CMVR_LOG(ERROR) << "[QuicEdgeService] registration failed" + << ", target=" << config_.server_host() << ':' + << config_.server_port() + << ", error=" << error + << ", backoff_ms=" << backoff.count(); + setState(QuicEdgeServiceState::BACKOFF, error); + if (waitForStop(jittered(backoff))) break; + backoff = nextBackoff(backoff); + continue; + } + setState(QuicEdgeServiceState::ONLINE); + CMVR_LOG(INFO) << "[QuicEdgeService] online" + << ", target=" << config_.server_host() << ':' + << config_.server_port() + << ", node_id=" << node_id_ + << ", session=" << session_id_; + + if (!startMediaWorker(&error)) { + transport_->disconnect(); + { + std::lock_guard lock(mutex_); + if (stop_requested_) break; + } + setState(QuicEdgeServiceState::BACKOFF, error); + if (waitForStop(jittered(backoff))) break; + backoff = nextBackoff(backoff); + continue; + } + + bool connection_failed = false; + bool connection_became_healthy = false; + while (true) { + { + std::lock_guard lock(mutex_); + if (stop_requested_) break; + } + + bool received_control = false; + if (!receiveAndDispatchControl( + &decoder, std::chrono::milliseconds(0), + &received_control, &error)) { + connection_failed = true; + break; + } + if (!transport_->isConnected()) { + // receiveControl drains bytes already delivered by MsQuic + // before it reports the disconnect on the next iteration. + continue; + } + if (!connection_became_healthy) { + std::lock_guard lock(mutex_); + if (stats_.heartbeats_acknowledged > + acknowledged_heartbeats_at_connect) { + backoff = std::chrono::milliseconds( + config_.reconnect().initial_delay_ms()); + connection_became_healthy = true; + } + } + + const auto now = std::chrono::steady_clock::now(); + std::uint64_t outstanding_sequence = 0U; + std::chrono::steady_clock::time_point heartbeat_deadline; + { + std::lock_guard lock(mutex_); + outstanding_sequence = outstanding_heartbeat_sequence_; + heartbeat_deadline = heartbeat_deadline_; + } + if (outstanding_sequence != 0U && now >= heartbeat_deadline) { + { + std::lock_guard lock(mutex_); + ++stats_.heartbeat_timeouts; + } + error = "QUIC node heartbeat acknowledgement timed out"; + connection_failed = true; + break; + } + + if (outstanding_sequence == 0U && now >= next_heartbeat_) { + std::uint64_t sequence = 0U; + { + std::lock_guard lock(mutex_); + if (heartbeat_sequence_ == + std::numeric_limits::max()) { + heartbeat_sequence_ = 0U; + } + sequence = ++heartbeat_sequence_; + } + if (!sendHeartbeat(sequence, &error)) { + connection_failed = true; + break; + } + const auto heartbeat_sent_at = + std::chrono::steady_clock::now(); + { + std::lock_guard lock(mutex_); + outstanding_heartbeat_sequence_ = sequence; + heartbeat_deadline_ = + heartbeat_sent_at + std::chrono::milliseconds( + config_.control_response_timeout_ms()); + ++stats_.heartbeats_sent; + } + next_heartbeat_ = + heartbeat_sent_at + std::chrono::milliseconds( + effective_heartbeat_interval_ms_); + } + + { + std::lock_guard lock(mutex_); + if (media_connection_failed_) { + error = media_connection_error_.empty() + ? "QUIC media worker reported a connection failure" + : media_connection_error_; + connection_failed = true; + } + } + if (connection_failed) break; + + if (!receiveAndDispatchControl( + &decoder, kControlPollInterval, + &received_control, &error)) { + connection_failed = true; + break; + } + } + + stopMediaWorker(); + transport_->disconnect(); + resetConnectionStatus(); + { + std::lock_guard lock(mutex_); + if (stop_requested_) break; + } + setState(QuicEdgeServiceState::BACKOFF, + error.empty() ? "QUIC edge connection closed" : error); + if (waitForStop(jittered(backoff))) break; + backoff = nextBackoff(backoff); + } + stopMediaWorker(); + transport_->disconnect(); + resetConnectionStatus(); + setState(QuicEdgeServiceState::STOPPED); + } catch (const std::exception& exception) { + stopMediaWorker(); + transport_->disconnect(); + resetConnectionStatus(); + setState(QuicEdgeServiceState::FAILED, + std::string("QUIC edge worker exception: ") + exception.what()); + } catch (...) { + stopMediaWorker(); + transport_->disconnect(); + resetConnectionStatus(); + setState(QuicEdgeServiceState::FAILED, "unknown QUIC edge worker exception"); + } +} + +bool QuicEdgeService::startMediaWorker(std::string* error) +{ + if (!hasEnabledMediaTracks()) { + std::lock_guard lock(mutex_); + active_media_tracks_ = 0U; + return true; + } + if (media_worker_.joinable()) { + setError(error, "QUIC media worker is already running"); + return false; + } + { + std::lock_guard lock(mutex_); + if (stop_requested_) { + setError(error, "QUIC edge service is stopping"); + return false; + } + media_stop_requested_ = false; + media_connection_failed_ = false; + media_connection_error_.clear(); + active_media_tracks_ = 0U; + } + try { + media_worker_ = std::thread(&QuicEdgeService::runMedia, this); + } catch (const std::exception& exception) { + const std::string message = + std::string("failed to start QUIC media worker: ") + exception.what(); + recordMediaConnectionFailure(message); + setError(error, message); + return false; + } + return true; +} + +void QuicEdgeService::stopMediaWorker() +{ + { + std::lock_guard lock(mutex_); + media_stop_requested_ = true; + } + media_stop_cv_.notify_all(); + if (media_worker_.joinable()) media_worker_.join(); + std::lock_guard lock(mutex_); + active_media_tracks_ = 0U; +} + +void QuicEdgeService::recordMediaConnectionFailure(const std::string& error) +{ + { + std::lock_guard lock(mutex_); + if (!media_connection_failed_) { + media_connection_error_ = error.empty() + ? "unknown QUIC media connection failure" : error; + } + media_connection_failed_ = true; + media_stop_requested_ = true; + last_media_error_ = media_connection_error_; + } + media_stop_cv_.notify_all(); +} + +void QuicEdgeService::runMedia() +{ + std::vector tracks; + try { + next_media_source_retry_ = std::chrono::steady_clock::now(); + while (true) { + { + std::lock_guard lock(mutex_); + if (stop_requested_ || media_stop_requested_) break; + } + if (!transport_->isConnected()) break; + + const auto now = std::chrono::steady_clock::now(); + std::string error; + if (now >= next_media_source_retry_) { + tracks.erase( + std::remove_if(tracks.begin(), tracks.end(), + [](const ActiveTrack& track) { + return !track.subscription.valid(); + }), + tracks.end()); + if (!openMediaSession(&tracks, &error)) { + recordMediaConnectionFailure(error); + break; + } + next_media_source_retry_ = now + kMediaSourceRetryInterval; + } + + bool sent_anything = false; + bool failed = false; + for (auto& track : tracks) { + if (!processTrack(&track, &sent_anything, &error)) { + failed = true; + break; + } + } + if (failed) { + recordMediaConnectionFailure(error); + break; + } + tracks.erase( + std::remove_if(tracks.begin(), tracks.end(), + [](const ActiveTrack& track) { + return !track.subscription.valid(); + }), + tracks.end()); + { + std::lock_guard lock(mutex_); + active_media_tracks_ = tracks.size(); + } + + if (!sent_anything) { + const auto poll_wait = tracks.empty() + ? kControlPollInterval + : std::chrono::milliseconds(config_.media_poll_interval_ms()); + std::unique_lock lock(mutex_); + media_stop_cv_.wait_for(lock, poll_wait, [this]() { + return stop_requested_ || media_stop_requested_; + }); + } + } + } catch (const std::exception& exception) { + recordMediaConnectionFailure( + std::string("QUIC media worker exception: ") + exception.what()); + } catch (...) { + recordMediaConnectionFailure("unknown QUIC media worker exception"); + } + tracks.clear(); + std::lock_guard lock(mutex_); + active_media_tracks_ = 0U; +} + +bool QuicEdgeService::performRegistration(ControlFrameDecoder* decoder, + std::string* error) +{ + if (!decoder) { + setError(error, "control frame decoder is null"); + return false; + } + if (!sendNodeRegistration(error)) return false; + + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.control_response_timeout_ms()); + while (transport_->isConnected()) { + { + std::lock_guard lock(mutex_); + if (stop_requested_) { + setError(error, "QUIC edge service is stopping"); + return false; + } + if (registered_) return true; + } + const auto now = std::chrono::steady_clock::now(); + if (now >= deadline) { + setError(error, "QUIC node registration response timed out"); + return false; + } + const auto remaining = std::chrono::duration_cast( + deadline - now); + bool received = false; + if (!receiveAndDispatchControl( + decoder, std::min(remaining, kControlPollInterval), + &received, error)) { + return false; + } + } + setError(error, "QUIC connection closed during node registration"); + return false; +} + +bool QuicEdgeService::sendNodeRegistration(std::string* error) +{ + std::lock_guard control_lock(control_send_mutex_); + v1::EdgeControlEnvelope envelope; + envelope.set_protocol_version(kProtocolVersion); + envelope.set_message_sequence(control_message_sequence_++); + auto* request = envelope.mutable_node_register_request(); + populateDescriptor(config_, node_id_, boot_id_, software_version_, + request->mutable_node()); + request->set_sent_at_unix_ms(unixTimeMs()); + std::string serialized; + if (!envelope.SerializeToString(&serialized)) { + setError(error, "failed to serialize NodeRegisterRequest"); + return false; + } + if (!sendControlEnvelope(serialized, error)) return false; + const auto& node = request->node(); + CMVR_LOG(INFO) << "[QuicEdgeService] sent NodeRegisterRequest" + << ", message_sequence=" << envelope.message_sequence() + << ", payload_bytes=" << serialized.size() + << ", node_id=" << node.node_id() + << ", boot_id=" << node.boot_id() + << ", grpc_endpoint=" << node.grpc_endpoint().host() + << ':' << node.grpc_endpoint().port() + << ", grpc_tls=" << node.grpc_endpoint().tls() + << ", interfaces=" << node.local_interfaces_size() + << ", sent_at_unix_ms=" << request->sent_at_unix_ms(); + std::lock_guard lock(mutex_); + ++stats_.registrations_sent; + return true; +} + +bool QuicEdgeService::sendHeartbeat(const std::uint64_t sequence, + std::string* error) +{ + std::optional device_manager_snapshot; + if (device_snapshot_provider_) { + try { + device_manager_snapshot = device_snapshot_provider_(); + } catch (const std::exception& exception) { + setError( + error, + std::string("failed to snapshot DeviceManager for heartbeat: ") + + exception.what()); + return false; + } catch (...) { + setError(error, + "failed to snapshot DeviceManager for heartbeat: " + "unknown exception"); + return false; + } + } + + std::lock_guard control_lock(control_send_mutex_); + std::string session_id; + { + std::lock_guard lock(mutex_); + session_id = session_id_; + } + if (session_id.empty()) { + setError(error, "cannot send heartbeat before node registration"); + return false; + } + + v1::EdgeControlEnvelope envelope; + envelope.set_protocol_version(kProtocolVersion); + envelope.set_message_sequence(control_message_sequence_++); + auto* heartbeat = envelope.mutable_node_heartbeat(); + heartbeat->set_node_id(node_id_); + heartbeat->set_boot_id(boot_id_); + heartbeat->set_session_id(session_id); + heartbeat->set_sequence(sequence); + const std::uint64_t sent_at_unix_ms = unixTimeMs(); + heartbeat->set_sent_at_unix_ms(sent_at_unix_ms); + heartbeat->set_software_version(software_version_); + populateHeartbeatNetwork(config_, heartbeat); + if (device_manager_snapshot.has_value()) { + populateDeviceManagerSnapshot( + *device_manager_snapshot, sent_at_unix_ms, + heartbeat->mutable_device_manager()); + } + std::string serialized; + if (!envelope.SerializeToString(&serialized)) { + setError(error, "failed to serialize NodeHeartbeat"); + return false; + } + if (!sendControlEnvelope(serialized, error)) return false; + // CMVR_LOG(INFO) << "[QuicEdgeService] sent NodeHeartbeat" + // << ", message_sequence=" << envelope.message_sequence() + // << ", payload_bytes=" << serialized.size() + // << ", session=" << session_id + // << ", heartbeat_sequence=" << sequence + // << ", grpc_endpoint=" << heartbeat->grpc_endpoint().host() + // << ':' << heartbeat->grpc_endpoint().port() + // << ", interfaces=" << heartbeat->local_interfaces_size() + // << ", devices=" << heartbeat->device_manager().devices_size() + // << ", sent_at_unix_ms=" << sent_at_unix_ms; + return true; +} + +bool QuicEdgeService::receiveAndDispatchControl( + ControlFrameDecoder* decoder, + const std::chrono::milliseconds timeout, + bool* received, + std::string* error) +{ + if (!decoder || !received) { + setError(error, "control receive arguments are null"); + return false; + } + *received = false; + std::vector chunk; + const auto result = transport_->receiveControl(&chunk, timeout, error); + if (result == TransportReceiveResult::TIMEOUT) return true; + if (result == TransportReceiveResult::DISCONNECTED) { + if (error && error->empty()) *error = "QUIC control stream disconnected"; + return false; + } + if (result == TransportReceiveResult::ERROR) { + if (error && error->empty()) *error = "QUIC control stream receive failed"; + return false; + } + if (chunk.empty()) { + setError(error, "QUIC control stream returned an empty data chunk"); + return false; + } + + std::vector> frames; + if (!decoder->push(chunk, &frames, error)) return false; + *received = true; + for (const auto& frame : frames) { + if (!dispatchControlFrame(frame, error)) return false; + } + return true; +} + +bool QuicEdgeService::dispatchControlFrame( + const std::vector& frame, + std::string* error) +{ + v1::EdgeControlEnvelope envelope; + if (!envelope.ParseFromArray(frame.data(), static_cast(frame.size()))) { + setError(error, "failed to parse EdgeControlEnvelope"); + return false; + } + if (envelope.protocol_version() != kProtocolVersion) { + setError(error, "unsupported QUIC edge protocol version"); + return false; + } + if (has_inbound_message_sequence_ && + envelope.message_sequence() <= inbound_message_sequence_) { + setError(error, "QUIC control message sequence did not increase"); + return false; + } + inbound_message_sequence_ = envelope.message_sequence(); + has_inbound_message_sequence_ = true; + + if (envelope.has_node_register_response()) { + const auto& response = envelope.node_register_response(); + if (!response.accepted()) { + { + std::lock_guard lock(mutex_); + ++stats_.registrations_rejected; + } + setError(error, response.message().empty() + ? "gateway rejected node registration" + : response.message()); + return false; + } + if (response.session_id().empty()) { + setError(error, "gateway accepted registration without a session ID"); + return false; + } + const std::uint32_t requested_interval = + response.heartbeat_interval_ms() == 0U + ? config_.heartbeat_interval_ms() + : response.heartbeat_interval_ms(); + { + std::lock_guard lock(mutex_); + if (registered_) { + setError(error, "duplicate node registration response"); + return false; + } + registered_ = true; + session_id_ = response.session_id(); + observed_source_ip_ = response.observed_source_ip(); + effective_heartbeat_interval_ms_ = std::clamp( + requested_interval, kMinimumHeartbeatIntervalMs, + kMaximumHeartbeatIntervalMs); + next_heartbeat_ = std::chrono::steady_clock::now(); + ++stats_.registrations_accepted; + } + return true; + } + + if (envelope.has_node_heartbeat_ack()) { + const auto& ack = envelope.node_heartbeat_ack(); + std::lock_guard lock(mutex_); + if (!registered_ || outstanding_heartbeat_sequence_ == 0U) { + setError(error, "unexpected heartbeat acknowledgement"); + return false; + } + if (!ack.accepted()) { + setError(error, ack.message().empty() + ? "gateway rejected node heartbeat" + : ack.message()); + return false; + } + if (ack.session_id() != session_id_ || + ack.acknowledged_sequence() != outstanding_heartbeat_sequence_) { + setError(error, "heartbeat acknowledgement session or sequence mismatch"); + return false; + } + outstanding_heartbeat_sequence_ = 0U; + if (!ack.observed_source_ip().empty()) { + observed_source_ip_ = ack.observed_source_ip(); + } + last_heartbeat_ack_unix_ms_ = unixTimeMs(); + ++stats_.heartbeats_acknowledged; + return true; + } + + if (envelope.has_protocol_error()) { + const auto& protocol_error = envelope.protocol_error(); + { + std::lock_guard lock(mutex_); + ++stats_.protocol_errors; + last_error_ = protocol_error.message(); + } + if (protocol_error.fatal()) { + setError(error, protocol_error.message().empty() + ? "gateway reported a fatal protocol error" + : protocol_error.message()); + return false; + } + return true; + } + + setError(error, "gateway sent an unexpected QUIC control message"); + return false; +} + +bool QuicEdgeService::openMediaSession(std::vector* tracks, + std::string* error) +{ + if (!tracks) { + setError(error, "active track output is null"); + return false; + } + if (!hasEnabledMediaTracks()) { + std::lock_guard lock(mutex_); + active_media_tracks_ = 0U; + return true; + } + if (!media_session_announced_) { + if (!sendSessionOpen(error)) { + if (!transport_->isConnected()) return false; + recordMediaError(error ? *error : "failed to open media session"); + if (error) error->clear(); + return true; + } + media_session_announced_ = true; + { + std::lock_guard lock(mutex_); + ++stats_.media_sessions_opened; + } + } + refreshMediaTracks(tracks); + return true; +} + +void QuicEdgeService::refreshMediaTracks(std::vector* tracks) +{ + if (!tracks) return; + for (const auto& track_config : config_.tracks()) { + if (!track_config.enable()) continue; + const bool already_active = std::any_of( + tracks->begin(), tracks->end(), + [&track_config](const ActiveTrack& active) { + return active.config.track_id() == track_config.track_id(); + }); + if (already_active) continue; + + ActiveTrack track; + track.config = track_config; + track.source_track_id = sourceTrackId(track_config); + std::string source_error; + if (!ensureSourceRegistered(track_config, track.source_track_id, + &source_error)) { + recordMediaError(source_error); + continue; + } + track.subscription = media_hub_->subscribe( + track.source_track_id, + media::MediaSourceHub::StartPosition::LATEST_AVAILABLE, + [this] { + std::lock_guard lock(mutex_); + return stop_requested_ || media_stop_requested_; + }); + if (!track.subscription.valid()) { + recordMediaError( + "MediaSourceHub source unavailable: " + track.source_track_id); + continue; + } + track.waiting_for_keyframe = + expectedKind(track_config) == media::MediaKind::VIDEO; + track.next_frame_discontinuous = true; + if (track.waiting_for_keyframe) requestKeyFrame(&track); + tracks->push_back(std::move(track)); + } + std::lock_guard lock(mutex_); + active_media_tracks_ = tracks->size(); +} + +bool QuicEdgeService::hasEnabledMediaTracks() const +{ + return std::any_of(config_.tracks().begin(), config_.tracks().end(), + [](const config::QuicEdgeTrackConfig& track) { + return track.enable(); + }); +} + +bool QuicEdgeService::ensureSourceRegistered( + const config::QuicEdgeTrackConfig& track, + const std::string& source_track_id, + std::string* error) +{ + if (media_hub_->hasSource(source_track_id)) return true; + if (!using_global_media_hub_ || !source_registrar_) { + setError(error, "MediaSourceHub source unavailable: " + source_track_id); + return false; + } + const bool registered = source_registrar_(track, source_track_id, error); + if (!registered && !media_hub_->hasSource(source_track_id)) { + if (!error || error->empty()) { + setError(error, + "failed to register MediaSourceHub source: " + source_track_id); + } + return false; + } + if (!media_hub_->hasSource(source_track_id)) { + setError(error, + "configured source_track_id does not match the device adapter track: " + + source_track_id); + return false; + } + return true; +} + +bool QuicEdgeService::sendSessionOpen(std::string* error) +{ + std::lock_guard control_lock(control_send_mutex_); + std::string session_id; + { + std::lock_guard lock(mutex_); + session_id = session_id_; + } + if (session_id.empty()) { + setError(error, "cannot open media before node registration"); + return false; + } + v1::EdgeControlEnvelope envelope; + envelope.set_protocol_version(kProtocolVersion); + envelope.set_message_sequence(control_message_sequence_++); + auto* open = envelope.mutable_media_session_open(); + open->set_node_id(node_id_); + open->set_session_epoch(session_epoch_); + open->set_session_id(session_id); + std::string serialized; + if (!envelope.SerializeToString(&serialized)) { + setError(error, "failed to serialize MediaSessionOpen"); + return false; + } + if (!sendControlEnvelope(serialized, error)) return false; + CMVR_LOG(INFO) << "[QuicEdgeService] sent MediaSessionOpen" + << ", message_sequence=" << envelope.message_sequence() + << ", payload_bytes=" << serialized.size() + << ", session=" << session_id + << ", session_epoch=" << session_epoch_ + << ", node_id=" << node_id_; + return true; +} + +bool QuicEdgeService::sendTrackDescription( + const MediaTrackDescription& description, + std::string* error) +{ + std::lock_guard control_lock(control_send_mutex_); + v1::EdgeControlEnvelope envelope; + envelope.set_protocol_version(kProtocolVersion); + envelope.set_message_sequence(control_message_sequence_++); + auto* descriptor = envelope.mutable_media_track_descriptor(); + descriptor->set_track_id(description.track_id); + descriptor->set_kind(toProtoKind(description.kind)); + descriptor->set_device_id(description.source_id); + descriptor->set_codec(codecName(description.codec)); + descriptor->set_codec_generation(description.codec_generation); + descriptor->set_source_track_id(description.source_track_id); + descriptor->set_codec_generation_token(description.codec_generation_token); + descriptor->set_payload_format(payloadFormatName(description.payload_format)); + descriptor->set_width(description.width); + descriptor->set_height(description.height); + descriptor->set_frames_per_second(description.nominal_rate); + descriptor->set_sample_rate(description.sample_rate); + descriptor->set_channels(description.channels); + if (!description.codec_config.empty()) { + descriptor->set_codec_config(description.codec_config.data(), + description.codec_config.size()); + } + std::string serialized; + if (!envelope.SerializeToString(&serialized)) { + setError(error, "failed to serialize MediaTrackDescriptor"); + return false; + } + if (!sendControlEnvelope(serialized, error)) return false; + CMVR_LOG(INFO) << "[QuicEdgeService] sent MediaTrackDescriptor" + << ", message_sequence=" << envelope.message_sequence() + << ", payload_bytes=" << serialized.size() + << ", track_id=" << description.track_id + << ", source_track_id=" << description.source_track_id + << ", source_id=" << description.source_id + << ", kind=" << static_cast(description.kind) + << ", codec=" << codecName(description.codec) + << ", payload_format=" << payloadFormatName(description.payload_format) + << ", codec_generation=" << description.codec_generation + << ", codec_generation_token=" + << description.codec_generation_token + << ", size=" << description.width << 'x' << description.height + << ", fps=" << description.nominal_rate + << ", sample_rate=" << description.sample_rate + << ", channels=" << description.channels + << ", codec_config_bytes=" << description.codec_config.size(); + return true; +} + +bool QuicEdgeService::sendControlEnvelope(const std::string& serialized, + std::string* error) +{ + std::vector framed; + if (!ControlFrameEncoder::encode( + reinterpret_cast(serialized.data()), + serialized.size(), config_.maximum_control_frame_bytes(), + &framed, error)) { + return false; + } + const auto result = transport_->sendControl(std::move(framed), error); + if (result == TransportSendResult::QUEUED) return true; + if (error && error->empty()) { + *error = result == TransportSendResult::WOULD_BLOCK + ? "QUIC reliable control stream is congested" + : "QUIC reliable control stream is disconnected"; + } + return false; +} + +bool QuicEdgeService::processTrack(ActiveTrack* track, + bool* sent_anything, + std::string* error) +{ + if (!track || !sent_anything) { + setError(error, "active track or sent flag is null"); + return false; + } + auto read = track->subscription.tryRead(); + if (!read) { + if (!track->subscription.valid()) { + recordMediaError( + "MediaSourceHub subscription stopped: " + track->source_track_id); + } + return true; + } + *sent_anything = true; + const media::MediaFramePtr& frame = read->value; + if (!frame || !frame->descriptor || + frame->descriptor->id != track->source_track_id || + frame->descriptor->kind != expectedKind(track->config)) { + recordMediaError("MediaSourceHub returned an invalid or mismatched frame"); + track->next_frame_discontinuous = true; + return true; + } + + bool discontinuity = track->next_frame_discontinuous || + frame->discontinuity || + read->generation_changed || + read->dropped_since_last_read != 0U; + if (read->dropped_since_last_read != 0U) { + std::lock_guard lock(mutex_); + stats_.frames_dropped_source += read->dropped_since_last_read; + } + + const auto description = describeTrack(track->config.track_id(), + *frame->descriptor); + const bool descriptor_changed = + !track->last_description || *track->last_description != description; + if (track->last_description && descriptor_changed && + description.codec_generation < track->last_description->codec_generation) { + recordMediaError("MediaSourceHub descriptor generation regressed"); + track->next_frame_discontinuous = true; + return true; + } + if (descriptor_changed) { + if (!sendTrackDescription(description, error)) { + if (!transport_->isConnected()) return false; + recordMediaError(error ? *error : "failed to send media track descriptor"); + if (error) error->clear(); + track->next_frame_discontinuous = true; + return true; + } + track->last_description = description; + discontinuity = true; + if (description.kind == media::MediaKind::VIDEO) { + track->waiting_for_keyframe = true; + track->keyframe_requested = false; + requestKeyFrame(track); + } + } + + if (description.kind == media::MediaKind::VIDEO && discontinuity) { + if (!track->waiting_for_keyframe) { + track->keyframe_requested = false; + } + track->waiting_for_keyframe = true; + requestKeyFrame(track); + } + + if (frame->descriptor->kind == media::MediaKind::VIDEO && + track->waiting_for_keyframe && !frame->key_frame) { + track->next_frame_discontinuous = discontinuity; + requestKeyFrame(track); + std::lock_guard lock(mutex_); + ++stats_.frames_skipped_waiting_keyframe; + return true; + } + + if (frame->empty() || frame->size() > maximumFrameBytes(*track)) { + const auto drained = shedBufferedFrames(track); + track->next_frame_discontinuous = true; + if (frame->descriptor->kind == media::MediaKind::VIDEO) { + track->waiting_for_keyframe = true; + track->keyframe_requested = false; + requestKeyFrame(track); + } + std::lock_guard lock(mutex_); + stats_.frames_dropped_oversize += 1U + drained; + return true; + } + + const std::size_t peer_maximum = transport_->maximumDatagramBytes(); + if (peer_maximum <= kDatagramHeaderBytes) { + const auto drained = shedBufferedFrames(track); + track->next_frame_discontinuous = true; + if (frame->descriptor->kind == media::MediaKind::VIDEO) { + track->waiting_for_keyframe = true; + track->keyframe_requested = false; + requestKeyFrame(track); + } + std::lock_guard lock(mutex_); + stats_.frames_dropped_no_datagram += 1U + drained; + last_media_error_ = "peer/path has no usable QUIC DATAGRAM support"; + return true; + } + const std::size_t effective_maximum = + std::min(config_.maximum_datagram_bytes(), peer_maximum); + const std::size_t fragment_payload_capacity = + effective_maximum - kDatagramHeaderBytes; + const std::size_t required_fragments = + (frame->size() + fragment_payload_capacity - 1U) / + fragment_payload_capacity; + const std::size_t maximum_batch_packets = + transport_->maximumDatagramBatchPackets(); + if (maximum_batch_packets == 0U || + required_fragments > maximum_batch_packets) { + const auto drained = shedBufferedFrames(track); + track->next_frame_discontinuous = true; + if (frame->descriptor->kind == media::MediaKind::VIDEO) { + track->waiting_for_keyframe = true; + track->keyframe_requested = false; + requestKeyFrame(track); + } + std::lock_guard lock(mutex_); + stats_.frames_dropped_oversize += 1U + drained; + last_media_error_ = + "media frame exceeds the atomic DATAGRAM batch capacity"; + return true; + } + auto& next_wire_sequence = + next_wire_frame_sequence_[track->config.track_id()]; + if (next_wire_sequence == 0U) next_wire_sequence = 1U; + if (next_wire_sequence == std::numeric_limits::max()) { + setError(error, "QUIC media wire frame sequence exhausted"); + return false; + } + const std::uint64_t wire_frame_sequence = next_wire_sequence++; + std::vector packets; + if (!packetizer_.packetize( + *frame, track->config.track_id(), + description.codec_generation_token, session_epoch_, + wire_frame_sequence, discontinuity, effective_maximum, + &packets, error)) { + const std::string packet_error = + error && !error->empty() ? *error : "media frame packetization failed"; + track->next_frame_discontinuous = true; + if (frame->descriptor->kind == media::MediaKind::VIDEO) { + track->waiting_for_keyframe = true; + track->keyframe_requested = false; + requestKeyFrame(track); + } + std::lock_guard lock(mutex_); + ++stats_.frames_dropped_oversize; + last_media_error_ = packet_error; + if (error) error->clear(); + return true; + } + + const std::size_t datagram_count = packets.size(); + const auto result = transport_->sendDatagramBatch(std::move(packets), error); + if (result == TransportSendResult::QUEUED) { + track->next_frame_discontinuous = false; + if (frame->descriptor->kind == media::MediaKind::VIDEO && frame->key_frame) { + track->waiting_for_keyframe = false; + track->keyframe_requested = false; + } + std::lock_guard lock(mutex_); + ++stats_.frames_queued; + stats_.datagrams_queued += datagram_count; + CMVR_LOG(INFO) << "[QuicEdgeService] queued media frame" + << ", track_id=" << track->config.track_id() + << ", source_track_id=" << track->source_track_id + << ", frame_sequence=" << frame->sequence + << ", wire_frame_sequence=" << wire_frame_sequence + << ", payload_bytes=" << frame->size() + << ", datagrams=" << datagram_count + << ", key_frame=" << frame->key_frame + << ", discontinuity=" << discontinuity + << ", codec=" << codecName(description.codec) + << ", payload_format=" + << payloadFormatName(description.payload_format) + << ", capture_timestamp_us=" + << frame->capture_time_ns / 1000U + << ", source_timestamp=" << frame->source_timestamp + << ", source_frame_number=" << frame->source_frame_number; + return true; + } + if (result == TransportSendResult::WOULD_BLOCK) { + const auto drained = shedBufferedFrames(track); + track->next_frame_discontinuous = true; + if (frame->descriptor->kind == media::MediaKind::VIDEO) { + track->waiting_for_keyframe = true; + track->keyframe_requested = false; + requestKeyFrame(track); + } + std::lock_guard lock(mutex_); + stats_.frames_dropped_backpressure += 1U + drained; + return true; + } + if (error && error->empty()) { + *error = result == TransportSendResult::DISCONNECTED + ? "QUIC transport disconnected while sending media" + : "QUIC transport failed while sending media"; + } + if (result == TransportSendResult::DISCONNECTED || + !transport_->isConnected()) { + return false; + } + recordMediaError(error ? *error : "QUIC DATAGRAM send failed"); + if (error) error->clear(); + track->next_frame_discontinuous = true; + return true; +} + +std::uint64_t QuicEdgeService::shedBufferedFrames(ActiveTrack* track) +{ + if (!track) return 0U; + std::uint64_t drained = 0U; + while (drained < kMaximumDrainPerPoll && track->subscription.tryRead()) { + ++drained; + } + return drained; +} + +void QuicEdgeService::requestKeyFrame(ActiveTrack* track) +{ + if (!track || track->keyframe_requested || !media_hub_ || + expectedKind(track->config) != media::MediaKind::VIDEO) { + return; + } + (void)media_hub_->requestKeyFrame(track->source_track_id); + track->keyframe_requested = true; +} + +std::uint32_t QuicEdgeService::maximumFrameBytes(const ActiveTrack& track) const +{ + return track.config.max_frame_bytes() == 0U + ? config_.maximum_frame_bytes() + : track.config.max_frame_bytes(); +} + +void QuicEdgeService::initializeIdentity() +{ + node_id_ = resolveNodeId(config_.node_id()); + boot_id_ = resolveBootId(); + software_version_ = config_.software_version().empty() + ? "unknown" : config_.software_version(); + effective_heartbeat_interval_ms_ = config_.heartbeat_interval_ms(); +} + +void QuicEdgeService::resetConnectionStatus() +{ + std::lock_guard lock(mutex_); + registered_ = false; + session_id_.clear(); + observed_source_ip_.clear(); + outstanding_heartbeat_sequence_ = 0U; + active_media_tracks_ = 0U; + last_media_error_.clear(); + media_stop_requested_ = false; + media_connection_failed_ = false; + media_connection_error_.clear(); + effective_heartbeat_interval_ms_ = config_.heartbeat_interval_ms(); + next_wire_frame_sequence_.clear(); + media_session_announced_ = false; + heartbeat_deadline_ = {}; + next_heartbeat_ = {}; +} + +void QuicEdgeService::recordMediaError(const std::string& error) +{ + std::lock_guard lock(mutex_); + ++stats_.source_errors; + last_media_error_ = error.empty() ? "unknown media source error" : error; +} + +void QuicEdgeService::setState(const QuicEdgeServiceState state, + const std::string& error) +{ + std::lock_guard lock(mutex_); + state_ = state; + if (!error.empty()) { + last_error_ = error; + } else if (state == QuicEdgeServiceState::ONLINE || + state == QuicEdgeServiceState::STOPPED) { + last_error_.clear(); + } +} + +bool QuicEdgeService::waitForStop(const std::chrono::milliseconds duration) +{ + std::unique_lock lock(mutex_); + return stop_cv_.wait_for(lock, duration, [this]() { return stop_requested_; }); +} + +std::chrono::milliseconds QuicEdgeService::nextBackoff( + const std::chrono::milliseconds current) +{ + const double multiplied = static_cast(current.count()) * + config_.reconnect().multiplier(); + const auto bounded = std::min( + multiplied, static_cast(config_.reconnect().maximum_delay_ms())); + return std::chrono::milliseconds(static_cast(bounded)); +} + +std::chrono::milliseconds QuicEdgeService::jittered( + const std::chrono::milliseconds base) +{ + const auto jitter_percent = config_.reconnect().jitter_percent(); + if (jitter_percent == 0U) return base; + thread_local std::mt19937 generator(std::random_device{}()); + const double fraction = static_cast(jitter_percent) / 100.0; + std::uniform_real_distribution distribution(1.0 - fraction, + 1.0 + fraction); + const double value = static_cast(base.count()) * distribution(generator); + return std::chrono::milliseconds( + std::max(1, static_cast(value))); +} + +std::uint64_t QuicEdgeService::nextSessionEpoch() +{ + if (session_epoch_ == 0U) { + const auto wall_clock = static_cast( + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count()); + std::random_device random; + session_epoch_ = wall_clock ^ + (static_cast(random()) << 32U) ^ random(); + if (session_epoch_ == 0U) session_epoch_ = 1U; + return session_epoch_; + } + session_epoch_ = session_epoch_ == std::numeric_limits::max() + ? 1U : session_epoch_ + 1U; + return session_epoch_; +} + +} // namespace cmvr::quic_edge diff --git a/cmvr-es/service/quic_edge/src/quic_edge_types.cpp b/cmvr-es/service/quic_edge/src/quic_edge_types.cpp new file mode 100644 index 00000000..f12303da --- /dev/null +++ b/cmvr-es/service/quic_edge/src/quic_edge_types.cpp @@ -0,0 +1,40 @@ +#include "service/quic_edge/include/quic_edge_types.h" + +namespace cmvr::quic_edge { + +std::uint32_t descriptorGenerationToken(const std::uint64_t generation) +{ + // Fold both halves because the shared device adapter embeds its stream + // epoch in the upper 32 bits and its codec generation in the lower half. + std::uint32_t token = static_cast(generation) ^ + static_cast(generation >> 32U); + return token == 0U ? 1U : token; +} + +const char* codecName(const media::Codec codec) +{ + switch (codec) { + case media::Codec::H264: return "h264"; + case media::Codec::H265: return "h265"; + case media::Codec::OPUS: return "opus"; + case media::Codec::PCM_S16LE: return "pcm_s16le"; + case media::Codec::AAC: return "aac"; + case media::Codec::UNKNOWN: return "unknown"; + } + return "unknown"; +} + +const char* payloadFormatName(const media::PayloadFormat format) +{ + switch (format) { + case media::PayloadFormat::ANNEX_B: return "annex_b"; + case media::PayloadFormat::AVCC: return "avcc"; + case media::PayloadFormat::RAW: return "raw"; + case media::PayloadFormat::OPUS_PACKET: return "opus_packet"; + case media::PayloadFormat::AAC_ADTS: return "aac_adts"; + case media::PayloadFormat::UNKNOWN: return "unknown"; + } + return "unknown"; +} + +} // namespace cmvr::quic_edge diff --git a/cmvr-es/service/quic_edge/src/quic_transport.cpp b/cmvr-es/service/quic_edge/src/quic_transport.cpp new file mode 100644 index 00000000..cc1d4a4b --- /dev/null +++ b/cmvr-es/service/quic_edge/src/quic_transport.cpp @@ -0,0 +1,31 @@ +#include "service/quic_edge/include/quic_transport.h" + +#include + +namespace cmvr::quic_edge { +std::unique_ptr createMsQuicTransport( + std::size_t send_queue_depth); + +TransportReceiveResult QuicTransport::receiveControl( + std::vector* chunk, + std::chrono::milliseconds, + std::string* error) +{ + if (!chunk) { + if (error) *error = "control receive output must not be null"; + return TransportReceiveResult::ERROR; + } + chunk->clear(); + if (error) { + *error = "control stream receive is not supported by this transport"; + } + return TransportReceiveResult::DISCONNECTED; +} + +std::unique_ptr createDefaultQuicTransport( + const std::size_t send_queue_depth) +{ + return createMsQuicTransport(send_queue_depth); +} + +} // namespace cmvr::quic_edge diff --git a/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp new file mode 100644 index 00000000..a6d9b6a8 --- /dev/null +++ b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp @@ -0,0 +1,1039 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cmvr/quic_edge/v1/quic_edge.pb.h" +#include "manager/media_source_hub/include/media_source_hub.h" +#include "service/quic_edge/include/control_framing.h" +#include "service/quic_edge/include/datagram_packetizer.h" +#include "service/quic_edge/include/quic_edge_service.h" + +namespace { + +#define CHECK_TRUE(expression) \ + do { \ + if (!(expression)) { \ + std::cerr << "CHECK failed at line " << __LINE__ << ": " \ + << #expression << '\n'; \ + return false; \ + } \ + } while (false) + +using namespace cmvr; + +class FakeTransport final : public quic_edge::QuicTransport { +public: + explicit FakeTransport(const bool accept_registration = true, + const bool acknowledge_heartbeats = true, + const bool valid_heartbeat_session = true, + const std::size_t media_session_would_block_count = 0U, + const std::uint32_t registration_heartbeat_interval_ms = + 250U, + const std::uint32_t heartbeat_ack_delay_ms = 0U) + : accept_registration_(accept_registration), + acknowledge_heartbeats_(acknowledge_heartbeats), + valid_heartbeat_session_(valid_heartbeat_session), + media_session_would_block_count_(media_session_would_block_count), + registration_heartbeat_interval_ms_( + registration_heartbeat_interval_ms), + heartbeat_ack_delay_ms_(heartbeat_ack_delay_ms) + { + } + + bool connect(const config::QuicEdgeConfig&, + std::chrono::milliseconds, + std::string*) override + { + connected_.store(true); + ++connect_count_; + { + std::lock_guard lock(mutex_); + connect_times_.push_back(std::chrono::steady_clock::now()); + } + condition_.notify_all(); + return true; + } + + void disconnect() override + { + connected_.store(false); + { + std::lock_guard lock(mutex_); + delayed_heartbeat_ack_.reset(); + } + condition_.notify_all(); + } + bool isConnected() const override { return connected_.load(); } + std::size_t maximumDatagramBytes() const override { return 1200U; } + std::size_t maximumDatagramBatchPackets() const override { return 32U; } + + quic_edge::TransportSendResult sendControl( + std::vector message, std::string*) override + { + if (!connected_.load()) return quic_edge::TransportSendResult::DISCONNECTED; + std::vector> frames; + std::string decode_error; + quic_edge::ControlFrameDecoder decoder(1024U * 1024U); + if (!decoder.push(message, &frames, &decode_error) || frames.size() != 1U) { + return quic_edge::TransportSendResult::ERROR; + } + cmvr::quic_edge::v1::EdgeControlEnvelope envelope; + if (!envelope.ParseFromArray(frames.front().data(), + static_cast(frames.front().size()))) { + return quic_edge::TransportSendResult::ERROR; + } + { + std::lock_guard lock(mutex_); + if (envelope.has_media_session_open() && + media_session_would_block_count_ != 0U) { + --media_session_would_block_count_; + return quic_edge::TransportSendResult::WOULD_BLOCK; + } + edge_message_sequences_.push_back(envelope.message_sequence()); + controls_.push_back(std::move(message)); + if (envelope.has_node_register_request()) { + const auto& request = envelope.node_register_request(); + last_registered_node_id_ = request.node().node_id(); + last_grpc_endpoint_port_ = request.node().grpc_endpoint().port(); + last_interface_count_ = request.node().local_interfaces_size(); + cmvr::quic_edge::v1::EdgeControlEnvelope response; + response.set_protocol_version(quic_edge::kProtocolVersion); + response.set_message_sequence(server_message_sequence_++); + auto* registration = response.mutable_node_register_response(); + registration->set_accepted(accept_registration_); + registration->set_session_id( + accept_registration_ ? "test-session" : ""); + registration->set_message( + accept_registration_ ? "accepted" : "rejected for test"); + registration->set_heartbeat_interval_ms( + registration_heartbeat_interval_ms_); + registration->set_observed_source_ip("203.0.113.10"); + enqueueEnvelopeLocked(response); + } else if (envelope.has_node_heartbeat()) { + const auto& heartbeat = envelope.node_heartbeat(); + last_heartbeat_ = heartbeat; + has_last_heartbeat_ = true; + heartbeat_times_.push_back( + std::chrono::steady_clock::now()); + if (!acknowledge_heartbeats_) { + condition_.notify_all(); + return quic_edge::TransportSendResult::QUEUED; + } + cmvr::quic_edge::v1::EdgeControlEnvelope response; + response.set_protocol_version(quic_edge::kProtocolVersion); + response.set_message_sequence(server_message_sequence_++); + auto* ack = response.mutable_node_heartbeat_ack(); + ack->set_accepted(true); + ack->set_acknowledged_sequence(heartbeat.sequence()); + ack->set_session_id(valid_heartbeat_session_ + ? heartbeat.session_id() : ""); + ack->set_observed_source_ip("203.0.113.11"); + if (heartbeat_ack_delay_ms_ == 0U) { + enqueueEnvelopeLocked(response); + } else { + delayed_heartbeat_ack_ = std::move(response); + delayed_heartbeat_ack_ready_at_ = + std::chrono::steady_clock::now() + + std::chrono::milliseconds( + heartbeat_ack_delay_ms_); + } + } + } + condition_.notify_all(); + return quic_edge::TransportSendResult::QUEUED; + } + + quic_edge::TransportReceiveResult receiveControl( + std::vector* chunk, + const std::chrono::milliseconds timeout, + std::string*) override + { + if (!chunk) return quic_edge::TransportReceiveResult::ERROR; + std::unique_lock lock(mutex_); + releaseDelayedHeartbeatAckLocked(); + if (control_receive_queue_.empty() && connected_.load()) { + auto wait_duration = timeout; + if (delayed_heartbeat_ack_) { + const auto now = std::chrono::steady_clock::now(); + if (now < delayed_heartbeat_ack_ready_at_) { + wait_duration = std::min( + wait_duration, + std::chrono::duration_cast( + delayed_heartbeat_ack_ready_at_ - now) + + std::chrono::milliseconds(1)); + } + } + condition_.wait_for(lock, wait_duration, [this]() { + return !control_receive_queue_.empty() || + !connected_.load(); + }); + releaseDelayedHeartbeatAckLocked(); + } + if (!control_receive_queue_.empty()) { + *chunk = std::move(control_receive_queue_.front()); + control_receive_queue_.pop_front(); + return quic_edge::TransportReceiveResult::DATA; + } + return connected_.load() + ? quic_edge::TransportReceiveResult::TIMEOUT + : quic_edge::TransportReceiveResult::DISCONNECTED; + } + + quic_edge::TransportSendResult sendDatagramBatch( + std::vector packets, std::string*) override + { + if (!connected_.load()) return quic_edge::TransportSendResult::DISCONNECTED; + std::lock_guard lock(mutex_); + datagrams_.push_back(std::move(packets)); + return quic_edge::TransportSendResult::QUEUED; + } + + std::size_t datagramBatchCount() const + { + std::lock_guard lock(mutex_); + return datagrams_.size(); + } + + std::size_t controlCount() const + { + std::lock_guard lock(mutex_); + return controls_.size(); + } + + bool controlSequencesStrictlyIncreasing() const + { + std::lock_guard lock(mutex_); + for (std::size_t index = 1U; + index < edge_message_sequences_.size(); ++index) { + if (edge_message_sequences_[index] <= + edge_message_sequences_[index - 1U]) { + return false; + } + } + return !edge_message_sequences_.empty(); + } + + std::vector datagramFrameSequences() const + { + std::lock_guard lock(mutex_); + std::vector sequences; + for (const auto& batch : datagrams_) { + if (!batch.empty()) sequences.push_back(batch.front().header.frame_sequence); + } + return sequences; + } + + std::uint64_t connectCount() const { return connect_count_.load(); } + + std::vector connectIntervals() const + { + std::lock_guard lock(mutex_); + std::vector intervals; + for (std::size_t index = 1U; index < connect_times_.size(); ++index) { + intervals.push_back(std::chrono::duration_cast( + connect_times_[index] - connect_times_[index - 1U])); + } + return intervals; + } + + std::string lastRegisteredNodeId() const + { + std::lock_guard lock(mutex_); + return last_registered_node_id_; + } + + std::uint32_t lastGrpcEndpointPort() const + { + std::lock_guard lock(mutex_); + return last_grpc_endpoint_port_; + } + + int lastInterfaceCount() const + { + std::lock_guard lock(mutex_); + return last_interface_count_; + } + + std::size_t heartbeatCount() const + { + std::lock_guard lock(mutex_); + return heartbeat_times_.size(); + } + + std::vector heartbeatIntervals() const + { + std::lock_guard lock(mutex_); + std::vector intervals; + for (std::size_t index = 1U; + index < heartbeat_times_.size(); ++index) { + intervals.push_back( + std::chrono::duration_cast( + heartbeat_times_[index] - + heartbeat_times_[index - 1U])); + } + return intervals; + } + + cmvr::quic_edge::v1::NodeHeartbeat lastHeartbeat() const + { + std::lock_guard lock(mutex_); + return has_last_heartbeat_ + ? last_heartbeat_ + : cmvr::quic_edge::v1::NodeHeartbeat{}; + } + +private: + void releaseDelayedHeartbeatAckLocked() + { + if (!delayed_heartbeat_ack_ || + std::chrono::steady_clock::now() < + delayed_heartbeat_ack_ready_at_) { + return; + } + enqueueEnvelopeLocked(*delayed_heartbeat_ack_); + delayed_heartbeat_ack_.reset(); + } + + void enqueueEnvelopeLocked( + const cmvr::quic_edge::v1::EdgeControlEnvelope& envelope) + { + std::string serialized; + if (!envelope.SerializeToString(&serialized)) return; + std::vector framed; + std::string error; + if (quic_edge::ControlFrameEncoder::encode( + reinterpret_cast(serialized.data()), + serialized.size(), 1024U * 1024U, &framed, &error)) { + const auto split = framed.size() / 2U; + control_receive_queue_.emplace_back( + framed.begin(), framed.begin() + static_cast(split)); + control_receive_queue_.emplace_back( + framed.begin() + static_cast(split), framed.end()); + } + } + + mutable std::mutex mutex_; + std::condition_variable condition_; + std::atomic connected_{false}; + std::atomic connect_count_{0}; + bool accept_registration_{true}; + bool acknowledge_heartbeats_{true}; + bool valid_heartbeat_session_{true}; + std::size_t media_session_would_block_count_{0U}; + std::uint32_t registration_heartbeat_interval_ms_{250U}; + std::uint32_t heartbeat_ack_delay_ms_{0U}; + std::uint64_t server_message_sequence_{0}; + std::string last_registered_node_id_; + std::uint32_t last_grpc_endpoint_port_{0}; + int last_interface_count_{0}; + std::vector> controls_; + std::deque> control_receive_queue_; + std::vector> datagrams_; + std::vector edge_message_sequences_; + std::vector connect_times_; + std::vector heartbeat_times_; + bool has_last_heartbeat_{false}; + cmvr::quic_edge::v1::NodeHeartbeat last_heartbeat_; + std::optional + delayed_heartbeat_ack_; + std::chrono::steady_clock::time_point + delayed_heartbeat_ack_ready_at_{}; +}; + +config::QuicEdgeConfig validConfig(const std::string& source_track_id) +{ + config::QuicEdgeConfig config; + config.set_id("quic-test"); + config.set_server_host("127.0.0.1"); + config.set_server_port(4433); + config.set_alpn("cmvr-quic-edge/1"); + config.set_node_id("test-node"); + config.set_software_version("test-version"); + config.set_grpc_endpoint_host("auto"); + config.set_grpc_endpoint_port(50052U); + config.set_grpc_endpoint_tls(false); + config.set_heartbeat_interval_ms(250U); + config.set_control_response_timeout_ms(100U); + config.set_include_loopback_interfaces(true); + config.mutable_tls()->set_allow_insecure(true); + config.mutable_reconnect()->set_initial_delay_ms(5); + config.mutable_reconnect()->set_maximum_delay_ms(20); + config.mutable_reconnect()->set_multiplier(2.0); + config.mutable_reconnect()->set_connect_timeout_ms(50); + config.set_maximum_datagram_bytes(1200); + config.set_maximum_control_frame_bytes(4096); + config.set_maximum_frame_bytes(32 * 1024); + config.set_datagram_send_queue_depth(32); + config.set_media_poll_interval_ms(1); + auto* track = config.add_tracks(); + track->set_track_id(7); + track->set_source_kind(config::QuicEdgeTrackConfig::SOURCE_KIND_CAMERA); + track->set_device_id("camera-test"); + track->set_source_track_id(source_track_id); + track->set_enable(true); + return config; +} + +config::QuicEdgeConfig validPresenceOnlyConfig() +{ + auto config = validConfig("unused/video/color"); + config.clear_tracks(); + return config; +} + +media::TrackDescriptorPtr videoDescriptor(const std::string& id) +{ + media::TrackDescriptor::Config config; + config.id = id; + config.source_id = "camera-test"; + config.kind = media::MediaKind::VIDEO; + config.codec = media::Codec::H264; + config.payload_format = media::PayloadFormat::ANNEX_B; + config.time_base = {1, 90000}; + config.width = 640; + config.height = 480; + config.nominal_rate = 30; + config.generation = 0x0000000200000003ULL; + return media::makeTrackDescriptor(std::move(config)); +} + +bool waitUntil(const std::function& predicate) +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::seconds(2); + while (std::chrono::steady_clock::now() < deadline) { + if (predicate()) return true; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + return predicate(); +} + +bool testControlFraming() +{ + const std::vector payload{1, 2, 3, 4, 5}; + std::vector encoded; + std::string error; + CHECK_TRUE(quic_edge::ControlFrameEncoder::encode( + payload, 64, &encoded, &error)); + quic_edge::ControlFrameDecoder decoder(64); + std::vector> decoded; + CHECK_TRUE(decoder.push(encoded.data(), 2, &decoded, &error)); + CHECK_TRUE(decoded.empty()); + CHECK_TRUE(decoder.push(encoded.data() + 2, encoded.size() - 2, + &decoded, &error)); + CHECK_TRUE(decoded.size() == 1 && decoded.front() == payload); + return true; +} + +bool testPacketizer() +{ + auto descriptor = videoDescriptor("camera-test/video/color"); + media::MediaFrame::Config frame_config; + frame_config.descriptor = descriptor; + frame_config.payload.resize(2500, 0x5a); + frame_config.sequence = 11; + frame_config.capture_time_ns = 1234567000ULL; + frame_config.key_frame = true; + auto frame = media::makeMediaFrame(std::move(frame_config)); + + quic_edge::DatagramPacketizer packetizer(100); + std::vector packets; + std::string error; + const std::uint32_t token = + quic_edge::descriptorGenerationToken(descriptor->generation); + CHECK_TRUE(packetizer.packetize(*frame, 7, token, 99, 11, true, 1200, + &packets, &error)); + CHECK_TRUE(packets.size() == 3); + std::size_t payload_bytes = 0; + for (std::size_t index = 0; index < packets.size(); ++index) { + quic_edge::DatagramHeader header; + CHECK_TRUE(quic_edge::DatagramPacketizer::decodeHeader( + packets[index].bytes, &header, &error)); + CHECK_TRUE(header.track_id == 7 && header.session_epoch == 99); + CHECK_TRUE(header.fragment_index == index && header.fragment_count == 3); + CHECK_TRUE((header.flags & quic_edge::DATAGRAM_FLAG_KEY_FRAME) != 0); + CHECK_TRUE((header.flags & quic_edge::DATAGRAM_FLAG_DISCONTINUITY) != 0); + payload_bytes += header.payload_size; + } + CHECK_TRUE(payload_bytes == frame->size()); + return true; +} + +bool testServiceWithSharedHub() +{ + const std::string track_id = "camera-test/video/color"; + media::MediaSourceHub hub; + media::MediaSourceHub::FrameSink sink; + std::mutex sink_mutex; + std::atomic source_started{false}; + std::atomic keyframe_requests{0}; + media::MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceHub::FrameSink& value, + const media::MediaSourceHub::CancelPredicate&) { + std::lock_guard lock(sink_mutex); + sink = value; + source_started.store(true); + return true; + }; + callbacks.stop = [&]() { source_started.store(false); }; + callbacks.request_key_frame = [&]() { + ++keyframe_requests; + return true; + }; + auto descriptor = videoDescriptor(track_id); + CHECK_TRUE(hub.registerSource(descriptor, std::move(callbacks), 8)); + + auto transport = std::make_unique(true, true, true, 1U); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService service( + validConfig(track_id), std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { return source_started.load(); })); + CHECK_TRUE(waitUntil([&]() { + return service.stats().registrations_accepted == 1U && + service.stats().heartbeats_acknowledged >= 1U; + })); + CHECK_TRUE(transport_view->lastRegisteredNodeId() == "test-node"); + CHECK_TRUE(transport_view->lastGrpcEndpointPort() == 50052U); + CHECK_TRUE(transport_view->connectCount() == 1U); + + media::MediaFrame::Config frame_config; + frame_config.descriptor = descriptor; + frame_config.payload.resize(1800, 0x11); + frame_config.sequence = 1; + frame_config.capture_time_ns = 1000000; + frame_config.key_frame = true; + media::MediaSourceHub::FrameSink publisher; + { + std::lock_guard lock(sink_mutex); + publisher = sink; + } + CHECK_TRUE(static_cast(publisher)); + publisher(media::makeMediaFrame(std::move(frame_config))); + CHECK_TRUE(waitUntil([&]() { return transport_view->datagramBatchCount() == 1; })); + CHECK_TRUE(service.stats().frames_queued == 1); + CHECK_TRUE(keyframe_requests.load() != 0); + + // Codec initialization bytes may be learned at the first encoder IDR + // without changing the source generation. The reliable descriptor must be + // refreshed rather than treating this legal enrichment as a source error. + auto enriched_descriptor_config = media::TrackDescriptor::Config{}; + enriched_descriptor_config.id = track_id; + enriched_descriptor_config.source_id = "camera-test"; + enriched_descriptor_config.kind = media::MediaKind::VIDEO; + enriched_descriptor_config.codec = media::Codec::H264; + enriched_descriptor_config.payload_format = media::PayloadFormat::ANNEX_B; + enriched_descriptor_config.time_base = {1, 90000}; + enriched_descriptor_config.width = 640; + enriched_descriptor_config.height = 480; + enriched_descriptor_config.nominal_rate = 30; + enriched_descriptor_config.generation = descriptor->generation; + enriched_descriptor_config.codec_config = {0, 0, 0, 1, 0x67}; + auto enriched_descriptor = + media::makeTrackDescriptor(std::move(enriched_descriptor_config)); + media::MediaFrame::Config enriched_frame; + enriched_frame.descriptor = std::move(enriched_descriptor); + enriched_frame.payload.resize(256, 0x22); + // Simulate a source restart that resets its device-local sequence. The + // QUIC wire sequence must remain monotonic inside the media epoch. + enriched_frame.sequence = 1; + enriched_frame.capture_time_ns = 2000000; + enriched_frame.key_frame = true; + enriched_frame.discontinuity = true; + publisher(media::makeMediaFrame(std::move(enriched_frame))); + CHECK_TRUE(waitUntil([&]() { return transport_view->datagramBatchCount() == 2; })); + const auto wire_sequences = transport_view->datagramFrameSequences(); + CHECK_TRUE(wire_sequences.size() == 2U && + wire_sequences[0] == 1U && wire_sequences[1] == 2U); + CHECK_TRUE(transport_view->controlCount() >= 3); + CHECK_TRUE(transport_view->controlSequencesStrictlyIncreasing()); + CHECK_TRUE(service.state() != quic_edge::QuicEdgeServiceState::FAILED); + service.stop(); + return true; +} + +bool testMissingInjectedSourceRetriesSafely() +{ + media::MediaSourceHub hub; + auto transport = std::make_unique(); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService service( + validConfig("missing/video/color"), std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { return service.stats().source_errors >= 2; })); + CHECK_TRUE(service.state() == quic_edge::QuicEdgeServiceState::ONLINE); + CHECK_TRUE(service.status().registered); + CHECK_TRUE(service.status().active_media_tracks == 0U); + CHECK_TRUE(transport_view->connectCount() == 1U); + service.stop(); + return true; +} + +bool testPresenceOnlyWithoutMedia() +{ + media::MediaSourceHub hub; + auto transport = std::make_unique(); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService service( + validPresenceOnlyConfig(), std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return service.stats().registrations_accepted == 1U && + service.stats().heartbeats_acknowledged >= 1U; + })); + const auto status = service.status(); + CHECK_TRUE(status.registered); + CHECK_TRUE(status.session_id == "test-session"); + CHECK_TRUE(status.observed_source_ip == "203.0.113.11"); + CHECK_TRUE(status.active_media_tracks == 0U); + CHECK_TRUE(transport_view->datagramBatchCount() == 0U); + CHECK_TRUE(transport_view->connectCount() == 1U); + CHECK_TRUE(service.state() == quic_edge::QuicEdgeServiceState::ONLINE); + service.stop(); + return true; +} + +bool testDeviceManagerSnapshotInHeartbeat() +{ + device::DeviceManagerSnapshot snapshot; + snapshot.name = "edge-device-manager"; + snapshot.version = "2.3.4"; + snapshot.description = "heartbeat snapshot test"; + + device::ManagedDeviceSnapshot disabled; + disabled.id = "camera-disabled"; + disabled.kind = device::DeviceKind::Camera; + disabled.type_name = "DEVICE_TYPE_CAMERA"; + disabled.enabled = false; + disabled.state = device::ManagedDeviceState::Disabled; + disabled.health.state = device::DeviceHealthState::Unknown; + disabled.status_updated_at_unix_ms = 101U; + snapshot.devices.push_back(disabled); + + device::ManagedDeviceSnapshot running; + running.id = "src1100"; + running.kind = device::DeviceKind::AGV; + running.type_name = "Src1100Agv"; + running.enabled = true; + running.state = device::ManagedDeviceState::Running; + running.health.state = device::DeviceHealthState::Healthy; + running.status_updated_at_unix_ms = 202U; + snapshot.devices.push_back(running); + + device::ManagedDeviceSnapshot failed; + failed.id = "microphone-failed"; + failed.kind = device::DeviceKind::Microphone; + failed.type_name = "FfmpegMicrophone"; + failed.enabled = true; + failed.state = device::ManagedDeviceState::Error; + failed.health.state = device::DeviceHealthState::Fault; + failed.abnormal = true; + failed.error_message = "device start returned false"; + failed.status_updated_at_unix_ms = 303U; + snapshot.devices.push_back(failed); + + media::MediaSourceHub hub; + auto transport = std::make_unique(); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService service( + validPresenceOnlyConfig(), std::move(transport), hub, + [snapshot]() { return snapshot; }); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return service.stats().heartbeats_acknowledged >= 1U; + })); + const auto heartbeat = transport_view->lastHeartbeat(); + service.stop(); + + CHECK_TRUE(heartbeat.has_device_manager()); + CHECK_TRUE(heartbeat.device_manager().manager_name() == + "edge-device-manager"); + CHECK_TRUE(heartbeat.device_manager().manager_version() == "2.3.4"); + CHECK_TRUE(heartbeat.device_manager().manager_description() == + "heartbeat snapshot test"); + CHECK_TRUE(heartbeat.device_manager().sampled_at_unix_ms() == + heartbeat.sent_at_unix_ms()); + CHECK_TRUE(heartbeat.device_manager().devices_size() == 2); + + const auto& wire_failed = heartbeat.device_manager().devices(0); + CHECK_TRUE(wire_failed.device_id() == "microphone-failed"); + CHECK_TRUE(wire_failed.enabled()); + CHECK_TRUE(wire_failed.kind() == + cmvr::quic_edge::v1::DEVICE_KIND_MICROPHONE); + CHECK_TRUE(wire_failed.manager_state() == + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_ERROR); + CHECK_TRUE(wire_failed.health() == + cmvr::quic_edge::v1::DEVICE_HEALTH_STATUS_FAULT); + CHECK_TRUE(wire_failed.has_error()); + CHECK_TRUE(wire_failed.error_message() == + "device start returned false"); + + const auto& wire_running = heartbeat.device_manager().devices(1); + CHECK_TRUE(wire_running.device_id() == "src1100"); + CHECK_TRUE(wire_running.enabled()); + CHECK_TRUE(wire_running.kind() == + cmvr::quic_edge::v1::DEVICE_KIND_AGV); + CHECK_TRUE(wire_running.manager_state() == + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_RUNNING); + CHECK_TRUE(wire_running.health() == + cmvr::quic_edge::v1::DEVICE_HEALTH_STATUS_HEALTHY); + CHECK_TRUE(!wire_running.has_error()); + for (const auto& wire_device : heartbeat.device_manager().devices()) { + CHECK_TRUE(wire_device.enabled()); + CHECK_TRUE(wire_device.device_id() != "camera-disabled"); + } + return true; +} + +bool testAllDeviceKindAndStateMappings() +{ + using ProtoKind = cmvr::quic_edge::v1::DeviceKind; + using ProtoState = cmvr::quic_edge::v1::ManagedDeviceState; + using ProtoHealth = cmvr::quic_edge::v1::DeviceHealthStatus; + const std::vector> kinds{ + {device::DeviceKind::Unknown, + cmvr::quic_edge::v1::DEVICE_KIND_UNSPECIFIED}, + {device::DeviceKind::AGV, cmvr::quic_edge::v1::DEVICE_KIND_AGV}, + {device::DeviceKind::Arm, cmvr::quic_edge::v1::DEVICE_KIND_ARM}, + {device::DeviceKind::Battery, + cmvr::quic_edge::v1::DEVICE_KIND_BATTERY}, + {device::DeviceKind::BioHead, + cmvr::quic_edge::v1::DEVICE_KIND_BIO_HEAD}, + {device::DeviceKind::Camera, + cmvr::quic_edge::v1::DEVICE_KIND_CAMERA}, + {device::DeviceKind::CanBus, + cmvr::quic_edge::v1::DEVICE_KIND_CAN_BUS}, + {device::DeviceKind::DexHand, + cmvr::quic_edge::v1::DEVICE_KIND_DEX_HAND}, + {device::DeviceKind::Gripper, + cmvr::quic_edge::v1::DEVICE_KIND_GRIPPER}, + {device::DeviceKind::Microphone, + cmvr::quic_edge::v1::DEVICE_KIND_MICROPHONE}, + {device::DeviceKind::Motor, + cmvr::quic_edge::v1::DEVICE_KIND_MOTOR}, + {device::DeviceKind::MotorSystem, + cmvr::quic_edge::v1::DEVICE_KIND_MOTOR_SYSTEM}, + {device::DeviceKind::Robot, + cmvr::quic_edge::v1::DEVICE_KIND_ROBOT}, + {device::DeviceKind::Speaker, + cmvr::quic_edge::v1::DEVICE_KIND_SPEAKER}, + }; + const std::vector> states{ + {device::ManagedDeviceState::Unknown, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_UNSPECIFIED}, + {device::ManagedDeviceState::Disabled, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_DISABLED}, + {device::ManagedDeviceState::Initializing, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_INITIALIZING}, + {device::ManagedDeviceState::Registered, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_REGISTERED}, + {device::ManagedDeviceState::Ready, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_READY}, + {device::ManagedDeviceState::Running, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_RUNNING}, + {device::ManagedDeviceState::Stopped, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_STOPPED}, + {device::ManagedDeviceState::Error, + cmvr::quic_edge::v1::MANAGED_DEVICE_STATE_ERROR}, + }; + const std::vector> health{ + {device::DeviceHealthState::Unknown, + cmvr::quic_edge::v1::DEVICE_HEALTH_STATUS_UNSPECIFIED}, + {device::DeviceHealthState::Healthy, + cmvr::quic_edge::v1::DEVICE_HEALTH_STATUS_HEALTHY}, + {device::DeviceHealthState::Degraded, + cmvr::quic_edge::v1::DEVICE_HEALTH_STATUS_DEGRADED}, + {device::DeviceHealthState::Fault, + cmvr::quic_edge::v1::DEVICE_HEALTH_STATUS_FAULT}, + }; + + device::DeviceManagerSnapshot snapshot; + snapshot.name = "mapping-test"; + for (std::size_t index = 0U; index < kinds.size(); ++index) { + device::ManagedDeviceSnapshot row; + row.id = std::string("kind-") + (index < 10U ? "0" : "") + + std::to_string(index); + row.kind = kinds[index].first; + row.type_name = "mapping"; + row.enabled = true; + row.state = states[index % states.size()].first; + row.health.state = health[index % health.size()].first; + row.abnormal = index % 2U != 0U; + snapshot.devices.push_back(std::move(row)); + } + + media::MediaSourceHub hub; + auto transport = std::make_unique(); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService service( + validPresenceOnlyConfig(), std::move(transport), hub, + [snapshot]() { return snapshot; }); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return service.stats().heartbeats_acknowledged >= 1U; + })); + const auto heartbeat = transport_view->lastHeartbeat(); + service.stop(); + + CHECK_TRUE(heartbeat.device_manager().devices_size() == + static_cast(kinds.size())); + for (std::size_t index = 0U; index < kinds.size(); ++index) { + const auto& row = + heartbeat.device_manager().devices(static_cast(index)); + CHECK_TRUE(row.kind() == kinds[index].second); + CHECK_TRUE(row.manager_state() == + states[index % states.size()].second); + CHECK_TRUE(row.health() == health[index % health.size()].second); + CHECK_TRUE(row.has_error() == (index % 2U != 0U)); + } + return true; +} + +bool testConfiguredHeartbeatIntervalWithoutGatewayOverride() +{ + media::MediaSourceHub hub; + auto transport = std::make_unique( + true, true, true, 0U, 0U); + FakeTransport* transport_view = transport.get(); + auto config = validPresenceOnlyConfig(); + config.set_heartbeat_interval_ms(400U); + quic_edge::QuicEdgeService service( + config, std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return transport_view->heartbeatCount() >= 3U; + })); + const auto intervals = transport_view->heartbeatIntervals(); + service.stop(); + + CHECK_TRUE(intervals.size() >= 2U); + CHECK_TRUE(intervals[0].count() >= 350); + CHECK_TRUE(intervals[1].count() >= 350); + CHECK_TRUE(intervals[0].count() <= 900); + CHECK_TRUE(intervals[1].count() <= 900); + return true; +} + +bool testSnapshotLatencyDoesNotConsumeAckDeadline() +{ + media::MediaSourceHub hub; + auto transport = std::make_unique( + true, true, true, 0U, 250U, 70U); + FakeTransport* transport_view = transport.get(); + auto config = validPresenceOnlyConfig(); + config.set_control_response_timeout_ms(100U); + quic_edge::QuicEdgeService service( + config, std::move(transport), hub, [] { + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + return device::DeviceManagerSnapshot{}; + }); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return service.stats().heartbeats_acknowledged >= 2U; + })); + CHECK_TRUE(transport_view->connectCount() == 1U); + CHECK_TRUE(service.stats().heartbeat_timeouts == 0U); + service.stop(); + return true; +} + +bool testHeartbeatTimeoutReconnectsWithoutTaskFailure() +{ + media::MediaSourceHub hub; + auto transport = std::make_unique(true, false); + FakeTransport* transport_view = transport.get(); + auto config = validPresenceOnlyConfig(); + config.set_control_response_timeout_ms(20U); + quic_edge::QuicEdgeService service(config, std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return service.stats().heartbeat_timeouts >= 1U && + transport_view->connectCount() >= 2U; + })); + CHECK_TRUE(service.state() != quic_edge::QuicEdgeServiceState::FAILED); + service.stop(); + return true; +} + +bool testRegistrationRejectionBacksOff() +{ + media::MediaSourceHub hub; + auto transport = std::make_unique(false, true); + FakeTransport* transport_view = transport.get(); + auto config = validPresenceOnlyConfig(); + config.mutable_reconnect()->set_initial_delay_ms(20U); + config.mutable_reconnect()->set_maximum_delay_ms(80U); + quic_edge::QuicEdgeService service( + config, std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return service.stats().registrations_rejected >= 3U && + transport_view->connectCount() >= 4U; + })); + CHECK_TRUE(service.stats().heartbeats_sent == 0U); + const auto intervals = transport_view->connectIntervals(); + CHECK_TRUE(intervals.size() >= 3U); + CHECK_TRUE(intervals[0].count() >= 15); + CHECK_TRUE(intervals[1].count() >= 35); + CHECK_TRUE(intervals[2].count() >= 70); + CHECK_TRUE(service.state() != quic_edge::QuicEdgeServiceState::FAILED); + service.stop(); + return true; +} + +bool testHeartbeatAckRequiresSessionId() +{ + media::MediaSourceHub hub; + auto transport = std::make_unique(true, true, false); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService service( + validPresenceOnlyConfig(), std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return transport_view->connectCount() >= 2U && + service.stats().heartbeats_sent >= 2U; + })); + CHECK_TRUE(service.stats().heartbeats_acknowledged == 0U); + CHECK_TRUE(service.state() != quic_edge::QuicEdgeServiceState::FAILED); + service.stop(); + return true; +} + +bool testSlowMediaStartDoesNotBlockHeartbeat() +{ + const std::string track_id = "slow-camera/video/color"; + media::MediaSourceHub hub; + std::atomic start_entered{false}; + std::atomic start_exited{false}; + std::atomic release_start{false}; + media::MediaSourceHub::SourceCallbacks callbacks; + callbacks.start = [&](const media::MediaSourceHub::FrameSink&, + const media::MediaSourceHub::CancelPredicate& cancelled) { + start_entered.store(true); + while (!release_start.load() && !cancelled()) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + start_exited.store(true); + return !cancelled(); + }; + callbacks.stop = []() {}; + CHECK_TRUE(hub.registerSource( + videoDescriptor(track_id), std::move(callbacks), 8U)); + + auto transport = std::make_unique(); + FakeTransport* transport_view = transport.get(); + quic_edge::QuicEdgeService service( + validConfig(track_id), std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + + const bool entered = waitUntil([&]() { return start_entered.load(); }); + const bool heartbeat_while_blocked = entered && waitUntil([&]() { + return service.stats().heartbeats_acknowledged >= 1U; + }); + const bool registered_while_blocked = service.status().registered; + auto stop_future = std::async(std::launch::async, [&service] { + service.stop(); + }); + const bool stop_while_start_blocked = + stop_future.wait_for(std::chrono::milliseconds(500)) == + std::future_status::ready; + release_start.store(true); + stop_future.get(); + const bool start_cancelled = waitUntil([&]() { return start_exited.load(); }); + + CHECK_TRUE(entered); + CHECK_TRUE(heartbeat_while_blocked); + CHECK_TRUE(registered_while_blocked); + CHECK_TRUE(stop_while_start_blocked); + CHECK_TRUE(start_cancelled); + CHECK_TRUE(transport_view->connectCount() == 1U); + CHECK_TRUE(transport_view->controlSequencesStrictlyIncreasing()); + return true; +} + +bool testLifecycleStateGuards() +{ + media::MediaSourceHub hub; + { + auto transport = std::make_unique(); + quic_edge::QuicEdgeService service( + validPresenceOnlyConfig(), std::move(transport), hub); + service.stop(); + CHECK_TRUE(service.state() == + quic_edge::QuicEdgeServiceState::UNINITIALIZED); + std::string error; + CHECK_TRUE(!service.start(&error)); + } + + { + auto transport = std::make_unique(); + quic_edge::QuicEdgeService service( + validPresenceOnlyConfig(), std::move(transport), hub); + std::string error; + CHECK_TRUE(service.initialize(&error)); + CHECK_TRUE(service.start(&error)); + CHECK_TRUE(waitUntil([&]() { + return service.stats().registrations_accepted == 1U; + })); + error.clear(); + CHECK_TRUE(!service.initialize(&error)); + CHECK_TRUE(!error.empty()); + service.stop(); + } + return true; +} + +} // namespace + +int main() +{ + if (!testControlFraming() || !testPacketizer() || + !testServiceWithSharedHub() || !testMissingInjectedSourceRetriesSafely() || + !testPresenceOnlyWithoutMedia() || + !testDeviceManagerSnapshotInHeartbeat() || + !testAllDeviceKindAndStateMappings() || + !testConfiguredHeartbeatIntervalWithoutGatewayOverride() || + !testSnapshotLatencyDoesNotConsumeAckDeadline() || + !testHeartbeatTimeoutReconnectsWithoutTaskFailure() || + !testRegistrationRejectionBacksOff() || + !testHeartbeatAckRequiresSessionId() || + !testSlowMediaStartDoesNotBlockHeartbeat() || + !testLifecycleStateGuards()) { + return 1; + } + std::cout << "quic_edge_protocol_test: PASS\n"; + return 0; +} diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index c8a2960f..e16bbb5f 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/cmvr-es/task/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(task touch_screen_task/src/touch_screen_task.cpp + self_collision_task/src/self_collision_task.cpp ) target_include_directories(task PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -10,6 +11,7 @@ target_link_libraries(task cmvr_es::common cmvr_es::ik_solver cmvr_es::base_motion + cmvr_es::self_collision_checker PRIVATE cmvr_es::device_manager ) diff --git a/cmvr-es/task/README.md b/cmvr-es/task/README.md new file mode 100644 index 00000000..aa6fc735 --- /dev/null +++ b/cmvr-es/task/README.md @@ -0,0 +1,184 @@ +# Task 模块开发指南 + +`task/` 保存由 TaskManager 管理的可运行模块。任务分为周期调度任务和自持线程/事件循环的服务任务。 + +返回[项目总览](../../README.md)。 + +## Task 契约 + +所有任务实现 [`task.h`](task.h) 中的接口： + +- `id()`：配置和管理器使用的稳定唯一 ID； +- `runMode()`：周期任务或服务任务； +- `init()`：配置校验和本地资源准备； +- `start()`：启动执行，必须快速返回； +- `step(dt)`：周期任务的一次计算； +- `stop()`：幂等停止并回收任务拥有的资源； +- `state()` 和状态辅助接口：线程安全地暴露状态； +- `detailStatusString()`：提供可诊断状态，不包含密钥。 + +析构函数应安全调用 `stop()`。异步状态、错误和统计必须使用 mutex/atomic 保护。 + +## 运行模式 + +### PERIODIC_STEP + +- 由 TaskManager scheduler 调用 `step()`； +- 配置 `control_period_s` 必须是有限正数； +- 只有 task state 为 `RUNNING` 时执行 step； +- 所有周期任务共享一个 scheduler 线程； +- step 不能做阻塞网络 I/O 或无界计算； +- step 返回失败时应更新任务状态和可诊断错误； +- 不要在 step 内同步停止 TaskManager。 + +### BLOCKING_SERVICE + +- TaskManager 不调用其 `step()`； +- start 必须创建自己的 server/worker 后快速返回； +- stop 必须关闭 server、唤醒等待并 join 线程； +- 不能把 `BLOCKING_SERVICE` 理解为 start 可以永久阻塞。 + +`GrpcServerTask` 和 `QuicEdgeTask` 是当前服务任务参考实现。 + +## 新增任务的完整链路 + +### 1. 配置 Proto + +在 `protos/cmvr/config/` 新增： + +```protobuf +message ExampleTaskConfig { + string id = 1; +} + +message ExampleTaskRootConfig { + ExampleTaskConfig example_task = 1; +} +``` + +在 `TaskConfigEntry::TaskType` 中使用新的、稳定的枚举值。当前 `2` 和 `4` 已 reserved，不得复用。 + +### 2. 实现目录 + +```text +task/example_task/ +├── CMakeLists.txt +├── include/example_task.h +├── src/example_task.cpp +└── tests/example_task_test.cpp +``` + +实现 `Task` 全部接口。持续运行任务的典型状态转换为： + +```text +UNINITIALIZED -> IDLE -> RUNNING -> STOPPED + └-------> FAILED +``` + +持续周期任务必须在 start 后进入 `RUNNING`，否则 scheduler 不会调用 step。命令驱动任务可以在 start 后保持 `IDLE`，收到外部命令后再进入 `RUNNING`，当前 TouchScreenTask 就采用这种方式。 + +init、start 或 step 都可能进入 `FAILED`，一次性任务也可以进入 `SUCCEEDED`。失败后是否允许重新 init/start 必须在类注释和测试中明确。 + +### 3. Creator 与注册 + +creator 应： + +1. 校验 manager entry ID 和 config file； +2. 使用 `ConfigHelper` 加载 root config； +3. 校验子配置 ID 与 manager entry ID 一致； +4. 只创建对象，不进行耗时外部连接； +5. 返回 `nullptr` 并记录明确错误。 + +提供： + +```cpp +void registerExampleTaskFactory(); +``` + +通过 [`task_factory.h`](task_factory.h) 的 `TaskFactory::registerCreator()` 注册。 + +`registerCreator()` 对相同 TaskType 会覆盖旧 creator，当前不会报错。不要重复注册。creator 在 registry mutex 持有期间执行，因此不能从 creator 递归调用 register/create。 + +### 4. main 注册 + +在 `TaskManager::getInstance()` 之前显式调用注册函数。当前位置见 [`../main.cpp`](../main.cpp)。 + +TouchScreen 是 TaskFactory 内硬编码的历史实现；新任务优先使用显式 registry 模式。 + +### 5. TaskManager 映射 + +在 [`../manager/task_manager/src/task_manager.cpp`](../manager/task_manager/src/task_manager.cpp) 的 TaskType 字符串映射中增加新类型，否则计划日志会显示 unknown。 + +### 6. 默认配置 + +新增： + +```text +cmvr-es/config/tasks/example_task/example_task.pb.txt +``` + +并在 `config/manager/task_manager.pb.txt` 增加 entry。依赖硬件、网络或证书的新任务默认 `enable: false`。 + +配置 `run_mode` 必须和 task 的 `runMode()` 一致，否则 TaskManager 会跳过该任务。 + +### 7. CMake + +- 为任务建立独立 library target； +- 添加项目命名空间 alias； +- 在 [`CMakeLists.txt`](CMakeLists.txt) 或 [`../CMakeLists.txt`](../CMakeLists.txt) 加入子目录； +- 根 `cmvr_es` 必须链接注册函数所在 target，避免静态库未被带入； +- 测试放在 `if(BUILD_TESTING)`； +- 使用 `add_test()` 登记到 CTest。 + +参考 [`quic_edge_task/CMakeLists.txt`](quic_edge_task/CMakeLists.txt)。 + +## TaskManager 当前语义 + +- init 按配置顺序执行； +- init 失败只跳过该任务，进程继续； +- ID 重复会跳过后加入项； +- run mode 不匹配会跳过； +- `TASK_RUN_MODE_UNKNOWN` 当前会记录错误并退化为 `PERIODIC_STEP`，配置不得依赖该历史行为； +- start 阶段任一任务失败会停止此前已启动任务； +- 返回 false 的 task 必须自行清理此次 start 的部分资源； +- 任务保存在 unordered_map，不能依赖 start/stop 顺序； +- manager 处于 running 状态时，stop 先停止 scheduler，再调用每个任务 stop； +- 未启动或已经停止时，`stopRunTask()` 会直接返回； +- TaskManager 单例已存在时传入新配置不会热更新。 + +任务之间有依赖时，应由一个协调 task 显式管理，或增加明确依赖模型；不能依赖配置顺序碰巧变成启动顺序。 + +## 测试最低要求 + +- 缺失配置、空 ID 和 ID mismatch； +- run-mode mismatch； +- init/start/stop 状态转换； +- stop 重复调用； +- start 失败后的资源清理； +- 周期 dt、调度延迟和 step 失败； +- service worker 正常 shutdown； +- stop 发生在阻塞等待期间； +- 析构时仍处于 RUNNING； +- fake 依赖下的无设备运行。 + +单项测试： + +```bash +cmake --build build --target quic_edge_task_test +ctest \ + --test-dir build \ + -R '^quic_edge_task_test$' \ + --output-on-failure +``` + +## 提交检查 + +- [ ] 使用新的 TaskType 枚举号 +- [ ] 配置 root message、默认配置和 ID 一致 +- [ ] creator 与 main 注册均已完成 +- [ ] run mode 与配置一致 +- [ ] start 快速返回 +- [ ] stop 幂等并 join 所有线程 +- [ ] 状态查询线程安全 +- [ ] CMake target 被根程序链接 +- [ ] 测试已登记到 CTest diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index b865e8f1..055e2eed 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -55,6 +55,7 @@ private: std::unique_ptr dexhand_service_; std::unique_ptr biohand_service_; std::unique_ptr arm_service_; + std::unique_ptr agv_service_; std::unique_ptr hlc_service_; }; diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index 4bab32b9..60d8c697 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -8,6 +8,7 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" #include "common/config/config_files.h" +#include "service/grpc/include/grpc_agv_service.h" #include "service/grpc/include/grpc_arm_service.h" #include "service/grpc/include/grpc_camera_service.h" #include "service/grpc/include/grpc_dexhand_service.h" @@ -94,13 +95,17 @@ bool GrpcServerTask::start() const std::string port = cfg_.port().empty() ? "50051" : cfg_.port(); const std::string local_address = host + ":" + port; - camera_service_ = std::make_unique(); + camera_service_ = std::make_unique( + service::makeCameraStreamLowLatencyConfig( + cfg_.camera_stream_max_pending_frames(), + cfg_.camera_stream_max_frame_age_ms())); system_service_ = std::make_unique(); speaker_service_ = std::make_unique(); microphone_service_ = std::make_unique(); dexhand_service_ = std::make_unique(); biohand_service_ = std::make_unique(); arm_service_ = std::make_unique(); + agv_service_ = std::make_unique(); hlc_service_ = std::make_unique(); grpc::ServerBuilder builder; @@ -112,6 +117,7 @@ bool GrpcServerTask::start() builder.RegisterService(dexhand_service_.get()); builder.RegisterService(biohand_service_.get()); builder.RegisterService(arm_service_.get()); + builder.RegisterService(agv_service_.get()); builder.RegisterService(hlc_service_.get()); server_ = builder.BuildAndStart(); @@ -247,6 +253,7 @@ void GrpcServerTask::waitLoop() void GrpcServerTask::clearServices() { hlc_service_.reset(); + agv_service_.reset(); arm_service_.reset(); biohand_service_.reset(); dexhand_service_.reset(); diff --git a/cmvr-es/task/quic_edge_task/CMakeLists.txt b/cmvr-es/task/quic_edge_task/CMakeLists.txt new file mode 100644 index 00000000..009388df --- /dev/null +++ b/cmvr-es/task/quic_edge_task/CMakeLists.txt @@ -0,0 +1,27 @@ +add_library(quic_edge_task STATIC src/quic_edge_task.cpp) +target_compile_features(quic_edge_task PUBLIC cxx_std_17) +target_include_directories(quic_edge_task PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es) +target_link_libraries(quic_edge_task + PUBLIC + cmvr_es::task + cmvr_es::quic_edge_service + PRIVATE + cmvr_es::proto + cmvr_es::logging + cmvr_es::device_manager +) +add_library(cmvr_es::quic_edge_task ALIAS quic_edge_task) + +if(BUILD_TESTING) + add_executable(quic_edge_task_test tests/quic_edge_task_test.cpp) + target_compile_features(quic_edge_task_test PRIVATE cxx_std_17) + target_link_libraries(quic_edge_task_test PRIVATE cmvr_es::quic_edge_task) + add_test(NAME quic_edge_task_test COMMAND quic_edge_task_test) + if(UNIX AND NOT APPLE) + get_property(_quic_task_test_library_dirs DIRECTORY PROPERTY LINK_DIRECTORIES) + list(PREPEND _quic_task_test_library_dirs "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime") + list(JOIN _quic_task_test_library_dirs ":" _quic_task_test_library_path) + set_tests_properties(quic_edge_task_test PROPERTIES + ENVIRONMENT "LD_LIBRARY_PATH=${_quic_task_test_library_path}") + endif() +endif() diff --git a/cmvr-es/task/quic_edge_task/include/quic_edge_task.h b/cmvr-es/task/quic_edge_task/include/quic_edge_task.h new file mode 100644 index 00000000..b5c760b5 --- /dev/null +++ b/cmvr-es/task/quic_edge_task/include/quic_edge_task.h @@ -0,0 +1,49 @@ +#ifndef CMVR_ES_QUIC_EDGE_TASK_H +#define CMVR_ES_QUIC_EDGE_TASK_H + +#include +#include +#include + +#include "cmvr/config/quic_edge_config/quic_edge_config.pb.h" +#include "service/quic_edge/include/quic_edge_service.h" +#include "task/task.h" + +namespace cmvr::task { + +class QuicEdgeTask final : public Task { +public: + explicit QuicEdgeTask(const config::QuicEdgeConfig& config); + ~QuicEdgeTask() override; + + const std::string& id() const override { return id_; } + TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; } + bool init() override; + bool start() override; + bool step(double dt) override; + void stop() override; + + TaskState state() const override; + bool isBusy() const override; + bool isFinished() const override; + bool isFailed() const override; + std::string stateString() const override; + std::string detailStatusString() const override; + +private: + TaskState mappedState() const; + + config::QuicEdgeConfig config_; + std::string id_; + std::unique_ptr service_; + + mutable std::mutex mutex_; + TaskState state_{TaskState::UNINITIALIZED}; + std::string last_error_; +}; + +void registerQuicEdgeTaskFactory(); + +} // namespace cmvr::task + +#endif // CMVR_ES_QUIC_EDGE_TASK_H diff --git a/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp new file mode 100644 index 00000000..7d32ecde --- /dev/null +++ b/cmvr-es/task/quic_edge_task/src/quic_edge_task.cpp @@ -0,0 +1,195 @@ +#include "task/quic_edge_task/include/quic_edge_task.h" + +#include +#include + +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" +#include "common/base/logging/logger.h" +#include "common/config/config_files.h" +#include "manager/device_manager/include/device_manager.h" +#include "task/task_factory.h" + +namespace cmvr::task { +namespace { + +std::shared_ptr createQuicEdgeTask(const config::TaskConfigEntry& entry) +{ + if (entry.id().empty() || entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[QuicEdgeTask] Task id or config_file is empty"; + return nullptr; + } + config::QuicEdgeRootConfig root; + if (!ConfigHelper::loadConfigFile(entry.config_file(), root)) { + CMVR_LOG(ERROR) << "[QuicEdgeTask] Failed to load config: " + << entry.config_file(); + return nullptr; + } + config::QuicEdgeConfig config = root.quic_edge(); + if (config.id().empty() || config.id() != entry.id()) { + CMVR_LOG(ERROR) << "[QuicEdgeTask] Task ID mismatch: manager=" << entry.id() + << ", config=" << config.id(); + return nullptr; + } + if (!config.tls().ca_file().empty()) { + config.mutable_tls()->set_ca_file( + ConfigHelper::resolveConfigFile(config.tls().ca_file())); + } + if (!config.tls().certificate_file().empty()) { + config.mutable_tls()->set_certificate_file( + ConfigHelper::resolveConfigFile(config.tls().certificate_file())); + } + if (!config.tls().private_key_file().empty()) { + config.mutable_tls()->set_private_key_file( + ConfigHelper::resolveConfigFile(config.tls().private_key_file())); + } + if (config.software_version().empty()) { + config.set_software_version(device::DeviceManager::getInstance().version()); + } + return std::make_shared(config); +} + +} // namespace + +QuicEdgeTask::QuicEdgeTask(const config::QuicEdgeConfig& config) + : config_(config), id_(config.id()) +{ +} + +QuicEdgeTask::~QuicEdgeTask() +{ + stop(); +} + +bool QuicEdgeTask::init() +{ + std::lock_guard lock(mutex_); + if (id_.empty()) { + last_error_ = "QUIC edge task id is empty"; + state_ = TaskState::FAILED; + return false; + } + service_ = std::make_unique(config_); + std::string error; + if (!service_->initialize(&error)) { + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + last_error_.clear(); + state_ = TaskState::IDLE; + return true; +} + +bool QuicEdgeTask::start() +{ + std::lock_guard lock(mutex_); + if (state_ == TaskState::RUNNING) return true; + if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) || !service_) { + last_error_ = "QUIC edge task is not initialized"; + state_ = TaskState::FAILED; + return false; + } + std::string error; + if (!service_->start(&error)) { + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + last_error_.clear(); + state_ = TaskState::RUNNING; + CMVR_LOG(INFO) << "[QuicEdgeTask] Started, id=" << id_; + return true; +} + +bool QuicEdgeTask::step(const double dt) +{ + (void)dt; + return !isFailed(); +} + +void QuicEdgeTask::stop() +{ + std::lock_guard lock(mutex_); + // Keep ownership stable for the complete stop. init() may replace the + // unique service instance and therefore must not race a raw pointer here. + if (service_) service_->stop(); + if (state_ != TaskState::FAILED) state_ = TaskState::STOPPED; +} + +TaskState QuicEdgeTask::mappedState() const +{ + if (!service_ || state_ != TaskState::RUNNING) return state_; + return service_->state() == quic_edge::QuicEdgeServiceState::FAILED + ? TaskState::FAILED : state_; +} + +TaskState QuicEdgeTask::state() const +{ + std::lock_guard lock(mutex_); + return mappedState(); +} + +bool QuicEdgeTask::isBusy() const +{ + return state() == TaskState::RUNNING; +} + +bool QuicEdgeTask::isFinished() const +{ + return state() == TaskState::STOPPED; +} + +bool QuicEdgeTask::isFailed() const +{ + return state() == TaskState::FAILED; +} + +std::string QuicEdgeTask::stateString() const +{ + return taskStateToString(state()); +} + +std::string QuicEdgeTask::detailStatusString() const +{ + std::lock_guard lock(mutex_); + std::ostringstream output; + output << taskStateToString(mappedState()); + if (service_) { + const auto stats = service_->stats(); + const auto service_status = service_->status(); + output << " service=" << quic_edge::toString(service_->state()) + << " target=" << config_.server_host() << ':' << config_.server_port() + << " node_id=" << service_status.node_id + << " registered=" << service_status.registered + << " connections=" << stats.successful_connections + << " registrations=" << stats.registrations_accepted + << " heartbeat_sequence=" << service_status.heartbeat_sequence + << " heartbeat_acks=" << stats.heartbeats_acknowledged + << " media_tracks=" << service_status.active_media_tracks + << " frames=" << stats.frames_queued + << " datagrams=" << stats.datagrams_queued; + if (!service_status.session_id.empty()) { + output << " session=" << service_status.session_id; + } + if (!service_status.observed_source_ip.empty()) { + output << " observed_ip=" << service_status.observed_source_ip; + } + if (!service_status.last_media_error.empty()) { + output << " media_error=" << service_status.last_media_error; + } + const std::string service_error = service_->lastError(); + if (!service_error.empty()) output << " error=" << service_error; + } else if (!last_error_.empty()) { + output << " error=" << last_error_; + } + return output.str(); +} + +void registerQuicEdgeTaskFactory() +{ + TaskFactory::registerCreator( + config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE, + createQuicEdgeTask); +} + +} // namespace cmvr::task diff --git a/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp new file mode 100644 index 00000000..3157e4b6 --- /dev/null +++ b/cmvr-es/task/quic_edge_task/tests/quic_edge_task_test.cpp @@ -0,0 +1,22 @@ +#include + +#include "task/quic_edge_task/include/quic_edge_task.h" + +int main() +{ + cmvr::config::QuicEdgeConfig config; + config.set_id("quic-invalid-config-test"); + + cmvr::task::QuicEdgeTask task(config); + if (task.init() || task.state() != cmvr::task::TaskState::FAILED) { + std::cerr << "invalid QUIC task config was not rejected\n"; + return 1; + } + task.stop(); + if (task.state() != cmvr::task::TaskState::FAILED) { + std::cerr << "failed QUIC task did not preserve its failure state\n"; + return 1; + } + std::cout << "quic_edge_task_test: PASS\n"; + return 0; +} diff --git a/cmvr-es/task/self_collision_task/include/self_collision_task.h b/cmvr-es/task/self_collision_task/include/self_collision_task.h new file mode 100644 index 00000000..938207e8 --- /dev/null +++ b/cmvr-es/task/self_collision_task/include/self_collision_task.h @@ -0,0 +1,98 @@ +#ifndef CMVR_ES_SELF_COLLISION_TASK_H +#define CMVR_ES_SELF_COLLISION_TASK_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h" +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" +#include "cmvr/config/self_collision_task_config/self_collision_task_config.pb.h" +#include "devices/arm/robot_arm.h" +#include "task/task.h" + +namespace cmvr::task { + +enum class CollisionSafetyLevel { + UNKNOWN = 0, + SAFE, + WARNING, + STOP, +}; + +enum class ProtectiveRecoveryState { + IDLE = 0, + AVAILABLE, + RECOVERING, + SUCCEEDED, + FAILED, +}; + +struct SelfCollisionTaskStatus { + CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN}; + SelfCollisionResult result; + bool stop_latched{false}; + std::uint64_t event_id{0}; + ProtectiveRecoveryState recovery_state{ProtectiveRecoveryState::IDLE}; + std::size_t recovery_sample_count{0}; + std::string recovery_error; +}; + +class SelfCollisionTask final : public Task { +public: + explicit SelfCollisionTask(const config::SelfCollisionTaskConfig& config); + + const std::string& id() const override { return id_; } + TaskRunMode runMode() const override { return TaskRunMode::PERIODIC_STEP; } + + bool init() override; + bool start() override; + bool step(double dt) override; + void stop() override; + + TaskState state() const override; + bool isBusy() const override; + bool isFinished() const override; + bool isFailed() const override; + std::string stateString() const override; + std::string detailStatusString() const override; + + SelfCollisionTaskStatus latestStatus() const; + device::Result requestRecovery(std::uint64_t event_id); + +private: + static bool validateConfig(const config::SelfCollisionTaskConfig& config, + std::string* error); + static const char* safetyLevelToString(CollisionSafetyLevel level); + static const char* recoveryStateToString(ProtectiveRecoveryState state); + void recordJointSample_(const device::JointGroupState& joint_state, + DistanceSamplingPolicy::Clock::time_point now); + + config::SelfCollisionTaskConfig config_; + std::string id_; + std::shared_ptr arm_; + SelfCollisionChecker checker_; + DistanceSamplingPolicy sampling_; + + mutable std::mutex mutex_; + std::condition_variable recovery_cv_; + TaskState state_{TaskState::UNINITIALIZED}; + SelfCollisionTaskStatus latest_status_{}; + std::deque joint_history_; + device::JointTrajectory recovery_path_; + DistanceSamplingPolicy::Clock::time_point history_epoch_{}; + std::optional recovery_clear_since_; + double recovery_best_distance_m_{0.0}; + bool recovery_clear_confirmed_{false}; + std::uint64_t next_event_id_{1}; + std::string last_error_; +}; + +} // namespace cmvr::task + +#endif // CMVR_ES_SELF_COLLISION_TASK_H diff --git a/cmvr-es/task/self_collision_task/src/self_collision_task.cpp b/cmvr-es/task/self_collision_task/src/self_collision_task.cpp new file mode 100644 index 00000000..00464c60 --- /dev/null +++ b/cmvr-es/task/self_collision_task/src/self_collision_task.cpp @@ -0,0 +1,597 @@ +#include "task/self_collision_task/include/self_collision_task.h" + +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::task { + +SelfCollisionTask::SelfCollisionTask(const config::SelfCollisionTaskConfig& config) + : config_(config), + id_(config.id()) +{ +} + +bool SelfCollisionTask::validateConfig(const config::SelfCollisionTaskConfig& config, + std::string* error) +{ + auto fail = [error](const std::string& message) { + if (error) { + *error = message; + } + return false; + }; + + if (config.id().empty()) { + return fail("Self-collision task id is empty"); + } + if (config.arm_id().empty()) { + return fail("Self-collision task arm_id is empty"); + } + if (config.checker().urdf_path().empty()) { + return fail("Self-collision checker URDF path is empty"); + } + const auto& sampling = config.sampling(); + if (!std::isfinite(sampling.max_geometry_displacement_m()) || + sampling.max_geometry_displacement_m() <= 0.0) { + return fail("max_geometry_displacement_m must be finite and positive"); + } + if (!std::isfinite(sampling.max_check_period_s()) || + sampling.max_check_period_s() <= 0.0) { + return fail("max_check_period_s must be finite and positive"); + } + + const auto& safety = config.safety(); + if (!std::isfinite(safety.stop_distance_m()) || safety.stop_distance_m() < 0.0) { + return fail("stop_distance_m must be finite and non-negative"); + } + if (!std::isfinite(safety.warning_distance_m()) || + safety.warning_distance_m() < safety.stop_distance_m()) { + return fail("warning_distance_m must be finite and not less than stop_distance_m"); + } + const auto& recovery = config.recovery(); + if (!std::isfinite(recovery.clear_distance_m()) || + recovery.clear_distance_m() <= safety.warning_distance_m()) { + return fail("recovery clear_distance_m must be finite and greater than warning_distance_m"); + } + if (!std::isfinite(recovery.stable_period_s()) || + recovery.stable_period_s() <= 0.0) { + return fail("recovery stable_period_s must be finite and positive"); + } + if (!std::isfinite(recovery.max_joint_velocity_rad_s()) || + recovery.max_joint_velocity_rad_s() <= 0.0) { + return fail("recovery max_joint_velocity_rad_s must be finite and positive"); + } + if (!std::isfinite(recovery.max_joint_acceleration_rad_s2()) || + recovery.max_joint_acceleration_rad_s2() <= 0.0) { + return fail("recovery max_joint_acceleration_rad_s2 must be finite and positive"); + } + if (!std::isfinite(recovery.history_duration_s()) || + recovery.history_duration_s() <= 0.0) { + return fail("recovery history_duration_s must be finite and positive"); + } + if (!std::isfinite(recovery.max_distance_regression_m()) || + recovery.max_distance_regression_m() < 0.0) { + return fail("recovery max_distance_regression_m must be finite and non-negative"); + } + for (const auto& pair : config.checker().ignored_pairs()) { + if (pair.first().empty() || pair.second().empty() || pair.first() == pair.second()) { + return fail("ignored_pairs entries require two different non-empty links"); + } + } + if (error) { + error->clear(); + } + return true; +} + +bool SelfCollisionTask::init() +{ + std::string error; + if (!validateConfig(config_, &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + auto arm = device::DeviceManager::getInstance().getDevice( + config_.arm_id()); + if (!arm) { + std::lock_guard lock(mutex_); + last_error_ = "Robot arm not found: " + config_.arm_id(); + state_ = TaskState::FAILED; + return false; + } + + const auto model = arm->getRobotModel(); + if (!model.valid()) { + std::lock_guard lock(mutex_); + last_error_ = "Robot arm model is invalid: " + config_.arm_id(); + state_ = TaskState::FAILED; + return false; + } + + SelfCollisionOptions checker_options; + checker_options.ignored_pairs.reserve(config_.checker().ignored_pairs_size()); + for (const auto& pair : config_.checker().ignored_pairs()) { + checker_options.ignored_pairs.push_back({pair.first(), pair.second()}); + } + if (!checker_.init(config_.checker().urdf_path(), + model.joint_names, + checker_options, + &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + const DistanceSamplingOptions sampling_options{ + config_.sampling().max_geometry_displacement_m(), + config_.sampling().max_check_period_s(), + }; + if (!sampling_.configure(sampling_options, &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + { + std::lock_guard lock(mutex_); + arm_ = std::move(arm); + latest_status_ = {}; + joint_history_.clear(); + recovery_path_.clear(); + history_epoch_ = DistanceSamplingPolicy::Clock::now(); + recovery_clear_since_.reset(); + recovery_best_distance_m_ = 0.0; + recovery_clear_confirmed_ = false; + next_event_id_ = 1; + last_error_.clear(); + state_ = TaskState::IDLE; + } + CMVR_LOG(INFO) << "[SelfCollisionTask] Initialized id=" << id_ + << ", arm=" << config_.arm_id() + << ", dof=" << checker_.dof() + << ", active_pairs=" << checker_.activePairCount(); + return true; +} + +bool SelfCollisionTask::start() +{ + std::lock_guard lock(mutex_); + if (state_ == TaskState::RUNNING) { + return true; + } + if (state_ != TaskState::IDLE && state_ != TaskState::STOPPED) { + last_error_ = "Self-collision task is not initialized"; + state_ = TaskState::FAILED; + return false; + } + sampling_.reset(); + latest_status_ = {}; + joint_history_.clear(); + recovery_path_.clear(); + history_epoch_ = DistanceSamplingPolicy::Clock::now(); + recovery_clear_since_.reset(); + recovery_best_distance_m_ = 0.0; + recovery_clear_confirmed_ = false; + last_error_.clear(); + state_ = TaskState::RUNNING; + return true; +} + +bool SelfCollisionTask::step(const double dt) +{ + (void)dt; + std::shared_ptr arm; + { + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING) { + return false; + } + arm = arm_; + } + + const auto fail_monitoring = [&](std::string error) { + bool cancel_recovery = false; + { + std::lock_guard lock(mutex_); + cancel_recovery = + latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING; + if (cancel_recovery) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = error; + } + last_error_ = std::move(error); + state_ = TaskState::FAILED; + recovery_cv_.notify_all(); + } + if (cancel_recovery) { + (void)arm->protectiveStop(); + } + return false; + }; + + const auto joint_state = arm->getJointState(); + const auto model = arm->getRobotModel(); + if (!joint_state.validForModel(model)) { + return fail_monitoring("Robot arm returned an invalid joint state"); + } + + const auto now = DistanceSamplingPolicy::Clock::now(); + recordJointSample_(joint_state, now); + + CollisionGeometrySnapshot snapshot; + std::string error; + if (!checker_.makeSnapshot(joint_state.position, &snapshot, &error)) { + return fail_monitoring(std::move(error)); + } + + if (!sampling_.shouldCheck(snapshot, now)) { + return true; + } + + SelfCollisionResult result = checker_.check(snapshot); + if (!result.valid) { + return fail_monitoring( + result.error.empty() ? "Self-collision distance check failed" : result.error); + } + sampling_.markChecked(snapshot, now); + + CollisionSafetyLevel level = CollisionSafetyLevel::SAFE; + if (result.minimum_distance_m <= config_.safety().stop_distance_m()) { + level = CollisionSafetyLevel::STOP; + } else if (result.minimum_distance_m <= config_.safety().warning_distance_m()) { + level = CollisionSafetyLevel::WARNING; + } + + bool trigger_stop = false; + bool abort_recovery = false; + CollisionSafetyLevel previous_level = CollisionSafetyLevel::UNKNOWN; + { + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING) { + return true; + } + previous_level = latest_status_.level; + latest_status_.level = level; + latest_status_.result = result; + + if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) { + if (arm->isEmergencyStopped()) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = + "Protective recovery interrupted by emergency stop"; + recovery_clear_since_.reset(); + abort_recovery = true; + recovery_cv_.notify_all(); + } else if (result.minimum_distance_m + + config_.recovery().max_distance_regression_m() < + recovery_best_distance_m_) { + std::ostringstream stream; + stream << "Protective recovery distance regressed from " + << recovery_best_distance_m_ << " m to " + << result.minimum_distance_m << " m"; + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = stream.str(); + recovery_clear_since_.reset(); + abort_recovery = true; + recovery_cv_.notify_all(); + } else { + recovery_best_distance_m_ = std::max( + recovery_best_distance_m_, result.minimum_distance_m); + if (result.minimum_distance_m >= + config_.recovery().clear_distance_m()) { + if (!recovery_clear_since_) { + recovery_clear_since_ = now; + } else if (std::chrono::duration( + now - *recovery_clear_since_).count() >= + config_.recovery().stable_period_s()) { + recovery_clear_confirmed_ = true; + recovery_cv_.notify_all(); + } + } else { + recovery_clear_since_.reset(); + recovery_clear_confirmed_ = false; + } + } + } else if (level == CollisionSafetyLevel::STOP && + !latest_status_.stop_latched) { + latest_status_.stop_latched = true; + latest_status_.event_id = next_event_id_++; + latest_status_.recovery_state = ProtectiveRecoveryState::AVAILABLE; + latest_status_.recovery_error.clear(); + recovery_path_.assign(joint_history_.begin(), joint_history_.end()); + latest_status_.recovery_sample_count = recovery_path_.size(); + trigger_stop = true; + } else if (!latest_status_.stop_latched && + result.minimum_distance_m >= + config_.recovery().clear_distance_m()) { + joint_history_.clear(); + device::JointTrajectoryPoint sample; + sample.time_s = std::chrono::duration(now - history_epoch_).count(); + sample.position = joint_state.position; + sample.velocity = joint_state.velocity; + joint_history_.push_back(std::move(sample)); + } + } + + if (level != previous_level) { + if (level == CollisionSafetyLevel::SAFE) { + CMVR_LOG(INFO) << "[SelfCollisionTask] level=" << safetyLevelToString(level) + << ", distance_m=" << result.minimum_distance_m + << ", pair=" << result.first << "/" << result.second; + } else { + CMVR_LOG(WARNING) << "[SelfCollisionTask] level=" << safetyLevelToString(level) + << ", distance_m=" << result.minimum_distance_m + << ", pair=" << result.first << "/" << result.second; + } + } + + if (abort_recovery) { + (void)arm->protectiveStop(); + } else if (trigger_stop) { + const auto stop_result = arm->protectiveStop(); + if (!stop_result.ok()) { + std::lock_guard lock(mutex_); + last_error_ = "Protective stop failed: " + stop_result.message; + state_ = TaskState::FAILED; + return false; + } + } + return true; +} + +void SelfCollisionTask::recordJointSample_( + const device::JointGroupState& joint_state, + const DistanceSamplingPolicy::Clock::time_point now) +{ + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING || latest_status_.stop_latched) { + return; + } + + device::JointTrajectoryPoint sample; + sample.time_s = std::chrono::duration(now - history_epoch_).count(); + sample.position = joint_state.position; + sample.velocity = joint_state.velocity; + if (!joint_history_.empty() && sample.time_s <= joint_history_.back().time_s) { + return; + } + joint_history_.push_back(std::move(sample)); + + const double oldest_time_s = joint_history_.back().time_s - + config_.recovery().history_duration_s(); + while (joint_history_.size() > 1 && + joint_history_.front().time_s < oldest_time_s) { + joint_history_.pop_front(); + } +} + +device::Result SelfCollisionTask::requestRecovery(const std::uint64_t event_id) +{ + std::shared_ptr arm; + device::JointTrajectory path; + device::MotionOptions options; + { + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING) { + return device::Result::failure( + device::ArmErrorCode::RobotNotReady, + "Protective recovery rejected: collision task is not running"); + } + if (!latest_status_.stop_latched || + latest_status_.recovery_state != ProtectiveRecoveryState::AVAILABLE) { + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + "Protective recovery rejected: no recoverable collision stop is available"); + } + if (event_id == 0 || event_id != latest_status_.event_id) { + return device::Result::failure( + device::ArmErrorCode::InvalidArgument, + "Protective recovery rejected: event_id does not match the active stop"); + } + if (!arm_ || !arm_->isProtectiveStopped() || arm_->isEmergencyStopped()) { + return device::Result::failure( + arm_ && arm_->isEmergencyStopped() + ? device::ArmErrorCode::RobotInEmergencyStop + : device::ArmErrorCode::RobotNotReady, + "Protective recovery rejected: arm safety state is invalid"); + } + if (recovery_path_.size() < 2) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = + "Protective recovery path contains fewer than two samples"; + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + latest_status_.recovery_error); + } + + arm = arm_; + path = recovery_path_; + options.velocity = config_.recovery().max_joint_velocity_rad_s(); + options.acceleration = + config_.recovery().max_joint_acceleration_rad_s2(); + latest_status_.recovery_state = ProtectiveRecoveryState::RECOVERING; + latest_status_.recovery_error.clear(); + recovery_best_distance_m_ = latest_status_.result.minimum_distance_m; + recovery_clear_since_.reset(); + recovery_clear_confirmed_ = false; + } + + CMVR_LOG(INFO) << "[SelfCollisionTask] recovery requested event_id=" + << event_id << ", samples=" << path.size() + << ", max_joint_velocity_rad_s=" + << options.velocity + << ", max_joint_acceleration_rad_s2=" + << options.acceleration; + const auto playback_result = arm->recoverProtectiveStop(path, options); + if (!playback_result.ok()) { + std::lock_guard lock(mutex_); + if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = playback_result.message; + } + const std::string recovery_error = latest_status_.recovery_error; + recovery_cv_.notify_all(); + return device::Result::failure(playback_result.code, recovery_error); + } + + { + std::unique_lock lock(mutex_); + const auto clear_timeout = std::chrono::duration( + config_.recovery().stable_period_s() + 2.0); + const bool completed = recovery_cv_.wait_for(lock, clear_timeout, [&] { + return state_ != TaskState::RUNNING || recovery_clear_confirmed_ || + latest_status_.recovery_state != ProtectiveRecoveryState::RECOVERING; + }); + if (!completed || !recovery_clear_confirmed_) { + if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = completed + ? "Protective recovery ended before the clear distance was confirmed" + : "Protective recovery clear-distance confirmation timed out"; + } + const std::string error = latest_status_.recovery_error; + lock.unlock(); + (void)arm->protectiveStop(); + return device::Result::failure( + device::ArmErrorCode::CommandFailed, error); + } + + latest_status_.stop_latched = false; + latest_status_.recovery_state = ProtectiveRecoveryState::SUCCEEDED; + latest_status_.recovery_error.clear(); + joint_history_.clear(); + joint_history_.push_back(path.front()); + recovery_path_.clear(); + latest_status_.recovery_sample_count = 0; + } + + const auto unlock_result = arm->unlockProtectiveStop(); + if (!unlock_result.ok()) { + std::lock_guard lock(mutex_); + latest_status_.stop_latched = true; + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = + "Failed to unlock protective stop: " + unlock_result.message; + return device::Result::failure( + unlock_result.code, latest_status_.recovery_error); + } + + CMVR_LOG(INFO) << "[SelfCollisionTask] recovery completed event_id=" + << event_id << ", clear_distance_m=" + << config_.recovery().clear_distance_m(); + return device::Result::success(); +} + +void SelfCollisionTask::stop() +{ + std::shared_ptr arm; + bool cancel_recovery = false; + { + std::lock_guard lock(mutex_); + cancel_recovery = + latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING; + arm = arm_; + if (state_ != TaskState::FAILED) { + state_ = TaskState::STOPPED; + } + recovery_cv_.notify_all(); + } + if (cancel_recovery && arm) { + (void)arm->protectiveStop(); + } +} + +TaskState SelfCollisionTask::state() const +{ + std::lock_guard lock(mutex_); + return state_; +} + +bool SelfCollisionTask::isBusy() const +{ + return state() == TaskState::RUNNING; +} + +bool SelfCollisionTask::isFinished() const +{ + return state() == TaskState::STOPPED; +} + +bool SelfCollisionTask::isFailed() const +{ + return state() == TaskState::FAILED; +} + +std::string SelfCollisionTask::stateString() const +{ + return taskStateToString(state()); +} + +std::string SelfCollisionTask::detailStatusString() const +{ + std::lock_guard lock(mutex_); + if (!last_error_.empty()) { + return std::string(taskStateToString(state_)) + " " + last_error_; + } + std::ostringstream stream; + stream << taskStateToString(state_) + << " level=" << safetyLevelToString(latest_status_.level) + << " stop_latched=" << latest_status_.stop_latched + << " event_id=" << latest_status_.event_id + << " recovery=" + << recoveryStateToString(latest_status_.recovery_state); + if (latest_status_.result.valid) { + stream << " distance_m=" << std::setprecision(6) + << latest_status_.result.minimum_distance_m + << " pair=" << latest_status_.result.first + << "/" << latest_status_.result.second; + } + if (!latest_status_.recovery_error.empty()) { + stream << " recovery_error=" << latest_status_.recovery_error; + } + return stream.str(); +} + +SelfCollisionTaskStatus SelfCollisionTask::latestStatus() const +{ + std::lock_guard lock(mutex_); + return latest_status_; +} + +const char* SelfCollisionTask::safetyLevelToString(const CollisionSafetyLevel level) +{ + switch (level) { + case CollisionSafetyLevel::UNKNOWN: return "UNKNOWN"; + case CollisionSafetyLevel::SAFE: return "SAFE"; + case CollisionSafetyLevel::WARNING: return "WARNING"; + case CollisionSafetyLevel::STOP: return "STOP"; + } + return "UNKNOWN"; +} + +const char* SelfCollisionTask::recoveryStateToString( + const ProtectiveRecoveryState state) +{ + switch (state) { + case ProtectiveRecoveryState::IDLE: return "IDLE"; + case ProtectiveRecoveryState::AVAILABLE: return "AVAILABLE"; + case ProtectiveRecoveryState::RECOVERING: return "RECOVERING"; + case ProtectiveRecoveryState::SUCCEEDED: return "SUCCEEDED"; + case ProtectiveRecoveryState::FAILED: return "FAILED"; + } + return "UNKNOWN"; +} + +} // namespace cmvr::task diff --git a/cmvr-es/task/task_factory.h b/cmvr-es/task/task_factory.h index e3efe733..dfb1ce62 100644 --- a/cmvr-es/task/task_factory.h +++ b/cmvr-es/task/task_factory.h @@ -14,6 +14,8 @@ #include "common/config/config_files.h" #include "task/task.h" #include "task/touch_screen_task/include/touch_screen_task.h" +#include "task/self_collision_task/include/self_collision_task.h" +#include "cmvr/config/self_collision_task_config/self_collision_task_config.pb.h" namespace cmvr::task { @@ -52,6 +54,28 @@ inline std::shared_ptr createTouchScreenTask(const config::TaskConfigEntry return std::make_shared(cfg); } +inline std::shared_ptr createSelfCollisionTask(const config::TaskConfigEntry& entry) +{ + if (entry.id().empty() || entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[TaskFactory] Invalid SelfCollision task entry: " << entry.id(); + return nullptr; + } + + config::SelfCollisionTaskRootConfig root_cfg; + if (!ConfigHelper::loadConfigFile(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[TaskFactory] Failed to load SelfCollision config: " + << entry.config_file(); + return nullptr; + } + const auto& cfg = root_cfg.self_collision_task(); + if (cfg.id().empty() || cfg.id() != entry.id()) { + CMVR_LOG(ERROR) << "[TaskFactory] SelfCollision task ID mismatch: manager id=" + << entry.id() << ", config id=" << cfg.id(); + return nullptr; + } + return std::make_shared(cfg); +} + } // namespace task_factory_detail class TaskFactory { @@ -70,6 +94,8 @@ public: switch (entry.type()) { case config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN: return task_factory_detail::createTouchScreenTask(entry); + case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION: + return task_factory_detail::createSelfCollisionTask(entry); default: break; } diff --git a/dependency/x86/third_party/aubo_sdk/v0.27.1/lib/libstdc++.so.6 b/dependency/x86/third_party/aubo_sdk/v0.27.1/lib/libstdc++.so.6 new file mode 100644 index 00000000..470f7733 --- /dev/null +++ b/dependency/x86/third_party/aubo_sdk/v0.27.1/lib/libstdc++.so.6 @@ -0,0 +1 @@ +libstdc++.so.6.0.25 \ No newline at end of file diff --git a/dependency/x86/third_party/aubo_sdk/v0.27.1/lib/libstdc++.so.6.0.25 b/dependency/x86/third_party/aubo_sdk/v0.27.1/lib/libstdc++.so.6.0.25 new file mode 100644 index 00000000..c47b099c Binary files /dev/null and b/dependency/x86/third_party/aubo_sdk/v0.27.1/lib/libstdc++.so.6.0.25 differ diff --git a/model/gen2/assets/10100.part b/model/gen2/assets/10100.part new file mode 100644 index 00000000..a5799f3a --- /dev/null +++ b/model/gen2/assets/10100.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M4Rmvs8/9nOQASQoz", + "isStandardContent": false, + "name": "10100 <1>", + "partId": "JyD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/10100.stl b/model/gen2/assets/10100.stl new file mode 100644 index 00000000..f7c27440 Binary files /dev/null and b/model/gen2/assets/10100.stl differ diff --git a/model/gen2/assets/10100__2.part b/model/gen2/assets/10100__2.part new file mode 100644 index 00000000..b79e9263 --- /dev/null +++ b/model/gen2/assets/10100__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "MHaJriwfEEKCaNP0z", + "isStandardContent": false, + "name": "10100 <2>", + "partId": "J5D", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/10100__2.stl b/model/gen2/assets/10100__2.stl new file mode 100644 index 00000000..5814b233 Binary files /dev/null and b/model/gen2/assets/10100__2.stl differ diff --git a/model/gen2/assets/10100__3.part b/model/gen2/assets/10100__3.part new file mode 100644 index 00000000..fe8f6c01 --- /dev/null +++ b/model/gen2/assets/10100__3.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "8073bb18db19f7fa7997ad99", + "documentMicroversion": "428a1bfe2c594dd041131bcd", + "documentVersion": "102e666e83d6b78c25c9ce78", + "elementId": "5decfde3994a7031e4d53265", + "fullConfiguration": "default", + "id": "MMSYOwzMqbgilS4dL", + "isStandardContent": false, + "name": "10100 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/10100__3.stl b/model/gen2/assets/10100__3.stl new file mode 100644 index 00000000..2f81fa0e Binary files /dev/null and b/model/gen2/assets/10100__3.stl differ diff --git a/model/gen2/assets/1020001.part b/model/gen2/assets/1020001.part new file mode 100644 index 00000000..38a2e880 --- /dev/null +++ b/model/gen2/assets/1020001.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "387522184bef9b82eac1fdfc", + "documentMicroversion": "caf63b0301e581fe867e46ba", + "documentVersion": "961b145da2fcbaf35ddb6766", + "elementId": "58a93d37d32edd9f1f80c4a6", + "fullConfiguration": "default", + "id": "MtGpd/bL/Q+stBW6B", + "isStandardContent": false, + "name": "1020001 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/1020001.stl b/model/gen2/assets/1020001.stl new file mode 100644 index 00000000..7f21adc5 Binary files /dev/null and b/model/gen2/assets/1020001.stl differ diff --git a/model/gen2/assets/arm_link_1.part b/model/gen2/assets/arm_link_1.part new file mode 100644 index 00000000..52a02296 --- /dev/null +++ b/model/gen2/assets/arm_link_1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "M77c5kvnmFYX3+E1/", + "isStandardContent": false, + "name": "arm_link_1 <2>", + "partId": "RHDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_1.stl b/model/gen2/assets/arm_link_1.stl new file mode 100644 index 00000000..c5221527 Binary files /dev/null and b/model/gen2/assets/arm_link_1.stl differ diff --git a/model/gen2/assets/arm_link_2.part b/model/gen2/assets/arm_link_2.part new file mode 100644 index 00000000..466137f5 --- /dev/null +++ b/model/gen2/assets/arm_link_2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MAagFsEi4vOqUYBnE", + "isStandardContent": false, + "name": "arm_link_2 <2>", + "partId": "JvD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_2.stl b/model/gen2/assets/arm_link_2.stl new file mode 100644 index 00000000..e0f07b54 Binary files /dev/null and b/model/gen2/assets/arm_link_2.stl differ diff --git a/model/gen2/assets/arm_link_3.part b/model/gen2/assets/arm_link_3.part new file mode 100644 index 00000000..9020c9d7 --- /dev/null +++ b/model/gen2/assets/arm_link_3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MODZ2heuMlDmDK6JN", + "isStandardContent": false, + "name": "arm_link_3 <2>", + "partId": "RRBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_3.stl b/model/gen2/assets/arm_link_3.stl new file mode 100644 index 00000000..ec969414 Binary files /dev/null and b/model/gen2/assets/arm_link_3.stl differ diff --git a/model/gen2/assets/arm_link_4.part b/model/gen2/assets/arm_link_4.part new file mode 100644 index 00000000..b6e91f6c --- /dev/null +++ b/model/gen2/assets/arm_link_4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MmEi+ZosIdG9Wp+iP", + "isStandardContent": false, + "name": "arm_link_4 <2>", + "partId": "RwCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_4.stl b/model/gen2/assets/arm_link_4.stl new file mode 100644 index 00000000..b7c392c1 Binary files /dev/null and b/model/gen2/assets/arm_link_4.stl differ diff --git a/model/gen2/assets/arm_link_5.part b/model/gen2/assets/arm_link_5.part new file mode 100644 index 00000000..7e3ea789 --- /dev/null +++ b/model/gen2/assets/arm_link_5.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MBPP2k1l9Xc6L3QvC", + "isStandardContent": false, + "name": "arm_link_5 <2>", + "partId": "RxCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_5.stl b/model/gen2/assets/arm_link_5.stl new file mode 100644 index 00000000..7aec226c Binary files /dev/null and b/model/gen2/assets/arm_link_5.stl differ diff --git a/model/gen2/assets/arm_link_6.part b/model/gen2/assets/arm_link_6.part new file mode 100644 index 00000000..96e7540e --- /dev/null +++ b/model/gen2/assets/arm_link_6.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MIcvuqnQVxIZF1ql0", + "isStandardContent": false, + "name": "arm_link_6 <2>", + "partId": "RyCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_6.stl b/model/gen2/assets/arm_link_6.stl new file mode 100644 index 00000000..a019833c Binary files /dev/null and b/model/gen2/assets/arm_link_6.stl differ diff --git a/model/gen2/assets/arm_link_7.part b/model/gen2/assets/arm_link_7.part new file mode 100644 index 00000000..13512a37 --- /dev/null +++ b/model/gen2/assets/arm_link_7.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "Mv//Etw9ckikOO5Dz", + "isStandardContent": false, + "name": "arm_link_7 <2>", + "partId": "RrCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_7.stl b/model/gen2/assets/arm_link_7.stl new file mode 100644 index 00000000..6af8cf87 Binary files /dev/null and b/model/gen2/assets/arm_link_7.stl differ diff --git a/model/gen2/assets/body_link.part b/model/gen2/assets/body_link.part new file mode 100644 index 00000000..d9c1dc62 --- /dev/null +++ b/model/gen2/assets/body_link.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "0384359606fafa70e61c4a9c", + "fullConfiguration": "default", + "id": "MdJXCUi9nHtYjnfh4", + "isStandardContent": false, + "name": "body_link <1>", + "partId": "RgQD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/body_link.stl b/model/gen2/assets/body_link.stl new file mode 100644 index 00000000..bccfeeff Binary files /dev/null and b/model/gen2/assets/body_link.stl differ diff --git a/model/gen2/assets/ethercat挂杆_上.part b/model/gen2/assets/ethercat挂杆_上.part new file mode 100644 index 00000000..07e4fef2 --- /dev/null +++ b/model/gen2/assets/ethercat挂杆_上.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M6a8atbQgryvAh+2G", + "isStandardContent": false, + "name": "ethercat\u6302\u6746-\u4e0a <1>", + "partId": "RMDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ethercat挂杆_上.stl b/model/gen2/assets/ethercat挂杆_上.stl new file mode 100644 index 00000000..f3214420 Binary files /dev/null and b/model/gen2/assets/ethercat挂杆_上.stl differ diff --git a/model/gen2/assets/ethercat挂杆_下.part b/model/gen2/assets/ethercat挂杆_下.part new file mode 100644 index 00000000..576f381e --- /dev/null +++ b/model/gen2/assets/ethercat挂杆_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M6IPrfKh9NKHG6KNc", + "isStandardContent": false, + "name": "ethercat\u6302\u6746-\u4e0b <1>", + "partId": "RMDH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ethercat挂杆_下.stl b/model/gen2/assets/ethercat挂杆_下.stl new file mode 100644 index 00000000..04e843f0 Binary files /dev/null and b/model/gen2/assets/ethercat挂杆_下.stl differ diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.part b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.part new file mode 100644 index 00000000..d87c155c --- /dev/null +++ b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.part @@ -0,0 +1,14 @@ +{ + "configuration": "JTQwc2NwPU9ITmFJVkFLb3VXVUNGQnNjWjhrOE1zRG1nMFNRSU9JOUxqWXlZc1dGZEklM0Q7QXZlcmFnZURpYW1ldGVyPTAuMDAzNjAwMDAwMDAwMDAwMDAwMyttZXRlcjtCYXNpY0RpYW1ldGVyPTAuMDAzK21ldGVyO0hlYWREaWFtZXRlcj0wLjAwNTUrbWV0ZXI7SGVhZEZpbGxldD0zLjBFLTQrbWV0ZXI7SGVhZEhlaWdodD0wLjAwMyttZXRlcjtIZXhEZXB0aD0wLjAwMTMwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMjUrbWV0ZXI7TGVuZ3RoPTAuMDI1K21ldGVyO1BpdGNoPTUuMEUtNCttZXRlcjtUaHJlYWRMZW5ndGg9MC4wMTgwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7VHJhbnNpdGlvbkxlbmd0aD01LjFFLTQrbWV0ZXI7VHJpYW5nbGVIZWlnaHQ9NC4zMzAxMjdFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTEuMEUtNCttZXRlcg", + "documentId": "da5fe16b33cc63bf8b9e7e78", + "documentMicroversion": "e36ce5d010b02c39f1a32b51", + "documentVersion": "dc1a15dae669d740ea2d555e", + "elementId": "5b44a050e0b24df3e47c76dc", + "fullConfiguration": "JTQwc2NwPU9ITmFJVkFLb3VXVUNGQnNjWjhrOE1zRG1nMFNRSU9JOUxqWXlZc1dGZEklM0Q7QXZlcmFnZURpYW1ldGVyPTAuMDAzNjAwMDAwMDAwMDAwMDAwMyttZXRlcjtCYXNpY0RpYW1ldGVyPTAuMDAzK21ldGVyO0hlYWREaWFtZXRlcj0wLjAwNTUrbWV0ZXI7SGVhZEZpbGxldD0zLjBFLTQrbWV0ZXI7SGVhZEhlaWdodD0wLjAwMyttZXRlcjtIZXhEZXB0aD0wLjAwMTMwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMjUrbWV0ZXI7TGVuZ3RoPTAuMDI1K21ldGVyO1BpdGNoPTUuMEUtNCttZXRlcjtUaHJlYWRMZW5ndGg9MC4wMTgwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7VHJhbnNpdGlvbkxlbmd0aD01LjFFLTQrbWV0ZXI7VHJpYW5nbGVIZWlnaHQ9NC4zMzAxMjdFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTEuMEUtNCttZXRlcg", + "id": "Mz77l4SnYYxPYDxga", + "isStandardContent": true, + "name": "Hex socket head cap screw M3x0.50 x 25 x 18 <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.stl b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.stl new file mode 100644 index 00000000..26cbae94 Binary files /dev/null and b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.stl differ diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.part b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.part new file mode 100644 index 00000000..a31206cb --- /dev/null +++ b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.part @@ -0,0 +1,14 @@ +{ + "configuration": "JTQwc2NwPXZaeDNQUzV0NWU2YlI2R3BJR25ZZ1ZoczdzREJNOWpvdmNXYiUyQjVIU1JnOCUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDM2MDAwMDAwMDAwMDAwMDAzK21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDMrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA1NSttZXRlcjtIZWFkRmlsbGV0PTMuMEUtNCttZXRlcjtIZWFkSGVpZ2h0PTAuMDAzK21ldGVyO0hleERlcHRoPTAuMDAxMzAwMDAwMDAwMDAwMDAwMittZXRlcjtIZXhTaXplPTAuMDAyNSttZXRlcjtMZW5ndGg9MC4wNSttZXRlcjtQaXRjaD01LjBFLTQrbWV0ZXI7VGhyZWFkTGVuZ3RoPTAuMDE4MDAwMDAwMDAwMDAwMDAyK21ldGVyO1RyYW5zaXRpb25MZW5ndGg9NS4xRS00K21ldGVyO1RyaWFuZ2xlSGVpZ2h0PTQuMzMwMTI3RS00K21ldGVyO1VuZGVySGVhZEZpbGxldD0xLjBFLTQrbWV0ZXI", + "documentId": "da5fe16b33cc63bf8b9e7e78", + "documentMicroversion": "e36ce5d010b02c39f1a32b51", + "documentVersion": "dc1a15dae669d740ea2d555e", + "elementId": "5b44a050e0b24df3e47c76dc", + "fullConfiguration": "JTQwc2NwPXZaeDNQUzV0NWU2YlI2R3BJR25ZZ1ZoczdzREJNOWpvdmNXYiUyQjVIU1JnOCUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDM2MDAwMDAwMDAwMDAwMDAzK21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDMrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA1NSttZXRlcjtIZWFkRmlsbGV0PTMuMEUtNCttZXRlcjtIZWFkSGVpZ2h0PTAuMDAzK21ldGVyO0hleERlcHRoPTAuMDAxMzAwMDAwMDAwMDAwMDAwMittZXRlcjtIZXhTaXplPTAuMDAyNSttZXRlcjtMZW5ndGg9MC4wNSttZXRlcjtQaXRjaD01LjBFLTQrbWV0ZXI7VGhyZWFkTGVuZ3RoPTAuMDE4MDAwMDAwMDAwMDAwMDAyK21ldGVyO1RyYW5zaXRpb25MZW5ndGg9NS4xRS00K21ldGVyO1RyaWFuZ2xlSGVpZ2h0PTQuMzMwMTI3RS00K21ldGVyO1VuZGVySGVhZEZpbGxldD0xLjBFLTQrbWV0ZXI", + "id": "MzS3hV4uHEP51PCyW", + "isStandardContent": true, + "name": "Hex socket head cap screw M3x0.50 x 50 x 18 <5>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.stl b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.stl new file mode 100644 index 00000000..203ef536 Binary files /dev/null and b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.stl differ diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.part b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.part new file mode 100644 index 00000000..948acca9 --- /dev/null +++ b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.part @@ -0,0 +1,14 @@ +{ + "configuration": "JTQwc2NwPWxOSElNUXhVSTdPY3d2VXJqaDNJcVhHZHV3elVUZWYlMkY1N3cydklXNTZ1ZyUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDQ3K21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDQrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA3K21ldGVyO0hlYWRGaWxsZXQ9NC4wRS00K21ldGVyO0hlYWRIZWlnaHQ9MC4wMDQrbWV0ZXI7SGV4RGVwdGg9MC4wMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMyttZXRlcjtMZW5ndGg9MC4wMDgrbWV0ZXI7UGl0Y2g9Ny4wRS00K21ldGVyO1RocmVhZExlbmd0aD0wLjAwOCttZXRlcjtUcmFuc2l0aW9uTGVuZ3RoPTYuMEUtNCttZXRlcjtUcmlhbmdsZUhlaWdodD02LjA2MjE3NzhFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTIuMEUtNCttZXRlcg", + "documentId": "da5fe16b33cc63bf8b9e7e78", + "documentMicroversion": "e36ce5d010b02c39f1a32b51", + "documentVersion": "dc1a15dae669d740ea2d555e", + "elementId": "5b44a050e0b24df3e47c76dc", + "fullConfiguration": "JTQwc2NwPWxOSElNUXhVSTdPY3d2VXJqaDNJcVhHZHV3elVUZWYlMkY1N3cydklXNTZ1ZyUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDQ3K21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDQrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA3K21ldGVyO0hlYWRGaWxsZXQ9NC4wRS00K21ldGVyO0hlYWRIZWlnaHQ9MC4wMDQrbWV0ZXI7SGV4RGVwdGg9MC4wMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMyttZXRlcjtMZW5ndGg9MC4wMDgrbWV0ZXI7UGl0Y2g9Ny4wRS00K21ldGVyO1RocmVhZExlbmd0aD0wLjAwOCttZXRlcjtUcmFuc2l0aW9uTGVuZ3RoPTYuMEUtNCttZXRlcjtUcmlhbmdsZUhlaWdodD02LjA2MjE3NzhFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTIuMEUtNCttZXRlcg", + "id": "Mwr1cbw7PCKF93OWT", + "isStandardContent": true, + "name": "Hex socket head cap screw M4x0.70 x 8 <7>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.stl b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.stl new file mode 100644 index 00000000..568a174b Binary files /dev/null and b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.stl differ diff --git a/model/gen2/assets/j2关节.part b/model/gen2/assets/j2关节.part new file mode 100644 index 00000000..095fb6d6 --- /dev/null +++ b/model/gen2/assets/j2关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "8b7e66f798cbb082509293d8", + "documentMicroversion": "887ff4907089063547dfb683", + "documentVersion": "05b6b05241fd0e72cdcb2a58", + "elementId": "a4d6cca76f08dc255e08b999", + "fullConfiguration": "default", + "id": "MPjW04ncnuOxOzkZ0", + "isStandardContent": false, + "name": "J2\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j2关节.stl b/model/gen2/assets/j2关节.stl new file mode 100644 index 00000000..0848ee6d Binary files /dev/null and b/model/gen2/assets/j2关节.stl differ diff --git a/model/gen2/assets/j2关节盖板.part b/model/gen2/assets/j2关节盖板.part new file mode 100644 index 00000000..1e36dd10 --- /dev/null +++ b/model/gen2/assets/j2关节盖板.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "639734872365a5c6ab1e6dc0", + "documentMicroversion": "87a57a1cf4702ec8bbc72b83", + "documentVersion": "bc5b603bcba394f302ef13b6", + "elementId": "2a770d046c759ce59d9fb447", + "fullConfiguration": "default", + "id": "M3lE/jcPA4MBiXRl+", + "isStandardContent": false, + "name": "J2\u5173\u8282\u76d6\u677f <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j2关节盖板.stl b/model/gen2/assets/j2关节盖板.stl new file mode 100644 index 00000000..e6fc0a4b Binary files /dev/null and b/model/gen2/assets/j2关节盖板.stl differ diff --git a/model/gen2/assets/j3关节.part b/model/gen2/assets/j3关节.part new file mode 100644 index 00000000..05d24dcc --- /dev/null +++ b/model/gen2/assets/j3关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "15fc539038cd4e87718802cf", + "documentMicroversion": "dd0168517fb915b74ab9f650", + "documentVersion": "c78f7c31e667644647771463", + "elementId": "cce50e82bbd0f737c1086291", + "fullConfiguration": "default", + "id": "ML+TSApNzCC0D52rC", + "isStandardContent": false, + "name": "J3\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j3关节.stl b/model/gen2/assets/j3关节.stl new file mode 100644 index 00000000..aba09b4f Binary files /dev/null and b/model/gen2/assets/j3关节.stl differ diff --git a/model/gen2/assets/j4关节.part b/model/gen2/assets/j4关节.part new file mode 100644 index 00000000..8d11e737 --- /dev/null +++ b/model/gen2/assets/j4关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "7b3f5b2e877e0152778a9ca2", + "documentMicroversion": "0f0440eac84e1ad6c9e4d950", + "documentVersion": "6d55eb8af3364ff18181e064", + "elementId": "deecc12ddde91edb0deb20c9", + "fullConfiguration": "default", + "id": "MGEe7vYxsi98IEyH5", + "isStandardContent": false, + "name": "J4\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4关节.stl b/model/gen2/assets/j4关节.stl new file mode 100644 index 00000000..1940bc4d Binary files /dev/null and b/model/gen2/assets/j4关节.stl differ diff --git a/model/gen2/assets/j4轴承支撑.part b/model/gen2/assets/j4轴承支撑.part new file mode 100644 index 00000000..74ca7542 --- /dev/null +++ b/model/gen2/assets/j4轴承支撑.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "38be5a8344ea08045e614079", + "fullConfiguration": "default", + "id": "MMhyQcMwjwq7kE227", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u652f\u6491 <1>", + "partId": "RWBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承支撑.stl b/model/gen2/assets/j4轴承支撑.stl new file mode 100644 index 00000000..c14cdd08 Binary files /dev/null and b/model/gen2/assets/j4轴承支撑.stl differ diff --git a/model/gen2/assets/j4轴承支撑__2.part b/model/gen2/assets/j4轴承支撑__2.part new file mode 100644 index 00000000..7375e897 --- /dev/null +++ b/model/gen2/assets/j4轴承支撑__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "a51a941965828d24d7f9ab81", + "fullConfiguration": "default", + "id": "MsSXN2rPmtY0dur6R", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u652f\u6491 <2>", + "partId": "RMCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承支撑__2.stl b/model/gen2/assets/j4轴承支撑__2.stl new file mode 100644 index 00000000..1f917818 Binary files /dev/null and b/model/gen2/assets/j4轴承支撑__2.stl differ diff --git a/model/gen2/assets/j4轴承盖板.part b/model/gen2/assets/j4轴承盖板.part new file mode 100644 index 00000000..c60543b2 --- /dev/null +++ b/model/gen2/assets/j4轴承盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "38be5a8344ea08045e614079", + "fullConfiguration": "default", + "id": "MKrYD2pl4tMWf+WQQ", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u76d6\u677f <1>", + "partId": "RjBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承盖板.stl b/model/gen2/assets/j4轴承盖板.stl new file mode 100644 index 00000000..a56635ee Binary files /dev/null and b/model/gen2/assets/j4轴承盖板.stl differ diff --git a/model/gen2/assets/j4轴承盖板__2.part b/model/gen2/assets/j4轴承盖板__2.part new file mode 100644 index 00000000..50afd215 --- /dev/null +++ b/model/gen2/assets/j4轴承盖板__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "a51a941965828d24d7f9ab81", + "fullConfiguration": "default", + "id": "MqrQMlBzm6clwBYhN", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u76d6\u677f <2>", + "partId": "RLCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承盖板__2.stl b/model/gen2/assets/j4轴承盖板__2.stl new file mode 100644 index 00000000..e40d5610 Binary files /dev/null and b/model/gen2/assets/j4轴承盖板__2.stl differ diff --git a/model/gen2/assets/j5主动法兰.part b/model/gen2/assets/j5主动法兰.part new file mode 100644 index 00000000..ba4cb08d --- /dev/null +++ b/model/gen2/assets/j5主动法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "95a669e868ab89618b756342", + "fullConfiguration": "default", + "id": "MkPc/e6vvvzfReU1g", + "isStandardContent": false, + "name": "J5\u4e3b\u52a8\u6cd5\u5170 <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5主动法兰.stl b/model/gen2/assets/j5主动法兰.stl new file mode 100644 index 00000000..f873e14e Binary files /dev/null and b/model/gen2/assets/j5主动法兰.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板.part b/model/gen2/assets/j5从动轴心盖板.part new file mode 100644 index 00000000..c7178894 --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "M++0c8F/OA1VcSYPL", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <2>", + "partId": "R1BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板.stl b/model/gen2/assets/j5从动轴心盖板.stl new file mode 100644 index 00000000..eec1e308 Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板__2.part b/model/gen2/assets/j5从动轴心盖板__2.part new file mode 100644 index 00000000..793e5578 --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "M+lDhmwR1TmaWXh/y", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <1>", + "partId": "R0BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板__2.stl b/model/gen2/assets/j5从动轴心盖板__2.stl new file mode 100644 index 00000000..efe21ac5 Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板__2.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板__3.part b/model/gen2/assets/j5从动轴心盖板__3.part new file mode 100644 index 00000000..2e4eb6fc --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板__3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "Mb3FWJ4KHbCKAG26B", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <3>", + "partId": "RDBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板__3.stl b/model/gen2/assets/j5从动轴心盖板__3.stl new file mode 100644 index 00000000..e8375f6c Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板__3.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板__4.part b/model/gen2/assets/j5从动轴心盖板__4.part new file mode 100644 index 00000000..5f1c4005 --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板__4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "MjEKCYXkYWHg0gqf+", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <4>", + "partId": "RSBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板__4.stl b/model/gen2/assets/j5从动轴心盖板__4.stl new file mode 100644 index 00000000..8600d762 Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板__4.stl differ diff --git a/model/gen2/assets/j5关节.part b/model/gen2/assets/j5关节.part new file mode 100644 index 00000000..ef2b4497 --- /dev/null +++ b/model/gen2/assets/j5关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "6738eff574c7bac74278c035", + "documentMicroversion": "7b5e69869d9b02be1ce0581d", + "documentVersion": "a0db86e78c7d2dc110a9791b", + "elementId": "60347bdca2a24e7714baa4d6", + "fullConfiguration": "default", + "id": "MjQlv+qNUV6RZuD6P", + "isStandardContent": false, + "name": "J5\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5关节.stl b/model/gen2/assets/j5关节.stl new file mode 100644 index 00000000..3d5e4944 Binary files /dev/null and b/model/gen2/assets/j5关节.stl differ diff --git a/model/gen2/assets/j6轴承支撑.part b/model/gen2/assets/j6轴承支撑.part new file mode 100644 index 00000000..2f161626 --- /dev/null +++ b/model/gen2/assets/j6轴承支撑.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "ec3810b640410692b6d75e6d", + "documentMicroversion": "2677b5c61e0ce625e61f2de1", + "documentVersion": "260d5320b9d212f7376901b9", + "elementId": "9b8fab3d8fa54635da3b9281", + "fullConfiguration": "default", + "id": "M1oxqvy7AYCLJRufY", + "isStandardContent": false, + "name": "J6\u8f74\u627f\u652f\u6491 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j6轴承支撑.stl b/model/gen2/assets/j6轴承支撑.stl new file mode 100644 index 00000000..54b5846e Binary files /dev/null and b/model/gen2/assets/j6轴承支撑.stl differ diff --git a/model/gen2/assets/j6轴承支撑1.part b/model/gen2/assets/j6轴承支撑1.part new file mode 100644 index 00000000..b88940bc --- /dev/null +++ b/model/gen2/assets/j6轴承支撑1.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "aab3081dc8a1b17396b04ac8", + "documentMicroversion": "989f9da17530022533d4b1b5", + "documentVersion": "431a46f24ed967bcf94385d0", + "elementId": "55a51320ef47236b54856bc1", + "fullConfiguration": "default", + "id": "MG/AppKFXBSV39O6i", + "isStandardContent": false, + "name": "J6\u8f74\u627f\u652f\u64911 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j6轴承支撑1.stl b/model/gen2/assets/j6轴承支撑1.stl new file mode 100644 index 00000000..96d0b61d Binary files /dev/null and b/model/gen2/assets/j6轴承支撑1.stl differ diff --git a/model/gen2/assets/j7关节a.part b/model/gen2/assets/j7关节a.part new file mode 100644 index 00000000..7b2f9539 --- /dev/null +++ b/model/gen2/assets/j7关节a.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "41e7681508301de01c2f4658", + "documentMicroversion": "e0b55cb820aadaa0f053d95b", + "documentVersion": "9b9bafefdd3dbfa2715fbe98", + "elementId": "28e76f0bd60c1f60bb95ed8c", + "fullConfiguration": "default", + "id": "MjmlvjYKU6BQqrtfC", + "isStandardContent": false, + "name": "J7\u5173\u8282A <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j7关节a.stl b/model/gen2/assets/j7关节a.stl new file mode 100644 index 00000000..7ffc5f5a Binary files /dev/null and b/model/gen2/assets/j7关节a.stl differ diff --git a/model/gen2/assets/jetson安装杆_右.part b/model/gen2/assets/jetson安装杆_右.part new file mode 100644 index 00000000..dc9d6425 --- /dev/null +++ b/model/gen2/assets/jetson安装杆_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MK0IY8wljRAjD4RsR", + "isStandardContent": false, + "name": "jetson\u5b89\u88c5\u6746-\u53f3 <1>", + "partId": "RwLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/jetson安装杆_右.stl b/model/gen2/assets/jetson安装杆_右.stl new file mode 100644 index 00000000..a072d0ad Binary files /dev/null and b/model/gen2/assets/jetson安装杆_右.stl differ diff --git a/model/gen2/assets/jetson安装杆_左.part b/model/gen2/assets/jetson安装杆_左.part new file mode 100644 index 00000000..d8f085d0 --- /dev/null +++ b/model/gen2/assets/jetson安装杆_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mmwlrnd+kZGc2EiJK", + "isStandardContent": false, + "name": "jetson\u5b89\u88c5\u6746-\u5de6 <1>", + "partId": "R3LD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/jetson安装杆_左.stl b/model/gen2/assets/jetson安装杆_左.stl new file mode 100644 index 00000000..8905eb74 Binary files /dev/null and b/model/gen2/assets/jetson安装杆_左.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_2_collision.stl b/model/gen2/assets/merged/arm_link_1_2_collision.stl new file mode 100644 index 00000000..593caf4f Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_2_visual.stl b/model/gen2/assets/merged/arm_link_1_2_visual.stl new file mode 100644 index 00000000..87509b29 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_collision.stl b/model/gen2/assets/merged/arm_link_1_collision.stl new file mode 100644 index 00000000..2be06fc6 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_visual.stl b/model/gen2/assets/merged/arm_link_1_visual.stl new file mode 100644 index 00000000..7b8bba96 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_2_collision.stl b/model/gen2/assets/merged/arm_link_2_2_collision.stl new file mode 100644 index 00000000..f63fde64 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_2_visual.stl b/model/gen2/assets/merged/arm_link_2_2_visual.stl new file mode 100644 index 00000000..07b07c92 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_collision.stl b/model/gen2/assets/merged/arm_link_2_collision.stl new file mode 100644 index 00000000..0ad57d21 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_visual.stl b/model/gen2/assets/merged/arm_link_2_visual.stl new file mode 100644 index 00000000..ce6ea378 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_2_collision.stl b/model/gen2/assets/merged/arm_link_3_2_collision.stl new file mode 100644 index 00000000..dbf4fa53 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_2_visual.stl b/model/gen2/assets/merged/arm_link_3_2_visual.stl new file mode 100644 index 00000000..a01d9ba6 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_collision.stl b/model/gen2/assets/merged/arm_link_3_collision.stl new file mode 100644 index 00000000..d6c0abbf Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_visual.stl b/model/gen2/assets/merged/arm_link_3_visual.stl new file mode 100644 index 00000000..4a3b6785 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_2_collision.stl b/model/gen2/assets/merged/arm_link_4_2_collision.stl new file mode 100644 index 00000000..4569f480 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_2_visual.stl b/model/gen2/assets/merged/arm_link_4_2_visual.stl new file mode 100644 index 00000000..4569f480 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_collision.stl b/model/gen2/assets/merged/arm_link_4_collision.stl new file mode 100644 index 00000000..a2cc8b37 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_visual.stl b/model/gen2/assets/merged/arm_link_4_visual.stl new file mode 100644 index 00000000..a2cc8b37 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_2_collision.stl b/model/gen2/assets/merged/arm_link_5_2_collision.stl new file mode 100644 index 00000000..196c86a3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_2_visual.stl b/model/gen2/assets/merged/arm_link_5_2_visual.stl new file mode 100644 index 00000000..196c86a3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_collision.stl b/model/gen2/assets/merged/arm_link_5_collision.stl new file mode 100644 index 00000000..92156354 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_visual.stl b/model/gen2/assets/merged/arm_link_5_visual.stl new file mode 100644 index 00000000..92156354 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_2_collision.stl b/model/gen2/assets/merged/arm_link_6_2_collision.stl new file mode 100644 index 00000000..875d6cc5 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_2_visual.stl b/model/gen2/assets/merged/arm_link_6_2_visual.stl new file mode 100644 index 00000000..875d6cc5 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_collision.stl b/model/gen2/assets/merged/arm_link_6_collision.stl new file mode 100644 index 00000000..38249bf3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_visual.stl b/model/gen2/assets/merged/arm_link_6_visual.stl new file mode 100644 index 00000000..38249bf3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_2_collision.stl b/model/gen2/assets/merged/arm_link_7_2_collision.stl new file mode 100644 index 00000000..9343e212 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_2_visual.stl b/model/gen2/assets/merged/arm_link_7_2_visual.stl new file mode 100644 index 00000000..9343e212 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_collision.stl b/model/gen2/assets/merged/arm_link_7_collision.stl new file mode 100644 index 00000000..e71dab05 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_visual.stl b/model/gen2/assets/merged/arm_link_7_visual.stl new file mode 100644 index 00000000..e71dab05 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_visual.stl differ diff --git a/model/gen2/assets/merged/body_link_collision.stl b/model/gen2/assets/merged/body_link_collision.stl new file mode 100644 index 00000000..094fdf95 Binary files /dev/null and b/model/gen2/assets/merged/body_link_collision.stl differ diff --git a/model/gen2/assets/merged/body_link_visual.stl b/model/gen2/assets/merged/body_link_visual.stl new file mode 100644 index 00000000..ddb3442e Binary files /dev/null and b/model/gen2/assets/merged/body_link_visual.stl differ diff --git a/model/gen2/assets/merged/j5从动轴心盖板_collision.stl b/model/gen2/assets/merged/j5从动轴心盖板_collision.stl new file mode 100644 index 00000000..6175d231 Binary files /dev/null and b/model/gen2/assets/merged/j5从动轴心盖板_collision.stl differ diff --git a/model/gen2/assets/merged/j5从动轴心盖板_visual.stl b/model/gen2/assets/merged/j5从动轴心盖板_visual.stl new file mode 100644 index 00000000..6175d231 Binary files /dev/null and b/model/gen2/assets/merged/j5从动轴心盖板_visual.stl differ diff --git a/model/gen2/assets/merged/part_1_2_collision.stl b/model/gen2/assets/merged/part_1_2_collision.stl new file mode 100644 index 00000000..5b28dbf9 Binary files /dev/null and b/model/gen2/assets/merged/part_1_2_collision.stl differ diff --git a/model/gen2/assets/merged/part_1_2_visual.stl b/model/gen2/assets/merged/part_1_2_visual.stl new file mode 100644 index 00000000..5b28dbf9 Binary files /dev/null and b/model/gen2/assets/merged/part_1_2_visual.stl differ diff --git a/model/gen2/assets/merged/part_1_collision.stl b/model/gen2/assets/merged/part_1_collision.stl new file mode 100644 index 00000000..5dbe4db7 Binary files /dev/null and b/model/gen2/assets/merged/part_1_collision.stl differ diff --git a/model/gen2/assets/merged/part_1_visual.stl b/model/gen2/assets/merged/part_1_visual.stl new file mode 100644 index 00000000..5dbe4db7 Binary files /dev/null and b/model/gen2/assets/merged/part_1_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_2_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_2_collision.stl new file mode 100644 index 00000000..8dc088dd Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_2_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_2_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_2_visual.stl new file mode 100644 index 00000000..b1300cdb Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_2_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_3_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_3_collision.stl new file mode 100644 index 00000000..5601939c Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_3_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_3_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_3_visual.stl new file mode 100644 index 00000000..79da68c0 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_3_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_4_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_4_collision.stl new file mode 100644 index 00000000..98ea5646 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_4_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_4_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_4_visual.stl new file mode 100644 index 00000000..78feead0 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_4_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_collision.stl new file mode 100644 index 00000000..571394bb Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_visual.stl new file mode 100644 index 00000000..c0d3fdfd Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_2_collision.stl b/model/gen2/assets/merged/rmd_x15_p20_450_2_collision.stl new file mode 100644 index 00000000..341d220d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_2_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_2_visual.stl b/model/gen2/assets/merged/rmd_x15_p20_450_2_visual.stl new file mode 100644 index 00000000..f0bbbb3b Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_2_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_collision.stl b/model/gen2/assets/merged/rmd_x15_p20_450_collision.stl new file mode 100644 index 00000000..7eeb1460 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_visual.stl b/model/gen2/assets/merged/rmd_x15_p20_450_visual.stl new file mode 100644 index 00000000..1ccc820c Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x6_60_collision.stl b/model/gen2/assets/merged/rmd_x6_60_collision.stl new file mode 100644 index 00000000..8cd4da1d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x6_60_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x6_60_visual.stl b/model/gen2/assets/merged/rmd_x6_60_visual.stl new file mode 100644 index 00000000..8cd4da1d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x6_60_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_2_collision.stl b/model/gen2/assets/merged/rmd_x8_120_2_collision.stl new file mode 100644 index 00000000..179f15ab Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_2_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_2_visual.stl b/model/gen2/assets/merged/rmd_x8_120_2_visual.stl new file mode 100644 index 00000000..179f15ab Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_2_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_collision.stl b/model/gen2/assets/merged/rmd_x8_120_collision.stl new file mode 100644 index 00000000..a9462a5d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_visual.stl b/model/gen2/assets/merged/rmd_x8_120_visual.stl new file mode 100644 index 00000000..a9462a5d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_visual.stl differ diff --git a/model/gen2/assets/merged/waist_link_1_collision.stl b/model/gen2/assets/merged/waist_link_1_collision.stl new file mode 100644 index 00000000..a10076ce Binary files /dev/null and b/model/gen2/assets/merged/waist_link_1_collision.stl differ diff --git a/model/gen2/assets/merged/waist_link_1_visual.stl b/model/gen2/assets/merged/waist_link_1_visual.stl new file mode 100644 index 00000000..d1fbb1ca Binary files /dev/null and b/model/gen2/assets/merged/waist_link_1_visual.stl differ diff --git a/model/gen2/assets/merged/waist_link_2_collision.stl b/model/gen2/assets/merged/waist_link_2_collision.stl new file mode 100644 index 00000000..2d0419f1 Binary files /dev/null and b/model/gen2/assets/merged/waist_link_2_collision.stl differ diff --git a/model/gen2/assets/merged/waist_link_2_visual.stl b/model/gen2/assets/merged/waist_link_2_visual.stl new file mode 100644 index 00000000..6222cb48 Binary files /dev/null and b/model/gen2/assets/merged/waist_link_2_visual.stl differ diff --git a/model/gen2/assets/part3.part b/model/gen2/assets/part3.part new file mode 100644 index 00000000..ef9a84f2 --- /dev/null +++ b/model/gen2/assets/part3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "MZbolAETzbjTWQxGh", + "isStandardContent": false, + "name": "part3 <3>", + "partId": "SfEHJ", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part3.stl b/model/gen2/assets/part3.stl new file mode 100644 index 00000000..9d10b000 Binary files /dev/null and b/model/gen2/assets/part3.stl differ diff --git a/model/gen2/assets/part_1.part b/model/gen2/assets/part_1.part new file mode 100644 index 00000000..e2b20911 --- /dev/null +++ b/model/gen2/assets/part_1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8e795471a63f9aeeb3f38bc9", + "fullConfiguration": "default", + "id": "M/fsWjcYigJevNTSo", + "isStandardContent": false, + "name": "Part 1 <10>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1.stl b/model/gen2/assets/part_1.stl new file mode 100644 index 00000000..02f96fca Binary files /dev/null and b/model/gen2/assets/part_1.stl differ diff --git a/model/gen2/assets/part_1__2.part b/model/gen2/assets/part_1__2.part new file mode 100644 index 00000000..921fbf70 --- /dev/null +++ b/model/gen2/assets/part_1__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "b62d5cd0a79626536433550c", + "fullConfiguration": "default", + "id": "Ml8S+i6AJBUQrvKPI", + "isStandardContent": false, + "name": "Part 1 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__2.stl b/model/gen2/assets/part_1__2.stl new file mode 100644 index 00000000..416c6253 Binary files /dev/null and b/model/gen2/assets/part_1__2.stl differ diff --git a/model/gen2/assets/part_1__3.part b/model/gen2/assets/part_1__3.part new file mode 100644 index 00000000..cf5897ac --- /dev/null +++ b/model/gen2/assets/part_1__3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "75bf03a59d086af969a33c01", + "fullConfiguration": "default", + "id": "MMSWppHq6Yjq0A4GJ", + "isStandardContent": false, + "name": "Part 1 <7>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__3.stl b/model/gen2/assets/part_1__3.stl new file mode 100644 index 00000000..86db7640 Binary files /dev/null and b/model/gen2/assets/part_1__3.stl differ diff --git a/model/gen2/assets/part_1__4.part b/model/gen2/assets/part_1__4.part new file mode 100644 index 00000000..c6d767db --- /dev/null +++ b/model/gen2/assets/part_1__4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "ea07b7966a3a2629b98a8575", + "fullConfiguration": "default", + "id": "MHhqsoHbvZI16AyiH", + "isStandardContent": false, + "name": "Part 1 <8>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__4.stl b/model/gen2/assets/part_1__4.stl new file mode 100644 index 00000000..fa42f58e Binary files /dev/null and b/model/gen2/assets/part_1__4.stl differ diff --git a/model/gen2/assets/part_1__5.part b/model/gen2/assets/part_1__5.part new file mode 100644 index 00000000..215e2ff0 --- /dev/null +++ b/model/gen2/assets/part_1__5.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "4efa01c02aae2d87545c9f16", + "fullConfiguration": "default", + "id": "MniQ8B1Rcr+eQTXJ6", + "isStandardContent": false, + "name": "Part 1 <9>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__5.stl b/model/gen2/assets/part_1__5.stl new file mode 100644 index 00000000..15d3e282 Binary files /dev/null and b/model/gen2/assets/part_1__5.stl differ diff --git a/model/gen2/assets/part_1__6.part b/model/gen2/assets/part_1__6.part new file mode 100644 index 00000000..712007df --- /dev/null +++ b/model/gen2/assets/part_1__6.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "6d51c3e57aab39664d6b1921", + "fullConfiguration": "default", + "id": "MxXxHbGlThzhJ469V", + "isStandardContent": false, + "name": "Part 1 <6>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__6.stl b/model/gen2/assets/part_1__6.stl new file mode 100644 index 00000000..fee8b326 Binary files /dev/null and b/model/gen2/assets/part_1__6.stl differ diff --git a/model/gen2/assets/part_1__7.part b/model/gen2/assets/part_1__7.part new file mode 100644 index 00000000..ba1bce44 --- /dev/null +++ b/model/gen2/assets/part_1__7.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M6WjFDvOl7wQzdTgE", + "isStandardContent": false, + "name": "Part 1 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__7.stl b/model/gen2/assets/part_1__7.stl new file mode 100644 index 00000000..ece3c67e Binary files /dev/null and b/model/gen2/assets/part_1__7.stl differ diff --git a/model/gen2/assets/part_1__8.part b/model/gen2/assets/part_1__8.part new file mode 100644 index 00000000..bbb9208c --- /dev/null +++ b/model/gen2/assets/part_1__8.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M7sFILMoD0PvVpFyz", + "isStandardContent": false, + "name": "Part 1 <2>", + "partId": "SWBXB", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__8.stl b/model/gen2/assets/part_1__8.stl new file mode 100644 index 00000000..f4620b67 Binary files /dev/null and b/model/gen2/assets/part_1__8.stl differ diff --git a/model/gen2/assets/part_2.part b/model/gen2/assets/part_2.part new file mode 100644 index 00000000..578cbc90 --- /dev/null +++ b/model/gen2/assets/part_2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "ea07b7966a3a2629b98a8575", + "fullConfiguration": "default", + "id": "MJXJUqhkZxXZYFfWJ", + "isStandardContent": false, + "name": "Part 2 <3>", + "partId": "JTD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_2.stl b/model/gen2/assets/part_2.stl new file mode 100644 index 00000000..3aab8358 Binary files /dev/null and b/model/gen2/assets/part_2.stl differ diff --git a/model/gen2/assets/part_2__2.part b/model/gen2/assets/part_2__2.part new file mode 100644 index 00000000..f3b4534a --- /dev/null +++ b/model/gen2/assets/part_2__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "4efa01c02aae2d87545c9f16", + "fullConfiguration": "default", + "id": "MvgANl+gNFugUkrMM", + "isStandardContent": false, + "name": "Part 2 <4>", + "partId": "JQD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_2__2.stl b/model/gen2/assets/part_2__2.stl new file mode 100644 index 00000000..46bc8f08 Binary files /dev/null and b/model/gen2/assets/part_2__2.stl differ diff --git a/model/gen2/assets/part_3.part b/model/gen2/assets/part_3.part new file mode 100644 index 00000000..e6cbda9f --- /dev/null +++ b/model/gen2/assets/part_3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "ea07b7966a3a2629b98a8575", + "fullConfiguration": "default", + "id": "M3lQHqrq4hZJcX0rS", + "isStandardContent": false, + "name": "Part 3 <3>", + "partId": "JmD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_3.stl b/model/gen2/assets/part_3.stl new file mode 100644 index 00000000..172ce3aa Binary files /dev/null and b/model/gen2/assets/part_3.stl differ diff --git a/model/gen2/assets/part_3__2.part b/model/gen2/assets/part_3__2.part new file mode 100644 index 00000000..743fb1b2 --- /dev/null +++ b/model/gen2/assets/part_3__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "4efa01c02aae2d87545c9f16", + "fullConfiguration": "default", + "id": "MHE2bGvITgPh7wTVN", + "isStandardContent": false, + "name": "Part 3 <4>", + "partId": "JfD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_3__2.stl b/model/gen2/assets/part_3__2.stl new file mode 100644 index 00000000..b0f1d141 Binary files /dev/null and b/model/gen2/assets/part_3__2.stl differ diff --git a/model/gen2/assets/ph11_n_51_101_e.part b/model/gen2/assets/ph11_n_51_101_e.part new file mode 100644 index 00000000..89c42643 --- /dev/null +++ b/model/gen2/assets/ph11_n_51_101_e.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "6a3e0b943524913fea5a4822", + "documentMicroversion": "cf8035afbce324bdc7a9e006", + "documentVersion": "351c8cbd1b321620e615819b", + "elementId": "f7464a1f93ca23cb11010a6f", + "fullConfiguration": "default", + "id": "MvgDf/uZU1+Rtwreu", + "isStandardContent": false, + "name": "PH11-N-51&101-E <3>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ph11_n_51_101_e.stl b/model/gen2/assets/ph11_n_51_101_e.stl new file mode 100644 index 00000000..a5850d84 Binary files /dev/null and b/model/gen2/assets/ph11_n_51_101_e.stl differ diff --git a/model/gen2/assets/ph17_2_2.part b/model/gen2/assets/ph17_2_2.part new file mode 100644 index 00000000..570907cb --- /dev/null +++ b/model/gen2/assets/ph17_2_2.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "e255f9ff9fda9450c77eb8f1", + "documentMicroversion": "20b63849a1768546e32fe1c1", + "documentVersion": "7c38174c464a84fcb345a027", + "elementId": "36ec63486326e8a1e99f315c", + "fullConfiguration": "default", + "id": "MIcouqN6LN1fhDLKv", + "isStandardContent": false, + "name": "PH17-2_2 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ph17_2_2.stl b/model/gen2/assets/ph17_2_2.stl new file mode 100644 index 00000000..e0a05a7c Binary files /dev/null and b/model/gen2/assets/ph17_2_2.stl differ diff --git a/model/gen2/assets/rmd_x12_p20_320.part b/model/gen2/assets/rmd_x12_p20_320.part new file mode 100644 index 00000000..66dbd7b3 --- /dev/null +++ b/model/gen2/assets/rmd_x12_p20_320.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "62d7fd99f6f8bc18d006f61d", + "documentMicroversion": "dbf97b0b2f1de35e36aa78e5", + "documentVersion": "947d0353c559b9c880806e11", + "elementId": "e1b8602fc8f4029943e2295e", + "fullConfiguration": "default", + "id": "M4evZkgyz6yFULD1S", + "isStandardContent": false, + "name": "RMD-X12-P20-320 <4>", + "partId": "JGD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x12_p20_320.stl b/model/gen2/assets/rmd_x12_p20_320.stl new file mode 100644 index 00000000..e7476420 Binary files /dev/null and b/model/gen2/assets/rmd_x12_p20_320.stl differ diff --git a/model/gen2/assets/rmd_x15_p20_450.part b/model/gen2/assets/rmd_x15_p20_450.part new file mode 100644 index 00000000..1021d5f6 --- /dev/null +++ b/model/gen2/assets/rmd_x15_p20_450.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "ec857d60fcb70103cc12860c", + "documentMicroversion": "1a2acfb65995cbdcea466261", + "documentVersion": "16d2b0eab42ae6c11ddb97f7", + "elementId": "6ba9083deb449364f7d336a0", + "fullConfiguration": "default", + "id": "MYOenBISeP8bZc8Qo", + "isStandardContent": false, + "name": "RMD-X15-P20-450 <2>", + "partId": "JGD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x15_p20_450.stl b/model/gen2/assets/rmd_x15_p20_450.stl new file mode 100644 index 00000000..e6ac780b Binary files /dev/null and b/model/gen2/assets/rmd_x15_p20_450.stl differ diff --git a/model/gen2/assets/rmd_x6_60.part b/model/gen2/assets/rmd_x6_60.part new file mode 100644 index 00000000..859e38e4 --- /dev/null +++ b/model/gen2/assets/rmd_x6_60.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "b60b2b1541b6caba79879c87", + "documentMicroversion": "e89aeab6208ada7cddc14963", + "documentVersion": "e6dd4bc54af7df350f2b61f5", + "elementId": "4210d66ddc6c74d4a3225701", + "fullConfiguration": "default", + "id": "MJXQun7eeHfI/CBG4", + "isStandardContent": false, + "name": "RMD-X6-60 <2>", + "partId": "JJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x6_60.stl b/model/gen2/assets/rmd_x6_60.stl new file mode 100644 index 00000000..cc6a9970 Binary files /dev/null and b/model/gen2/assets/rmd_x6_60.stl differ diff --git a/model/gen2/assets/rmd_x8_120.part b/model/gen2/assets/rmd_x8_120.part new file mode 100644 index 00000000..17ff104f --- /dev/null +++ b/model/gen2/assets/rmd_x8_120.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "7ab7b9fa4ac0bac59e4b7f31", + "documentMicroversion": "3f29b3a24d3d3bc9d28f8291", + "documentVersion": "f7cb5e3afb025dafb27bbb69", + "elementId": "d519a1f4dd46602465d25754", + "fullConfiguration": "default", + "id": "MGNRNjyq5zHPCRaGM", + "isStandardContent": false, + "name": "RMD-X8-120 <3>", + "partId": "JJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x8_120.stl b/model/gen2/assets/rmd_x8_120.stl new file mode 100644 index 00000000..459cde68 Binary files /dev/null and b/model/gen2/assets/rmd_x8_120.stl differ diff --git a/model/gen2/assets/waist_link_1.part b/model/gen2/assets/waist_link_1.part new file mode 100644 index 00000000..59d28b59 --- /dev/null +++ b/model/gen2/assets/waist_link_1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "MaMIM7xMUAfPCsjBw", + "isStandardContent": false, + "name": "waist_link_1 <1>", + "partId": "R6ED", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/waist_link_1.stl b/model/gen2/assets/waist_link_1.stl new file mode 100644 index 00000000..a5c12747 Binary files /dev/null and b/model/gen2/assets/waist_link_1.stl differ diff --git a/model/gen2/assets/waist_link_2.part b/model/gen2/assets/waist_link_2.part new file mode 100644 index 00000000..86a753f1 --- /dev/null +++ b/model/gen2/assets/waist_link_2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M751ldLqHrvZy5x87", + "isStandardContent": false, + "name": "waist_link_2 <1>", + "partId": "RaFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/waist_link_2.stl b/model/gen2/assets/waist_link_2.stl new file mode 100644 index 00000000..b6000c4c Binary files /dev/null and b/model/gen2/assets/waist_link_2.stl differ diff --git a/model/gen2/assets/不锈钢推拉杆.part b/model/gen2/assets/不锈钢推拉杆.part new file mode 100644 index 00000000..dc5a6fd1 --- /dev/null +++ b/model/gen2/assets/不锈钢推拉杆.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "be59b5b3fc36c192f76143e2", + "fullConfiguration": "default", + "id": "Mt9kcvd/03CDMTgIs", + "isStandardContent": false, + "name": "\u4e0d\u9508\u94a2\u63a8\u62c9\u6746 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/不锈钢推拉杆.stl b/model/gen2/assets/不锈钢推拉杆.stl new file mode 100644 index 00000000..13ad6356 Binary files /dev/null and b/model/gen2/assets/不锈钢推拉杆.stl differ diff --git a/model/gen2/assets/关节轴承__外.part b/model/gen2/assets/关节轴承__外.part new file mode 100644 index 00000000..5b9fc3e1 --- /dev/null +++ b/model/gen2/assets/关节轴承__外.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "ebe12e2f11c612cfba1a391d", + "fullConfiguration": "default", + "id": "Mpi9R8DS1HtWAKAPT", + "isStandardContent": false, + "name": "\u5173\u8282\u8f74\u627f -\u5916 <4>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/关节轴承__外.stl b/model/gen2/assets/关节轴承__外.stl new file mode 100644 index 00000000..17755efe Binary files /dev/null and b/model/gen2/assets/关节轴承__外.stl differ diff --git a/model/gen2/assets/右侧板_下.part b/model/gen2/assets/右侧板_下.part new file mode 100644 index 00000000..a74ec8de --- /dev/null +++ b/model/gen2/assets/右侧板_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M3XYgvzxX84GAVmSg", + "isStandardContent": false, + "name": "\u53f3\u4fa7\u677f\uff08\u4e0b\uff09 <1>", + "partId": "JYD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右侧板_下.stl b/model/gen2/assets/右侧板_下.stl new file mode 100644 index 00000000..1cafe0b4 Binary files /dev/null and b/model/gen2/assets/右侧板_下.stl differ diff --git a/model/gen2/assets/右滑槽.part b/model/gen2/assets/右滑槽.part new file mode 100644 index 00000000..13d731df --- /dev/null +++ b/model/gen2/assets/右滑槽.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MZVXkKtoaUSIqtlGn", + "isStandardContent": false, + "name": "\u53f3\u6ed1\u69fd <1>", + "partId": "JxD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右滑槽.stl b/model/gen2/assets/右滑槽.stl new file mode 100644 index 00000000..71f7f65d Binary files /dev/null and b/model/gen2/assets/右滑槽.stl differ diff --git a/model/gen2/assets/右滑盖.part b/model/gen2/assets/右滑盖.part new file mode 100644 index 00000000..03d91a19 --- /dev/null +++ b/model/gen2/assets/右滑盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "M80GcYpLl6V+aHmLK", + "isStandardContent": false, + "name": "\u53f3\u6ed1\u76d6 <1>", + "partId": "RTBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右滑盖.stl b/model/gen2/assets/右滑盖.stl new file mode 100644 index 00000000..71c1b73e Binary files /dev/null and b/model/gen2/assets/右滑盖.stl differ diff --git a/model/gen2/assets/右臂法兰.part b/model/gen2/assets/右臂法兰.part new file mode 100644 index 00000000..142c1140 --- /dev/null +++ b/model/gen2/assets/右臂法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M/96q4NYqfHK8laya", + "isStandardContent": false, + "name": "\u53f3\u81c2\u6cd5\u5170 <1>", + "partId": "RiBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右臂法兰.stl b/model/gen2/assets/右臂法兰.stl new file mode 100644 index 00000000..e5d0d417 Binary files /dev/null and b/model/gen2/assets/右臂法兰.stl differ diff --git a/model/gen2/assets/右臂电机.part b/model/gen2/assets/右臂电机.part new file mode 100644 index 00000000..4876bb78 --- /dev/null +++ b/model/gen2/assets/右臂电机.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MP8GXDKWNCto3hS8V", + "isStandardContent": false, + "name": "\u53f3\u81c2\u7535\u673a <1>", + "partId": "R/BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右臂电机.stl b/model/gen2/assets/右臂电机.stl new file mode 100644 index 00000000..a9f70a89 Binary files /dev/null and b/model/gen2/assets/右臂电机.stl differ diff --git a/model/gen2/assets/吊装_右.part b/model/gen2/assets/吊装_右.part new file mode 100644 index 00000000..0ceb2f3c --- /dev/null +++ b/model/gen2/assets/吊装_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MQBanFl24uiIqwBsw", + "isStandardContent": false, + "name": "\u540a\u88c5-\u53f3 <1>", + "partId": "RzHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/吊装_右.stl b/model/gen2/assets/吊装_右.stl new file mode 100644 index 00000000..79518b87 Binary files /dev/null and b/model/gen2/assets/吊装_右.stl differ diff --git a/model/gen2/assets/吊装_左.part b/model/gen2/assets/吊装_左.part new file mode 100644 index 00000000..40c72253 --- /dev/null +++ b/model/gen2/assets/吊装_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MvYDW8Ut5YsNBqjh/", + "isStandardContent": false, + "name": "\u540a\u88c5-\u5de6 <1>", + "partId": "R5HD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/吊装_左.stl b/model/gen2/assets/吊装_左.stl new file mode 100644 index 00000000..38d8f9a4 Binary files /dev/null and b/model/gen2/assets/吊装_左.stl differ diff --git a/model/gen2/assets/外壳1.part b/model/gen2/assets/外壳1.part new file mode 100644 index 00000000..56f7ad0c --- /dev/null +++ b/model/gen2/assets/外壳1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MTqNjRvBwQ+HQVcHs", + "isStandardContent": false, + "name": "\u5916\u58f31 <1>", + "partId": "JKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳1.stl b/model/gen2/assets/外壳1.stl new file mode 100644 index 00000000..ef49e798 Binary files /dev/null and b/model/gen2/assets/外壳1.stl differ diff --git a/model/gen2/assets/外壳2.part b/model/gen2/assets/外壳2.part new file mode 100644 index 00000000..58f28146 --- /dev/null +++ b/model/gen2/assets/外壳2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MC4nkIik1bp1s556v", + "isStandardContent": false, + "name": "\u5916\u58f32 <1>", + "partId": "JZD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳2.stl b/model/gen2/assets/外壳2.stl new file mode 100644 index 00000000..905fc07a Binary files /dev/null and b/model/gen2/assets/外壳2.stl differ diff --git a/model/gen2/assets/外壳_侧盖_右.part b/model/gen2/assets/外壳_侧盖_右.part new file mode 100644 index 00000000..6e911c9d --- /dev/null +++ b/model/gen2/assets/外壳_侧盖_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MKQXi3/0fXRuXGG40", + "isStandardContent": false, + "name": "\u5916\u58f3-\u4fa7\u76d6-\u53f3 <1>", + "partId": "RSMH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_侧盖_右.stl b/model/gen2/assets/外壳_侧盖_右.stl new file mode 100644 index 00000000..5bd255d2 Binary files /dev/null and b/model/gen2/assets/外壳_侧盖_右.stl differ diff --git a/model/gen2/assets/外壳_侧盖_左.part b/model/gen2/assets/外壳_侧盖_左.part new file mode 100644 index 00000000..8418e394 --- /dev/null +++ b/model/gen2/assets/外壳_侧盖_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "ML9n/y47JVCppqO4O", + "isStandardContent": false, + "name": "\u5916\u58f3-\u4fa7\u76d6-\u5de6 <1>", + "partId": "RSMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_侧盖_左.stl b/model/gen2/assets/外壳_侧盖_左.stl new file mode 100644 index 00000000..41027c9e Binary files /dev/null and b/model/gen2/assets/外壳_侧盖_左.stl differ diff --git a/model/gen2/assets/外壳_前盖.part b/model/gen2/assets/外壳_前盖.part new file mode 100644 index 00000000..99e26e9d --- /dev/null +++ b/model/gen2/assets/外壳_前盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MjC0LlUXkhLEI3+sQ", + "isStandardContent": false, + "name": "\u5916\u58f3-\u524d\u76d6 <1>", + "partId": "RjJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_前盖.stl b/model/gen2/assets/外壳_前盖.stl new file mode 100644 index 00000000..ed0012d9 Binary files /dev/null and b/model/gen2/assets/外壳_前盖.stl differ diff --git a/model/gen2/assets/外壳_前盖_下.part b/model/gen2/assets/外壳_前盖_下.part new file mode 100644 index 00000000..9a119be9 --- /dev/null +++ b/model/gen2/assets/外壳_前盖_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MXzWnzC8eVA1K9AOp", + "isStandardContent": false, + "name": "\u5916\u58f3-\u524d\u76d6-\u4e0b <1>", + "partId": "RpJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_前盖_下.stl b/model/gen2/assets/外壳_前盖_下.stl new file mode 100644 index 00000000..fa4ff1e4 Binary files /dev/null and b/model/gen2/assets/外壳_前盖_下.stl differ diff --git a/model/gen2/assets/外壳_后盖.part b/model/gen2/assets/外壳_后盖.part new file mode 100644 index 00000000..25a78225 --- /dev/null +++ b/model/gen2/assets/外壳_后盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MEpXtt7oMgb6bd4K7", + "isStandardContent": false, + "name": "\u5916\u58f3-\u540e\u76d6 <1>", + "partId": "R6MD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_后盖.stl b/model/gen2/assets/外壳_后盖.stl new file mode 100644 index 00000000..f236debb Binary files /dev/null and b/model/gen2/assets/外壳_后盖.stl differ diff --git a/model/gen2/assets/外壳_后盖_下.part b/model/gen2/assets/外壳_后盖_下.part new file mode 100644 index 00000000..06a6f04a --- /dev/null +++ b/model/gen2/assets/外壳_后盖_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M00Wo3f4AHVleGN4f", + "isStandardContent": false, + "name": "\u5916\u58f3-\u540e\u76d6-\u4e0b <1>", + "partId": "RSML", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_后盖_下.stl b/model/gen2/assets/外壳_后盖_下.stl new file mode 100644 index 00000000..8b432a9b Binary files /dev/null and b/model/gen2/assets/外壳_后盖_下.stl differ diff --git a/model/gen2/assets/小腿.part b/model/gen2/assets/小腿.part new file mode 100644 index 00000000..e23ebc77 --- /dev/null +++ b/model/gen2/assets/小腿.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "38be5a8344ea08045e614079", + "fullConfiguration": "default", + "id": "M+9mGjSFXYjvXOSGj", + "isStandardContent": false, + "name": "\u5c0f\u817f <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/小腿.stl b/model/gen2/assets/小腿.stl new file mode 100644 index 00000000..6320ed25 Binary files /dev/null and b/model/gen2/assets/小腿.stl differ diff --git a/model/gen2/assets/小腿__2.part b/model/gen2/assets/小腿__2.part new file mode 100644 index 00000000..fc30f489 --- /dev/null +++ b/model/gen2/assets/小腿__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "a51a941965828d24d7f9ab81", + "fullConfiguration": "default", + "id": "MwioXAIZK7wvJewBS", + "isStandardContent": false, + "name": "\u5c0f\u817f <2>", + "partId": "RKCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/小腿__2.stl b/model/gen2/assets/小腿__2.stl new file mode 100644 index 00000000..a6ae2ebb Binary files /dev/null and b/model/gen2/assets/小腿__2.stl differ diff --git a/model/gen2/assets/左侧板_下.part b/model/gen2/assets/左侧板_下.part new file mode 100644 index 00000000..f99f3177 --- /dev/null +++ b/model/gen2/assets/左侧板_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MGrb7N3ttkchxFr+R", + "isStandardContent": false, + "name": "\u5de6\u4fa7\u677f\uff08\u4e0b\uff09 <1>", + "partId": "RQCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左侧板_下.stl b/model/gen2/assets/左侧板_下.stl new file mode 100644 index 00000000..4e6e6f3f Binary files /dev/null and b/model/gen2/assets/左侧板_下.stl differ diff --git a/model/gen2/assets/左滑槽.part b/model/gen2/assets/左滑槽.part new file mode 100644 index 00000000..677955da --- /dev/null +++ b/model/gen2/assets/左滑槽.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MtSjvzM/U6qF2/2vK", + "isStandardContent": false, + "name": "\u5de6\u6ed1\u69fd <1>", + "partId": "JkD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左滑槽.stl b/model/gen2/assets/左滑槽.stl new file mode 100644 index 00000000..1851e191 Binary files /dev/null and b/model/gen2/assets/左滑槽.stl differ diff --git a/model/gen2/assets/左滑盖.part b/model/gen2/assets/左滑盖.part new file mode 100644 index 00000000..08f1633b --- /dev/null +++ b/model/gen2/assets/左滑盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MqVnOcEE4czuCcGrq", + "isStandardContent": false, + "name": "\u5de6\u6ed1\u76d6 <1>", + "partId": "RBBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左滑盖.stl b/model/gen2/assets/左滑盖.stl new file mode 100644 index 00000000..3439abd0 Binary files /dev/null and b/model/gen2/assets/左滑盖.stl differ diff --git a/model/gen2/assets/左臂法兰.part b/model/gen2/assets/左臂法兰.part new file mode 100644 index 00000000..a0ccc976 --- /dev/null +++ b/model/gen2/assets/左臂法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mv50nGvmKTJai12AS", + "isStandardContent": false, + "name": "\u5de6\u81c2\u6cd5\u5170 <1>", + "partId": "RSCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左臂法兰.stl b/model/gen2/assets/左臂法兰.stl new file mode 100644 index 00000000..eae8893d Binary files /dev/null and b/model/gen2/assets/左臂法兰.stl differ diff --git a/model/gen2/assets/左臂电机.part b/model/gen2/assets/左臂电机.part new file mode 100644 index 00000000..12667db8 --- /dev/null +++ b/model/gen2/assets/左臂电机.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MJ40cA8u6yvxua234", + "isStandardContent": false, + "name": "\u5de6\u81c2\u7535\u673a <1>", + "partId": "RRCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左臂电机.stl b/model/gen2/assets/左臂电机.stl new file mode 100644 index 00000000..eae14638 Binary files /dev/null and b/model/gen2/assets/左臂电机.stl differ diff --git a/model/gen2/assets/底座_6.part b/model/gen2/assets/底座_6.part new file mode 100644 index 00000000..6ca56176 --- /dev/null +++ b/model/gen2/assets/底座_6.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "0354ef7293b27d43612c5d0d", + "fullConfiguration": "default", + "id": "MtDL1ISqaj7ZwQD10", + "isStandardContent": false, + "name": "\u5e95\u5ea7-6 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底座_6.stl b/model/gen2/assets/底座_6.stl new file mode 100644 index 00000000..74886465 Binary files /dev/null and b/model/gen2/assets/底座_6.stl differ diff --git a/model/gen2/assets/底板.part b/model/gen2/assets/底板.part new file mode 100644 index 00000000..c4e6b71e --- /dev/null +++ b/model/gen2/assets/底板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MkFlrF8wJBcmivM2P", + "isStandardContent": false, + "name": "\u5e95\u677f <1>", + "partId": "RMBX", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底板.stl b/model/gen2/assets/底板.stl new file mode 100644 index 00000000..3198f6c4 Binary files /dev/null and b/model/gen2/assets/底板.stl differ diff --git a/model/gen2/assets/底部外壳.part b/model/gen2/assets/底部外壳.part new file mode 100644 index 00000000..4be02a59 --- /dev/null +++ b/model/gen2/assets/底部外壳.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MPmUT9gi4bKLP++VU", + "isStandardContent": false, + "name": "\u5e95\u90e8\u5916\u58f3 <1>", + "partId": "RRKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底部外壳.stl b/model/gen2/assets/底部外壳.stl new file mode 100644 index 00000000..8441ccc3 Binary files /dev/null and b/model/gen2/assets/底部外壳.stl differ diff --git a/model/gen2/assets/底部法兰.part b/model/gen2/assets/底部法兰.part new file mode 100644 index 00000000..ec4978a8 --- /dev/null +++ b/model/gen2/assets/底部法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MmuoU/+5K/tn1Uuuz", + "isStandardContent": false, + "name": "\u5e95\u90e8\u6cd5\u5170 <1>", + "partId": "RxJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底部法兰.stl b/model/gen2/assets/底部法兰.stl new file mode 100644 index 00000000..aba996b5 Binary files /dev/null and b/model/gen2/assets/底部法兰.stl differ diff --git a/model/gen2/assets/开关按钮.part b/model/gen2/assets/开关按钮.part new file mode 100644 index 00000000..635248ef --- /dev/null +++ b/model/gen2/assets/开关按钮.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mv0YjhCiv6JK7HE7+", + "isStandardContent": false, + "name": "\u5f00\u5173\u6309\u94ae <1>", + "partId": "R6MH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/开关按钮.stl b/model/gen2/assets/开关按钮.stl new file mode 100644 index 00000000..f7c4692a Binary files /dev/null and b/model/gen2/assets/开关按钮.stl differ diff --git a/model/gen2/assets/开关按钮盖板.part b/model/gen2/assets/开关按钮盖板.part new file mode 100644 index 00000000..cbef0ff8 --- /dev/null +++ b/model/gen2/assets/开关按钮盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MvoOTXWzNfTNyzNg1", + "isStandardContent": false, + "name": "\u5f00\u5173\u6309\u94ae\u76d6\u677f <1>", + "partId": "RJND", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/开关按钮盖板.stl b/model/gen2/assets/开关按钮盖板.stl new file mode 100644 index 00000000..328e8ffc Binary files /dev/null and b/model/gen2/assets/开关按钮盖板.stl differ diff --git a/model/gen2/assets/手掌骨架_a.part b/model/gen2/assets/手掌骨架_a.part new file mode 100644 index 00000000..49871eab --- /dev/null +++ b/model/gen2/assets/手掌骨架_a.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "899055c6cd6af50017fdf8fa", + "fullConfiguration": "default", + "id": "MdjPNuoKJPr1LXuLe", + "isStandardContent": false, + "name": "\u624b\u638c\u9aa8\u67b6-A <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/手掌骨架_a.stl b/model/gen2/assets/手掌骨架_a.stl new file mode 100644 index 00000000..8481c61c Binary files /dev/null and b/model/gen2/assets/手掌骨架_a.stl differ diff --git a/model/gen2/assets/折叠驱动器v3.part b/model/gen2/assets/折叠驱动器v3.part new file mode 100644 index 00000000..59d55321 --- /dev/null +++ b/model/gen2/assets/折叠驱动器v3.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "2d4e9f935bf8977ffd5ac0d5", + "fullConfiguration": "default", + "id": "MBoO2CX+KIuRAoE1a", + "isStandardContent": false, + "name": "\u6298\u53e0\u9a71\u52a8\u5668V3 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/折叠驱动器v3.stl b/model/gen2/assets/折叠驱动器v3.stl new file mode 100644 index 00000000..788b3e64 Binary files /dev/null and b/model/gen2/assets/折叠驱动器v3.stl differ diff --git a/model/gen2/assets/指尖_金属部分b.part b/model/gen2/assets/指尖_金属部分b.part new file mode 100644 index 00000000..052f44af --- /dev/null +++ b/model/gen2/assets/指尖_金属部分b.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "c06c3339cd9e4fa2c01e2e2d", + "fullConfiguration": "default", + "id": "MY80To9lh13ublyqF", + "isStandardContent": false, + "name": "\u6307\u5c16-\u91d1\u5c5e\u90e8\u5206B <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指尖_金属部分b.stl b/model/gen2/assets/指尖_金属部分b.stl new file mode 100644 index 00000000..8ac7cf89 Binary files /dev/null and b/model/gen2/assets/指尖_金属部分b.stl differ diff --git a/model/gen2/assets/指尖短_金属部分b.part b/model/gen2/assets/指尖短_金属部分b.part new file mode 100644 index 00000000..8bb283e2 --- /dev/null +++ b/model/gen2/assets/指尖短_金属部分b.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "b0d8e8ffedfa915e327340f4", + "fullConfiguration": "default", + "id": "MmminKSlDg1YFLKmR", + "isStandardContent": false, + "name": "\u6307\u5c16\u77ed-\u91d1\u5c5e\u90e8\u5206B <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指尖短_金属部分b.stl b/model/gen2/assets/指尖短_金属部分b.stl new file mode 100644 index 00000000..86072233 Binary files /dev/null and b/model/gen2/assets/指尖短_金属部分b.stl differ diff --git a/model/gen2/assets/指尖短_金属部分b__2.part b/model/gen2/assets/指尖短_金属部分b__2.part new file mode 100644 index 00000000..f5837f98 --- /dev/null +++ b/model/gen2/assets/指尖短_金属部分b__2.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "ac4bb6bd62c1ad7b487a0ee3", + "documentMicroversion": "034d2f4cc6593a6848f3ae5d", + "documentVersion": "8c333a8c296052edbafa6c2a", + "elementId": "894c7841ae7eb4bf59bb7d60", + "fullConfiguration": "default", + "id": "Mtdn25hbOsANtFGGl", + "isStandardContent": false, + "name": "\u6307\u5c16\u77ed-\u91d1\u5c5e\u90e8\u5206B <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指尖短_金属部分b__2.stl b/model/gen2/assets/指尖短_金属部分b__2.stl new file mode 100644 index 00000000..ae172e13 Binary files /dev/null and b/model/gen2/assets/指尖短_金属部分b__2.stl differ diff --git a/model/gen2/assets/指节_金属b.part b/model/gen2/assets/指节_金属b.part new file mode 100644 index 00000000..017e15f1 --- /dev/null +++ b/model/gen2/assets/指节_金属b.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "9118cb28ca995c46f4e34f4b", + "fullConfiguration": "default", + "id": "MnshumUO1YlD+Ko9U", + "isStandardContent": false, + "name": "\u6307\u8282-\u91d1\u5c5eB <4>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指节_金属b.stl b/model/gen2/assets/指节_金属b.stl new file mode 100644 index 00000000..75e6e7ad Binary files /dev/null and b/model/gen2/assets/指节_金属b.stl differ diff --git a/model/gen2/assets/控制器挂杆_上.part b/model/gen2/assets/控制器挂杆_上.part new file mode 100644 index 00000000..6006a515 --- /dev/null +++ b/model/gen2/assets/控制器挂杆_上.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mh5IYfyGrF6wvmGzK", + "isStandardContent": false, + "name": "\u63a7\u5236\u5668\u6302\u6746-\u4e0a <1>", + "partId": "R3CD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/控制器挂杆_上.stl b/model/gen2/assets/控制器挂杆_上.stl new file mode 100644 index 00000000..396d0c64 Binary files /dev/null and b/model/gen2/assets/控制器挂杆_上.stl differ diff --git a/model/gen2/assets/控制器挂杆_下.part b/model/gen2/assets/控制器挂杆_下.part new file mode 100644 index 00000000..0a98c366 --- /dev/null +++ b/model/gen2/assets/控制器挂杆_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MgLDlq/TavYbYmODE", + "isStandardContent": false, + "name": "\u63a7\u5236\u5668\u6302\u6746-\u4e0b <1>", + "partId": "R3CH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/控制器挂杆_下.stl b/model/gen2/assets/控制器挂杆_下.stl new file mode 100644 index 00000000..91b20963 Binary files /dev/null and b/model/gen2/assets/控制器挂杆_下.stl differ diff --git a/model/gen2/assets/末端关节.part b/model/gen2/assets/末端关节.part new file mode 100644 index 00000000..1d65e974 --- /dev/null +++ b/model/gen2/assets/末端关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "fa23ed383daf64f447915c2b", + "documentMicroversion": "293169235e93895e758df8bb", + "documentVersion": "0369694b643f0badb7315400", + "elementId": "0da12e30653684e6b7330282", + "fullConfiguration": "default", + "id": "MeCqnhCcxfa176sxX", + "isStandardContent": false, + "name": "\u672b\u7aef\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/末端关节.stl b/model/gen2/assets/末端关节.stl new file mode 100644 index 00000000..1bd0336f Binary files /dev/null and b/model/gen2/assets/末端关节.stl differ diff --git a/model/gen2/assets/末端关节电机安装组件.part b/model/gen2/assets/末端关节电机安装组件.part new file mode 100644 index 00000000..32b418cd --- /dev/null +++ b/model/gen2/assets/末端关节电机安装组件.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "551d2d195f1f472f8549e41c", + "documentMicroversion": "228c31412604fbd205c7c7f0", + "documentVersion": "5b355140b1fda9df977e21e8", + "elementId": "915704befc3a0a3894e0594e", + "fullConfiguration": "default", + "id": "MJQBjMB33DiZ2dP41", + "isStandardContent": false, + "name": "\u672b\u7aef\u5173\u8282\u7535\u673a\u5b89\u88c5\u7ec4\u4ef6 <2>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/末端关节电机安装组件.stl b/model/gen2/assets/末端关节电机安装组件.stl new file mode 100644 index 00000000..78de11c1 Binary files /dev/null and b/model/gen2/assets/末端关节电机安装组件.stl differ diff --git a/model/gen2/assets/横杆2.part b/model/gen2/assets/横杆2.part new file mode 100644 index 00000000..e5bbcb96 --- /dev/null +++ b/model/gen2/assets/横杆2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MFmdDenJlDZ5t+msP", + "isStandardContent": false, + "name": "\u6a2a\u67462 <1>", + "partId": "RMBP", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/横杆2.stl b/model/gen2/assets/横杆2.stl new file mode 100644 index 00000000..3b7ff533 Binary files /dev/null and b/model/gen2/assets/横杆2.stl differ diff --git a/model/gen2/assets/横杆3.part b/model/gen2/assets/横杆3.part new file mode 100644 index 00000000..69325329 --- /dev/null +++ b/model/gen2/assets/横杆3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MOT0xoI5qNjvxnH0z", + "isStandardContent": false, + "name": "\u6a2a\u67463 <1>", + "partId": "RMBf", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/横杆3.stl b/model/gen2/assets/横杆3.stl new file mode 100644 index 00000000..c2a3be68 Binary files /dev/null and b/model/gen2/assets/横杆3.stl differ diff --git a/model/gen2/assets/横杆4.part b/model/gen2/assets/横杆4.part new file mode 100644 index 00000000..43a7e0ca --- /dev/null +++ b/model/gen2/assets/横杆4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MLqo4Lsvzz5zxWbrl", + "isStandardContent": false, + "name": "\u6a2a\u67464 <1>", + "partId": "RMBj", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/横杆4.stl b/model/gen2/assets/横杆4.stl new file mode 100644 index 00000000..bbe793e0 Binary files /dev/null and b/model/gen2/assets/横杆4.stl differ diff --git a/model/gen2/assets/电池仓.part b/model/gen2/assets/电池仓.part new file mode 100644 index 00000000..16a9373a --- /dev/null +++ b/model/gen2/assets/电池仓.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MDvrSJHryY8/2Hi0J", + "isStandardContent": false, + "name": "\u7535\u6c60\u4ed3 <1>", + "partId": "RBHz", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/电池仓.stl b/model/gen2/assets/电池仓.stl new file mode 100644 index 00000000..fd6f401a Binary files /dev/null and b/model/gen2/assets/电池仓.stl differ diff --git a/model/gen2/assets/电芯.part b/model/gen2/assets/电芯.part new file mode 100644 index 00000000..2b99efc0 --- /dev/null +++ b/model/gen2/assets/电芯.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "Mdy8jYdL+09IxJQ6R", + "isStandardContent": false, + "name": "\u7535\u82af <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/电芯.stl b/model/gen2/assets/电芯.stl new file mode 100644 index 00000000..cc91c8a2 Binary files /dev/null and b/model/gen2/assets/电芯.stl differ diff --git a/model/gen2/assets/盖子.part b/model/gen2/assets/盖子.part new file mode 100644 index 00000000..1229749e --- /dev/null +++ b/model/gen2/assets/盖子.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MolC3SwHEf2Qq06MU", + "isStandardContent": false, + "name": "\u76d6\u5b50 <1>", + "partId": "RIBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/盖子.stl b/model/gen2/assets/盖子.stl new file mode 100644 index 00000000..bc29455a Binary files /dev/null and b/model/gen2/assets/盖子.stl differ diff --git a/model/gen2/assets/胸部支撑_右.part b/model/gen2/assets/胸部支撑_右.part new file mode 100644 index 00000000..84e9b1ac --- /dev/null +++ b/model/gen2/assets/胸部支撑_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MCqQDpJWvDl+yXTAF", + "isStandardContent": false, + "name": "\u80f8\u90e8\u652f\u6491-\u53f3 <1>", + "partId": "RdDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/胸部支撑_右.stl b/model/gen2/assets/胸部支撑_右.stl new file mode 100644 index 00000000..dbc3321b Binary files /dev/null and b/model/gen2/assets/胸部支撑_右.stl differ diff --git a/model/gen2/assets/胸部支撑_左.part b/model/gen2/assets/胸部支撑_左.part new file mode 100644 index 00000000..c11cb00b --- /dev/null +++ b/model/gen2/assets/胸部支撑_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MI7nrS71IHygPjEQu", + "isStandardContent": false, + "name": "\u80f8\u90e8\u652f\u6491-\u5de6 <1>", + "partId": "RVDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/胸部支撑_左.stl b/model/gen2/assets/胸部支撑_左.stl new file mode 100644 index 00000000..29337f3b Binary files /dev/null and b/model/gen2/assets/胸部支撑_左.stl differ diff --git a/model/gen2/assets/脚踝.part b/model/gen2/assets/脚踝.part new file mode 100644 index 00000000..f1e26cd6 --- /dev/null +++ b/model/gen2/assets/脚踝.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "MADl4hipSxmbYTIZt", + "isStandardContent": false, + "name": "\u811a\u8e1d <1>", + "partId": "RzBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝.stl b/model/gen2/assets/脚踝.stl new file mode 100644 index 00000000..4442a13e Binary files /dev/null and b/model/gen2/assets/脚踝.stl differ diff --git a/model/gen2/assets/脚踝__2.part b/model/gen2/assets/脚踝__2.part new file mode 100644 index 00000000..a9f9cfec --- /dev/null +++ b/model/gen2/assets/脚踝__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "MErAFGluT0GUcFkk0", + "isStandardContent": false, + "name": "\u811a\u8e1d <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝__2.stl b/model/gen2/assets/脚踝__2.stl new file mode 100644 index 00000000..28cf6678 Binary files /dev/null and b/model/gen2/assets/脚踝__2.stl differ diff --git a/model/gen2/assets/脚踝盖板.part b/model/gen2/assets/脚踝盖板.part new file mode 100644 index 00000000..2d13ea6a --- /dev/null +++ b/model/gen2/assets/脚踝盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "MmXV24uIhptbe5WAb", + "isStandardContent": false, + "name": "\u811a\u8e1d\u76d6\u677f <1>", + "partId": "R2BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝盖板.stl b/model/gen2/assets/脚踝盖板.stl new file mode 100644 index 00000000..1401ca79 Binary files /dev/null and b/model/gen2/assets/脚踝盖板.stl differ diff --git a/model/gen2/assets/脚踝盖板__2.part b/model/gen2/assets/脚踝盖板__2.part new file mode 100644 index 00000000..e7f89d1c --- /dev/null +++ b/model/gen2/assets/脚踝盖板__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "MY1VzHUkp29mkYztr", + "isStandardContent": false, + "name": "\u811a\u8e1d\u76d6\u677f <2>", + "partId": "JrD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝盖板__2.stl b/model/gen2/assets/脚踝盖板__2.stl new file mode 100644 index 00000000..1f26d34d Binary files /dev/null and b/model/gen2/assets/脚踝盖板__2.stl differ diff --git a/model/gen2/assets/路由器挂杆_上.part b/model/gen2/assets/路由器挂杆_上.part new file mode 100644 index 00000000..256987fc --- /dev/null +++ b/model/gen2/assets/路由器挂杆_上.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MydLgTIuREEzGv2GG", + "isStandardContent": false, + "name": "\u8def\u7531\u5668\u6302\u6746-\u4e0a <1>", + "partId": "RoCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/路由器挂杆_上.stl b/model/gen2/assets/路由器挂杆_上.stl new file mode 100644 index 00000000..a9fa5415 Binary files /dev/null and b/model/gen2/assets/路由器挂杆_上.stl differ diff --git a/model/gen2/assets/路由器挂杆_下.part b/model/gen2/assets/路由器挂杆_下.part new file mode 100644 index 00000000..329efa8a --- /dev/null +++ b/model/gen2/assets/路由器挂杆_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MdJHUl2i3izPMt9Ct", + "isStandardContent": false, + "name": "\u8def\u7531\u5668\u6302\u6746-\u4e0b <1>", + "partId": "RrCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/路由器挂杆_下.stl b/model/gen2/assets/路由器挂杆_下.stl new file mode 100644 index 00000000..251f1e90 Binary files /dev/null and b/model/gen2/assets/路由器挂杆_下.stl differ diff --git a/model/gen2/assets/转接件.part b/model/gen2/assets/转接件.part new file mode 100644 index 00000000..0b138df7 --- /dev/null +++ b/model/gen2/assets/转接件.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "80ba63b613294cdd216840c7", + "documentMicroversion": "988d004c15b7387e8f7ce387", + "documentVersion": "de65ed1f664a5219ffb57b25", + "elementId": "46406d0b66f4940cc911ad8a", + "fullConfiguration": "default", + "id": "MN9ebc5PQdTrTHfSL", + "isStandardContent": false, + "name": "\u8f6c\u63a5\u4ef6 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/转接件.stl b/model/gen2/assets/转接件.stl new file mode 100644 index 00000000..3be11f9d Binary files /dev/null and b/model/gen2/assets/转接件.stl differ diff --git a/model/gen2/assets/转接法兰.part b/model/gen2/assets/转接法兰.part new file mode 100644 index 00000000..3fb97f87 --- /dev/null +++ b/model/gen2/assets/转接法兰.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "c56408749058b658d80e6d2e", + "documentMicroversion": "15b7b67a921cb84d8b985bd8", + "documentVersion": "e686f077317bdcd23693af0b", + "elementId": "d97365e7f25c01f7e677c7c4", + "fullConfiguration": "default", + "id": "MLD5bO1BfctD8vbcp", + "isStandardContent": false, + "name": "\u8f6c\u63a5\u6cd5\u5170 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/转接法兰.stl b/model/gen2/assets/转接法兰.stl new file mode 100644 index 00000000..eb619ec5 Binary files /dev/null and b/model/gen2/assets/转接法兰.stl differ diff --git a/model/gen2/assets/轴承轴向定位支撑.part b/model/gen2/assets/轴承轴向定位支撑.part new file mode 100644 index 00000000..48210dba --- /dev/null +++ b/model/gen2/assets/轴承轴向定位支撑.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "be59b5b3fc36c192f76143e2", + "fullConfiguration": "default", + "id": "MhYFeXPznLasm6sLJ", + "isStandardContent": false, + "name": "\u8f74\u627f\u8f74\u5411\u5b9a\u4f4d\u652f\u6491 <1>", + "partId": "JMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/轴承轴向定位支撑.stl b/model/gen2/assets/轴承轴向定位支撑.stl new file mode 100644 index 00000000..c5d9d0fe Binary files /dev/null and b/model/gen2/assets/轴承轴向定位支撑.stl differ diff --git a/model/gen2/assets/轴承轴向定位支撑__2.part b/model/gen2/assets/轴承轴向定位支撑__2.part new file mode 100644 index 00000000..55c77c5f --- /dev/null +++ b/model/gen2/assets/轴承轴向定位支撑__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "be59b5b3fc36c192f76143e2", + "fullConfiguration": "default", + "id": "MsOmGauaviz3dyT5u", + "isStandardContent": false, + "name": "\u8f74\u627f\u8f74\u5411\u5b9a\u4f4d\u652f\u6491 <2>", + "partId": "JUD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/轴承轴向定位支撑__2.stl b/model/gen2/assets/轴承轴向定位支撑__2.stl new file mode 100644 index 00000000..a150efa8 Binary files /dev/null and b/model/gen2/assets/轴承轴向定位支撑__2.stl differ diff --git a/model/gen2/assets/音响.part b/model/gen2/assets/音响.part new file mode 100644 index 00000000..5b527a42 --- /dev/null +++ b/model/gen2/assets/音响.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mlqrn7VrYrssl1rlG", + "isStandardContent": false, + "name": "\u97f3\u54cd <1>", + "partId": "RIJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/音响.stl b/model/gen2/assets/音响.stl new file mode 100644 index 00000000..9e275a09 Binary files /dev/null and b/model/gen2/assets/音响.stl differ diff --git a/model/gen2/assets/颈部支撑板.part b/model/gen2/assets/颈部支撑板.part new file mode 100644 index 00000000..a6418a7f --- /dev/null +++ b/model/gen2/assets/颈部支撑板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MEqAIszUMOTcIvTSN", + "isStandardContent": false, + "name": "\u9888\u90e8\u652f\u6491\u677f <1>", + "partId": "RkDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/颈部支撑板.stl b/model/gen2/assets/颈部支撑板.stl new file mode 100644 index 00000000..cf9efb8b Binary files /dev/null and b/model/gen2/assets/颈部支撑板.stl differ diff --git a/model/gen2/assets/马达应变片螺母.part b/model/gen2/assets/马达应变片螺母.part new file mode 100644 index 00000000..ea0e2f7f --- /dev/null +++ b/model/gen2/assets/马达应变片螺母.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "f2384e9af7a4e4dc42e82439", + "fullConfiguration": "default", + "id": "MgSXd7s3VkizXEC4M", + "isStandardContent": false, + "name": "\u9a6c\u8fbe\u5e94\u53d8\u7247\u87ba\u6bcd <2>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/马达应变片螺母.stl b/model/gen2/assets/马达应变片螺母.stl new file mode 100644 index 00000000..72dbb9c4 Binary files /dev/null and b/model/gen2/assets/马达应变片螺母.stl differ diff --git a/model/gen2/collision/gen2_collision.toml b/model/gen2/collision/gen2_collision.toml new file mode 100644 index 00000000..a1e07f96 --- /dev/null +++ b/model/gen2/collision/gen2_collision.toml @@ -0,0 +1,9 @@ +# Simplified primitive colliders generated from each link's visual mesh. +[defaults] +primitive = "box" +padding = 0.0 +scale = 1.0 + +# Keep the torso collider aligned with the body frame for predictable arm clearance. +[links.body_link] +alignment = "link" diff --git a/model/gen2/collision/robot_collision.urdf b/model/gen2/collision/robot_collision.urdf new file mode 100644 index 00000000..3a3c11de --- /dev/null +++ b/model/gen2/collision/robot_collision.urdf @@ -0,0 +1,926 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/gen2/config.json b/model/gen2/config.json new file mode 100644 index 00000000..1a65ecd4 --- /dev/null +++ b/model/gen2/config.json @@ -0,0 +1,10 @@ +// config.json general options +// for urdf or mujoco specific options, see documentation +{ + // Onshape assembly URL + "url": "https://cad.onshape.com/documents/3fb6fc43af8407628b8a436e/w/97510b712561d1cd4f61c300/e/926f5c272b7ea2919e153a0d", + // Output format: urdf or mujoco (required) + "output_format": "urdf", + "merge_stls": true, + "simplify_stls": "visual" +} diff --git a/model/gen2/gen2_fixed.xml b/model/gen2/gen2_fixed.xml new file mode 100644 index 00000000..0cf56738 --- /dev/null +++ b/model/gen2/gen2_fixed.xml @@ -0,0 +1,262 @@ + + + + diff --git a/model/gen2/robot.urdf b/model/gen2/robot.urdf new file mode 100644 index 00000000..f3327b16 --- /dev/null +++ b/model/gen2/robot.urdf @@ -0,0 +1,928 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/xiaoyan_description/dual_arm.urdf b/model/xiaoyan_description/dual_arm.urdf index f1cc20d0..7e7ab687 100644 --- a/model/xiaoyan_description/dual_arm.urdf +++ b/model/xiaoyan_description/dual_arm.urdf @@ -8,44 +8,30 @@ - - - - - - - - - - + + + + + + + + + + - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - + + + + + @@ -68,12 +54,6 @@ - - - - - - @@ -97,12 +77,6 @@ - - - - - - @@ -134,12 +108,6 @@ - - - - - - @@ -172,12 +140,6 @@ - - - - - - @@ -209,12 +171,6 @@ - - - - - - @@ -246,12 +202,6 @@ - - - - - - @@ -283,12 +233,6 @@ - - - - - - @@ -320,12 +264,6 @@ - - - - - - @@ -357,12 +295,6 @@ - - - - - - @@ -394,12 +326,6 @@ - - - - - - @@ -432,12 +358,6 @@ - - - - - - @@ -469,12 +389,6 @@ - - - - - - @@ -506,12 +420,6 @@ - - - - - - @@ -543,12 +451,6 @@ - - - - - - @@ -581,12 +483,6 @@ - - - - - - diff --git a/model/xiaoyan_description/dual_arm/dual_arm.usda b/model/xiaoyan_description/dual_arm/dual_arm.usda new file mode 100644 index 00000000..8da4d998 --- /dev/null +++ b/model/xiaoyan_description/dual_arm/dual_arm.usda @@ -0,0 +1,52 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend references = @./payloads/base.usda@ + variants = { + string Physics = "physx" + } + append variantSets = "Physics" +) +{ + variantSet "Physics" = { + "mujoco" ( + prepend payload = @./payloads/Physics/mujoco.usda@ + ) { + + } + "none" { + + } + "physics" ( + prepend payload = @./payloads/Physics/physics.usda@ + ) { + + } + "physx" ( + prepend payload = @./payloads/Physics/physx.usda@ + ) { + + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/Physics/mujoco.usda b/model/xiaoyan_description/dual_arm/payloads/Physics/mujoco.usda new file mode 100644 index 00000000..dc5e41fe --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/Physics/mujoco.usda @@ -0,0 +1,444 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + def MjcActuator "L_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + over "root_joint" + { + } + + over "L_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_FINGER_TIP_FIXED" + { + } + + over "R_CAM_FIXED" + { + } + } + + over "Geometry" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" + { + over "L_WRIST_R_S" + { + } + + over "L_WRIST_R_S_1" + { + } + } + + over "L_WRIST_Y_S" + { + } + + over "L_WRIST_Y_S_1" + { + } + } + + over "L_WRIST_P_S" + { + } + + over "L_WRIST_P_S_1" + { + } + } + + over "L_ELBOW_R_S" + { + } + + over "L_ELBOW_R_S_1" + { + } + } + + over "L_SHOULDER_Y_S" + { + } + + over "L_SHOULDER_Y_S_1" + { + } + } + + over "L_SHOULDER_R_S" + { + } + + over "L_SHOULDER_R_S_1" + { + } + } + + over "L_SHOULDER_P_S" + { + } + + over "L_SHOULDER_P_S_1" + { + } + } + + over "R_SHOULDER_P_S" + { + over "R_SHOULDER_R_S" + { + over "R_SHOULDER_Y_S" + { + over "R_ELBOW_R_S" + { + over "R_WRIST_P_S" + { + over "R_WRIST_Y_S" + { + over "R_WRIST_R_S" + { + over "R_FINGER_TIP" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_CAM" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_WRIST_R_S" + { + } + + over "R_WRIST_R_S_1" + { + } + } + + over "R_WRIST_Y_S" + { + } + + over "R_WRIST_Y_S_1" + { + } + } + + over "R_WRIST_P_S" + { + } + + over "R_WRIST_P_S_1" + { + } + } + + over "R_ELBOW_R_S" + { + } + + over "R_ELBOW_R_S_1" + { + } + } + + over "R_SHOULDER_Y_S" + { + } + + over "R_SHOULDER_Y_S_1" + { + } + } + + over "R_SHOULDER_R_S" + { + } + + over "R_SHOULDER_R_S_1" + { + } + } + + over "R_SHOULDER_P_S" + { + } + + over "R_SHOULDER_P_S_1" + { + } + } + + over "PELVIS_S" + { + } + + over "PELVIS_S_1" + { + } + } + } + + over "Materials" + { + over "material_16" + { + } + + over "material_17" + { + } + } + + over "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/Physics/physics.usda b/model/xiaoyan_description/dual_arm/payloads/Physics/physics.usda new file mode 100644 index 00000000..ece6b107 --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/Physics/physics.usda @@ -0,0 +1,530 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "PELVIS_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI", "PhysicsMassAPI"] + ) + { + bool newton:selfCollisionEnabled = 0 + point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627) + float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747) + float physics:mass = 2.106246 + quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252) + + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823) + + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278) + + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022) + + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766) + + over "L_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155) + + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "L_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925) + float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117) + float physics:mass = 0.50489414 + quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542) + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165) + + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957) + + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771) + + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363) + + over "R_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155) + + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "R_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534) + float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028) + float physics:mass = 0.50436604 + quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874) + + over "R_FINGER_TIP" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + + over "R_CAM" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + def PhysicsRevoluteJoint "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, 0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -114.59156 + float physics:upperLimit = 114.59156 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, 0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -124.9048 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "L_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, 0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -117.456345 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, 0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "L_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.0258, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 14.896903 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, -0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, -0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, -0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 117.456345 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, -0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "R_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.03, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -32.658596 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsFixedJoint "root_joint" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_FINGER_TIP_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_CAM_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677) + quatf physics:localRot1 = (1, 0, 0, 0) + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/Physics/physx.usda b/model/xiaoyan_description/dual_arm/payloads/Physics/physx.usda new file mode 100644 index 00000000..f65b957d --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/Physics/physx.usda @@ -0,0 +1,129 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + over "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/base.usda b/model/xiaoyan_description/dual_arm/payloads/base.usda new file mode 100644 index 00000000..2d9719a2 --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/base.usda @@ -0,0 +1,594 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./robot.usda@ + ] + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend apiSchemas = ["GeomModelAPI"] + assetInfo = { + string name = "dual_arm" + } + kind = "component" +) +{ + float3[] extentsHint = [(-0.043500002, -0.958077, -0.011133737), (0.080300845, 0.9075668, 0.12106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.043500002, -0.959077, -0.011133737), (0.080300845, 0.9075668, 0.12206)] + + def Scope "Materials" + { + def Material "material_16" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_17" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + + def Scope "Geometry" + { + def Xform "PELVIS_S" + { + def Xform "PELVIS_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, 0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, 0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, 0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, 0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.0258, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S_1" ( + displayName = "L_WRIST_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_WRIST_Y_S_1" ( + displayName = "L_WRIST_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_WRIST_P_S_1" ( + displayName = "L_WRIST_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_ELBOW_R_S_1" ( + displayName = "L_ELBOW_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_SHOULDER_Y_S_1" ( + displayName = "L_SHOULDER_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_SHOULDER_R_S_1" ( + displayName = "L_SHOULDER_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_SHOULDER_P_S_1" ( + displayName = "L_SHOULDER_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, -0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, -0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, -0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, -0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.03, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_FINGER_TIP" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_CAM" + { + quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.01212, -0.17655, 0.07506) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_WRIST_R_S_1" ( + displayName = "R_WRIST_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_WRIST_Y_S_1" ( + displayName = "R_WRIST_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_WRIST_P_S_1" ( + displayName = "R_WRIST_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_ELBOW_R_S_1" ( + displayName = "R_ELBOW_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_Y_S_1" ( + displayName = "R_SHOULDER_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_R_S_1" ( + displayName = "R_SHOULDER_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_P_S_1" ( + displayName = "R_SHOULDER_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "PELVIS_S_1" ( + displayName = "PELVIS_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + + def Scope "Physics" + { + } + + def Scope "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/instances.usda b/model/xiaoyan_description/dual_arm/payloads/instances.usda new file mode 100644 index 00000000..54238edb --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/instances.usda @@ -0,0 +1,564 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Instances" +{ + def Xform "PELVIS_S" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_1" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "PELVIS_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_2" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_3" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_4" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_5" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_ELBOW_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_6" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_7" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_8" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_9" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_10" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_11" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_12" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_ELBOW_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_13" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_14" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_15" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/materials.usda b/model/xiaoyan_description/dual_arm/payloads/materials.usda new file mode 100644 index 00000000..5801264d --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/materials.usda @@ -0,0 +1,222 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Materials" +{ + def Material "material_16" + { + color3f inputs:diffuseColor = (0, 1, 1) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_17" + { + color3f inputs:diffuseColor = (0, 1, 0) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_1" + { + color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_2" + { + color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_3" + { + color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_7" + { + color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/robot.usda b/model/xiaoyan_description/dual_arm/payloads/robot.usda new file mode 100644 index 00000000..5ebf95bb --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/robot.usda @@ -0,0 +1,260 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" ( + prepend apiSchemas = ["IsaacRobotAPI"] +) +{ + prepend rel isaac:physics:robotJoints = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + prepend rel isaac:physics:robotLinks = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + token isaac:robotType = "Default" + + over "Geometry" + { + over "PELVIS_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_FINGER_TIP" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + + over "R_CAM" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + over "root_joint" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_FINGER_TIP_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_CAM_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/dual_arm.usda b/model/xiaoyan_description/dual_arm_1/dual_arm.usda new file mode 100644 index 00000000..6b9fc6f6 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/dual_arm.usda @@ -0,0 +1,52 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend references = @./payloads/base.usda@ + variants = { + string Physics = "physx" + } + append variantSets = "Physics" +) +{ + variantSet "Physics" = { + "mujoco" ( + prepend payload = @./payloads/Physics/mujoco.usda@ + ) { + + } + "none" { + + } + "physics" ( + prepend payload = @./payloads/Physics/physics.usda@ + ) { + + } + "physx" ( + prepend payload = @./payloads/Physics/physx.usda@ + ) { + + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/Physics/mujoco.usda b/model/xiaoyan_description/dual_arm_1/payloads/Physics/mujoco.usda new file mode 100644 index 00000000..a6382414 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/Physics/mujoco.usda @@ -0,0 +1,400 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + def MjcActuator "L_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + over "root_joint" + { + } + + over "base_fixed" + { + } + + over "L_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_FINGER_TIP_FIXED" + { + } + + over "R_CAM_FIXED" + { + } + } + + over "Geometry" + { + over "base_link" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" + { + } + + over "L_WRIST_Y_S" + { + } + } + + over "L_WRIST_P_S" + { + } + } + + over "L_ELBOW_R_S" + { + } + } + + over "L_SHOULDER_Y_S" + { + } + } + + over "L_SHOULDER_R_S" + { + } + } + + over "L_SHOULDER_P_S" + { + } + } + + over "R_SHOULDER_P_S" + { + over "R_SHOULDER_R_S" + { + over "R_SHOULDER_Y_S" + { + over "R_ELBOW_R_S" + { + over "R_WRIST_P_S" + { + over "R_WRIST_Y_S" + { + over "R_WRIST_R_S" + { + over "R_FINGER_TIP" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_CAM" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_WRIST_R_S" + { + } + } + + over "R_WRIST_Y_S" + { + } + } + + over "R_WRIST_P_S" + { + } + } + + over "R_ELBOW_R_S" + { + } + } + + over "R_SHOULDER_Y_S" + { + } + } + + over "R_SHOULDER_R_S" + { + } + } + + over "R_SHOULDER_P_S" + { + } + } + + over "PELVIS_S" + { + } + } + + over "cylinder" + { + } + + over "base_column" + { + } + } + } + + over "Materials" + { + over "gray" + { + } + + over "material_16" + { + } + + over "material_17" + { + } + } + + over "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/Physics/physics.usda b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physics.usda new file mode 100644 index 00000000..ccac6fc0 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physics.usda @@ -0,0 +1,554 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI"] + ) + { + bool newton:selfCollisionEnabled = 0 + + over "PELVIS_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627) + float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747) + float physics:mass = 2.106246 + quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252) + + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823) + + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278) + + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022) + + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766) + + over "L_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155) + + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "L_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925) + float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117) + float physics:mass = 0.50489414 + quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542) + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165) + + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957) + + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771) + + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363) + + over "R_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155) + + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "R_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534) + float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028) + float physics:mass = 0.50436604 + quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874) + + over "R_FINGER_TIP" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + + over "R_CAM" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + + over "base_column" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + + over "Physics" + { + def PhysicsRevoluteJoint "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, 0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -114.59156 + float physics:upperLimit = 114.59156 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, 0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -124.9048 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "L_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, 0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -117.456345 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, 0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "L_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.0258, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 14.896903 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, -0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, -0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, -0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 117.456345 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, -0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "R_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.03, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -32.658596 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsFixedJoint "root_joint" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "base_fixed" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 1.2) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_FINGER_TIP_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_CAM_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677) + quatf physics:localRot1 = (1, 0, 0, 0) + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/Physics/physx.usda b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physx.usda new file mode 100644 index 00000000..638500a6 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physx.usda @@ -0,0 +1,129 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + over "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/base.usda b/model/xiaoyan_description/dual_arm_1/payloads/base.usda new file mode 100644 index 00000000..dee6c966 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/base.usda @@ -0,0 +1,451 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./robot.usda@ + ] + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend apiSchemas = ["GeomModelAPI"] + assetInfo = { + string name = "dual_arm" + } + kind = "component" +) +{ + float3[] extentsHint = [(-0.05, -0.958077, -2.3841858e-8), (0.080300845, 0.9075668, 1.32106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.05, -0.959077, -2.3841858e-8), (0.05, 0.05, 1.32206)] + + def Scope "Materials" + { + def Material "gray" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_16" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_17" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + + def Scope "Geometry" + { + def Xform "base_link" + { + def Cylinder "cylinder" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + rel material:binding = + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "PELVIS_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 1.2) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "PELVIS_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, 0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, 0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, 0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, 0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.0258, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + + def Xform "R_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, -0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, -0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, -0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, -0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.03, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_FINGER_TIP" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_CAM" + { + quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.01212, -0.17655, 0.07506) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + } + } + } + + def Cylinder "base_column" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + uniform token purpose = "guide" + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + + def Scope "Physics" + { + } + + def Scope "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/instances.usda b/model/xiaoyan_description/dual_arm_1/payloads/instances.usda new file mode 100644 index 00000000..c2e7a10a --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/instances.usda @@ -0,0 +1,369 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Instances" +{ + def Xform "PELVIS_S" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_1" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_2" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_3" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_4" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_5" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_6" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_7" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_8" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_9" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_10" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_11" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_12" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_13" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_14" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_15" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/materials.usda b/model/xiaoyan_description/dual_arm_1/payloads/materials.usda new file mode 100644 index 00000000..79051996 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/materials.usda @@ -0,0 +1,255 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Materials" +{ + def Material "gray" + { + color3f inputs:diffuseColor = (0.21404114, 0.21404114, 0.21404114) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_16" + { + color3f inputs:diffuseColor = (0, 1, 1) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_17" + { + color3f inputs:diffuseColor = (0, 1, 0) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_1" + { + color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_2" + { + color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_3" + { + color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_7" + { + color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/robot.usda b/model/xiaoyan_description/dual_arm_1/payloads/robot.usda new file mode 100644 index 00000000..14a8afe8 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/robot.usda @@ -0,0 +1,273 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" ( + prepend apiSchemas = ["IsaacRobotAPI"] +) +{ + prepend rel isaac:physics:robotJoints = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + prepend rel isaac:physics:robotLinks = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + token isaac:robotType = "Manipulator" + + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "PELVIS_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_FINGER_TIP" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + + over "R_CAM" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + over "root_joint" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "base_fixed" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_FINGER_TIP_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_CAM_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/dual_arm.usda b/model/xiaoyan_description/dual_arm_2/dual_arm.usda new file mode 100644 index 00000000..b6ed6a5b --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/dual_arm.usda @@ -0,0 +1,52 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend references = @./payloads/base.usda@ + variants = { + string Physics = "physx" + } + append variantSets = "Physics" +) +{ + variantSet "Physics" = { + "mujoco" ( + prepend payload = @./payloads/Physics/mujoco.usda@ + ) { + + } + "none" { + + } + "physics" ( + prepend payload = @./payloads/Physics/physics.usda@ + ) { + + } + "physx" ( + prepend payload = @./payloads/Physics/physx.usda@ + ) { + + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/Physics/mujoco.usda b/model/xiaoyan_description/dual_arm_2/payloads/Physics/mujoco.usda new file mode 100644 index 00000000..23566fce --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/Physics/mujoco.usda @@ -0,0 +1,400 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + def MjcActuator "L_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + over "root_joint" + { + } + + over "base_fixed" + { + } + + over "L_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_FINGER_TIP_FIXED" + { + } + + over "R_CAM_FIXED" + { + } + } + + over "Geometry" + { + over "base_link" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" + { + } + + over "L_WRIST_Y_S" + { + } + } + + over "L_WRIST_P_S" + { + } + } + + over "L_ELBOW_R_S" + { + } + } + + over "L_SHOULDER_Y_S" + { + } + } + + over "L_SHOULDER_R_S" + { + } + } + + over "L_SHOULDER_P_S" + { + } + } + + over "R_SHOULDER_P_S" + { + over "R_SHOULDER_R_S" + { + over "R_SHOULDER_Y_S" + { + over "R_ELBOW_R_S" + { + over "R_WRIST_P_S" + { + over "R_WRIST_Y_S" + { + over "R_WRIST_R_S" + { + over "R_FINGER_TIP" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_CAM" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_WRIST_R_S" + { + } + } + + over "R_WRIST_Y_S" + { + } + } + + over "R_WRIST_P_S" + { + } + } + + over "R_ELBOW_R_S" + { + } + } + + over "R_SHOULDER_Y_S" + { + } + } + + over "R_SHOULDER_R_S" + { + } + } + + over "R_SHOULDER_P_S" + { + } + } + + over "PELVIS_S" + { + } + } + + over "cylinder" + { + } + + over "base_column" + { + } + } + } + + over "Materials" + { + over "gray" + { + } + + over "material_16" + { + } + + over "material_17" + { + } + } + + over "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/Physics/physics.usda b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physics.usda new file mode 100644 index 00000000..b0444420 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physics.usda @@ -0,0 +1,554 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI"] + ) + { + bool newton:selfCollisionEnabled = 0 + + over "PELVIS_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627) + float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747) + float physics:mass = 2.106246 + quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252) + + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823) + + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278) + + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022) + + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766) + + over "L_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155) + + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "L_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925) + float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117) + float physics:mass = 0.50489414 + quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542) + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165) + + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957) + + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771) + + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363) + + over "R_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155) + + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "R_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534) + float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028) + float physics:mass = 0.50436604 + quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874) + + over "R_FINGER_TIP" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + + over "R_CAM" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + + over "base_column" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + + over "Physics" + { + def PhysicsRevoluteJoint "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, 0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -114.59156 + float physics:upperLimit = 114.59156 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, 0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -124.9048 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "L_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, 0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -117.456345 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, 0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "L_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.0258, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 14.896903 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, -0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, -0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, -0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 117.456345 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, -0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "R_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.03, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -32.658596 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsFixedJoint "root_joint" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "base_fixed" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 1.2) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_FINGER_TIP_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_CAM_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677) + quatf physics:localRot1 = (1, 0, 0, 0) + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/Physics/physx.usda b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physx.usda new file mode 100644 index 00000000..778984fd --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physx.usda @@ -0,0 +1,129 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + over "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/base.usda b/model/xiaoyan_description/dual_arm_2/payloads/base.usda new file mode 100644 index 00000000..58f9e8ef --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/base.usda @@ -0,0 +1,451 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./robot.usda@ + ] + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend apiSchemas = ["GeomModelAPI"] + assetInfo = { + string name = "dual_arm" + } + kind = "component" +) +{ + float3[] extentsHint = [(-0.05, -0.958077, -2.3841858e-8), (0.080300845, 0.9075668, 1.32106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.05, -0.959077, -2.3841858e-8), (0.05, 0.05, 1.32206)] + + def Scope "Materials" + { + def Material "gray" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_16" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_17" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + + def Scope "Geometry" + { + def Xform "base_link" + { + def Cylinder "cylinder" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + rel material:binding = + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "PELVIS_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 1.2) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "PELVIS_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, 0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, 0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, 0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, 0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.0258, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + + def Xform "R_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, -0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, -0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, -0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, -0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.03, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_FINGER_TIP" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_CAM" + { + quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.01212, -0.17655, 0.07506) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + } + } + } + + def Cylinder "base_column" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + uniform token purpose = "guide" + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + + def Scope "Physics" + { + } + + def Scope "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/geometries.usd b/model/xiaoyan_description/dual_arm_2/payloads/geometries.usd new file mode 100644 index 00000000..8015a4a5 Binary files /dev/null and b/model/xiaoyan_description/dual_arm_2/payloads/geometries.usd differ diff --git a/model/xiaoyan_description/dual_arm_2/payloads/instances.usda b/model/xiaoyan_description/dual_arm_2/payloads/instances.usda new file mode 100644 index 00000000..dfc8b3f8 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/instances.usda @@ -0,0 +1,369 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Instances" +{ + def Xform "PELVIS_S" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_1" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_2" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_3" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_4" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_5" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_6" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_7" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_8" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_9" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_10" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_11" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_12" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_13" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_14" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_15" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/materials.usda b/model/xiaoyan_description/dual_arm_2/payloads/materials.usda new file mode 100644 index 00000000..b404b460 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/materials.usda @@ -0,0 +1,255 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Materials" +{ + def Material "gray" + { + color3f inputs:diffuseColor = (0.21404114, 0.21404114, 0.21404114) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_16" + { + color3f inputs:diffuseColor = (0, 1, 1) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_17" + { + color3f inputs:diffuseColor = (0, 1, 0) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_1" + { + color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_2" + { + color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_3" + { + color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_7" + { + color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/robot.usda b/model/xiaoyan_description/dual_arm_2/payloads/robot.usda new file mode 100644 index 00000000..fd7f6ff7 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/robot.usda @@ -0,0 +1,273 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" ( + prepend apiSchemas = ["IsaacRobotAPI"] +) +{ + prepend rel isaac:physics:robotJoints = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + prepend rel isaac:physics:robotLinks = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + token isaac:robotType = "Manipulator" + + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "PELVIS_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_FINGER_TIP" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + + over "R_CAM" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + over "root_joint" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "base_fixed" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_FINGER_TIP_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_CAM_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_collision.urdf b/model/xiaoyan_description/dual_arm_collision.urdf new file mode 100644 index 00000000..1ce06c46 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_collision.urdf @@ -0,0 +1,517 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/model/xiaoyan_description/dual_arm_collision.usda b/model/xiaoyan_description/dual_arm_collision.usda new file mode 100644 index 00000000..08a8e167 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_collision.usda @@ -0,0 +1,324 @@ +#usda 1.0 +( + defaultPrim = "dual_arm" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @dual_arm_2/dual_arm.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "base_link" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" ( + instanceable = false + ) + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.11902186, 0.25356677, 0.069732234) + double3 xformOp:translate = (-0.0050100889056921005, 0.11078338418155909, -0.01045313011854887) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.5000000000000001, -0.5000000000000001, -0.5000000000000001, 0.5000000000000001) + float3 xformOp:scale = (0.0409999, 0.053493902, 0.0595) + double3 xformOp:translate = (-0.0037500001490116098, -5.408977040638672e-19, -0.03274694923311472) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cylinder "AUTO_COLLISION_CYLINDER" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + uniform token axis = "Z" + custom token collision:primitiveType = "cylinder" + double height = 0.16749374149367213 + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double radius = 0.03765367431379833 + quatd xformOp:orient = (0.7071067811865475, -0.7071067811865475, 0, 0) + double3 xformOp:translate = (3.469446951953614e-18, 0.08874687016941607, 0.012211145890315668) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.069, 0.1285, 0.058) + double3 xformOp:translate = (-0.034500000427457156, 0.03925000037997961, 1.862645149230957e-9) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.060999997, 0.14400001, 0.062992066) + double3 xformOp:translate = (-0.0008999994024634361, 0.062000001315027475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.0785, 0.1725, 0.065) + double3 xformOp:translate = (-0.03925000161955211, 0.056249999441206455, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.000040139854472664035, -0.00004013985447262077, 0.7071067800472512, 0.7071067800472514) + float3 xformOp:scale = (0.07170313, 0.07299983, 0.104499996) + double3 xformOp:translate = (-0.004348363594192078, 0.06074999878183007, 3.992215372663401e-8) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.07699999, 0.216, 0.0805) + double3 xformOp:translate = (0, 0, 0.04024999989568795) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_SHOULDER_P_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.7071067800472514, 0.7071067800472515, -0.0000401398544727094, 0.00004013985447257145) + float3 xformOp:scale = (0.07170313, 0.07299983, 0.104499996) + double3 xformOp:translate = (-0.004348363594192072, -0.06074999924749136, 3.992215372663644e-8) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_SHOULDER_R_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.0785, 0.1725, 0.065) + double3 xformOp:translate = (-0.03925000161954845, -0.056249999441206455, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_SHOULDER_Y_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.060999997, 0.14400001, 0.062992066) + double3 xformOp:translate = (-0.0008999994024634361, -0.06200000178068876, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_ELBOW_R_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.069, 0.1285, 0.058) + double3 xformOp:translate = (-0.03450000042745616, -0.03925000037997961, 1.862645149230957e-9) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_WRIST_P_S" + { + def Cylinder "AUTO_COLLISION_CYLINDER" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + uniform token axis = "Z" + custom token collision:primitiveType = "cylinder" + double height = 0.16749374056234956 + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double radius = 0.03765373180894358 + quatd xformOp:orient = (0.7071067811865475, -0.7071067811865475, 0, 0) + double3 xformOp:translate = (3.469446951953614e-18, -0.08874687063507736, 0.012211077787740575) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"] + } + + over "R_WRIST_Y_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.5000000000000001, -0.5000000000000001, -0.5000000000000001, 0.5000000000000001) + float3 xformOp:scale = (0.0409999, 0.053493902, 0.0595) + double3 xformOp:translate = (-0.0037500001490116098, -5.408977040638672e-19, -0.03274694923311472) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_WRIST_R_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.11152348, 0.25179133, 0.0779598) + double3 xformOp:translate = (-0.01363389752805233, -0.1093915430828929, -0.014153839088976383) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + } + } + } + } +} + diff --git a/protos/README.md b/protos/README.md new file mode 100644 index 00000000..fef77e4e --- /dev/null +++ b/protos/README.md @@ -0,0 +1,191 @@ +# Protobuf 与协议开发指南 + +`protos/` 是 CMVR-ES 配置、gRPC API、共享消息和 QUIC 控制面的契约源。生成代码由 CMake 创建，不应手工编辑或提交。 + +返回[项目总览](../README.md)。 + +## 目录职责 + +| 目录 | 职责 | +| --- | --- | +| `cmvr/api/` | gRPC service 和 command DTO | +| `cmvr/msgs/` | 设备、状态和错误等共享消息 | +| `cmvr/config/` | Proto Text 运行配置 Schema | +| `cmvr/common/` | 通用几何等跨领域消息 | +| `cmvr/quic_edge/v1/` | QUIC reliable control message 和 v1 wire 文档 | +| `rbk/` | 第三方/兼容协议 Schema | + +## C++ 生成流程 + +根 [`CMakeLists.txt`](../CMakeLists.txt)： + +1. 递归收集 `protos/**/*.proto`； +2. 使用 `protoc` 生成 `.pb.h/.pb.cc`； +3. 使用 `grpc_cpp_plugin` 生成 `.grpc.pb.h/.grpc.pb.cc`； +4. 输出到 `build/_protobuf/`； +5. 打包为 `cmvr_es::proto`。 + +`build/_protobuf/` 是构建产物，不手改、不提交。 + +当前使用 `file(GLOB_RECURSE ...)` 且没有 `CONFIGURE_DEPENDS`。新增 Proto 文件后必须重新执行 CMake configure： + +```bash +cmake -S . -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMVR_ARCH=x86 + +cmake --build build -j"$(nproc)" +``` + +`protoc` 和 `grpc_cpp_plugin` 当前固定从 `output/bin/` 使用。干净环境首次构建先执行根 README 的工具引导步骤。 + +## 通用兼容规则 + +已发布协议中禁止： + +- 修改或复用字段号； +- 修改 enum 数值含义； +- 修改 oneof tag； +- 修改字段 wire type； +- 修改 gRPC package、service 或 method 全名； +- 将原本可选的字段改成必填语义； +- 把同一字段单位从 m 改成 mm 等隐式破坏。 + +删除字段时同时 reserved 字段号和名称： + +```protobuf +message Example { + reserved 2; + reserved "old_field"; + + string id = 1; + string new_field = 3; +} +``` + +新增字段使用新 tag。需要区分“缺失”和“零值”时使用 proto3 `optional`，并验证目标语言工具链支持。 + +破坏性 gRPC 变化应创建版本化 package/service，而不是原地改变旧方法。 + +## 配置 Proto + +配置也必须考虑已部署 `.pb.txt`： + +- 新 scalar 的零值不能自动成为危险默认值； +- C++ 必须做范围和跨字段校验； +- enum unknown、oneof 未设置应明确失败； +- root message 命名保持 `XxxRootConfig`； +- 字段注释写明单位、范围和安全语义； +- Schema 变更同步 [`../cmvr-es/config/README.md`](../cmvr-es/config/README.md) 与样例配置。 + +TextFormat 使用字段名，因此随意重命名字段同样会破坏旧配置。 + +## gRPC API + +推荐一个领域拆分为： + +```text +cmvr/api/example_command.proto +cmvr/api/example_service.proto +``` + +service 文件 import command 文件。公共 header、错误和时间戳复用 `common.proto`，不要复制出多个略有差异的定义。 + +新增 API 后还需要： + +1. 实现 C++ service； +2. 加入 service CMake target； +3. 在 GrpcServerTask 中构造和注册； +4. 生成 Java client； +5. 增加 handler、reflection 和 grpcurl 测试。 + +完整流程见 [`../cmvr-es/service/README.md`](../cmvr-es/service/README.md)。 + +`cmvr/api/test_service.proto` 当前没有实现或注册，且包含历史拼写 `TestReqeust`，不要把它作为新 API 模板。 + +## QUIC Edge v1 + +当前固定契约： + +- package：`cmvr.quic_edge.v1` +- `protocol_version = 1` +- ALPN：`cmvr-quic-edge/1` +- reliable stream：`uint32_be length + EdgeControlEnvelope` +- media DATAGRAM：固定 64 字节非 Protobuf header + payload + +完整字段和偏移见 [`cmvr/quic_edge/v1/README.md`](cmvr/quic_edge/v1/README.md)。 + +破坏 framing、固定头、状态机或时序时，必须建立新协议版本、package 和 ALPN，不能只修改 Proto。 + +新增 control envelope payload 时： + +- 使用未占用的新 oneof tag； +- 同步 edge 发送/dispatch 和状态机； +- 同步 message/session/heartbeat sequence 校验； +- 同步 Java Gateway； +- 更新 wire 文档； +- 增加 golden vector 和 fake transport 测试。 + +Edge 当前入站只实现 RegisterResponse、HeartbeatAck 和 ProtocolError。Proto 中存在的消息不等于两个方向都已实现。 + +每次 QUIC 连接建立后，Edge 的出站 control `message_sequence` 从 `0` 重新开始，因此首个 `NodeRegisterRequest.message_sequence` 当前为 `0`。Gateway 必须接受该起始值，不能假设从 `1` 开始；后续只要求同一发送方向严格递增，允许出现间隔。 + +QUIC stream 与 DATAGRAM 没有跨通道到达顺序保证。Gateway 不能假设 session/descriptor 一定先于媒体到达。 + +## Java 生成 + +仓库只自动生成 C++。Java 平台应在自己的 Gradle/Maven 工程中锁定： + +- protoc +- protobuf-java +- protoc-gen-grpc-java +- grpc-java + +并以 `protos/` 作为 import root。 + +QUIC Gateway 只需普通 Protobuf Java message： + +```bash +output/bin/protoc \ + -I protos \ + --java_out=platform-gateway/src/main/java \ + protos/cmvr/quic_edge/v1/quic_edge.proto +``` + +gRPC Java client 还需要 grpc-java plugin。当前 Proto 没有统一 `java_package` 和 `java_multiple_files`，增加这些 option 时应同步两端生成结果和平台 import。 + +`cmvr.api.FrameData` 的字段 7–13 是可选的实时流诊断信息，包括边缘端采集 UTC +时间、源序列、PTS/DTS、SDK 帧率、SDK 原始时间戳和帧号。旧客户端会安全忽略 +这些字段；Java 平台需要重新生成 message 和 stub 才能读取它们。SDK 原始时间戳 +的单位和时钟域由设备定义，不能按 Unix 时间直接解释。 + +浏览器不能直接消费自定义 QUIC ALPN，不应从这些 Proto 推导“浏览器可直连 Edge”。 + +## Review 与验证 + +```bash +# 重新生成并编译 C++ +cmake -S . -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMVR_ARCH=x86 +cmake --build build -j"$(nproc)" + +# 验证当前自动测试 +ctest --test-dir build --output-on-failure + +# 验证运行时 reflection +/tmp/grpcurl -plaintext 127.0.0.1:50052 list +``` + +评审清单： + +- [ ] 新字段只使用新 tag +- [ ] 删除字段同时 reserved number 和 name +- [ ] enum 数值和语义兼容 +- [ ] 旧 pb.txt 仍可解析 +- [ ] C++ 重新 configure 并生成 +- [ ] Java/其他语言生成成功 +- [ ] service 已真正注册 +- [ ] grpcurl reflection 与预期一致 +- [ ] QUIC wire 文档和测试同步更新 +- [ ] 破坏性变化采用新版本 diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto new file mode 100644 index 00000000..267c88f3 --- /dev/null +++ b/protos/cmvr/api/agv_command.proto @@ -0,0 +1,212 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; +import "cmvr/msgs/agv.proto"; + +// 查询 AGV 运行状态命令。 +message AgvRuntimeStateCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + // AGV 当前运行状态快照。 + cmvr.msgs.AgvRuntimeState state = 2; + } +} + +// 查询 AGV 当前导航任务状态命令。 +message AgvNavigationStatusCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + // 当前导航任务状态。 + cmvr.msgs.AgvNavigationStatus status = 2; + } +} + +// 导航到指定地图位姿命令。 +message AgvNavigateToPoseCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 目标位姿。x/y 单位：米，theta 单位：弧度。 + cmvr.msgs.AgvPose2d pose = 2; + // 通用运动约束和执行选项。 + cmvr.msgs.AgvMotionOptions options = 3; + // AGV 适配器扩展参数，用于传递厂商特有选项。 + cmvr.msgs.AgvAdapterParams adapter_params = 4; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + } +} + +// 导航到指定站点命令。 +message AgvNavigateToStationCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 目标站点 id。 + string station_id = 2; + // 通用运动约束和执行选项。 + cmvr.msgs.AgvMotionOptions options = 3; + // AGV 适配器扩展参数，用于传递厂商特有选项。 + cmvr.msgs.AgvAdapterParams adapter_params = 4; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + } +} + +// 按显式站点路径导航命令。 +message AgvFollowPathCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 路径段列表。每段包含起点站点 id 和终点站点 id。 + repeated cmvr.msgs.AgvPathSegment path = 2; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + } +} + +// 下发底盘速度命令。 +message AgvSetVelocityCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 目标车体速度。vx/vy 单位：米/秒，wz 单位：弧度/秒。 + cmvr.msgs.AgvVelocity velocity = 2; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + } +} + +// 查询可用地图列表命令。 +message AgvListMapsCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + // 地图名称列表。 + repeated string maps = 2; + } +} + +// 查询当前地图站点列表命令。 +message AgvListStationsCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + // 当前地图中的站点列表。 + repeated cmvr.msgs.AgvStation stations = 2; + } +} + +// 地图上传、下载、切换等通用地图命令。 +message AgvMapCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 地图名称。切换/下载时表示目标地图，上传时表示写入的地图名称。 + string map_name = 2; + // 地图内容。上传地图时使用；下载或切换地图时可为空。 + string content = 3; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + // 地图内容。下载地图时返回；其他命令通常为空。 + string content = 2; + } +} + +// 开始建图/扫图命令。 +message AgvStartMappingCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 请求建图维度：2D、3D 或二者都要。未指定时由适配器选择最合适模式。 + cmvr.msgs.AgvMapDimension dimension = 2; + // 目标地图名称。为空表示由 AGV 或适配器创建/选择默认地图名。 + string map_name = 3; + // 是否请求实时建图更新。true 表示希望实时推送；false 表示允许离线/批处理。 + bool real_time = 4; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + // 建图会话 id。该值由服务端生成，作为调试和日志关联标识。 + string session_id = 2; + } +} + +// 地图流命令。用于获取当前地图、最近地图、全量快照和后续增量更新。 +message AgvMapStreamCommand { + // 请求体。 + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 请求地图维度：2D、3D 或二者都要。该字段不是厂商格式选择器。 + cmvr.msgs.AgvMapDimension dimension = 2; + // 地图名称。为空表示当前加载地图；若无当前地图，适配器应尝试使用最近保存/创建的地图。 + string map_name = 3; + // 断点续传令牌。为空表示上位机没有缓存，服务端应先发送全量快照。 + string resume_token = 4; + // 是否请求全量快照。首次请求通常应为 true。 + bool snapshot = 5; + // 是否在快照之后保持流并发送增量更新。若适配器不支持增量，可继续发送全量并标记 update_type。 + bool incremental = 6; + // 单条流消息建议最大载荷大小，单位：字节。小于等于 0 表示使用服务端默认值。 + int32 max_chunk_bytes = 7; + } + // 反馈体。 + message Feedback { + // 通用反馈头。包含成功标志、错误信息和反馈时间戳。 + CommandHeader.Feedback header = 1; + // 统一地图更新。payload 只会是 map_2d 或 map_3d，不暴露厂商原始格式。 + cmvr.msgs.AgvUnifiedMapUpdate update = 2; + } +} diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto new file mode 100644 index 00000000..f0bdc5b2 --- /dev/null +++ b/protos/cmvr/api/agv_service.proto @@ -0,0 +1,72 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; +import "cmvr/api/agv_command.proto"; + +// AGV 通用服务。该服务只暴露控制器无关的能力， +// 厂商协议、地图文件格式和控制器特有参数由具体 AGV 适配器内部处理。 +service AgvService { + // 获取 AGV 当前运行状态快照。 + rpc getRuntimeState(AgvRuntimeStateCommand.Request) returns (AgvRuntimeStateCommand.Feedback); + + // 获取当前导航任务状态。 + rpc getNavigationStatus(AgvNavigationStatusCommand.Request) returns (AgvNavigationStatusCommand.Feedback); + + // 执行急停或等效安全停止动作。 + rpc emergencyStop(CommandHeader.Request) returns (CommandHeader.Feedback); + + // 清除可恢复故障或告警。 + rpc clearFault(CommandHeader.Request) returns (CommandHeader.Feedback); + + // 导航到指定地图位姿。目标位姿 x/y 单位为米，theta 单位为弧度。 + rpc navigateToPose(AgvNavigateToPoseCommand.Request) returns (AgvNavigateToPoseCommand.Feedback); + + // 导航到指定地图站点。 + rpc navigateToStation(AgvNavigateToStationCommand.Request) returns (AgvNavigateToStationCommand.Feedback); + + // 按显式站点路径执行导航。 + rpc followPath(AgvFollowPathCommand.Request) returns (AgvFollowPathCommand.Feedback); + + // 暂停当前导航任务。 + rpc pauseNavigation(CommandHeader.Request) returns (CommandHeader.Feedback); + + // 恢复已暂停的导航任务。 + rpc resumeNavigation(CommandHeader.Request) returns (CommandHeader.Feedback); + + // 取消当前导航任务。 + rpc cancelNavigation(CommandHeader.Request) returns (CommandHeader.Feedback); + + // 下发底盘速度控制指令。vx/vy 单位为米/秒，wz 单位为弧度/秒。 + rpc setVelocity(AgvSetVelocityCommand.Request) returns (AgvSetVelocityCommand.Feedback); + + // 停止底盘速度控制。该接口不等价于取消导航任务。 + rpc stopVelocityControl(CommandHeader.Request) returns (CommandHeader.Feedback); + + // 查询 AGV 控制器可用地图名称列表。 + rpc listMaps(AgvListMapsCommand.Request) returns (AgvListMapsCommand.Feedback); + + // 查询当前地图中的站点列表。 + rpc listStations(AgvListStationsCommand.Request) returns (AgvListStationsCommand.Feedback); + + // 切换当前使用地图。 + rpc switchMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback); + + // 上传地图内容到 AGV 控制器。地图内容字段由适配器解释。 + rpc uploadMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback); + + // 下载指定地图内容。 + rpc downloadMap(AgvMapCommand.Request) returns (AgvMapCommand.Feedback); + + // 开始建图/扫图会话。请求只选择 2D、3D 或二者都要， + // 控制器特有地图格式由 AGV 适配器内部转换。 + rpc startMapping(AgvStartMappingCommand.Request) returns (AgvStartMappingCommand.Feedback); + + // 以服务端流方式发送统一地图。resume_token 为空时应先发送全量快照； + // 后续是否发送增量由 AGV 适配器能力决定，并通过 update_type 标记。 + rpc streamMap(AgvMapStreamCommand.Request) returns (stream AgvMapStreamCommand.Feedback); + + // 停止当前建图/扫图会话。 + rpc stopMapping(CommandHeader.Request) returns (CommandHeader.Feedback); +} diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index 565b4939..92562f4a 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/protos/cmvr/api/arm_service.proto @@ -8,6 +8,7 @@ import "cmvr/api/arm_command.proto"; service ArmService { rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback); rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc clearFault(CommandHeader.Request) returns (CommandHeader.Feedback); rpc moveJ(MoveJ.Request) returns (MoveJ.Response); rpc moveL(MoveL.Request) returns (MoveL.Response); rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response); diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index c8aee52e..84d1c9bd 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -1,5 +1,6 @@ syntax = "proto3"; +import "cmvr/api/common.proto"; import "cmvr/api/system_command.proto"; package cmvr.api; @@ -10,6 +11,7 @@ service SystemService { rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {} rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {} + rpc ExecuteJsonCommand(JsonDeviceCommand.Request) returns (JsonDeviceCommand.Feedback) {} rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {} -} \ No newline at end of file +} diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto index 004e2155..c8ec8c56 100644 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -1,24 +1,78 @@ syntax = "proto3"; package cmvr.config; +// 示例/测试 AGV 后端配置。 message MyAgvConfig { + // 设备 id。为空时通常由外层 AGVDeviceConfig.id 补齐。 string id = 1; + // AGV 控制器 IP 地址或主机名。 string ip = 2; + // AGV 控制器端口号。 int32 port = 3; } +// 仙工 SRC1100 AGV 后端配置。 +message Src1100AgvConfig { + // 设备 id。为空时通常由外层 AGVDeviceConfig.id 补齐。 + string id = 1; + // SRC1100 控制器 IP 地址。 + string ip = 2; + // 是否启用该后端配置。当前设备是否创建仍以设备管理器配置为准。 + bool enable = 3; + // 状态查询端口，默认 19204。 + int32 port_status = 4; + // 控制命令端口，默认 19205。 + int32 port_control = 5; + // 导航任务端口，默认 19206。 + int32 port_nav = 6; + // 地图/配置文件端口，默认 19207。 + int32 port_config = 7; + // 其他功能端口，默认 19210，例如扫图开始/停止。 + int32 port_other = 8; + // 状态推送端口，默认 19301。 + int32 port_push = 9; + // 连接超时时间，单位：毫秒。0 表示使用适配器默认值。 + int32 connect_timeout_ms = 10; + // 接收超时时间，单位：毫秒。0 表示使用适配器默认值。 + int32 recv_timeout_ms = 11; + // 是否启用机器人状态实时推送。 + bool enable_state_push = 12; + // 状态推送间隔，单位：毫秒。0 表示不修改控制器默认间隔。 + int32 state_push_interval_ms = 13; + // 状态推送中显式包含的字段列表。该字段不能与 state_push_excluded_fields 同时配置。 + repeated string state_push_included_fields = 14; + // 状态推送中排除的字段列表。该字段不能与 state_push_included_fields 同时配置。 + repeated string state_push_excluded_fields = 15; + // 是否启用地图后台更新线程。启用后适配器会周期性抓取并解析地图，供 gRPC 地图流直接读取。 + bool enable_map_update = 16; + // 地图后台更新间隔，单位：毫秒。0 表示使用适配器默认值。 + int32 map_update_interval_ms = 17; + // 统一地图更新缓存条数。0 表示使用适配器默认值；缓存满后会丢弃最旧更新。 + uint32 map_update_history_size = 18; +} + +// 单个 AGV 设备配置。 message AGVDeviceConfig { + // 设备 id。必须与设备管理器中的 AGV 设备 id 对应。 string id = 1; + // 具体 AGV 后端配置。同一设备只能选择一个后端。 oneof backend { + // 示例/测试 AGV 后端。 MyAgvConfig my_agv = 10; + // 仙工 SRC1100 AGV 后端。 + Src1100AgvConfig src1100_agv = 11; } } +// AGV 设备配置集合。 message AGVConfig { + // AGV 设备列表。 repeated AGVDeviceConfig agvs = 1; } +// AGV 配置文件根节点。 message AGVRootConfig { + // AGV 配置集合。 AGVConfig agv = 1; } diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index e6d88215..7c166df9 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -10,6 +10,8 @@ message MotorConfigItem { double limit_q_ub = 4; double limit_qd = 5; double limit_qdd = 6; + double encoder_counts_per_rev = 7; + double gear_ratio = 8; } message MotorList { @@ -18,9 +20,33 @@ message MotorList { message EthercatSlaveConfig { int32 motor_id = 1; - int32 slave_index = 2; - uint32 vendor_id = 3; - uint32 product_code = 4; + uint32 alias = 2; + uint32 position = 3; +} + +message Cia402ProtocolConfig { + uint32 state_transition_timeout_ms = 2; + uint32 velocity_stop_timeout_ms = 3; + uint32 status_poll_period_ms = 4; + double stopped_velocity_tolerance_rad_s = 5; +} + +message ZeroCalibrationConfig { + uint32 timeout_ms = 1; + uint32 poll_period_ms = 2; + uint32 stable_sample_count = 3; + uint32 position_tolerance_counts = 4; + uint32 stable_delta_counts = 5; +} + +message EtherCATDcConfig { + optional bool enable = 1; + optional int32 reference_motor_id = 2; + optional uint32 sync0_cycle_us = 3; + optional int32 sync0_shift_us = 4; + optional uint32 sync_reference_clock_period = 5; + optional uint32 assign_activate = 6; + optional uint32 sync_monitor_period_ms = 7; } message SocketCanConfig { @@ -29,8 +55,13 @@ message SocketCanConfig { } message EtherCATConfig { - string master_id = 1; + uint32 master_index = 1; int32 cycle_us = 2; + Cia402ProtocolConfig cia402 = 3; + EtherCATDcConfig dc = 4; + optional uint32 slave_op_timeout_ms = 5; + optional uint32 slave_state_poll_period_ms = 6; + ZeroCalibrationConfig zero_calibration = 7; repeated EthercatSlaveConfig slaves = 10; } @@ -49,6 +80,7 @@ enum MotorVendor { MOTOR_VENDOR_UNKNOWN = 0; MOTOR_VENDOR_TI5 = 1; MOTOR_VENDOR_MUJOCO = 2; + MOTOR_VENDOR_EYOU = 3; } enum MotorProtocol { @@ -63,7 +95,6 @@ message MotorGroupConfig { MotorBusType bus_type = 2; MotorVendor vendor = 3; MotorProtocol protocol = 4; - string tool_frame = 5; oneof bus_config { SocketCanConfig can = 10; diff --git a/protos/cmvr/config/quic_edge_config/quic_edge_config.proto b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto new file mode 100644 index 00000000..904e5604 --- /dev/null +++ b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto @@ -0,0 +1,90 @@ +syntax = "proto3"; + +package cmvr.config; + +message QuicEdgeTlsConfig { + string ca_file = 1; + string certificate_file = 2; + string private_key_file = 3; + string server_name = 4; + + // Development-only escape hatch. Production configurations should keep + // this false and provide ca_file plus server_name. + bool allow_insecure = 5; +} + +message QuicEdgeReconnectConfig { + uint32 initial_delay_ms = 1; + uint32 maximum_delay_ms = 2; + double multiplier = 3; + uint32 jitter_percent = 4; + uint32 connect_timeout_ms = 5; +} + +message QuicEdgeTrackConfig { + enum SourceKind { + SOURCE_KIND_UNSPECIFIED = 0; + SOURCE_KIND_CAMERA = 1; + SOURCE_KIND_MICROPHONE = 2; + } + + uint32 track_id = 1; + SourceKind source_kind = 2; + string device_id = 3; + bool enable = 4; + + // Zero uses QuicEdgeConfig.maximum_frame_bytes. + uint32 max_frame_bytes = 5; + + // Protocol-neutral MediaSourceHub track ID. When empty, the service derives + // the default ID from source_kind and device_id (for example + // "right_hand_cam/video/color"). The Hub's descriptor is the authority for + // codec metadata and descriptor generation. + string source_track_id = 6; +} + +message QuicEdgeConfig { + string id = 1; + reserved 2; + string server_host = 3; + uint32 server_port = 4; + string alpn = 5; + string node_id = 6; + QuicEdgeTlsConfig tls = 7; + QuicEdgeReconnectConfig reconnect = 8; + + // Local safety limits. The effective DATAGRAM size is the minimum of this + // value and the peer/path value reported by the QUIC implementation. + uint32 maximum_datagram_bytes = 9; + uint32 maximum_control_frame_bytes = 10; + uint32 maximum_frame_bytes = 11; + uint32 datagram_send_queue_depth = 12; + uint32 media_poll_interval_ms = 13; + + // Node-presence fields migrated from the former gRPC edge agent. node_id may + // be "auto" when the implementation supports hostname-based resolution. + string software_version = 14; + + // Existing edge gRPC server endpoint advertised to the gateway. Empty or + // "auto" host selects a usable address from the current interface snapshot. + string grpc_endpoint_host = 15; + uint32 grpc_endpoint_port = 16; + bool grpc_endpoint_tls = 17; + + // Local heartbeat frequency in milliseconds. A zero + // NodeRegisterResponse.heartbeat_interval_ms keeps this value; a non-zero + // response is the platform's negotiated override. + // control_response_timeout_ms applies while waiting for registration and + // heartbeat acknowledgements on the reliable stream. + uint32 heartbeat_interval_ms = 18; + uint32 control_response_timeout_ms = 19; + + // An empty list, or a list with no enabled entry, is valid: node registration, + // IP reporting and heartbeat continue without a camera or microphone. + repeated QuicEdgeTrackConfig tracks = 20; + bool include_loopback_interfaces = 21; +} + +message QuicEdgeRootConfig { + QuicEdgeConfig quic_edge = 1; +} diff --git a/protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto b/protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto new file mode 100644 index 00000000..521fd5e1 --- /dev/null +++ b/protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto @@ -0,0 +1,45 @@ +syntax = "proto3"; + +package cmvr.config; + +message CollisionPairConfig { + string first = 1; + string second = 2; +} + +message SelfCollisionCheckerConfig { + string urdf_path = 1; + repeated CollisionPairConfig ignored_pairs = 2; +} + +message DistanceSamplingConfig { + double max_geometry_displacement_m = 1; + double max_check_period_s = 2; +} + +message CollisionSafetyConfig { + double warning_distance_m = 1; + double stop_distance_m = 2; +} + +message ProtectiveRecoveryConfig { + double clear_distance_m = 1; + double stable_period_s = 2; + double max_joint_velocity_rad_s = 3; + double max_joint_acceleration_rad_s2 = 4; + double history_duration_s = 5; + double max_distance_regression_m = 6; +} + +message SelfCollisionTaskConfig { + string id = 1; + string arm_id = 2; + SelfCollisionCheckerConfig checker = 10; + DistanceSamplingConfig sampling = 11; + CollisionSafetyConfig safety = 12; + ProtectiveRecoveryConfig recovery = 13; +} + +message SelfCollisionTaskRootConfig { + SelfCollisionTaskConfig self_collision_task = 1; +} diff --git a/protos/cmvr/config/task_manager_config/task_manager_config.proto b/protos/cmvr/config/task_manager_config/task_manager_config.proto index 6c13c845..8c60e370 100644 --- a/protos/cmvr/config/task_manager_config/task_manager_config.proto +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -6,8 +6,8 @@ message TaskConfigEntry { TASK_TYPE_UNKNOWN = 0; TASK_TYPE_TOUCH_SCREEN = 1; TASK_TYPE_GRPC_SERVER = 3; - reserved 2; - reserved "TASK_TYPE_ARM_CONTROL"; + TASK_TYPE_SELF_COLLISION = 4; + TASK_TYPE_QUIC_EDGE = 5; } enum TaskRunMode { diff --git a/protos/cmvr/msgs/agv.proto b/protos/cmvr/msgs/agv.proto new file mode 100644 index 00000000..54439eeb --- /dev/null +++ b/protos/cmvr/msgs/agv.proto @@ -0,0 +1,310 @@ +syntax = "proto3"; + +package cmvr.msgs; + +// AGV 在地图平面坐标系中的二维位姿。 +message AgvPose2d { + // X 坐标，单位：米。 + double x = 1; + // Y 坐标，单位：米。 + double y = 2; + // 航向角，单位：弧度，逆时针为正。 + double theta = 3; +} + +// AGV 车体坐标系下的平面速度。 +message AgvVelocity { + // 车体 X 方向线速度，单位：米/秒。 + double vx = 1; + // 车体 Y 方向线速度，单位：米/秒。 + double vy = 2; + // 绕 Z 轴角速度，单位：弧度/秒。 + double wz = 3; +} + +// AGV 电池状态。 +message AgvBatteryState { + // 电量比例，范围：[0, 1]，例如 0.8 表示 80%。 + double percentage = 1; + // 电池电压，单位：伏特。 + double voltage = 2; + // 电池电流，单位：安培；正负号含义由具体 AGV 适配器保持一致。 + double current = 3; + // 电池温度，单位：摄氏度。 + double temperature = 4; + // 是否正在充电。 + bool charging = 5; +} + +// 导航任务的通用运动约束和执行选项。 +message AgvMotionOptions { + // 最大线速度，单位：米/秒；0 表示使用 AGV 默认值。 + double max_speed = 1; + // 最大角速度，单位：弧度/秒；0 表示使用 AGV 默认值。 + double max_angular_speed = 2; + // 最大线加速度，单位：米/秒^2；0 表示使用 AGV 默认值。 + double max_acceleration = 3; + // 最大角加速度，单位：弧度/秒^2；0 表示使用 AGV 默认值。 + double max_angular_acceleration = 4; + // 到达目标点的距离容差，单位：米；0 表示使用 AGV 默认值。 + double reach_distance = 5; + // 到达目标角度的角度容差，单位：弧度；0 表示使用 AGV 默认值。 + double reach_angle = 6; + // 速度比例，范围通常为 [0, 1]；1 表示不降速。 + double speed_ratio = 7; + // 是否异步执行；true 表示下发任务后立即返回。 + bool asynchronous = 8; +} + +// AGV 适配器扩展参数。用于传递厂商或控制器特有的参数。 +message AgvAdapterParams { + // 参数键值表。键和值都使用字符串，具体含义由 AGV 适配器解释。 + map values = 1; +} + +// AGV 当前运行状态快照。 +message AgvRuntimeState { + // 状态采样时间，Unix 时间戳，单位：秒。 + double timestamp = 1; + // 运行模式，取值对应服务端内部 AgvMode 枚举的整数值。 + int32 mode = 2; + // 是否已连接 AGV 控制器。 + bool connected = 3; + // 是否已定位成功。 + bool localized = 4; + // AGV 是否处于运动状态。 + bool moving = 5; + // AGV 是否处于故障状态。 + bool fault = 6; + // AGV 是否处于急停状态。 + bool emergency_stopped = 7; + // 当前地图坐标系下的二维位姿。 + AgvPose2d pose = 8; + // 当前车体速度。 + AgvVelocity velocity = 9; + // 当前电池状态。 + AgvBatteryState battery = 10; + // 当前加载地图名称；为空表示未知或控制器未返回。 + string current_map = 11; + // 当前或最近站点 id；为空表示未知或当前不在站点附近。 + string current_station = 12; + // 最近一次错误信息；为空表示无错误或未知。 + string last_error = 13; +} + +// 地图中的站点信息。 +message AgvStation { + // 站点唯一 id。 + string id = 1; + // 站点类型；具体枚举由地图或控制器定义。 + string type = 2; + // 站点在地图坐标系下的二维位姿。 + AgvPose2d pose = 3; + // 站点描述或备注。 + string description = 4; +} + +// 显式站点路径中的一段路径。 +message AgvPathSegment { + // 起点站点 id。 + string source_station = 1; + // 目标站点 id。 + string target_station = 2; +} + +// 地图维度请求。这里只描述上位机想要 2D、3D 还是二者都要， +// 不用于指定厂商文件格式；厂商格式必须在 AGV 适配器内部转换。 +enum AgvMapDimension { + // 未指定。服务端应选择最有用的默认地图，通常是当前加载地图。 + AGV_MAP_DIMENSION_UNSPECIFIED = 0; + // 只请求统一 2D 地图。 + AGV_MAP_2D = 1; + // 只请求统一 3D 地图。 + AGV_MAP_3D = 2; + // 在同一个流中请求统一 2D 和统一 3D 地图。 + AGV_MAP_2D_AND_3D = 3; +} + +// 地图流更新类型。上位机应根据该字段判断是全量、增量还是缓存重置。 +enum AgvMapUpdateType { + // 未指定。 + AGV_MAP_UPDATE_UNSPECIFIED = 0; + // 全量地图快照。首次请求或无法增量续传时应发送该类型。 + AGV_MAP_UPDATE_SNAPSHOT = 1; + // 全量快照之后的增量更新。仅当适配器能够可靠生成差异时发送。 + AGV_MAP_UPDATE_INCREMENTAL = 2; + // 上位机本地缓存已失效，应丢弃缓存并等待后续全量快照。 + AGV_MAP_UPDATE_RESET = 3; +} + +// 2D/3D 地图共用的语义对象类型。 +enum AgvMapObjectType { + // 未指定。 + AGV_MAP_OBJECT_UNSPECIFIED = 0; + // 导航站点或路径点。 + AGV_MAP_OBJECT_STATION = 1; + // 路径线、禁行线、引导线等线对象。 + AGV_MAP_OBJECT_LINE = 2; + // 多边形区域，例如禁行区、限速区、作业区。 + AGV_MAP_OBJECT_AREA = 3; + // 二维码、天码或其他标签地标。 + AGV_MAP_OBJECT_QR_TAG = 4; + // 反光板或反光柱地标。 + AGV_MAP_OBJECT_REFLECTOR = 5; + // 库位、货位或储位。 + AGV_MAP_OBJECT_BIN_LOCATION = 6; + // 门、电梯、充电桩等外部设备。 + AGV_MAP_OBJECT_EXTERNAL_DEVICE = 7; +} + +// 地图坐标系下的三维点。单位：米。 +message AgvMapPoint3D { + // X 坐标，单位：米。 + double x = 1; + // Y 坐标，单位：米。 + double y = 2; + // Z 坐标，单位：米；纯 2D 几何可置为 0。 + double z = 3; +} + +// 统一 2D 地图。栅格数据按行优先排列：index = y * width + x。 +message AgvUnifiedMap2D { + // 坐标系名称，例如 "map"。 + string frame_id = 1; + // 地图采样或更新时间，Unix 时间戳，单位：秒。 + double timestamp = 2; + // 栅格分辨率，单位：米/格。 + double resolution = 3; + // 栅格宽度，单位：格。 + uint32 width = 4; + // 栅格高度，单位：格。 + uint32 height = 5; + // 栅格 (0, 0) 在世界/地图坐标系下的位姿；x/y 单位：米，theta 单位：弧度。 + AgvPose2d origin = 6; + // 占据值：-1 表示未知，0 表示空闲，100 表示占据。 + repeated int32 data = 7; + // 地图中的统一语义对象，例如站点、线、区域、标签、反光板等。 + repeated AgvMapObject objects = 8; +} + +// 统一 3D 点样本。坐标单位：米。 +message AgvMapPointSample3D { + // X 坐标，单位：米。 + double x = 1; + // Y 坐标，单位：米。 + double y = 2; + // Z 坐标，单位：米。 + double z = 3; + // 激光强度；无强度信息时置为 0。 + float intensity = 4; + // 激光雷达线束/通道编号；无该信息时置为 0。 + uint32 ring = 5; + // 相对地图时间戳的时间偏移，单位：秒；无该信息时置为 0。 + double time_offset = 6; +} + +// 统一 3D 体素。体素索引基于 AgvUnifiedMap3D.voxel_resolution。 +message AgvMapVoxel3D { + // 体素 X 索引。 + int32 x = 1; + // 体素 Y 索引。 + int32 y = 2; + // 体素 Z 索引。 + int32 z = 3; + // 占据概率，范围：[0, 1]；未知时置为 -1。 + float probability = 4; +} + +// 统一 3D 平面特征。平面方程： +// normal.x * x + normal.y * y + normal.z * z + d = 0。 +message AgvMapPlane3D { + // 平面中心点，单位：米。 + AgvMapPoint3D center = 1; + // 平面单位法向量。 + AgvMapPoint3D normal = 2; + // 平面方程偏移量，单位：米。 + double d = 3; + // 平面特征近似半径，单位：米。 + double radius = 4; +} + +// 统一语义对象。几何点均使用地图坐标系，单位：米。 +message AgvMapObject { + // 对象稳定 id 或名称。 + string id = 1; + // 对象类型。 + AgvMapObjectType type = 2; + // 对象几何点。点对象使用 1 个点，线对象使用多个点，区域对象使用多边形顶点。 + repeated AgvMapPoint3D points = 3; + // 朝向角，单位：弧度；不适用时置为 0。 + double heading = 4; + // 额外归一化属性。为兼容不同 AGV，属性值统一使用字符串。 + map properties = 5; +} + +// 统一 3D 地图。虽然内部包含点、体素、平面和语义对象， +// 但对上位机来说它仍然是唯一的 3D 地图格式。 +message AgvUnifiedMap3D { + // 坐标系名称，例如 "map"。 + string frame_id = 1; + // 地图采样或更新时间，Unix 时间戳，单位：秒。 + double timestamp = 2; + // 体素分辨率，单位：米/体素；没有体素数据时置为 0。 + double voxel_resolution = 3; + // 统一点云点样本。 + repeated AgvMapPointSample3D points = 4; + // 统一占据体素。 + repeated AgvMapVoxel3D voxels = 5; + // 统一平面特征。 + repeated AgvMapPlane3D planes = 6; + // 统一 3D 语义对象，例如站点、区域、标签、地标等。 + repeated AgvMapObject objects = 7; +} + +// 地图流中的单条更新。AGV 适配器必须先把厂商地图转换成 map_2d 或 map_3d， +// 再通过该消息发送给上位机。 +message AgvUnifiedMapUpdate { + // 实际发送的地图 id 或名称。请求 map_name 为空时，AGV 可选择当前或最近地图。 + string map_id = 1; + // 建图或地图流会话 id。 + string session_id = 2; + // 会话内单调递增序号，从 0 或 1 开始均可，但同一会话内必须保持递增。 + uint64 sequence = 3; + // 用于断点续传或增量订阅的不透明令牌。 + string resume_token = 4; + // 本条更新的数据维度。 + AgvMapDimension dimension = 5; + // 本条更新是全量、增量还是缓存重置。 + AgvMapUpdateType update_type = 6; + // 坐标系名称，例如 "map"。 + string frame_id = 7; + // 本条更新产生时间，Unix 时间戳，单位：秒。 + double timestamp = 8; + // 是否为全量快照的第一条消息。 + bool snapshot_begin = 9; + // 是否为全量快照的最后一条消息。 + bool snapshot_end = 10; + // 大地图分片发送时的分片序号，从 0 开始。 + uint32 chunk_index = 11; + // 大地图分片总数；0 表示未知或连续流。 + uint32 chunk_count = 12; + + oneof payload { + // 统一 2D 地图或 2D 地图增量。 + AgvUnifiedMap2D map_2d = 20; + // 统一 3D 地图或 3D 地图增量。 + AgvUnifiedMap3D map_3d = 21; + } +} + +// 当前导航任务状态。 +message AgvNavigationStatus { + // 导航状态，取值对应服务端内部 AgvTaskState 枚举的整数值。 + int32 state = 1; + // 导航任务类型，取值对应服务端内部 AgvTaskType 枚举的整数值。 + int32 type = 2; + // 当前任务进度，范围：[0, 1]；未知时置为 0。 + double progress = 3; + // 状态描述或错误信息。 + string message = 4; +} diff --git a/protos/cmvr/msgs/canopen.proto b/protos/cmvr/msgs/canopen.proto index 23914e79..14ab3279 100644 --- a/protos/cmvr/msgs/canopen.proto +++ b/protos/cmvr/msgs/canopen.proto @@ -5,8 +5,8 @@ package cmvr.msgs; message SdoFrame { uint32 node_id = 1; // 节点ID CommandSpecifier cs = 2; // SDO命令字 - ObIndex index = 3; // 对象字典索引 - ObSubIndex sub_index = 4; // 子索引 + uint32 index = 3; // 对象字典索引 + uint32 sub_index = 4; // 子索引 uint32 data = 5; // 数据区 } @@ -110,81 +110,38 @@ enum NmtCommand { } -// 索引 -enum ObIndex { - INDEX_ZERO = 0; - USER_SAVE_PARA_2000 = 0x2000; // 下发命令 1 保存参数 - POSITION_OFFSET_2008 = 0x2008; // 位置偏移，子索引 0x00，用于设置零点、起始位置 - // Error Codes - ERROR_CODE_6007 = 0x6007; - ERROR_CODE_603F = 0x603F; +// CANopen communication object dictionary indexes. +// CiA402 drive-profile objects are defined in cia402.proto. +enum CanopenObjectIndex { + CANOPEN_OBJECT_INDEX_ZERO = 0; - // Control and Status - CONTROL_WORD_6040 = 0x6040; - STATUS_WORD_6041 = 0x6041; + CANOPEN_PRODUCER_HEARTBEAT_TIME_1017 = 0x1017; - // Operation Modes - OPERATION_MODE_6060 = 0x6060; - MODE_DISPLAY_6061 = 0x6061; - - // Actual Values - ACTUAL_POSITION_6064 = 0x6064; - ACTUAL_SPEED_606C = 0x606C; - ACTUAL_CURRENT_6078 = 0x6078; - - // Torque-related - TARGET_TORQUE_6071 = 0x6071; - MAX_TORQUE_6072 = 0x6072; - DEMAND_TORQUE_6074 = 0x6074; - - // Position-related - TARGET_POSITION_607A = 0x607A; - SOFTWARE_POSITION_LIMIT_607D = 0x607D; // Sub-indexes: 1, 2 - - // Speed-related - MAX_SPEED_607F = 0x607F; - PROFILE_SPEED_6081 = 0x6081; - PROFILE_ACCELERATION_6083 = 0x6083; - PROFILE_DECELERATION_6084 = 0x6084; - - // Same as DEMAND_TORQUE? Verify correctness. - // TORQUE_SLOPE_6074 = 0x6074; - - // PID Control - CURRENT_LOOP_PID_60F6 = 0x60F6; // Sub-indexes: 1, 2 - SPEED_LOOP_PID_60F9 = 0x60F9; // Sub-indexes: 1, 2 - POSITION_LOOP_PID_60FB = 0x60FB; // Sub-indexes: 1, 2, 3 - - // Target Speed - TARGET_SPEED_60FF = 0x60FF; - - QUICK_STOP_OPTION_605A = 0x605A; - QUICK_STOP_DECEL_6085 = 0x6085; - - // ------------------------- // PDO 通信参数对象（Communication Object） - RPDO1_COMM_1400 = 0x1400; - RPDO2_COMM_1401 = 0x1401; - RPDO3_COMM_1402 = 0x1402; - RPDO4_COMM_1403 = 0x1403; + CANOPEN_RPDO1_COMM_1400 = 0x1400; + CANOPEN_RPDO2_COMM_1401 = 0x1401; + CANOPEN_RPDO3_COMM_1402 = 0x1402; + CANOPEN_RPDO4_COMM_1403 = 0x1403; - TPDO1_COMM_1800 = 0x1800; - TPDO2_COMM_1801 = 0x1801; - TPDO3_COMM_1802 = 0x1802; - TPDO4_COMM_1803 = 0x1803; + CANOPEN_TPDO1_COMM_1800 = 0x1800; + CANOPEN_TPDO2_COMM_1801 = 0x1801; + CANOPEN_TPDO3_COMM_1802 = 0x1802; + CANOPEN_TPDO4_COMM_1803 = 0x1803; // PDO 映射对象（Mapping Object） - RPDO1_MAP_1600 = 0x1600; - RPDO2_MAP_1601 = 0x1601; - RPDO3_MAP_1602 = 0x1602; - RPDO4_MAP_1603 = 0x1603; + CANOPEN_RPDO1_MAP_1600 = 0x1600; + CANOPEN_RPDO2_MAP_1601 = 0x1601; + CANOPEN_RPDO3_MAP_1602 = 0x1602; + CANOPEN_RPDO4_MAP_1603 = 0x1603; - TPDO1_MAP_1A00 = 0x1A00; - TPDO2_MAP_1A01 = 0x1A01; - TPDO3_MAP_1A02 = 0x1A02; - TPDO4_MAP_1A03 = 0x1A03; + CANOPEN_TPDO1_MAP_1A00 = 0x1A00; + CANOPEN_TPDO2_MAP_1A01 = 0x1A01; + CANOPEN_TPDO3_MAP_1A02 = 0x1A02; + CANOPEN_TPDO4_MAP_1A03 = 0x1A03; - PRODUCER_HEARTBEAT_TIME = 0x1017; + // Ti5 vendor-specific objects used through CANopen SDO. + CANOPEN_USER_SAVE_PARA_2000 = 0x2000; + CANOPEN_POSITION_OFFSET_2008 = 0x2008; } // 子索引 @@ -198,5 +155,3 @@ enum ObSubIndex { SUB_INDEX_6 = 6; SUB_INDEX_7 = 7; } - - diff --git a/protos/cmvr/msgs/cia402.proto b/protos/cmvr/msgs/cia402.proto new file mode 100644 index 00000000..96e46c74 --- /dev/null +++ b/protos/cmvr/msgs/cia402.proto @@ -0,0 +1,64 @@ +syntax = "proto3"; + +package cmvr.msgs; + +// CiA402 object dictionary indexes shared by CANopen and EtherCAT CoE drives. +enum Cia402ObjectIndex { + CIA402_OBJECT_INDEX_ZERO = 0; + + CIA402_ERROR_CODE_603F = 0x603F; + + CIA402_CONTROL_WORD_6040 = 0x6040; + CIA402_STATUS_WORD_6041 = 0x6041; + + CIA402_QUICK_STOP_OPTION_605A = 0x605A; + CIA402_SHUTDOWN_OPTION_605B = 0x605B; + CIA402_DISABLE_OPERATION_OPTION_605C = 0x605C; + CIA402_HALT_OPTION_605D = 0x605D; + CIA402_FAULT_REACTION_OPTION_605E = 0x605E; + + CIA402_OPERATION_MODE_6060 = 0x6060; + CIA402_MODE_DISPLAY_6061 = 0x6061; + + CIA402_POSITION_DEMAND_VALUE_6062 = 0x6062; + CIA402_ACTUAL_POSITION_6064 = 0x6064; + CIA402_MAX_FOLLOWING_ERROR_6065 = 0x6065; + CIA402_POSITION_WINDOW_6067 = 0x6067; + CIA402_POSITION_WINDOW_TIME_6068 = 0x6068; + + CIA402_VELOCITY_DEMAND_VALUE_606B = 0x606B; + CIA402_ACTUAL_VELOCITY_606C = 0x606C; + CIA402_VELOCITY_WINDOW_606D = 0x606D; + CIA402_VELOCITY_WINDOW_TIME_606E = 0x606E; + CIA402_VELOCITY_THRESHOLD_606F = 0x606F; + CIA402_VELOCITY_THRESHOLD_TIME_6070 = 0x6070; + + CIA402_TARGET_TORQUE_6071 = 0x6071; + CIA402_MAX_TORQUE_6072 = 0x6072; + CIA402_TORQUE_DEMAND_VALUE_6074 = 0x6074; + CIA402_MOTOR_RATED_TORQUE_6076 = 0x6076; + CIA402_ACTUAL_TORQUE_6077 = 0x6077; + CIA402_ACTUAL_CURRENT_6078 = 0x6078; + CIA402_DC_LINK_VOLTAGE_6079 = 0x6079; + + CIA402_TARGET_POSITION_607A = 0x607A; + CIA402_HOME_OFFSET_607C = 0x607C; + CIA402_SOFTWARE_POSITION_LIMIT_607D = 0x607D; + CIA402_MAX_PROFILE_VELOCITY_607F = 0x607F; + + CIA402_PROFILE_VELOCITY_6081 = 0x6081; + CIA402_PROFILE_ACCELERATION_6083 = 0x6083; + CIA402_PROFILE_DECELERATION_6084 = 0x6084; + CIA402_QUICK_STOP_DECELERATION_6085 = 0x6085; + CIA402_TORQUE_SLOPE_6087 = 0x6087; + + CIA402_GEAR_RATIO_6091 = 0x6091; + CIA402_VELOCITY_OFFSET_60B1 = 0x60B1; + CIA402_TORQUE_OFFSET_60B2 = 0x60B2; + CIA402_INTERPOLATION_DATA_RECORD_60C1 = 0x60C1; + CIA402_INTERPOLATION_TIME_PERIOD_60C2 = 0x60C2; + CIA402_FOLLOWING_ERROR_ACTUAL_VALUE_60F4 = 0x60F4; + CIA402_TARGET_VELOCITY_60FF = 0x60FF; + + CIA402_SUPPORTED_DRIVE_MODES_6502 = 0x6502; +} diff --git a/protos/cmvr/msgs/motor.proto b/protos/cmvr/msgs/motor.proto index a0336dea..55cda865 100644 --- a/protos/cmvr/msgs/motor.proto +++ b/protos/cmvr/msgs/motor.proto @@ -114,5 +114,3 @@ message MotorStatus { uint32 status_word = 44; } - - diff --git a/protos/cmvr/msgs/robot_detail.proto b/protos/cmvr/msgs/robot_detail.proto index 19a6595b..375fdfee 100644 --- a/protos/cmvr/msgs/robot_detail.proto +++ b/protos/cmvr/msgs/robot_detail.proto @@ -1,6 +1,5 @@ syntax = "proto3"; -import "cmvr/msgs/canopen.proto"; import "cmvr/msgs/motor.proto"; package cmvr.msgs; diff --git a/protos/cmvr/quic_edge/v1/README.md b/protos/cmvr/quic_edge/v1/README.md new file mode 100644 index 00000000..4361811e --- /dev/null +++ b/protos/cmvr/quic_edge/v1/README.md @@ -0,0 +1,251 @@ +# QUIC edge v1 wire format + +## Scope and topology + +QUIC edge v1 carries two functions over one authenticated QUIC connection: + +- a reliable bidirectional control stream for node registration, heartbeat, + current local IP addresses, the advertised gRPC endpoint and media metadata; +- QUIC DATAGRAM payloads for lossy, real-time audio and video. + +Robot arm, AGV and other device-control APIs remain on the existing cmvr-es +gRPC server. The QUIC node descriptor advertises that server's current endpoint; +it does not move robot-control RPCs onto QUIC. + +```text +cmvr-es -- custom QUIC edge v1 --> Java receiver / edge gateway + | + +-- WebTransport/HTTP3 --> browser +``` + +A browser cannot connect to this custom QUIC ALPN directly. The gateway must +terminate v1, register the node, reassemble media fragments and expose +WebTransport (or another browser-supported media API). Browser sessions do not +participate in edge-node registration or heartbeat. + +## Build and host-safe defaults + +cmvr-es pins MsQuic v2.5.9 and builds it from source into the repository +dependency tree. It is not installed system-wide: + +```bash +script/build_msquic.sh \ + --arch x86 \ + --version 2.5.9 \ + --jobs 2 \ + --clean + +cmake -S . -B build-quic \ + -DCMVR_ARCH=x86 \ + -DBUILD_TESTING=ON \ + -DCMVR_INSTALL_DEFAULT_RUNTIME_ASSETS=OFF +cmake --build build-quic -j2 +ctest --test-dir build-quic --output-on-failure +cmake --install build-quic +``` + +The source helper clones the official `v2.5.9` tag, verifies its reviewed +source commit, and initializes its QuicTLS submodule +into `build/third_party/`, then installs the relocatable public result under +`dependency/x86/third_party/msquic/v2.5.9`. Its first run therefore requires +network access, but neither root privileges nor a system MsQuic package. The +result uses a statically linked QuicTLS backend; `libmsquic.so` is copied to +`output/lib` when cmvr-es is installed. + +The repository dependency is declared in `request.txt`, which adds its include +and library directories and installs `libmsquic.so*` with the other bundled +runtime libraries. MsQuic is therefore a required build dependency for +cmvr-es. The repository runtime configuration keeps `quic_edge` disabled, and +the existing inbound gRPC server remains enabled independently. + +`CMVR_INSTALL_DEFAULT_RUNTIME_ASSETS=OFF` preserves an existing +`output/bin/config` and `output/bin/model` while updating the binary and +runtime libraries. Leave its default value `ON` when a clean copy of the +repository defaults is desired. + +## Local real-QUIC verification + +The development-only server in `test/quic_gateway` listens through real +MsQuic/TLS/UDP and implements the v1 control and DATAGRAM receiver. With the +strict build above, CTest registers: + +- `cmvr_quic_msquic_e2e_test`, which connects the production edge service and + feeds synthetic H.264 video plus AAC audio through `MediaSourceHub`, then + verifies registration, heartbeat, descriptors, DATAGRAMs and frame + reassembly; +- `cmvr_es_quic_process_smoke_test`, which starts the actual `cmvr_es` + executable with a temporary no-device configuration and verifies + registration and heartbeat ACKs against the local gateway. + +Both tests use a short-lived loopback certificate generated under the build +tree. They do not modify `output/bin/config` or require physical devices. See +[`test/e2e/README.md`](../../../../test/e2e/README.md) and +[`test/quic_gateway/README.md`](../../../../test/quic_gateway/README.md) for +their exact scope and manual gateway options. + +To enable node presence without media: + +1. Configure the gateway address, ALPN `cmvr-quic-edge/1`, TLS trust, node ID, + advertised gRPC endpoint and heartbeat values in + `cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt`. + Certificate paths are relative to the cmvr-es configuration root, for + example `certs/quic_gateway_ca.pem`. +2. Leave `tracks` empty, set `quic_edge.enable: true`, and enable the + `quic_edge` TaskManager entry. + +To add a media uplink, also enable the camera/microphone in DeviceManager and +add a matching `tracks` entry. A zero-track configuration is valid: unavailable +media must not suppress node registration, IP reporting or heartbeat. +`maximum_frame_bytes` must fit in one atomically admitted DATAGRAM batch; reduce +it or increase `datagram_send_queue_depth` when changing the DATAGRAM size. + +## Reliable control stream + +The edge opens one bidirectional reliable stream after the QUIC handshake. Both +directions carry a sequence of length-prefixed protobuf messages: + +```text +uint32_be protobuf_length +protobuf EdgeControlEnvelope +``` + +The length excludes the four-byte prefix. Each receiver must reject a frame +larger than its configured control-frame limit. `protocol_version` is `1`. +`message_sequence` increases independently in each direction; gaps are allowed, +but duplicates and reordering are rejected. The edge resets its outbound +control sequence on every connection, so its first `NodeRegisterRequest` +currently has `message_sequence=0`; gateways must not assume the sequence starts +at 1. Heartbeat uses a separate sequence so its acknowledgement remains +explicit. + +The legal session order is: + +1. The edge sends `NodeRegisterRequest` as its first application message after + every QUIC connect or reconnect. It includes `node_id`, `boot_id`, software + version, the current IPv4/IPv6 interface snapshot and advertised gRPC endpoint. +2. The gateway replies with `NodeRegisterResponse`. Media and heartbeat must not + start until `accepted=true` and a non-empty `session_id` are received. +3. The edge sends `NodeHeartbeat` at the negotiated interval. The gateway returns + `NodeHeartbeatAck` with the same `session_id` and exact + `acknowledged_sequence`. A missing, rejected or mismatched ACK causes the edge + to reconnect and register again after its configured backoff. Every heartbeat + also carries a fresh `DeviceManagerSnapshot`; its `devices` list contains + only enabled devices, including enabled devices whose construction, + initialization or start failed. +4. If media tracks are available, the edge sends `MediaSessionOpen`, binding its + `session_epoch` to the accepted `session_id`, followed by track descriptors. + The edge calls reliable descriptor send before DATAGRAM send for a new + generation. QUIC does not guarantee arrival order across a reliable stream + and DATAGRAMs, so a gateway must boundedly buffer or drop media whose session + or generation metadata has not arrived yet. +5. The gateway may send `ProtocolError`; a fatal error closes the connection. + The current edge receive path otherwise accepts only `NodeRegisterResponse` + and `NodeHeartbeatAck`. Although `MediaSessionClose` is present in the v1 + schema, the current edge neither sends it nor accepts it inbound. Gateways + must not depend on that message until both sides implement and test it. + +The edge sends a fresh local-interface snapshot in every heartbeat, so an IP +change is reported without a second protocol. These addresses are edge claims. +`observed_source_ip` is authoritative for the public/NAT-facing address and must +be derived by the gateway from the authenticated QUIC peer, never copied from a +client field. + +The heartbeat's `device_manager.devices` contains only entries with +`enabled=true`. Disabled configuration entries remain available in the edge's +local DeviceManager snapshot but are not transmitted. An enabled entry that +fails creation, initialization or start remains visible with +`MANAGED_DEVICE_STATE_ERROR`, `has_error=true` and an error detail. Its +independent health value may still be `UNSPECIFIED` when no trustworthy device +probe exists. `kind` is the stable category used for machine decisions, while +`type_name` is a concrete implementation name when the object exists and +otherwise a category label; it is intended only for display and diagnostics. +Senders order entries by `device_id` to make +snapshots deterministic. `sampled_at_unix_ms` is the snapshot time, whereas +`status_updated_at_unix_ms` records when DeviceManager last changed that row's +lifecycle/error record. The enclosing snapshot time is the freshness timestamp +for the health observation. + +Lifecycle and health are deliberately separate. In particular, +`DEVICE_HEALTH_STATUS_UNSPECIFIED` means that no trustworthy health observation +was available; it is not equivalent to `DEVICE_HEALTH_STATUS_HEALTHY`. +Similarly, `has_error=false` only means that no error is currently confirmed +and must not be used to turn unknown health into healthy health. A health probe +failure must degrade that row to an unknown or fault result without suppressing +the rest of the heartbeat. + +The edge caps each diagnostic string at 512 bytes without splitting a UTF-8 +code point. Device identifiers, implementation names and manager metadata are +not silently truncated because doing so would change identity. The deployment +must therefore size `maximum_control_frame_bytes` for its enabled inventory; +the sender and receiver both reject an oversized control frame. Very large +inventories require a future explicit pagination/truncation extension rather +than silently dropping rows from this enabled-device snapshot. + +The locally configured `QuicEdgeConfig.heartbeat_interval_ms` controls the +reporting interval before registration. A gateway may negotiate a different +interval through `NodeRegisterResponse.heartbeat_interval_ms`: zero keeps the +locally configured value, while a non-zero value overrides it for the current +registered QUIC connection. The edge clamps the negotiated value to its +supported safety range and returns to the local value on reconnect until a new +registration response is accepted. + +`device_manager = 9` is an additive protobuf field in `NodeHeartbeat`, so this +extension remains QUIC edge protocol v1. Existing gateways ignore the unknown +field. Updated gateways must continue accepting older v1 heartbeats where +`device_manager` is absent and must not treat an absent snapshot as an empty, +healthy DeviceManager. Enum values may only be appended; existing numeric +meanings must never be renumbered or reused. + +DATAGRAM negotiation is required only when at least one media track is enabled. +The reliable registration and heartbeat path remains valid for a zero-track +node or while all media sources are unavailable. + +Media source registration, device start/keyframe callbacks and DATAGRAM +packetization run on a dedicated media worker. Reliable registration and +heartbeat therefore continue while a media source is slow or unavailable; +the reliable send path serializes heartbeat and media metadata so envelope +sequence order remains strict. + +`MediaSourceHub` is protocol-neutral and gives each adapter an independent, +single-consumer subscription cursor. Its source-start callback receives a +cancellation predicate and must check it around potentially blocking device +startup. QUIC reconnect/stop and gRPC client cancellation can therefore abandon +startup without blocking presence or process shutdown. Ring overflow keeps the +newest bounded set of immutable frames, advances a slow cursor to the oldest +retained frame, and reports the exact dropped count so protocol adapters can +mark a discontinuity and request a new video keyframe. + +## Media DATAGRAM wire format + +Every media DATAGRAM starts with this fixed 64-byte, network-byte-order header: + +| Offset | Size | Field | +|---:|---:|---| +| 0 | 4 | magic `CMQD` (`0x434d5144`) | +| 4 | 1 | protocol version (`1`) | +| 5 | 1 | media kind (`1` video, `2` audio) | +| 6 | 2 | flags | +| 8 | 2 | header size (`64`) | +| 10 | 2 | fragment index | +| 12 | 2 | fragment count | +| 14 | 2 | payload size | +| 16 | 4 | track ID | +| 20 | 4 | codec generation token | +| 24 | 8 | media session epoch | +| 32 | 8 | DATAGRAM packet sequence | +| 40 | 8 | media frame sequence | +| 48 | 8 | monotonic capture timestamp in microseconds | +| 56 | 4 | complete frame size | +| 60 | 4 | fragment byte offset in the complete frame | + +Flag bit 0 marks a video keyframe, bit 1 is reserved and must currently be +zero, and bit 2 marks a discontinuity. Codec initialization bytes are carried +reliably in `MediaTrackDescriptor`. QUIC already authenticates each DATAGRAM, +so the application header has no redundant checksum. + +Receivers must key reassembly by `(session_epoch, track_id, frame_sequence)`, +drop incomplete frames at their playback deadline, and reject fragments whose +offset plus payload exceeds `frame_size`. A new session epoch invalidates all +fragments retained from a previous connection. `frame_sequence` is generated +by the QUIC edge service and remains strictly increasing per track throughout +the media epoch, including after a device source restarts. diff --git a/protos/cmvr/quic_edge/v1/quic_edge.proto b/protos/cmvr/quic_edge/v1/quic_edge.proto new file mode 100644 index 00000000..3dca447c --- /dev/null +++ b/protos/cmvr/quic_edge/v1/quic_edge.proto @@ -0,0 +1,230 @@ +syntax = "proto3"; + +package cmvr.quic_edge.v1; + +// Every application message on the edge-opened reliable bidirectional stream +// is carried by this envelope. message_sequence is strictly increasing per +// sender (gaps are allowed) and is independent from the heartbeat sequence used +// for liveness acknowledgement. +message EdgeControlEnvelope { + uint32 protocol_version = 1; + uint64 message_sequence = 2; + + oneof payload { + NodeRegisterRequest node_register_request = 10; + NodeRegisterResponse node_register_response = 11; + NodeHeartbeat node_heartbeat = 12; + NodeHeartbeatAck node_heartbeat_ack = 13; + + MediaSessionOpen media_session_open = 20; + MediaTrackDescriptor media_track_descriptor = 21; + MediaSessionClose media_session_close = 22; + + ProtocolError protocol_error = 30; + } +} + +message NetworkInterfaceAddress { + enum AddressFamily { + ADDRESS_FAMILY_UNSPECIFIED = 0; + ADDRESS_FAMILY_IPV4 = 1; + ADDRESS_FAMILY_IPV6 = 2; + } + + string interface_name = 1; + string ip_address = 2; + AddressFamily family = 3; + bool loopback = 4; +} + +// The existing cmvr-es gRPC server remains the robot-control endpoint. The +// edge advertises its current reachable address through the QUIC control plane. +message GrpcEndpoint { + string host = 1; + uint32 port = 2; + bool tls = 3; +} + +// Stable protocol-level categories for devices managed by cmvr-es. These +// values intentionally do not reuse the configuration or gRPC API enums: +// their zero values and supported categories have different semantics. +enum DeviceKind { + DEVICE_KIND_UNSPECIFIED = 0; + DEVICE_KIND_AGV = 1; + DEVICE_KIND_ARM = 2; + DEVICE_KIND_BATTERY = 3; + DEVICE_KIND_BIO_HEAD = 4; + DEVICE_KIND_CAMERA = 5; + DEVICE_KIND_CAN_BUS = 6; + DEVICE_KIND_DEX_HAND = 7; + DEVICE_KIND_GRIPPER = 8; + DEVICE_KIND_MICROPHONE = 9; + DEVICE_KIND_MOTOR = 10; + DEVICE_KIND_MOTOR_SYSTEM = 11; + DEVICE_KIND_ROBOT = 12; + DEVICE_KIND_SPEAKER = 13; +} + +// DeviceManager's view of a configured entry. REGISTERED means that the +// manager owns a device record but has no more specific lifecycle signal. +enum ManagedDeviceState { + MANAGED_DEVICE_STATE_UNSPECIFIED = 0; + MANAGED_DEVICE_STATE_DISABLED = 1; + MANAGED_DEVICE_STATE_INITIALIZING = 2; + MANAGED_DEVICE_STATE_REGISTERED = 3; + MANAGED_DEVICE_STATE_READY = 4; + MANAGED_DEVICE_STATE_RUNNING = 5; + MANAGED_DEVICE_STATE_STOPPED = 6; + MANAGED_DEVICE_STATE_ERROR = 7; +} + +// Health is independent of lifecycle. UNSPECIFIED means that no trustworthy +// health observation is available and must never be interpreted as healthy. +enum DeviceHealthStatus { + DEVICE_HEALTH_STATUS_UNSPECIFIED = 0; + DEVICE_HEALTH_STATUS_HEALTHY = 1; + DEVICE_HEALTH_STATUS_DEGRADED = 2; + DEVICE_HEALTH_STATUS_FAULT = 3; +} + +message ManagedDeviceStatus { + string device_id = 1; + DeviceKind kind = 2; + + // Concrete implementation name when a device object exists; otherwise a + // category label. It is for display/diagnostics only. Consumers use kind, + // rather than this free-form string, for machine decisions. + string type_name = 3; + + bool enabled = 4; + ManagedDeviceState manager_state = 5; + DeviceHealthStatus health = 6; + + // false means that no error is currently confirmed. It does not turn + // DEVICE_HEALTH_STATUS_UNSPECIFIED into a healthy observation. + bool has_error = 7; + string error_message = 8; + + // Time at which DeviceManager last changed the lifecycle/error record. + // DeviceManagerSnapshot.sampled_at_unix_ms is the freshness timestamp for + // the health observation carried by this heartbeat. + uint64 status_updated_at_unix_ms = 9; +} + +message DeviceManagerSnapshot { + string manager_name = 1; + string manager_version = 2; + string manager_description = 3; + + // Current cmvr-es senders include only enabled devices. The enabled field in + // each row and DISABLED enum value remain part of v1 for wire compatibility. + repeated ManagedDeviceStatus devices = 4; + uint64 sampled_at_unix_ms = 5; +} + +message NodeDescriptor { + string node_id = 1; + string boot_id = 2; + string software_version = 3; + repeated NetworkInterfaceAddress local_interfaces = 4; + GrpcEndpoint grpc_endpoint = 5; +} + +// This must be the first application message sent after each QUIC connection +// is established. A reconnect always creates a new registration session. +message NodeRegisterRequest { + NodeDescriptor node = 1; + uint64 sent_at_unix_ms = 2; +} + +message NodeRegisterResponse { + bool accepted = 1; + string session_id = 2; + string message = 3; + + // Zero tells the edge to keep its locally configured interval. + uint32 heartbeat_interval_ms = 4; + + // Derived by the receiver from the authenticated QUIC peer address. It is + // not copied from a client-supplied local interface. + string observed_source_ip = 5; +} + +// A heartbeat carries a fresh network snapshot so address changes are reported +// without opening a second protocol or connection. +message NodeHeartbeat { + string node_id = 1; + string boot_id = 2; + string session_id = 3; + uint64 sequence = 4; + uint64 sent_at_unix_ms = 5; + string software_version = 6; + repeated NetworkInterfaceAddress local_interfaces = 7; + GrpcEndpoint grpc_endpoint = 8; + DeviceManagerSnapshot device_manager = 9; +} + +message NodeHeartbeatAck { + bool accepted = 1; + uint64 acknowledged_sequence = 2; + string message = 3; + uint64 server_time_unix_ms = 4; + string observed_source_ip = 5; + string session_id = 6; +} + +message MediaSessionOpen { + string node_id = 1; + uint64 session_epoch = 2; + + // Binds the media epoch to the accepted node registration on this QUIC + // connection. Media must not start before this session is assigned. + string session_id = 3; +} + +enum MediaKind { + MEDIA_KIND_UNSPECIFIED = 0; + MEDIA_KIND_VIDEO = 1; + MEDIA_KIND_AUDIO = 2; +} + +message MediaTrackDescriptor { + uint32 track_id = 1; + MediaKind kind = 2; + string device_id = 3; + string codec = 4; + uint64 codec_generation = 5; + + // The exact MediaSourceHub track and the 32-bit token repeated in every + // DATAGRAM header. The full generation remains on the reliable stream. + string source_track_id = 6; + uint32 codec_generation_token = 7; + string payload_format = 8; + + // Video fields. They are zero for audio tracks. + uint32 width = 10; + uint32 height = 11; + uint32 frames_per_second = 12; + + // Audio fields. They are zero for video tracks. + uint32 sample_rate = 20; + uint32 channels = 21; + + // Decoder initialization bytes, for example AVCC/HVCC or AudioSpecificConfig. + // Existing cmvr-es sources may leave this empty when configuration NAL units + // are carried in-band. + bytes codec_config = 30; +} + +message MediaSessionClose { + string reason = 1; + string session_id = 2; + uint64 session_epoch = 3; +} + +message ProtocolError { + uint32 code = 1; + string message = 2; + uint64 related_message_sequence = 3; + bool fatal = 4; +} diff --git a/protos/rbk/protocol/src1100_map3d.proto b/protos/rbk/protocol/src1100_map3d.proto new file mode 100644 index 00000000..b33474f1 --- /dev/null +++ b/protos/rbk/protocol/src1100_map3d.proto @@ -0,0 +1,124 @@ +syntax = "proto3"; + +package rbk.protocol; + +// 仙工 SRC1100 3D 地图文件 0.3dsmap 的最小解析结构。 +// 这里只保留转换统一地图所需字段，未声明字段由 protobuf 作为未知字段跳过。 + +// 地图坐标系下的三维位置，单位：米。 +message Message_MapPos { + // X 坐标，单位：米。 + double x = 1; + // Y 坐标，单位：米。 + double y = 2; + // Z 坐标，单位：米。 + double z = 3; +} + +// 仙工地图头信息。 +message Message_MapHeader { + // 地图类型，例如 2D-Map 或 3D-Map。 + string map_type = 1; + // 地图名称，通常对应地图文件名。 + string map_name = 2; + // 地图最小边界点，单位：米。 + Message_MapPos min_pos = 3; + // 地图最大边界点，单位：米。 + Message_MapPos max_pos = 4; + // 地图分辨率，单位：米。 + double resolution = 5; + // 地图格式版本号。 + string version = 8; +} + +// 三维浮点向量。 +message Vec3f { + // X 分量。 + float x = 1; + // Y 分量。 + float y = 2; + // Z 分量。 + float z = 3; +} + +// 三维整数向量。 +message Vec3i { + // X 分量。 + int32 x = 1; + // Y 分量。 + int32 y = 2; + // Z 分量。 + int32 z = 3; +} + +// 仙工 3D 特征地图参数。 +message FeatureMapParams { + // 激光测距标准差，单位：米。 + float ranging_sigma = 1; + // 激光测角标准差，单位：度。 + float angle_sigma = 2; + // 最大体素边长，单位：米。 + float max_voxel_size = 3; + // 八叉树最大层数。 + uint32 max_layer = 4; + // 平面协方差停止更新的点数阈值。 + uint32 cov_fixed_pts_num = 5; + // 平面停止更新的点数阈值。 + uint32 plane_fixed_pts_num = 6; + // 有效平面协方差最小特征值阈值。 + float plane_min_eigen_value = 7; + // 每层评估平面所需的最少点数。 + repeated int32 each_layer_least_pts_num = 8; +} + +// 仙工 3D 平面特征。 +message FeatureMapPlane { + // 平面中心点，单位：米。 + Vec3f center = 1; + // 平面法向量。 + Vec3f normal = 2; + // 平面方程 Ax + By + Cz + D = 0 中的 D。 + float d = 3; + // 平面特征近似半径，单位：米。 + float radius = 4; + // 平面协方差矩阵，按 6x6 展平。 + repeated float plane_cov = 5; +} + +// 仙工特征地图中的八叉树节点。 +message OctoTree { + // 对应的平面 ID。 + uint32 plane_id = 1; + // 子节点序列，-1 表示当前层无其他子节点。 + repeated int32 child_id_list = 2; +} + +// 同一外层体素位置下的八叉树节点集合。 +message OctoTrees { + // 八叉树节点列表。 + repeated OctoTree octo_tree = 1; +} + +// 仙工 3D 特征地图。 +message FeatureMap3D { + // 特征地图参数。 + FeatureMapParams params = 1; + // 平面特征列表。 + repeated FeatureMapPlane planes = 2; + // 每个外层体素对应的八叉树节点集合。 + repeated OctoTrees octo_trees = 3; + // 最外层八叉树体素坐标。 + repeated Vec3i voxel_locs = 4; +} + +// 0.3dsmap 顶层消息。 +message Message_Map3D { + // 地图目录或地图包内部目录名。 + string map_directory = 1; + // 地图头信息。 + Message_MapHeader header = 2; + // 普通 3D 点云点。 + repeated Message_MapPos normal_pos3d_list = 3; + // 3D 特征地图。 + FeatureMap3D feature_map_3d = 4; +} diff --git a/request.txt b/request.txt index 752889e5..0ec7a5eb 100644 --- a/request.txt +++ b/request.txt @@ -1,8 +1,10 @@ +third_party/aubo_sdk/v0.27.1 third_party/coal/v3.0.2 third_party/console_bridge/v1.0.1 third_party/eigenpy/v3.0.0 third_party/Eigen3/v3.4.0 third_party/example-robot-data/v4.3.0 +third_party/ethercat/v1.7.0 third_party/fcl/v0.7.0 third_party/x264/v165 third_party/x265/v215 @@ -31,6 +33,7 @@ third_party/urdfdom/v5.0.3 third_party/urdfdom_headers/v2.0.1 third_party/opencv/4.13.0 third_party/modbus/3.1.11 +third_party/msquic/v2.5.9 third_party/visp/3.7.0 third_party/mainif/0.0.5 third_party/matplotplusplus/1.2.0 diff --git a/script/build_msquic.sh b/script/build_msquic.sh new file mode 100644 index 00000000..fe58f16d --- /dev/null +++ b/script/build_msquic.sh @@ -0,0 +1,411 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'EOF' +Usage: script/build_msquic.sh [options] + +Build the pinned MsQuic source into: + dependency//third_party/msquic/v + +Options: + --arch x86|arm Dependency architecture (default: native host) + --version VERSION Supported pinned version without leading v (default: 2.5.9) + --jobs N Parallel build jobs (default: nproc) + --clean Recreate the MsQuic build and staging directories + -h, --help Show this help + +Environment: + CMVR_CMAKE Absolute CMake executable override + CMVR_MSQUIC_TOOLCHAIN_FILE CMake toolchain file for cross-compilation + CC, CXX Native compiler overrides + +The first run needs network access to clone the official MsQuic tag and its +QuicTLS submodule. No sudo or system MsQuic installation is used. +EOF +} + +script_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd -P)" +repo_root="$(cd -- "${script_dir}/.." && pwd -P)" +version="2.5.9" +arch="" +jobs="" +clean_build=false + +while (($# > 0)); do + case "$1" in + --arch) + [[ $# -ge 2 ]] || { echo "missing value for --arch" >&2; exit 2; } + arch="$2" + shift 2 + ;; + --version) + [[ $# -ge 2 ]] || { echo "missing value for --version" >&2; exit 2; } + version="${2#v}" + shift 2 + ;; + --jobs) + [[ $# -ge 2 ]] || { echo "missing value for --jobs" >&2; exit 2; } + jobs="$2" + shift 2 + ;; + --clean) + clean_build=true + shift + ;; + -h|--help) + usage + exit 0 + ;; + *) + echo "unknown option: $1" >&2 + usage >&2 + exit 2 + ;; + esac +done + +host_machine="$(uname -m)" +case "${host_machine}" in + x86_64|amd64) + native_arch="x86" + ;; + aarch64|arm64|armv8*) + native_arch="arm" + ;; + *) + echo "unsupported host architecture: ${host_machine}" >&2 + exit 2 + ;; +esac + +arch="${arch:-${native_arch}}" +if [[ "${arch}" != "x86" && "${arch}" != "arm" ]]; then + echo "--arch must be x86 or arm" >&2 + exit 2 +fi +if [[ ! "${version}" =~ ^[0-9]+\.[0-9]+\.[0-9]+$ ]]; then + echo "--version must use the form MAJOR.MINOR.PATCH" >&2 + exit 2 +fi +case "${version}" in + 2.5.9) + expected_source_commit="87b53085d76bd7920d490a6f226c9999b6614d14" + ;; + *) + echo "unsupported MsQuic version: ${version}" >&2 + echo "add its reviewed tag and commit to script/build_msquic.sh first" >&2 + exit 2 + ;; +esac + +if [[ -z "${jobs}" ]]; then + jobs="$(nproc 2>/dev/null || getconf _NPROCESSORS_ONLN || echo 2)" +fi +if [[ ! "${jobs}" =~ ^[1-9][0-9]*$ ]]; then + echo "--jobs must be a positive integer" >&2 + exit 2 +fi + +toolchain_args=() +if [[ "${arch}" != "${native_arch}" ]]; then + if [[ -z "${CMVR_MSQUIC_TOOLCHAIN_FILE:-}" ]]; then + echo "cross-building ${arch} on ${host_machine} requires" >&2 + echo "CMVR_MSQUIC_TOOLCHAIN_FILE=/absolute/path/to/toolchain.cmake" >&2 + exit 2 + fi + if [[ ! -f "${CMVR_MSQUIC_TOOLCHAIN_FILE}" ]]; then + echo "toolchain file does not exist: ${CMVR_MSQUIC_TOOLCHAIN_FILE}" >&2 + exit 2 + fi + toolchain_file="$(realpath "${CMVR_MSQUIC_TOOLCHAIN_FILE}")" + toolchain_args+=("-DCMAKE_TOOLCHAIN_FILE=${toolchain_file}") +fi + +tag="v${version}" +source_dir="${repo_root}/build/third_party/msquic-src/${tag}" +build_dir="${repo_root}/build/third_party/msquic-build/${arch}-${tag}" +stage_dir="${repo_root}/build/third_party/msquic-stage/${arch}-${tag}" +stage_prefix="${stage_dir}/prefix" +install_root="${repo_root}/dependency/${arch}/third_party/msquic/${tag}" +backup_root="${install_root}.previous" + +for guarded_path in \ + "${source_dir}" "${build_dir}" "${stage_dir}" \ + "${install_root}" "${backup_root}"; do + case "${guarded_path}" in + "${repo_root}"/build/third_party/*|\ + "${repo_root}"/dependency/"${arch}"/third_party/msquic/"${tag}"|\ + "${repo_root}"/dependency/"${arch}"/third_party/msquic/"${tag}".previous) + ;; + *) + echo "refusing unsafe path: ${guarded_path}" >&2 + exit 2 + ;; + esac +done + +if [[ -n "${CMVR_CMAKE:-}" ]]; then + cmake_bin="$(realpath "${CMVR_CMAKE}")" +else + bundled_cmake="${repo_root}/dependency/${arch}/third_party/cmake/v3.30.3/cmake-3.30.3-linux-$( + [[ "${arch}" == "x86" ]] && echo x86_64 || echo aarch64 + )/bin/cmake" + if [[ "${arch}" == "${native_arch}" && -x "${bundled_cmake}" ]]; then + cmake_bin="${bundled_cmake}" + else + cmake_bin="$(command -v cmake)" + fi +fi +[[ -x "${cmake_bin}" ]] || { + echo "CMake executable is unavailable: ${cmake_bin}" >&2 + exit 2 +} + +git_bin="/usr/bin/git" +[[ -x "${git_bin}" ]] || git_bin="$(command -v git)" +[[ -x "${git_bin}" ]] || { echo "git is required" >&2; exit 2; } + +clean_path="$(dirname "${cmake_bin}"):/usr/local/bin:/usr/bin:/bin" +export PATH="${clean_path}" +unset CONDA_PREFIX CONDA_DEFAULT_ENV CMAKE_PREFIX_PATH PKG_CONFIG_PATH \ + OPENSSL_ROOT_DIR OPENSSL_DIR LD_LIBRARY_PATH LIBRARY_PATH CPATH \ + C_INCLUDE_PATH CPLUS_INCLUDE_PATH CFLAGS CXXFLAGS CPPFLAGS LDFLAGS \ + PERL5LIB PERL5OPT + +if [[ "${arch}" == "${native_arch}" ]]; then + export CC="${CC:-/usr/bin/cc}" + export CXX="${CXX:-/usr/bin/c++}" +fi + +mkdir -p "$(dirname "${source_dir}")" "$(dirname "${install_root}")" +if [[ ! -d "${source_dir}/.git" ]]; then + "${git_bin}" clone \ + --branch "${tag}" \ + --depth 1 \ + https://github.com/microsoft/msquic.git \ + "${source_dir}" +fi + +source_commit="$("${git_bin}" -C "${source_dir}" rev-parse HEAD)" +tag_commit="$("${git_bin}" -C "${source_dir}" rev-list -n 1 "${tag}")" +if [[ "${source_commit}" != "${tag_commit}" ]]; then + echo "${source_dir} is not checked out at ${tag}" >&2 + echo "remove that cache directory and rerun the script" >&2 + exit 2 +fi +if [[ "${source_commit}" != "${expected_source_commit}" ]]; then + echo "${tag} resolved to an unexpected source commit" >&2 + echo "expected: ${expected_source_commit}" >&2 + echo "actual: ${source_commit}" >&2 + exit 2 +fi + +unexpected_initialized_submodules="$( + "${git_bin}" -C "${source_dir}" submodule status | + awk '$2 != "submodules/quictls" && substr($1, 1, 1) != "-" { + print $2 " (" $1 ")" + }' +)" +if [[ -n "${unexpected_initialized_submodules}" ]]; then + echo "MsQuic source cache contains initialized non-QuicTLS submodules:" >&2 + echo "${unexpected_initialized_submodules}" >&2 + echo "deinitialize those submodules or use a clean source cache before building" >&2 + exit 2 +fi + +"${git_bin}" -C "${source_dir}" submodule sync -- submodules/quictls +"${git_bin}" -C "${source_dir}" submodule update \ + --init --depth 1 -- submodules/quictls + +source_changes="$("${git_bin}" -C "${source_dir}" status \ + --porcelain --untracked-files=all --ignore-submodules=all)" +if [[ -n "${source_changes}" ]]; then + echo "MsQuic source cache contains local changes:" >&2 + echo "${source_changes}" >&2 + echo "use a clean source cache before building" >&2 + exit 2 +fi +quictls_dir="${source_dir}/submodules/quictls" +expected_quictls_commit="$("${git_bin}" -C "${source_dir}" \ + rev-parse HEAD:submodules/quictls)" +actual_quictls_commit="$("${git_bin}" -C "${quictls_dir}" rev-parse HEAD)" +quictls_changes="$("${git_bin}" -C "${quictls_dir}" status \ + --porcelain --untracked-files=all)" +if [[ "${actual_quictls_commit}" != "${expected_quictls_commit}" || + -n "${quictls_changes}" ]]; then + echo "QuicTLS source cache is not at the clean pinned commit" >&2 + echo "expected: ${expected_quictls_commit}" >&2 + echo "actual: ${actual_quictls_commit}" >&2 + [[ -z "${quictls_changes}" ]] || echo "${quictls_changes}" >&2 + exit 2 +fi + +if [[ "${clean_build}" == true ]]; then + "${cmake_bin}" -E remove_directory "${build_dir}" +fi +"${cmake_bin}" -E remove_directory "${stage_dir}" +"${cmake_bin}" -E make_directory "${build_dir}" "${stage_prefix}" + +toolchain_fingerprint="native" +if [[ ${#toolchain_args[@]} -ne 0 ]]; then + toolchain_fingerprint="$( + sha256sum "${toolchain_file}" | awk '{print $1}' + )" +fi +build_recipe_version="5" +build_fingerprint="$( + printf '%s' \ + "${source_commit}|${build_recipe_version}|${arch}|" \ + "${CC:-toolchain}|${CXX:-toolchain}|" \ + "${toolchain_fingerprint}|${cmake_bin}" +)" +fingerprint_file="${build_dir}/cmvr-msquic-build.fingerprint" +if [[ -f "${build_dir}/CMakeCache.txt" ]]; then + if [[ ! -f "${fingerprint_file}" ]]; then + echo "existing MsQuic build cache predates compiler fingerprinting" >&2 + echo "rerun with --clean" >&2 + exit 2 + fi + existing_fingerprint="$(<"${fingerprint_file}")" + if [[ "${existing_fingerprint}" != "${build_fingerprint}" ]]; then + echo "MsQuic compiler/toolchain fingerprint changed" >&2 + echo "rerun with --clean" >&2 + exit 2 + fi +fi +printf '%s\n' "${build_fingerprint}" >"${fingerprint_file}" + +prefix_map_flags="\ +-ffile-prefix-map=${repo_root}=. -fmacro-prefix-map=${repo_root}=." +"${cmake_bin}" \ + -S "${source_dir}" \ + -B "${build_dir}" \ + -G "Unix Makefiles" \ + -DCMAKE_BUILD_TYPE=Release \ + "-DCMAKE_INSTALL_PREFIX=${stage_prefix}" \ + "-DCMAKE_MODULE_PATH=${repo_root}/cmake/msquic" \ + "-DCMVR_MSQUIC_PROCESSOR_COUNT=${jobs}" \ + "-DCMAKE_C_FLAGS=${prefix_map_flags}" \ + "-DCMAKE_CXX_FLAGS=${prefix_map_flags}" \ + -DQUIC_BUILD_SHARED=ON \ + -DQUIC_BUILD_TEST=OFF \ + -DQUIC_BUILD_TOOLS=OFF \ + -DQUIC_BUILD_PERF=OFF \ + -DQUIC_ENABLE_LOGGING=OFF \ + -DQUIC_TLS_LIB=quictls \ + -DQUIC_USE_SYSTEM_LIBCRYPTO=OFF \ + -DNUMA:STRING=FALSE \ + "${toolchain_args[@]}" + +# QuicTLS derives MODULESDIR from its temporary --prefix and compiles that +# absolute path into libcrypto. Dynamic providers are disabled above +# (no-shared, no-legacy and no-fips), so keep the unused fallback path stable +# instead of leaking the build workspace into the shipped MsQuic runtime. +openssl_makefile_target="_deps/opensslquic-build/submodules/quictls/Makefile" +openssl_build_rules="_deps/opensslquic-build/CMakeFiles/OpenSSL_Target.dir/build.make" +make_bin="$(command -v make)" +[[ -x "${make_bin}" ]] || { + echo "GNU Make is required to configure the bundled QuicTLS source" >&2 + exit 2 +} +"${make_bin}" -C "${build_dir}" -f "${openssl_build_rules}" \ + "${openssl_makefile_target}" +openssl_makefile="${build_dir}/${openssl_makefile_target}" +test -f "${openssl_makefile}" +if ! grep -Fx 'MODULESDIR=/usr/lib/ssl/ossl-modules' \ + "${openssl_makefile}" >/dev/null; then + sed -i -E \ + 's|^MODULESDIR=.*$|MODULESDIR=/usr/lib/ssl/ossl-modules|' \ + "${openssl_makefile}" +fi +grep -Fx 'MODULESDIR=/usr/lib/ssl/ossl-modules' \ + "${openssl_makefile}" >/dev/null + +"${cmake_bin}" --build "${build_dir}" --parallel "${jobs}" +"${cmake_bin}" --install "${build_dir}" + +# Keep only the public Linux API and shared runtime that cmvr-es consumes. +find "${stage_prefix}/include" -maxdepth 1 -type f \ + ! -name msquic.h \ + ! -name msquic_posix.h \ + ! -name quic_sal_stub.h \ + -delete +"${cmake_bin}" -E rm -f "${stage_prefix}/lib/libmsquic_platform.a" +"${cmake_bin}" -E remove_directory "${stage_prefix}/share" +"${cmake_bin}" -E make_directory "${stage_prefix}/share/licenses/msquic" +"${cmake_bin}" -E copy "${source_dir}/LICENSE" \ + "${stage_prefix}/share/licenses/msquic/LICENSE" +"${cmake_bin}" -E copy "${source_dir}/THIRD-PARTY-NOTICES" \ + "${stage_prefix}/share/licenses/msquic/THIRD-PARTY-NOTICES" + +cat >"${stage_prefix}/BUILD-INFO.txt" </dev/null 2>&1; then + if readelf -d "${stage_prefix}/lib/libmsquic.so.${version}" | + grep -E 'NEEDED.*lib(ssl|crypto|numa)' >/dev/null; then + echo "MsQuic unexpectedly depends on system TLS or NUMA libraries" >&2 + exit 1 + fi +fi +if grep -R -F --exclude='libmsquic.so*' \ + "${repo_root}" "${stage_prefix}" >/dev/null 2>&1; then + echo "staged MsQuic metadata contains a non-relocatable workspace path" >&2 + exit 1 +fi +if command -v strings >/dev/null 2>&1 && + strings "${stage_prefix}/lib/libmsquic.so.${version}" | + grep -F "${repo_root}" >/dev/null; then + echo "staged MsQuic runtime contains a non-relocatable workspace path" >&2 + exit 1 +fi + +swap_in_progress=false +restore_install_on_exit() { + if [[ "${swap_in_progress}" != true ]]; then + return + fi + if [[ -e "${install_root}" ]]; then + "${cmake_bin}" -E remove_directory "${backup_root}" || true + elif [[ -e "${backup_root}" ]]; then + mv "${backup_root}" "${install_root}" || true + fi +} +trap restore_install_on_exit EXIT + +if [[ -e "${backup_root}" ]]; then + if [[ ! -e "${install_root}" ]]; then + mv "${backup_root}" "${install_root}" + else + echo "stale MsQuic backup requires manual inspection:" >&2 + echo " ${backup_root}" >&2 + exit 2 + fi +fi +if [[ -e "${install_root}" ]]; then + swap_in_progress=true + mv "${install_root}" "${backup_root}" +fi +mv "${stage_prefix}" "${install_root}" +swap_in_progress=false +"${cmake_bin}" -E remove_directory "${backup_root}" +trap - EXIT + +echo "MsQuic ${tag} installed to:" +echo " ${install_root}" +echo "MsQuic is enabled through its request.txt dependency entry." diff --git a/script/ethercat/start_ethercat.sh b/script/ethercat/start_ethercat.sh new file mode 100644 index 00000000..c1b9deeb --- /dev/null +++ b/script/ethercat/start_ethercat.sh @@ -0,0 +1,136 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'USAGE' +Usage: + sudo script/ethercat/start_ethercat.sh [iface] [ethercat_dev] [start_wait_sec] + Any pure numeric argument is treated as start_wait_sec. + +Example: + sudo script/ethercat/start_ethercat.sh eno1 + sudo script/ethercat/start_ethercat.sh 10 + sudo script/ethercat/start_ethercat.sh eno1 10 + sudo script/ethercat/start_ethercat.sh eno1 /dev/EtherCAT1 + sudo script/ethercat/start_ethercat.sh eno1 /dev/EtherCAT0 10 + sudo script/ethercat/start_ethercat.sh eno1 10 /dev/EtherCAT0 + +Environment: + DEVICE_MODULES=generic IgH device module list. + IGH_ROOT=... Override bundled IgH install path. + ETHERCAT_CONF=... Override ethercatctl config path. + ETHERCAT_DEV=/dev/EtherCAT0 Override EtherCAT character device node. + ETHERCAT_GROUP=plugdev Group allowed to access the character device. + START_WAIT_SEC=5 Seconds to wait for link/slave discovery. +USAGE +} + +if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then + usage + exit 0 +fi + +if [[ "$(id -u)" -ne 0 ]]; then + echo "error: please run with sudo." >&2 + exit 1 +fi + +IFACE="${IFACE:-eno1}" +ETHERCAT_DEV="${ETHERCAT_DEV:-/dev/EtherCAT0}" +ETHERCAT_GROUP="${ETHERCAT_GROUP:-plugdev}" +START_WAIT_SEC="${START_WAIT_SEC:-5}" + +NON_NUMERIC_ARG_COUNT=0 +for arg in "$@"; do + if [[ "${arg}" =~ ^[0-9]+$ ]]; then + START_WAIT_SEC="${arg}" + continue + fi + + case "${NON_NUMERIC_ARG_COUNT}" in + 0) + IFACE="${arg}" + ;; + 1) + ETHERCAT_DEV="${arg}" + ;; + *) + echo "error: unexpected argument: ${arg}" >&2 + usage >&2 + exit 1 + ;; + esac + NON_NUMERIC_ARG_COUNT=$((NON_NUMERIC_ARG_COUNT + 1)) +done +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" +IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}" +DEVICE_MODULES="${DEVICE_MODULES:-generic}" +ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}" +ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl" +ETHERCAT="${IGH_ROOT}/bin/ethercat" + +if [[ ! -d "/sys/class/net/${IFACE}" ]]; then + echo "error: network interface '${IFACE}' does not exist." >&2 + exit 1 +fi + +if [[ ! -x "${ETHERCATCTL}" ]]; then + echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2 + exit 1 +fi + +if [[ ! -x "${ETHERCAT}" ]]; then + echo "error: ethercat command not found: ${ETHERCAT}" >&2 + exit 1 +fi + +MAC="$(cat "/sys/class/net/${IFACE}/address")" + +echo "EtherCAT interface: ${IFACE}" +echo "EtherCAT MAC: ${MAC}" +echo "EtherCAT device: ${ETHERCAT_DEV}" +echo "IgH root: ${IGH_ROOT}" +echo "IgH config: ${ETHERCAT_CONF}" +echo "Device modules: ${DEVICE_MODULES}" +echo "Start wait: ${START_WAIT_SEC}s" + +mkdir -p "$(dirname "${ETHERCAT_CONF}")" + +if [[ -f "${ETHERCAT_CONF}" ]]; then + echo "Stopping existing EtherCAT master with current config..." + "${ETHERCATCTL}" -c "${ETHERCAT_CONF}" stop >/dev/null 2>&1 || true + sleep 1 +fi + +cat >"${ETHERCAT_CONF}" </dev/null 2>&1; then + nmcli device disconnect "${IFACE}" >/dev/null 2>&1 || true +fi + +ip addr flush dev "${IFACE}" +ip link set "${IFACE}" up + +"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" start + +if [[ -e "${ETHERCAT_DEV}" ]]; then + chgrp "${ETHERCAT_GROUP}" "${ETHERCAT_DEV}" + chmod 660 "${ETHERCAT_DEV}" +else + echo "warning: ${ETHERCAT_DEV} not found; skip chmod. Check with: ls -l /dev/EtherCAT*" >&2 +fi + +for ((i = 0; i < START_WAIT_SEC; ++i)); do + if "${ETHERCAT}" slaves 2>/dev/null | grep -qE '^[0-9]+[[:space:]]'; then + break + fi + sleep 1 +done + +"${ETHERCAT}" master +"${ETHERCAT}" slaves || true diff --git a/script/ethercat/status_ethercat.sh b/script/ethercat/status_ethercat.sh new file mode 100644 index 00000000..0533b0fb --- /dev/null +++ b/script/ethercat/status_ethercat.sh @@ -0,0 +1,62 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'USAGE' +Usage: + script/ethercat/status_ethercat.sh + +Environment: + IGH_ROOT=... Override bundled IgH install path. + ETHERCAT_CONF=... Override ethercatctl config path. +USAGE +} + +if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then + usage + exit 0 +fi + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" +IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}" +ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}" +ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl" +ETHERCAT="${IGH_ROOT}/bin/ethercat" + +if [[ ! -x "${ETHERCATCTL}" ]]; then + echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2 + exit 1 +fi + +if [[ ! -x "${ETHERCAT}" ]]; then + echo "error: ethercat command not found: ${ETHERCAT}" >&2 + exit 1 +fi + +echo "== ${ETHERCAT_CONF} ==" +if [[ -f "${ETHERCAT_CONF}" ]]; then + sed -n '1,80p' "${ETHERCAT_CONF}" +else + echo "missing" +fi + +echo +echo "== kernel modules ==" +lsmod | grep -E '(^ec_master|^ec_generic|^ec_)' || true + +echo +echo "== ethercatctl ==" +"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" status || true + +echo +echo "== master ==" +"${ETHERCAT}" master || true + +echo +echo "== slaves ==" +"${ETHERCAT}" slaves || true + +echo +echo "== pdos ==" +"${ETHERCAT}" pdos || true diff --git a/script/ethercat/stop_ethercat.sh b/script/ethercat/stop_ethercat.sh new file mode 100644 index 00000000..2e0d6f33 --- /dev/null +++ b/script/ethercat/stop_ethercat.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'USAGE' +Usage: + sudo script/ethercat/stop_ethercat.sh [iface] [--restore-network] + +Examples: + sudo script/ethercat/stop_ethercat.sh eno1 + sudo script/ethercat/stop_ethercat.sh eno1 --restore-network + +Environment: + IGH_ROOT=... Override bundled IgH install path. + ETHERCAT_CONF=... Override ethercatctl config path. +USAGE +} + +if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then + usage + exit 0 +fi + +if [[ "$(id -u)" -ne 0 ]]; then + echo "error: please run with sudo." >&2 + exit 1 +fi + +IFACE="eno1" +RESTORE_NETWORK="false" + +for arg in "$@"; do + case "${arg}" in + --restore-network) + RESTORE_NETWORK="true" + ;; + -*) + echo "error: unknown option: ${arg}" >&2 + usage >&2 + exit 1 + ;; + *) + IFACE="${arg}" + ;; + esac +done + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" +IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}" +ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}" +ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl" + +if [[ ! -x "${ETHERCATCTL}" ]]; then + echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2 + exit 1 +fi + +"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" stop + +if [[ "${RESTORE_NETWORK}" == "true" ]]; then + ip link set "${IFACE}" up + if command -v nmcli >/dev/null 2>&1; then + nmcli device connect "${IFACE}" || true + fi + echo "Stopped EtherCAT and requested normal network restore on ${IFACE}." +else + echo "Stopped EtherCAT. Normal network restore skipped." + echo "Use '--restore-network' if this interface should return to NetworkManager." +fi