diff --git a/CMakeLists.txt b/CMakeLists.txt index ca7498b2..be448083 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) diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT new file mode 100644 index 00000000..74d127a1 --- /dev/null +++ b/MUJOCO_LOG.TXT @@ -0,0 +1,12 @@ +Fri Jul 24 15:39:05 2026 +ERROR: could not create window + +Fri Jul 24 15:40:37 2026 +ERROR: could not create window + +Fri Sep 11 13:10:38 2026 +ERROR: could not initialize GLFW + +Fri Sep 11 14:13:09 2026 +ERROR: could not initialize GLFW + diff --git a/README.md b/README.md index 03b74a5e..1bcb8565 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,25 @@ # CMVR-ES -## Overview +## 简介 -## Installation +CMVR-ES 工程。 -### 1. Git submodules install +## 安装 + +### 1. 拉取 Git 子模块 ``` git submodule update --init --recursive ``` -### 2. Dependency install +### 2. 安装系统依赖 ```shell -# basic +# 基础工具 sudo apt-get update sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev -# opencv +# OpenCV sudo apt install -y \ libjpeg-dev libpng-dev libtiff-dev \ libavcodec-dev libavformat-dev libswscale-dev \ @@ -39,7 +41,7 @@ sudo apt-get install libassimp-dev # visp sudo apt-get install -y libx11-dev liblapack-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev -# realsense +# RealSense sudo apt-get install -y \ libusb-1.0-0-dev libudev-dev \ libglu1-mesa-dev @@ -48,4 +50,91 @@ sudo apt-get install -y \ sudo apt install gnuplot-qt ``` +### 3. 配置 IgH EtherCAT +工程已内置 IgH EtherCAT 1.7.0 的 userspace 文件： + +```text +dependency/x86/third_party/ethercat/v1.7.0 +``` + +先设置本机路径： + +```shell +export CMVR_ES_ROOT=/path/to/cmvr-es +export IGH_ETHERCAT_ROOT=$CMVR_ES_ROOT/dependency/x86/third_party/ethercat/v1.7.0 +``` + +安装当前内核的 header： + +```shell +sudo apt-get update +sudo apt-get install -y linux-headers-$(uname -r) +``` + +如果内置目录里已经有当前内核版本的 EtherCAT 内核模块，直接安装到系统： + +```shell +sudo mkdir -p /lib/modules/$(uname -r)/ethercat +sudo cp -r $IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)/ethercat/* /lib/modules/$(uname -r)/ethercat/ +sudo depmod +``` + +内置内核模块只适用于相同内核版本。如果 `$IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)` 不存在，说明这台机器的内核版本不匹配，需要在这台机器上重新编译安装 IgH EtherCAT： + +```shell +sudo apt-get update +sudo apt-get install -y \ + build-essential autoconf automake libtool pkg-config git \ + linux-headers-$(uname -r) + +cd /tmp +git clone --branch stable-1.7 --depth 1 https://gitlab.com/etherlab.org/ethercat.git ethercat-stable-1.7 +cd ethercat-stable-1.7 + +./bootstrap +./configure \ + --prefix=$IGH_ETHERCAT_ROOT \ + --libdir=$IGH_ETHERCAT_ROOT/lib \ + --includedir=$IGH_ETHERCAT_ROOT/include \ + --sysconfdir=$IGH_ETHERCAT_ROOT/etc \ + --with-systemdsystemunitdir=$IGH_ETHERCAT_ROOT/lib/systemd/system \ + --enable-generic \ + --with-linux-dir=/lib/modules/$(uname -r)/build + +make -j$(nproc) all modules +make install +sudo make modules_install +sudo depmod +``` + +启动 EtherCAT。`eno1` 换成实际连接 EtherCAT 从站的网卡： + +```shell +sudo script/ethercat/start_ethercat.sh eno1 +``` + +脚本会写入内置 IgH 配置文件： + +```text +$IGH_ETHERCAT_ROOT/etc/ethercat.conf +``` + +脚本会把该网卡的 MAC 写到 `MASTER0_DEVICE`，使用 `DEVICE_MODULES="generic"`，把网卡从 NetworkManager 断开，并通过 `ethercatctl -c` 启动 IgH master。 + +查看状态： + +```shell +script/ethercat/status_ethercat.sh + +$IGH_ETHERCAT_ROOT/bin/ethercat master +$IGH_ETHERCAT_ROOT/bin/ethercat slaves +$IGH_ETHERCAT_ROOT/bin/ethercat pdos +``` + +停止 EtherCAT： + +```shell +sudo script/ethercat/stop_ethercat.sh eno1 +sudo script/ethercat/stop_ethercat.sh eno1 --restore-network +``` diff --git a/cmake/FindExternalLib.cmake b/cmake/FindExternalLib.cmake index b426441e..2a92692f 100644 --- a/cmake/FindExternalLib.cmake +++ b/cmake/FindExternalLib.cmake @@ -55,7 +55,17 @@ function(setup_external_libs ARCH) # ---- library dirs ---- if(EXISTS "${FULL_PATH}/lib") - list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib") + file(GLOB _BUNDLED_LIBSTDCXX_FILES + "${FULL_PATH}/lib/libstdc++.so" + "${FULL_PATH}/lib/libstdc++.so.*" + ) + if(_BUNDLED_LIBSTDCXX_FILES) + message(STATUS + "${LIB_NAME}: excluding vendor lib directory from global " + "link paths because it contains a private libstdc++") + else() + list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib") + endif() set(HAS_LIB TRUE) # Collect shared libs for install: *.so and *.so.* @@ -128,6 +138,10 @@ function(setup_external_libs ARCH) link_directories(${LIBRARY_DIRS}) endif() + # Use the system/toolchain C++ runtime. Vendor SDKs (e.g. Aubo) may bundle + # an older libstdc++ that cannot satisfy the rest of the application's ABI. + list(FILTER INSTALL_SO_FILES EXCLUDE REGEX "/libstdc\\+\\+\\.so(\\..*)?$") + # ---- install third-party shared libs into /lib ---- if(INSTALL_SO_FILES) list(REMOVE_DUPLICATES INSTALL_SO_FILES) 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/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/controllers/CMakeLists.txt b/cmvr-es/algorithms/controllers/CMakeLists.txt index 1921842a..07fe6948 100644 --- a/cmvr-es/algorithms/controllers/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/CMakeLists.txt @@ -12,7 +12,7 @@ add_subdirectory(arm_control) #) 其他动态库类似 file(GLOB SRC ${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/pbvs/src/pbvs_controller.cpp ) @@ -58,3 +58,13 @@ target_link_libraries(controller PUBLIC add_library(cmvr_es::algorithms::controller ALIAS controller) install(TARGETS controller LIBRARY DESTINATION lib) + +add_executable(pbvs_controller_test + pbvs/src/pbvs_controller_test.cpp +) + +target_link_libraries(pbvs_controller_test PRIVATE + cmvr_es::algorithms::controller + gtest + gtest_main +) diff --git a/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt b/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt index 120e91f9..3194deb1 100644 --- a/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt @@ -11,3 +11,7 @@ target_link_libraries(arm_control add_library(cmvr_es::algorithms::arm_control ALIAS arm_control) install(TARGETS arm_control LIBRARY DESTINATION lib) + +add_executable(cartesian_velocity_controller_test src/cartesian_velocity_controller_test.cpp) +target_link_libraries(cartesian_velocity_controller_test PRIVATE + cmvr_es::algorithms::arm_control gtest gtest_main pthread) diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index 9dd0fc29..cc6fc7ff 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h @@ -25,6 +25,7 @@ public: double stop_command_velocity_norm{1e-3}; double stop_measured_velocity_norm{1e-2}; double stop_acceleration{0.5}; + double stop_timeout_s{2.0}; }; using ReadStateCallback = std::function& q, std::vector& qd)>; @@ -45,14 +46,20 @@ public: double duration, FrameType frame); Result stop(std::optional acceleration = std::nullopt); + Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options, + double duration, FrameType frame); + SpeedLReference getReference() const; void shutdown(); bool busy() const { return busy_.load(); } + double stopTimeoutS() const { return config_.stop_timeout_s; } CartesianVelocity getCommandTwistBase() const; private: void ensureWorkerStarted_(); void workerLoop_(); + void requestStop_(std::optional acceleration = std::nullopt); + void abortCommand_(); void sendZero_(); static double velocityNorm_(const std::vector& velocity); @@ -73,6 +80,9 @@ private: CartesianVelocity target_twist_{}; FrameType target_frame_{FrameType::Base}; double target_acceleration_{0.25}; + SpeedLOptions target_options_{}; + SpeedLReference reference_{}; + CartesianVelocity command_twist_snapshot_{}; std::uint64_t command_version_{0}; std::atomic busy_{false}; }; diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index efad63b6..073a7d47 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp @@ -29,6 +29,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController: if (config.stop_acceleration <= 0.0) { config.stop_acceleration = defaults.stop_acceleration; } + if (!std::isfinite(config.stop_timeout_s) || config.stop_timeout_s <= 0.0) { + config.stop_timeout_s = defaults.stop_timeout_s; + } return config; } @@ -58,11 +61,26 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, const double duration, const FrameType frame) { - if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) { + SpeedLOptions options; + options.acceleration = acceleration; + return speedL(velocity, options, duration, frame); +} + +Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, + const SpeedLOptions& options, + const double duration, const FrameType frame) +{ + const double acceleration = options.acceleration; + if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || + !std::isfinite(acceleration) || acceleration <= 0.0 || + !std::isfinite(duration) || duration < 0.0 || !std::isfinite(twistNorm_(velocity)) || + (options.linear_jerk && (!std::isfinite(*options.linear_jerk) || *options.linear_jerk <= 0.0))) { return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input"); } - if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + if (!worker_ || !worker_->joinable()) { + if (busy_.exchange(true)) { + return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + } } ensureWorkerStarted_(); @@ -73,7 +91,10 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, target_twist_ = velocity; target_acceleration_ = acceleration; target_frame_ = frame; + target_options_ = options; + reference_ = {}; command_active_ = true; + busy_.store(true); command_version = ++command_version_; } cv_.notify_all(); @@ -103,15 +124,13 @@ Result CartesianVelocityController::stop(const std::optional acceleratio if (!worker_ || !worker_->joinable()) { return Result::success(); } - { - std::lock_guard lock(mutex_); - target_twist_ = {}; - target_frame_ = FrameType::Base; - target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration; - command_active_ = true; - ++command_version_; + // A completed speedL command leaves the worker thread joinable but idle. + // Do not turn that idle worker into a new command just because a caller + // requests a stop during a task transition. + if (!busy_.load()) { + return Result::success(); } - cv_.notify_all(); + requestStop_(acceleration); return Result::success(); } @@ -137,10 +156,14 @@ void CartesianVelocityController::shutdown() CartesianVelocity CartesianVelocityController::getCommandTwistBase() const { - if (!planner_) { - return {}; - } - return planner_->getSpeedLCommandTwistBase(); + std::lock_guard lock(mutex_); + return command_twist_snapshot_; +} + +SpeedLReference CartesianVelocityController::getReference() const +{ + std::lock_guard lock(mutex_); + return reference_; } void CartesianVelocityController::ensureWorkerStarted_() @@ -161,6 +184,9 @@ void CartesianVelocityController::workerLoop_() CartesianVelocity target_twist; double acceleration = 0.25; FrameType target_frame = FrameType::Base; + SpeedLOptions options; + std::uint64_t applied_version = 0; + bool capture_reference = false; { std::unique_lock lock(mutex_); cv_.wait(lock, [&]() { @@ -189,42 +215,55 @@ void CartesianVelocityController::workerLoop_() target_twist = target_twist_; acceleration = target_acceleration_; target_frame = target_frame_; + options = target_options_; + options.acceleration = acceleration; + applied_version = command_version_; + capture_reference = options.capture_reference && !reference_.valid; } - if (!planner_->updateSpeedLAcceleration(acceleration)) { - if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) { - std::lock_guard lock(mutex_); - command_active_ = false; - sendZero_(); - busy_.store(false); + if (!planner_->updateSpeedLLimits(options)) { + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); break; } CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" << acceleration; - sendZero_(); - busy_.store(false); - return; + requestStop_(); + continue; } std::vector q_now; std::vector qd_now; if (!read_state_(q_now, qd_now)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed"; - sendZero_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + break; + } + requestStop_(); + continue; } std::vector qd_cmd; + SpeedLReference reference; + if (capture_reference && + !planner_->captureSpeedLReference(q_now, target_twist, target_frame, reference)) { + CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] cannot capture motion reference"; + abortCommand_(); + break; + } if (!planner_->speedLStep(target_twist, dt, q_now, qd_now, qd_cmd, target_frame)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] speedLStep failed, target_twist=[" << target_twist.vx << ", " << target_twist.vy << ", " << target_twist.vz << ", " << target_twist.wx << ", " << target_twist.wy << ", " << target_twist.wz << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base"); - sendZero_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + break; + } + requestStop_(); + continue; } JointVelocityCommand velocity_command; @@ -233,9 +272,25 @@ void CartesianVelocityController::workerLoop_() if (!send_result.ok()) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: " << send_result.message; - sendZero_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + break; + } + requestStop_(); + continue; + } + + { + std::lock_guard lock(mutex_); + command_twist_snapshot_ = planner_->getSpeedLCommandTwistBase(); + if (capture_reference && command_version_ == applied_version) { + reference.command_version = applied_version; + reference_ = reference; + // Lock a captured Tool-frame translation in Base for the + // entire command, matching its distance reference axis. + target_twist_ = reference.target_base; + target_frame_ = FrameType::Base; + } } if (twistNorm_(target_twist) < config_.stop_twist_norm && @@ -243,10 +298,14 @@ void CartesianVelocityController::workerLoop_() velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) { { std::lock_guard lock(mutex_); + // A newer target may have arrived during planning/I/O. + if (command_version_ != applied_version) continue; command_active_ = false; + // Serialize the final zero and busy transition with new + // submissions, not only the version comparison. + sendZero_(); + busy_.store(false); } - sendZero_(); - busy_.store(false); break; } @@ -260,6 +319,34 @@ void CartesianVelocityController::workerLoop_() busy_.store(false); } +void CartesianVelocityController::requestStop_(const std::optional acceleration) +{ + { + std::lock_guard lock(mutex_); + target_twist_ = {}; + target_frame_ = FrameType::Base; + target_acceleration_ = acceleration.has_value() ? *acceleration + : config_.stop_acceleration; + target_options_.acceleration = target_acceleration_; + target_options_.capture_reference = false; + command_active_ = true; + ++command_version_; + } + cv_.notify_all(); +} + +void CartesianVelocityController::abortCommand_() +{ + { + std::lock_guard lock(mutex_); + command_active_ = false; + target_twist_ = {}; + target_frame_ = FrameType::Base; + sendZero_(); + busy_.store(false); + } +} + void CartesianVelocityController::sendZero_() { if (!send_velocity_) { diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp new file mode 100644 index 00000000..daf2ed72 --- /dev/null +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp @@ -0,0 +1,131 @@ +#include +#include "algorithms/controllers/arm_control/include/cartesian_velocity_controller.h" +#include +#include +#include +#include + +namespace cmvr::device { +namespace { +using namespace std::chrono_literals; +class Planner final : public CartesianMotionPlanner { +public: + bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override { return true; } + bool configureMoveL(const config::MoveLPlannerConfig&) override { return true; } + bool planMoveL(const CartesianPose&, const std::vector&, const std::vector&, + double, double, double, FrameType, CartesianJointTrajectory&) override { return false; } + bool updateSpeedLAcceleration(double) override { return true; } + bool updateSpeedLLimits(const SpeedLOptions& options) override { + applied_reversal.store(options.continuous_linear_reversal.value_or(false)); + applied_jerk.store(options.linear_jerk.value_or(10.0)); return true; + } + bool captureSpeedLReference(const std::vector&, const CartesianVelocity& target, + FrameType, SpeedLReference& ref) override { + ref.valid = true; + ref.tcp_pose_base.y = .5; + ref.target_base.vx = target.vy; + return true; + } + bool speedLStep(const CartesianVelocity& v, double, const std::vector&, + const std::vector&, std::vector& out, FrameType) override { + current = v; out = {v.vy}; return true; + } + CartesianVelocity getSpeedLCommandTwistBase() const override { return current; } + CartesianVelocity current; + std::atomic applied_jerk{0.0}; + std::atomic applied_reversal{false}; +}; +TEST(CartesianVelocityController, CompletedStopCannotClearNewReversal) { + std::mutex mutex; + std::condition_variable cv; + bool forward_sent = false, block_zero = false, zero_entered = false; + bool release_zero = false, reverse_sent = false; + auto planner = std::make_shared(); + CartesianVelocityController controller({}, planner, 1, + [](auto& q, auto& qd) { q = {0}; qd = {0}; return true; }, + [&](const JointVelocityCommand& command, double) { + std::unique_lock lock(mutex); + forward_sent |= command.velocity[0] > 0; + reverse_sent |= command.velocity[0] < 0; + if (command.velocity[0] == 0 && block_zero && !zero_entered) { + zero_entered = true; cv.notify_all(); + cv.wait_for(lock, 2s, [&] { return release_zero; }); + } + cv.notify_all(); return Result::success(); + }); + CartesianVelocity v; v.vy = .08; + ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok()); + { + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return forward_sent; })); + block_zero = true; + } + ASSERT_TRUE(controller.stop(3).ok()); + { + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return zero_entered; })); + } + v.vy = -.08; + ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok()); + { + std::unique_lock lock(mutex); + release_zero = true; cv.notify_all(); + EXPECT_TRUE(cv.wait_for(lock, 1s, [&] { return reverse_sent; })); + } + EXPECT_TRUE(controller.busy()); + controller.shutdown(); +} +TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimits) { + std::mutex mutex; + std::condition_variable cv; + bool entered = false, release = false; + auto planner = std::make_shared(); + CartesianVelocityController controller({}, planner, 1, + [](auto& q, auto& qd) { q = {0}; qd = {0}; return true; }, + [&](const JointVelocityCommand&, double) { + std::unique_lock lock(mutex); + if (!entered) { + entered = true; cv.notify_all(); + cv.wait_for(lock, 2s, [&] { return release; }); + } + return Result::success(); + }); + CartesianVelocity v; v.vy = -.08; + SpeedLOptions options; + options.acceleration = 3; + options.linear_jerk = 60; + options.continuous_linear_reversal = true; + options.capture_reference = true; + ASSERT_TRUE(controller.speedL(v, options, 0, FrameType::Tool).ok()); + { + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return entered; })); + EXPECT_FALSE(controller.getReference().valid); + release = true; cv.notify_all(); + } + const auto deadline = std::chrono::steady_clock::now() + 1s; + while (!controller.getReference().valid && std::chrono::steady_clock::now() < deadline) + std::this_thread::sleep_for(1ms); + const auto ref = controller.getReference(); + EXPECT_TRUE(ref.valid); + EXPECT_GT(ref.command_version, 0U); + EXPECT_DOUBLE_EQ(ref.tcp_pose_base.y, .5); + EXPECT_DOUBLE_EQ(ref.target_base.vx, -.08); + EXPECT_DOUBLE_EQ(planner->applied_jerk.load(), 60); + EXPECT_TRUE(planner->applied_reversal.load()); + controller.shutdown(); +} +TEST(CartesianVelocityController, RejectsInvalidMotionLimitsBeforeStarting) { + auto planner = std::make_shared(); + CartesianVelocityController controller({}, planner, 1, + [](auto&, auto&) { return false; }, + [](const auto&, double) { return Result::success(); }); + SpeedLOptions options; + options.linear_jerk = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok()); + options.linear_jerk = -1; + EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok()); + EXPECT_FALSE(controller.busy()); +} +} +} diff --git a/cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h b/cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h new file mode 100644 index 00000000..645ddd83 --- /dev/null +++ b/cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h @@ -0,0 +1,453 @@ +#pragma once + +#ifndef CMVR_PBVS_CONTROLLER_H +#define CMVR_PBVS_CONTROLLER_H + +#include + +#include + +#include "common/types/arm/arm_types.h" + +namespace cmvr { + +/** + * @brief 基于相对 3D 位姿的 PBVS 控制器。 + * + * 坐标系： + * + * G : Screen Tag 坐标系，同时作为屏幕参考坐标系 + * P : TCP / 触控点坐标系 + * + * 输入： + * + * ^G T_P_des : TCP 相对于 Screen Tag 的目标位姿 + * ^G T_P_cur : TCP 相对于 Screen Tag 的当前位姿 + * + * 输出： + * + * ^G V_P = + * + * [ vx ] + * [ vy ] + * [ vz ] + * [ wx ] + * [ wy ] + * [ wz ] + * + * 即 TCP 在 Screen Tag 坐标系 G 下表达的 6D Cartesian Twist。 + * + * 位置误差： + * + * e_p = p_des - p_cur + * + * 姿态误差： + * + * R_err = R_des * R_cur^T + * + * e_R = Log(R_err)^vee + * + * 控制律： + * + * v = Kp * e_p + * w = Kr * e_R + * + * 本类只负责视觉伺服 6D Twist 计算，不负责： + * + * - AprilTag 检测 + * - RobotArm + * - G -> Base 坐标转换 + * - IK + * - speedL 下发 + */ +class PbvsController { +public: + enum class ComputeStatus { + OK = 0, + TARGET_NOT_SET, + INVALID_INPUT, + INVALID_DT, + INVALID_TARGET, + INVALID_CURRENT_POSE + }; + + struct Output { + // 当前位置误差，单位 m + Eigen::Vector3d position_error_G{ + Eigen::Vector3d::Zero() + }; + + // 当前姿态误差 rotation-vector，单位 rad + Eigen::Vector3d rotation_error_G{ + Eigen::Vector3d::Zero() + }; + + // PBVS 计算得到的原始线速度，单位 m/s + Eigen::Vector3d raw_linear_velocity_G{ + Eigen::Vector3d::Zero() + }; + + // PBVS 计算得到的原始角速度，单位 rad/s + Eigen::Vector3d raw_angular_velocity_G{ + Eigen::Vector3d::Zero() + }; + + // 经过限幅 / 加速度 / 滤波后的线速度 + Eigen::Vector3d linear_velocity_G{ + Eigen::Vector3d::Zero() + }; + + // 经过限幅 / 加速度 / 滤波后的角速度 + Eigen::Vector3d angular_velocity_G{ + Eigen::Vector3d::Zero() + }; + + // 可直接取出的 CartesianVelocity。 + // + // 注意： + // 这里仍然是在 G / ScreenTag frame 下表达。 + device::CartesianVelocity twist_G{}; + + bool position_reached{false}; + bool orientation_reached{false}; + bool reached{false}; + + bool valid{false}; + }; + +public: + PbvsController(); + + /** + * @brief 设置完整目标位姿。 + * + * @param T_G_P_des TCP(P) 相对于 ScreenTag(G) 的目标位姿。 + */ + bool setTargetPose( + const Eigen::Matrix4d& T_G_P_des); + + /** + * @brief 使用目标位置 + 目标姿态设置期望位姿。 + */ + bool setTargetPose( + const Eigen::Vector3d& position_G, + const Eigen::Matrix3d& rotation_G_P); + + /** + * @brief 当前是否已经设置有效目标。 + */ + bool hasTarget() const { + return target_valid_; + } + + /** + * @brief 获取目标位姿。 + */ + const Eigen::Matrix4d& targetPose() const { + return T_G_P_des_; + } + + /** + * @brief 根据当前 TCP 位姿计算 PBVS 速度命令。 + * + * @param T_G_P_cur 当前 TCP 相对于 Screen Tag 的位姿。 + * @param dt 控制周期，单位 s。 + * @param output 输出结果。 + * + * @return 成功返回 true。 + */ + bool compute( + const Eigen::Matrix4d& T_G_P_cur, + double dt, + Output& output); + + /** + * @brief 便捷接口，只输出 CartesianVelocity。 + * + * 注意输出仍然是 G frame。 + */ + bool compute( + const Eigen::Matrix4d& T_G_P_cur, + double dt, + device::CartesianVelocity& twist_G_out); + + /** + * @brief 设置位置比例增益。 + * + * x/y/z 分别控制。 + */ + void setPositionGain( + const Eigen::Vector3d& kp); + + /** + * @brief 设置姿态比例增益。 + * + * rx/ry/rz 分别控制。 + */ + void setRotationGain( + const Eigen::Vector3d& kr); + + /** + * @brief 设置六维速度上限。 + * + * [vx vy vz wx wy wz] + * + * 前三维 m/s； + * 后三维 rad/s。 + */ + void setVelocityLimit6( + const std::array& vmax6); + + /** + * @brief 设置六维加速度限制。 + * + * 前三维 m/s^2； + * 后三维 rad/s^2。 + * + * <= 0 表示对应维度不限制。 + */ + void setAccelerationLimit6( + const std::array& amax6); + + /** + * @brief 设置六维误差阈值。 + * + * [x y z rx ry rz] + * + * 前三维 m； + * 后三维 rad。 + */ + void setTolerance6( + const std::array& tolerance6); + + /** + * @brief 设置一阶低通 alpha。 + * + * alpha = 1： + * 不滤波。 + * + * 0 < alpha < 1： + * + * cmd = + * alpha * current + * + + * (1-alpha) * previous + */ + void setTwistFilterAlpha(double alpha); + + /** + * @brief 是否启用每个轴。 + * + * 默认 6DoF 全部开启。 + * + * 例如以后如果不希望控制 yaw： + * + * enabled[5] = false; + */ + void setAxisEnabled( + const std::array& enabled); + + /** + * @brief 清空速度历史，但保留目标。 + */ + void resetTwistCommandState(); + + /** + * @brief 清空整个 PBVS 状态和目标。 + */ + void reset(); + + ComputeStatus lastComputeStatus() const { + return last_compute_status_; + } + + static const char* statusToString( + ComputeStatus status); + + const Eigen::Vector3d& lastPositionError() const { + return last_position_error_G_; + } + + const Eigen::Vector3d& lastRotationError() const { + return last_rotation_error_G_; + } + + const Eigen::Vector3d& positionGain() const { + return kp_position_; + } + + const Eigen::Vector3d& rotationGain() const { + return kp_rotation_; + } + + const std::array& velocityLimit6() const { + return vmax6_; + } + + const std::array& accelerationLimit6() const { + return amax6_; + } + + const std::array& tolerance6() const { + return tolerance6_; + } + + double twistFilterAlpha() const { + return twist_lpf_alpha_; + } + + const Eigen::Matrix& + lastTwistCommandG() const { + return last_twist_cmd_G_; + } + + bool lastReached() const { + return last_reached_; + } + +private: + static bool isFiniteTransform( + const Eigen::Matrix4d& T); + + static bool hasValidBottomRow( + const Eigen::Matrix4d& T, + double tolerance = 1e-6); + + static bool isValidTransform( + const Eigen::Matrix4d& T); + + /** + * @brief 将可能有微小数值误差的旋转矩阵投影到 SO(3)。 + */ + static Eigen::Matrix3d projectToSO3( + const Eigen::Matrix3d& R); + + /** + * @brief 计算空间旋转误差，在 G frame 表达。 + * + * R_err = R_des * R_cur^T + * + * e_R = Log(R_err)^vee + */ + static Eigen::Vector3d rotationError( + const Eigen::Matrix3d& R_des, + const Eigen::Matrix3d& R_cur); + + static double clampValue( + double value, + double lower, + double upper); + + static device::CartesianVelocity + toCartesianVelocity( + const Eigen::Matrix& twist); + +private: + // ---------------- target ---------------- + + Eigen::Matrix4d T_G_P_des_{ + Eigen::Matrix4d::Identity() + }; + + bool target_valid_{false}; + + // ---------------- gains ---------------- + + Eigen::Vector3d kp_position_{ + 2.0, + 2.0, + 1.5 + }; + + Eigen::Vector3d kp_rotation_{ + 1.5, + 1.5, + 1.5 + }; + + // ---------------- velocity limits ---------------- + + // + // [vx vy vz wx wy wz] + // + std::array vmax6_{{ + 0.10, + 0.10, + 0.05, + 0.50, + 0.50, + 0.50 + }}; + + // ---------------- acceleration limits ---------------- + + std::array amax6_{{ + 0.50, + 0.50, + 0.30, + 2.0, + 2.0, + 2.0 + }}; + + // ---------------- tolerance ---------------- + + std::array tolerance6_{{ + 0.0015, // x 1.5 mm + 0.0015, // y 1.5 mm + 0.0020, // z 2.0 mm + + 0.05, // rx ~2.9 deg + 0.05, // ry + 0.05 // rz + }}; + + // ---------------- axis enable ---------------- + + std::array axis_enabled_{{ + true, + true, + true, + true, + true, + true + }}; + + // ---------------- LPF ---------------- + + double twist_lpf_alpha_{1.0}; + + // ---------------- command history ---------------- + + Eigen::Matrix + previous_twist_cmd_G_{ + Eigen::Matrix::Zero() + }; + + bool has_previous_twist_{false}; + + // ---------------- last output ---------------- + + Eigen::Vector3d last_position_error_G_{ + Eigen::Vector3d::Zero() + }; + + Eigen::Vector3d last_rotation_error_G_{ + Eigen::Vector3d::Zero() + }; + + Eigen::Matrix + last_twist_cmd_G_{ + Eigen::Matrix::Zero() + }; + + bool last_reached_{false}; + + ComputeStatus last_compute_status_{ + ComputeStatus::TARGET_NOT_SET + }; +}; + +} // namespace cmvr + +#endif // CMVR_PBVS_CONTROLLER_H diff --git a/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp new file mode 100644 index 00000000..fedd1f87 --- /dev/null +++ b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp @@ -0,0 +1,774 @@ +#include "algorithms/controllers/pbvs/include/pbvs_controller.h" + +#include +#include + +#include +#include + +namespace cmvr { + +PbvsController::PbvsController() +{ + resetTwistCommandState(); +} + +bool PbvsController::setTargetPose( + const Eigen::Matrix4d& T_G_P_des) +{ + if (!isValidTransform(T_G_P_des)) { + target_valid_ = false; + last_compute_status_ = + ComputeStatus::INVALID_TARGET; + return false; + } + + T_G_P_des_ = T_G_P_des; + + // + // 视觉位姿可能有微小数值误差。 + // 强制把 rotation 投影到 SO(3)。 + // + T_G_P_des_.block<3, 3>(0, 0) = + projectToSO3( + T_G_P_des.block<3, 3>(0, 0)); + + target_valid_ = true; + last_compute_status_ = + ComputeStatus::OK; + + return true; +} + +bool PbvsController::setTargetPose( + const Eigen::Vector3d& position_G, + const Eigen::Matrix3d& rotation_G_P) +{ + if (!position_G.allFinite() || + !rotation_G_P.allFinite()) { + + target_valid_ = false; + last_compute_status_ = + ComputeStatus::INVALID_TARGET; + + return false; + } + + Eigen::Matrix4d T = + Eigen::Matrix4d::Identity(); + + T.block<3, 3>(0, 0) = + projectToSO3(rotation_G_P); + + T.block<3, 1>(0, 3) = + position_G; + + return setTargetPose(T); +} + +bool PbvsController::compute( + const Eigen::Matrix4d& T_G_P_cur, + const double dt, + Output& output) +{ + output = Output{}; + + last_reached_ = false; + last_position_error_G_.setZero(); + last_rotation_error_G_.setZero(); + last_twist_cmd_G_.setZero(); + + if (!target_valid_) { + last_compute_status_ = + ComputeStatus::TARGET_NOT_SET; + + return false; + } + + if (!std::isfinite(dt) || + dt <= 0.0) { + + last_compute_status_ = + ComputeStatus::INVALID_DT; + + return false; + } + + if (!isValidTransform(T_G_P_cur)) { + last_compute_status_ = + ComputeStatus::INVALID_CURRENT_POSE; + + return false; + } + + // ===================================================== + // 1. Current pose + // ===================================================== + + const Eigen::Vector3d p_cur_G = + T_G_P_cur.block<3, 1>(0, 3); + + const Eigen::Matrix3d R_cur_G_P = + projectToSO3( + T_G_P_cur.block<3, 3>(0, 0)); + + // ===================================================== + // 2. Desired pose + // ===================================================== + + const Eigen::Vector3d p_des_G = + T_G_P_des_.block<3, 1>(0, 3); + + const Eigen::Matrix3d R_des_G_P = + T_G_P_des_.block<3, 3>(0, 0); + + // ===================================================== + // 3. Position error + // + // e_p = p_des - p_cur + // + // expressed in G frame. + // ===================================================== + + Eigen::Vector3d e_pos_G = + p_des_G - + p_cur_G; + + // ===================================================== + // 4. Rotation error + // + // R_err = + // R_des * R_cur^T + // + // e_R = + // Log(R_err)^vee + // + // e_R is also expressed in G frame. + // ===================================================== + + Eigen::Vector3d e_rot_G = + rotationError( + R_des_G_P, + R_cur_G_P); + + if (!e_pos_G.allFinite() || + !e_rot_G.allFinite()) { + + last_compute_status_ = + ComputeStatus::INVALID_INPUT; + + return false; + } + + // ===================================================== + // 5. Disabled axes + // ===================================================== + + for (int i = 0; i < 3; ++i) { + if (!axis_enabled_[i]) { + e_pos_G[i] = 0.0; + } + + if (!axis_enabled_[i + 3]) { + e_rot_G[i] = 0.0; + } + } + + last_position_error_G_ = + e_pos_G; + + last_rotation_error_G_ = + e_rot_G; + + output.position_error_G = + e_pos_G; + + output.rotation_error_G = + e_rot_G; + + // ===================================================== + // 6. Reached check + // ===================================================== + + bool position_reached = true; + bool orientation_reached = true; + + for (int i = 0; i < 3; ++i) { + + if (axis_enabled_[i] && + std::abs(e_pos_G[i]) > + tolerance6_[i]) { + + position_reached = false; + } + + if (axis_enabled_[i + 3] && + std::abs(e_rot_G[i]) > + tolerance6_[i + 3]) { + + orientation_reached = false; + } + } + + const bool reached = + position_reached && + orientation_reached; + + output.position_reached = + position_reached; + + output.orientation_reached = + orientation_reached; + + output.reached = + reached; + + last_reached_ = + reached; + + // ===================================================== + // 7. PBVS P control + // + // v = Kp * e_pos + // + // w = Kr * e_rot + // ===================================================== + + Eigen::Matrix + twist_raw_G = + Eigen::Matrix::Zero(); + + twist_raw_G.head<3>() = + kp_position_.cwiseProduct( + e_pos_G); + + twist_raw_G.tail<3>() = + kp_rotation_.cwiseProduct( + e_rot_G); + + // ===================================================== + // 8. Dead zone + // + // 某个轴已经进入误差阈值，则这个轴不再主动运动。 + // ===================================================== + + for (int i = 0; i < 6; ++i) { + + if (!axis_enabled_[i]) { + twist_raw_G[i] = 0.0; + continue; + } + + const double error_value = + i < 3 + ? e_pos_G[i] + : e_rot_G[i - 3]; + + if (std::abs(error_value) <= + tolerance6_[i]) { + + twist_raw_G[i] = 0.0; + } + } + + output.raw_linear_velocity_G = + twist_raw_G.head<3>(); + + output.raw_angular_velocity_G = + twist_raw_G.tail<3>(); + + // ===================================================== + // 9. Velocity limits + // ===================================================== + + Eigen::Matrix + twist_vel_limited = + twist_raw_G; + + for (int i = 0; i < 6; ++i) { + + const double vmax = + vmax6_[i]; + + if (!std::isfinite(vmax) || + vmax <= 0.0) { + + twist_vel_limited[i] = 0.0; + continue; + } + + twist_vel_limited[i] = + clampValue( + twist_vel_limited[i], + -vmax, + vmax); + } + + // ===================================================== + // 10. Reached: + // + // 进入完整目标阈值后直接输出 0。 + // + // 上层可以随后调用 RobotArm::stopL()。 + // ===================================================== + + if (reached) { + + previous_twist_cmd_G_.setZero(); + has_previous_twist_ = true; + + last_twist_cmd_G_.setZero(); + + output.linear_velocity_G.setZero(); + output.angular_velocity_G.setZero(); + + output.twist_G = + device::CartesianVelocity{}; + + output.valid = true; + + last_compute_status_ = + ComputeStatus::OK; + + return true; + } + + // ===================================================== + // 11. Acceleration limits + // ===================================================== + + Eigen::Matrix + twist_acc_limited; + + if (!has_previous_twist_) { + + previous_twist_cmd_G_.setZero(); + has_previous_twist_ = true; + } + + twist_acc_limited = + previous_twist_cmd_G_; + + for (int i = 0; i < 6; ++i) { + + if (!axis_enabled_[i]) { + twist_acc_limited[i] = 0.0; + continue; + } + + const double amax = + amax6_[i]; + + // <=0：不做加速度限制 + if (!std::isfinite(amax) || + amax <= 0.0) { + + twist_acc_limited[i] = + twist_vel_limited[i]; + + continue; + } + + const double dv_max = + amax * dt; + + const double dv_des = + twist_vel_limited[i] + - + previous_twist_cmd_G_[i]; + + const double dv = + clampValue( + dv_des, + -dv_max, + dv_max); + + twist_acc_limited[i] = + previous_twist_cmd_G_[i] + + + dv; + } + + // ===================================================== + // 12. First-order LPF + // ===================================================== + + Eigen::Matrix + twist_filtered = + twist_acc_limited; + + const double alpha = + std::clamp( + twist_lpf_alpha_, + 0.0, + 1.0); + + if (alpha > 0.0 && + alpha < 1.0) { + + twist_filtered = + alpha * + twist_acc_limited + + + (1.0 - alpha) * + previous_twist_cmd_G_; + } + + // ===================================================== + // 13. Make sure disabled axis is zero + // ===================================================== + + for (int i = 0; i < 6; ++i) { + if (!axis_enabled_[i]) { + twist_filtered[i] = 0.0; + } + } + + // ===================================================== + // 14. Save state + // ===================================================== + + previous_twist_cmd_G_ = + twist_filtered; + + last_twist_cmd_G_ = + twist_filtered; + + // ===================================================== + // 15. Output + // ===================================================== + + output.linear_velocity_G = + twist_filtered.head<3>(); + + output.angular_velocity_G = + twist_filtered.tail<3>(); + + output.twist_G = + toCartesianVelocity( + twist_filtered); + + output.valid = true; + + last_compute_status_ = + ComputeStatus::OK; + + return true; +} + +bool PbvsController::compute( + const Eigen::Matrix4d& T_G_P_cur, + const double dt, + device::CartesianVelocity& twist_G_out) +{ + Output output; + + if (!compute( + T_G_P_cur, + dt, + output)) { + + twist_G_out = + device::CartesianVelocity{}; + + return false; + } + + twist_G_out = + output.twist_G; + + return true; +} + +void PbvsController::setPositionGain( + const Eigen::Vector3d& kp) +{ + for (int i = 0; i < 3; ++i) { + if (std::isfinite(kp[i]) && + kp[i] >= 0.0) { + + kp_position_[i] = + kp[i]; + } + } +} + +void PbvsController::setRotationGain( + const Eigen::Vector3d& kr) +{ + for (int i = 0; i < 3; ++i) { + if (std::isfinite(kr[i]) && + kr[i] >= 0.0) { + + kp_rotation_[i] = + kr[i]; + } + } +} + +void PbvsController::setVelocityLimit6( + const std::array& vmax6) +{ + for (int i = 0; i < 6; ++i) { + + if (std::isfinite(vmax6[i]) && + vmax6[i] >= 0.0) { + + vmax6_[i] = + vmax6[i]; + } + } +} + +void PbvsController::setAccelerationLimit6( + const std::array& amax6) +{ + for (int i = 0; i < 6; ++i) { + + if (std::isfinite(amax6[i])) { + amax6_[i] = + amax6[i]; + } + } +} + +void PbvsController::setTolerance6( + const std::array& tolerance6) +{ + for (int i = 0; i < 6; ++i) { + + if (std::isfinite(tolerance6[i]) && + tolerance6[i] >= 0.0) { + + tolerance6_[i] = + tolerance6[i]; + } + } +} + +void PbvsController::setTwistFilterAlpha( + const double alpha) +{ + if (!std::isfinite(alpha)) { + return; + } + + twist_lpf_alpha_ = + std::clamp( + alpha, + 0.0, + 1.0); +} + +void PbvsController::setAxisEnabled( + const std::array& enabled) +{ + axis_enabled_ = + enabled; +} + +void PbvsController::resetTwistCommandState() +{ + previous_twist_cmd_G_.setZero(); + last_twist_cmd_G_.setZero(); + + has_previous_twist_ = false; +} + +void PbvsController::reset() +{ + T_G_P_des_.setIdentity(); + + target_valid_ = false; + + last_position_error_G_.setZero(); + last_rotation_error_G_.setZero(); + + last_reached_ = false; + + resetTwistCommandState(); + + last_compute_status_ = + ComputeStatus::TARGET_NOT_SET; +} + +const char* PbvsController::statusToString( + const ComputeStatus status) +{ + switch (status) { + + case ComputeStatus::OK: + return "ok"; + + case ComputeStatus::TARGET_NOT_SET: + return "target_not_set"; + + case ComputeStatus::INVALID_INPUT: + return "invalid_input"; + + case ComputeStatus::INVALID_DT: + return "invalid_dt"; + + case ComputeStatus::INVALID_TARGET: + return "invalid_target"; + + case ComputeStatus::INVALID_CURRENT_POSE: + return "invalid_current_pose"; + + default: + return "unknown"; + } +} + +bool PbvsController::isFiniteTransform( + const Eigen::Matrix4d& T) +{ + return T.allFinite(); +} + +bool PbvsController::hasValidBottomRow( + const Eigen::Matrix4d& T, + const double tolerance) +{ + return + std::abs(T(3, 0)) <= tolerance && + std::abs(T(3, 1)) <= tolerance && + std::abs(T(3, 2)) <= tolerance && + std::abs(T(3, 3) - 1.0) <= tolerance; +} + +bool PbvsController::isValidTransform( + const Eigen::Matrix4d& T) +{ + if (!isFiniteTransform(T)) { + return false; + } + + if (!hasValidBottomRow(T)) { + return false; + } + + return true; +} + +Eigen::Matrix3d PbvsController::projectToSO3( + const Eigen::Matrix3d& R) +{ + if (!R.allFinite()) { + return Eigen::Matrix3d::Identity(); + } + + Eigen::JacobiSVD svd( + R, + Eigen::ComputeFullU | + Eigen::ComputeFullV); + + Eigen::Matrix3d U = + svd.matrixU(); + + const Eigen::Matrix3d V = + svd.matrixV(); + + Eigen::Matrix3d R_projected = + U * V.transpose(); + + // + // 确保 det = +1，而不是 reflection。 + // + if (R_projected.determinant() < 0.0) { + + U.col(2) *= -1.0; + + R_projected = + U * V.transpose(); + } + + return R_projected; +} + +Eigen::Vector3d PbvsController::rotationError( + const Eigen::Matrix3d& R_des, + const Eigen::Matrix3d& R_cur) +{ + const Eigen::Matrix3d R_d = + projectToSO3(R_des); + + const Eigen::Matrix3d R_c = + projectToSO3(R_cur); + + // + // 空间旋转误差，在 G frame 表达。 + // + // 当前为 I，目标绕 +Z 旋转 theta： + // + // R_err = R_des + // + // 得到 +theta Z， + // 因此角速度方向正确。 + // + const Eigen::Matrix3d R_err = + R_d * + R_c.transpose(); + + Eigen::AngleAxisd aa( + R_err); + + const double angle = + aa.angle(); + + if (!std::isfinite(angle) || + std::abs(angle) <= 1e-12) { + + return Eigen::Vector3d::Zero(); + } + + const Eigen::Vector3d axis = + aa.axis(); + + if (!axis.allFinite()) { + return Eigen::Vector3d::Zero(); + } + + return axis * angle; +} + +double PbvsController::clampValue( + const double value, + const double lower, + const double upper) +{ + return std::max( + lower, + std::min( + value, + upper)); +} + +device::CartesianVelocity +PbvsController::toCartesianVelocity( + const Eigen::Matrix& twist) +{ + device::CartesianVelocity velocity; + + velocity.vx = + twist[0]; + + velocity.vy = + twist[1]; + + velocity.vz = + twist[2]; + + velocity.wx = + twist[3]; + + velocity.wy = + twist[4]; + + velocity.wz = + twist[5]; + + return velocity; +} + +} // namespace cmvr \ No newline at end of file diff --git a/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp new file mode 100644 index 00000000..cde596f8 --- /dev/null +++ b/cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp @@ -0,0 +1,61 @@ +#include "gtest/gtest.h" + +#include + +#include + +#include "algorithms/controllers/pbvs/include/pbvs_controller.h" + +namespace { + +TEST(PbvsControllerTest, AppliesConfigurationSetters) { + cmvr::PbvsController controller; + + const Eigen::Vector3d position_gain(1.0, 2.0, 3.0); + const Eigen::Vector3d rotation_gain(4.0, 5.0, 6.0); + const std::array vmax{{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}}; + const std::array amax{{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}}; + const std::array tolerance{{0.001, 0.002, 0.003, 0.01, 0.02, 0.03}}; + + controller.setPositionGain(position_gain); + controller.setRotationGain(rotation_gain); + controller.setVelocityLimit6(vmax); + controller.setAccelerationLimit6(amax); + controller.setTolerance6(tolerance); + controller.setTwistFilterAlpha(0.75); + + EXPECT_TRUE(controller.positionGain().isApprox(position_gain)); + EXPECT_TRUE(controller.rotationGain().isApprox(rotation_gain)); + EXPECT_EQ(controller.velocityLimit6(), vmax); + EXPECT_EQ(controller.accelerationLimit6(), amax); + EXPECT_EQ(controller.tolerance6(), tolerance); + EXPECT_DOUBLE_EQ(controller.twistFilterAlpha(), 0.75); +} + +TEST(PbvsControllerTest, CommandsTowardPositionAndRotationError) { + cmvr::PbvsController controller; + controller.setPositionGain(Eigen::Vector3d::Ones()); + controller.setRotationGain(Eigen::Vector3d::Ones()); + controller.setVelocityLimit6({{1.0, 1.0, 1.0, 1.0, 1.0, 1.0}}); + controller.setAccelerationLimit6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); + controller.setTolerance6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}); + controller.setTwistFilterAlpha(1.0); + + Eigen::Matrix4d target = Eigen::Matrix4d::Identity(); + target.block<3, 3>(0, 0) = + Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + target.block<3, 1>(0, 3) = Eigen::Vector3d(0.1, -0.2, 0.3); + ASSERT_TRUE(controller.setTargetPose(target)); + + cmvr::PbvsController::Output output; + ASSERT_TRUE(controller.compute(Eigen::Matrix4d::Identity(), 0.01, output)); + ASSERT_TRUE(output.valid); + EXPECT_GT(output.linear_velocity_G.x(), 0.0); + EXPECT_LT(output.linear_velocity_G.y(), 0.0); + EXPECT_GT(output.linear_velocity_G.z(), 0.0); + EXPECT_NEAR(output.angular_velocity_G.x(), 0.0, 1e-12); + EXPECT_NEAR(output.angular_velocity_G.y(), 0.0, 1e-12); + EXPECT_GT(output.angular_velocity_G.z(), 0.0); +} + +} // namespace diff --git a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt index 310be1b7..1ade9e29 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt +++ b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt @@ -25,4 +25,15 @@ target_link_libraries(ik_solver PUBLIC add_library(cmvr_es::ik_solver ALIAS ik_solver) -install(TARGETS ik_solver LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS ik_solver LIBRARY DESTINATION lib) + +add_executable(pinocchio_qp_ik_solver_test + pinocchio/src/pinocchio_qp_ik_solver_test.cpp +) + +target_link_libraries(pinocchio_qp_ik_solver_test PRIVATE + cmvr_es::ik_solver + cmvr_es::proto + gtest + gtest_main +) diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h index 1fa37f8a..ffce089f 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h @@ -45,6 +45,11 @@ public: std::vector& qdot_out, double qdot_abs_max = std::numeric_limits::infinity()) const override; + // Projects a secondary joint velocity into the Cartesian task null space. + static Eigen::VectorXd projectJointLimitAvoidanceToNullspace( + const Eigen::MatrixXd& jacobian_base, + const Eigen::VectorXd& qdot_avoid); + /// 如你有更严格的速度 / 加速度限位，可以覆盖默认值 void setVelocityLimits(const Eigen::VectorXd &qd_max); void setAccelerationLimits(const Eigen::VectorXd &qdd_max); diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp index 823073a0..3ceba79e 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp @@ -164,7 +164,9 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity( const double margin_ratio = positiveOr(config.margin_ratio(), 0.08); const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02); - Eigen::VectorXd limited = qdot; + // Apply one common scale factor instead of changing individual joints. + // Per-joint scaling changes J*qdot and can disturb the Cartesian task. + double scale = 1.0; for (Eigen::Index i = 0; i < q_chain.size(); ++i) { const double lower = joint_pos_lower_limits_[i]; const double upper = joint_pos_upper_limits_[i]; @@ -174,21 +176,15 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity( const double span = upper - lower; const double margin = std::max(min_margin_rad, margin_ratio * span); - if (limited[i] < 0.0 && q_chain[i] < lower + margin) { + if (qdot[i] < 0.0 && q_chain[i] < lower + margin) { const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0); - limited[i] *= ratio; - if (q_chain[i] <= lower) { - limited[i] = std::max(0.0, limited[i]); - } - } else if (limited[i] > 0.0 && q_chain[i] > upper - margin) { + scale = std::min(scale, ratio); + } else if (qdot[i] > 0.0 && q_chain[i] > upper - margin) { const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0); - limited[i] *= ratio; - if (q_chain[i] >= upper) { - limited[i] = std::min(0.0, limited[i]); - } + scale = std::min(scale, ratio); } } - return limited; + return scale * qdot; } void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) { diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp index ce7af494..4825d0a5 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp @@ -13,11 +13,66 @@ #include #include // std::clamp, std::max, std::min +#include #include // std::sqrt +#include #include +#include #include +#include namespace cmvr { + namespace { + + Eigen::MatrixXd moorePenrosePseudoInverse(const Eigen::MatrixXd& matrix) + { + if (matrix.rows() == 0 || matrix.cols() == 0 || !matrix.allFinite()) { + return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows()); + } + + Eigen::JacobiSVD svd( + matrix, Eigen::ComputeFullU | Eigen::ComputeFullV); + if (svd.info() != Eigen::Success) { + return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows()); + } + + const Eigen::VectorXd singular_values = svd.singularValues(); + const double max_singular = singular_values.size() > 0 + ? singular_values.maxCoeff() + : 0.0; + const double tolerance = + std::numeric_limits::epsilon() * + static_cast(std::max(matrix.rows(), matrix.cols())) * + std::max(1.0, max_singular); + Eigen::VectorXd inverse_singular = singular_values; + for (Eigen::Index i = 0; i < inverse_singular.size(); ++i) { + inverse_singular[i] = singular_values[i] > tolerance + ? 1.0 / singular_values[i] + : 0.0; + } + + const Eigen::Index rank_dimension = singular_values.size(); + return svd.matrixV().leftCols(rank_dimension) * + inverse_singular.asDiagonal() * + svd.matrixU().leftCols(rank_dimension).transpose(); + } + + std::string vectorToString(const Eigen::VectorXd& value) + { + std::ostringstream stream; + stream << '['; + for (Eigen::Index i = 0; i < value.size(); ++i) { + if (i > 0) { + stream << ' '; + } + stream << value[i]; + } + stream << ']'; + return stream.str(); + } + + } // namespace + using Eigen::Matrix4d; using Eigen::VectorXd; using Eigen::MatrixXd; @@ -208,6 +263,24 @@ namespace cmvr { } } + Eigen::VectorXd PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace( + const Eigen::MatrixXd& jacobian_base, + const Eigen::VectorXd& qdot_avoid) + { + if (jacobian_base.cols() != qdot_avoid.size() || + jacobian_base.rows() == 0 || jacobian_base.cols() == 0 || + !jacobian_base.allFinite() || !qdot_avoid.allFinite()) { + return Eigen::VectorXd::Zero(qdot_avoid.size()); + } + + const Eigen::MatrixXd jacobian_pinv = + moorePenrosePseudoInverse(jacobian_base); + const Eigen::MatrixXd nullspace = + Eigen::MatrixXd::Identity(jacobian_base.cols(), jacobian_base.cols()) - + jacobian_pinv * jacobian_base; + return nullspace * qdot_avoid; + } + bool PinocchioQpIKSolver::ik(const Matrix4d &target_pose, std::vector &joints_angle, bool is_tcp) { @@ -389,7 +462,7 @@ namespace cmvr { const int dof = chain_v_dof_; const auto& avoidance = jointLimitPolicy().avoidance(); const bool use_joint_limit_avoidance = - !jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0; + !jointLimitsDisabled() && avoidance.enable() && avoidance.gain() > 0.0; const int avoidance_rows = use_joint_limit_avoidance ? dof : 0; MatrixXd cost(6 + dof + avoidance_rows, dof); @@ -406,7 +479,7 @@ namespace cmvr { VectorXd upper(dof); const Eigen::Map q_chain(q_chain_std.data(), dof); if (use_joint_limit_avoidance) { - const VectorXd qdot_avoid = + const VectorXd qdot_avoid_raw = cmvr::kinematics::computeJointLimitAvoidanceVelocity( q_chain, joint_pos_lower_limits_, @@ -415,10 +488,32 @@ namespace cmvr { positiveOr(avoidance.gain(), 0.2), positiveOr(avoidance.margin_ratio(), 0.15), positiveOr(avoidance.max_push(), 0.25)); - const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05)); + const Eigen::MatrixXd jacobian_pinv = + moorePenrosePseudoInverse(jacobian_base); + const bool jacobian_pinv_valid = + jacobian_pinv.rows() == dof && jacobian_pinv.cols() == 6 && + jacobian_pinv.allFinite(); + Eigen::MatrixXd nullspace = MatrixXd::Zero(dof, dof); + if (jacobian_pinv_valid) { + nullspace = MatrixXd::Identity(dof, dof) - + jacobian_pinv * jacobian_base; + } + const VectorXd qdot_avoid_null = nullspace * qdot_avoid_raw; cost.middleRows(6 + dof, dof) = - sqrt_weight * MatrixXd::Identity(dof, dof); - target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid; + nullspace; + target.segment(6 + dof, dof) = qdot_avoid_null; + + static std::atomic avoidance_debug_counter{0}; + const auto debug_index = + avoidance_debug_counter.fetch_add(1, std::memory_order_relaxed); + if (debug_index % 1000 == 0) { + CMVR_LOG(DEBUG) + << "[PinocchioQpIKSolver][JOINT_LIMIT_AVOIDANCE]" + << " qdot_avoid_raw=" << vectorToString(qdot_avoid_raw) + << " qdot_avoid_null=" << vectorToString(qdot_avoid_null) + << " norm(J*qdot_avoid_null)=" + << (jacobian_base * qdot_avoid_null).norm(); + } } for (int i = 0; i < dof; ++i) { double limit = std::numeric_limits::infinity(); @@ -452,6 +547,35 @@ namespace cmvr { } } } + + const auto& soft_limit = jointLimitPolicy().soft_limit(); + if (!jointLimitsDisabled() && soft_limit.enable() && + joint_pos_lower_limits_.size() == dof && + joint_pos_upper_limits_.size() == dof) { + const double q_min = joint_pos_lower_limits_[i]; + const double q_max = joint_pos_upper_limits_[i]; + if (std::isfinite(q_min) && std::isfinite(q_max) && q_max > q_min) { + const double span = q_max - q_min; + const double margin = std::max( + positiveOr(soft_limit.min_margin_rad(), 0.02), + positiveOr(soft_limit.margin_ratio(), 0.08) * span); + if (q_chain[i] < q_min + margin) { + const double ratio = std::clamp( + (q_chain[i] - q_min) / margin, 0.0, 1.0); + lower[i] = std::max(lower[i], -limit * ratio); + if (q_chain[i] <= q_min) { + lower[i] = std::max(0.0, lower[i]); + } + } else if (q_chain[i] > q_max - margin) { + const double ratio = std::clamp( + (q_max - q_chain[i]) / margin, 0.0, 1.0); + upper[i] = std::min(upper[i], limit * ratio); + if (q_chain[i] >= q_max) { + upper[i] = std::min(0.0, upper[i]); + } + } + } + } } QPSolver solver; @@ -475,7 +599,6 @@ namespace cmvr { if (qdot.size() != dof) { return false; } - qdot = applyJointSoftLimitsToVelocity(q_chain, qdot); qdot_out.assign(qdot.data(), qdot.data() + qdot.size()); return true; } diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp new file mode 100644 index 00000000..9edbb089 --- /dev/null +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp @@ -0,0 +1,104 @@ +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h" + +#include +#include + +#include + +#include "common/io/proto_file_io.h" +#include "cmvr/config/arm_config/arm_config.pb.h" + +namespace cmvr { +namespace { + +std::filesystem::path findProjectRoot() +{ + std::filesystem::path current = std::filesystem::current_path(); + while (!current.empty()) { + if (std::filesystem::exists( + current / "model/xiaoyan_description/dual_arm.urdf")) { + return current; + } + const auto parent = current.parent_path(); + if (parent == current) { + break; + } + current = parent; + } + return {}; +} + +config::PinocchioQpIKConfig loadQpConfig(const std::filesystem::path& root) +{ + config::ArmRootConfig root_config; + const auto config_path = + root / "cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt"; + EXPECT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(config_path.string(), &root_config)); + EXPECT_GT(root_config.arm().robot_arms_size(), 0); + auto solver_config = + root_config.arm().robot_arms(0).kinematics().pinocchio_qp_ik_solver(); + solver_config.set_urdf_path( + (root / "model/xiaoyan_description/dual_arm.urdf").string()); + return solver_config; +} + +TEST(PinocchioQpIKSolverTest, JointLimitAvoidanceIsInCartesianNullspace) +{ + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7); + jacobian.leftCols(6).setIdentity(); + jacobian.col(6) << 0.3, -0.2, 0.4, -0.1, 0.25, 0.15; + Eigen::VectorXd qdot_avoid(7); + qdot_avoid << 0.4, -0.3, 0.2, 0.1, -0.5, 0.6, -0.7; + + const Eigen::VectorXd qdot_null = + PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace( + jacobian, qdot_avoid); + + ASSERT_EQ(qdot_null.size(), 7); + EXPECT_LT((jacobian * qdot_null).norm(), 1e-12); + EXPECT_GT(qdot_null.norm(), 0.0); +} + +TEST(PinocchioQpIKSolverTest, AvoidanceDoesNotDisturbReachableCartesianTwist) +{ + const auto root = findProjectRoot(); + ASSERT_FALSE(root.empty()); + + auto disabled_config = loadQpConfig(root); + disabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(false); + auto enabled_config = disabled_config; + enabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(true); + + PinocchioQpIKSolver solver_disabled(disabled_config); + PinocchioQpIKSolver solver_enabled(enabled_config); + ASSERT_TRUE(solver_disabled.init()); + ASSERT_TRUE(solver_enabled.init()); + + Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7); + jacobian.leftCols(6).setIdentity(); + Eigen::Matrix target_twist; + target_twist << 0.15, -0.10, 0.08, 0.05, -0.04, 0.03; + + // The last joint is inside its configured soft-limit margin, while the + // first six columns fully span the Cartesian task. + const std::vector q_chain = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.56}; + std::vector qdot_disabled; + std::vector qdot_enabled; + ASSERT_TRUE(solver_disabled.solveVelocityBase( + jacobian, target_twist, q_chain, qdot_disabled, 10.0)); + ASSERT_TRUE(solver_enabled.solveVelocityBase( + jacobian, target_twist, q_chain, qdot_enabled, 10.0)); + + const Eigen::Map qdot_disabled_eigen( + qdot_disabled.data(), static_cast(qdot_disabled.size())); + const Eigen::Map qdot_enabled_eigen( + qdot_enabled.data(), static_cast(qdot_enabled.size())); + const Eigen::VectorXd achieved_disabled = jacobian * qdot_disabled_eigen; + const Eigen::VectorXd achieved_enabled = jacobian * qdot_enabled_eigen; + + EXPECT_LT((achieved_enabled - achieved_disabled).norm(), 1e-6); + EXPECT_LT((achieved_enabled - target_twist).norm(), 5e-5); +} + +} // 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..f5313688 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt @@ -14,3 +14,22 @@ 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 +) + +add_executable(pinocchio_speedl_limits_test + cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp +) +target_compile_definitions(pinocchio_speedl_limits_test PRIVATE + CMVR_TEST_SOURCE_DIR="${PROJECT_SOURCE_DIR}") +target_link_libraries(pinocchio_speedl_limits_test PRIVATE + cmvr_es::algorithms::arm_motion gtest gtest_main) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h index 42a553c3..816854f0 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h @@ -44,6 +44,12 @@ public: FrameType frame) = 0; virtual bool updateSpeedLAcceleration(double acceleration) = 0; + virtual bool updateSpeedLLimits(const SpeedLOptions& options) { + return !options.linear_jerk && !options.continuous_linear_reversal.value_or(false) && + updateSpeedLAcceleration(options.acceleration); + } + virtual bool captureSpeedLReference(const std::vector&, + const CartesianVelocity&, FrameType, SpeedLReference&) { return false; } virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0; }; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h index 9d6f070e..b83848dc 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h @@ -37,6 +37,9 @@ public: FrameType frame) override; bool updateSpeedLAcceleration(double acceleration) override; + bool updateSpeedLLimits(const SpeedLOptions& options) override; + bool captureSpeedLReference(const std::vector& q, const CartesianVelocity& target, + FrameType frame, SpeedLReference& reference) override; CartesianVelocity getSpeedLCommandTwistBase() const override; private: @@ -96,9 +99,16 @@ private: Eigen::Vector3d speedl_line_start_tcp_base_{Eigen::Vector3d::Zero()}; Eigen::Vector3d speedl_line_direction_base_{Eigen::Vector3d::Zero()}; double speedl_applied_acceleration_{0.25}; + double speedl_applied_angular_acceleration_{0.25}; + double speedl_applied_linear_jerk_{-1.0}; bool speedl_line_check_active_{false}; bool speedl_line_deviation_warned_{false}; bool speedl_line_direction_warned_{false}; + + // speedL 是否已经进入停止阶段。 + // 停止阶段不要每 1 ms 用 measured twist 重新点燃 Cartesian planner。 + bool speedl_stop_active_{false}; + bool speedl_configured_{false}; }; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp index 329c7b96..661b771a 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp @@ -121,13 +121,16 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne } cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_); + speedl_applied_linear_jerk_ = -1.0; prev_qdot_command_.assign(dof, 0.0); speedl_command_twist_base_.setZero(); speedl_line_check_active_ = false; speedl_line_deviation_warned_ = false; speedl_line_direction_warned_ = false; + speedl_stop_active_ = false; speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); + speedl_applied_angular_acceleration_ = positiveOr(speedl_config_.angular_acceleration_max(), 5.0); speedl_configured_ = true; return true; } @@ -894,15 +897,44 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target return false; } - const Eigen::Matrix target_twist = common::math::velocityToVector(target_velocity); - const bool is_stop_command = target_twist.squaredNorm() <= 1e-12; + const Eigen::Matrix target_twist = + common::math::velocityToVector(target_velocity); + + const bool is_stop_command = + target_twist.squaredNorm() <= 1e-12; + if (is_stop_command) { - twist_limiter_.synchronize(measured_twist_base, dt, true); - } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { - twist_limiter_.initialize(Eigen::Matrix::Zero()); + if (!speedl_stop_active_) { + // 只在 stop 边沿执行一次。 + // + // 非常重要： + // 不再调用 + // twist_limiter_.synchronize(measured_twist_base, dt, true); + // + // 停止应当从“上一拍已经发送出去的 command twist” + // 连续规划到 0，而不是每 1 ms 被 measured twist 重新点燃。 + speedl_stop_active_ = true; + + twist_limiter_.stop(); + } + } else { + // 收到新的非零 speedL，退出停止状态。 + speedl_stop_active_ = false; + + // 从静止开始一个新的 speedL command。 + if (speedl_command_twist_base_.squaredNorm() <= 1e-12 && + (!twist_limiter_.continuousLinearReversalEnabled() || !twist_limiter_.isMoving())) { + twist_limiter_.initialize( + Eigen::Matrix::Zero()); + } + + twist_limiter_.setTargetTwist( + target_twist, + common::math::toPlannerFrame(frame)); } - twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); - speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); + + speedl_command_twist_base_ = + twist_limiter_.update(dt, base_R_tool); if (!updateAndValidateSpeedLLineDeviation_(q_measured, is_stop_command, speedl_command_twist_base_.head<3>())) { @@ -932,7 +964,11 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target qdot = applyJointAccelerationLimits_(qdot, reference, dt); } const Eigen::Matrix achieved_twist_base = jacobian_base * qdot; - if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_, + // During a stop, the limiter intentionally commands a near-zero residual + // twist while the measured arm can still be moving in a different direction. + // Direction and speed-ratio checks are not meaningful for that transient. + if (!is_stop_command && + !validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_, achieved_twist_base, toEigenVector(q_measured), qdot)) { @@ -1216,18 +1252,64 @@ std::string PinocchioCartesianMotionPlanner::describeJointLimitCandidates_( bool PinocchioCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration) { - if (!speedl_configured_ || acceleration <= 0.0) { + SpeedLOptions options; + options.acceleration = acceleration; + return updateSpeedLLimits(options); +} + +bool PinocchioCartesianMotionPlanner::updateSpeedLLimits(const SpeedLOptions& options) +{ + const double jerk_max = positiveOr(speedl_config_.linear_jerk_max(), 10.0); + const double requested_jerk = options.linear_jerk.value_or(jerk_max); + // Validate before clamping: invalid requests must not become valid limits. + if (!speedl_configured_ || !std::isfinite(options.acceleration) || options.acceleration <= 0.0 || + !std::isfinite(requested_jerk) || requested_jerk <= 0.0) { return false; } - if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { + const double acceleration = std::min(options.acceleration, + positiveOr(speedl_config_.linear_acceleration_max(), 5.0)); + const double angular_acceleration = std::min(options.acceleration, + positiveOr(speedl_config_.angular_acceleration_max(), 5.0)); + const double jerk = std::min(requested_jerk, jerk_max); + // Apply the command's motion policy even when acceleration/jerk are + // unchanged. PBVS streams directions; contact retraction locks an axis. + twist_limiter_.setContinuousLinearReversal(options.continuous_linear_reversal.value_or( + !speedl_config_.has_continuous_linear_reversal() || speedl_config_.continuous_linear_reversal())); + if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9 && + std::abs(speedl_applied_angular_acceleration_ - angular_acceleration) <= 1e-9 && + std::abs(speedl_applied_linear_jerk_ - jerk) <= 1e-9) { return true; } - cartesian_motion::updateTwistLimiterAcceleration( - twist_limiter_, - speedl_config_, - acceleration); + twist_limiter_.setLinearConstraints( + positiveOr(speedl_config_.linear_velocity_max(), .55), + acceleration, jerk); + twist_limiter_.setAngularConstraints( + positiveOr(speedl_config_.angular_velocity_max(), 1.0), + angular_acceleration, positiveOr(speedl_config_.angular_jerk_max(), 12.0)); speedl_applied_acceleration_ = acceleration; + speedl_applied_angular_acceleration_ = angular_acceleration; + speedl_applied_linear_jerk_ = jerk; + return true; +} + +bool PinocchioCartesianMotionPlanner::captureSpeedLReference( + const std::vector& q, const CartesianVelocity& target, + const FrameType frame, SpeedLReference& reference) +{ + Eigen::Matrix4d pose; + if (!solver_ || !solver_->fk(q, pose, true) || !pose.allFinite()) return false; + auto twist = common::math::velocityToVector(target); + if (frame == FrameType::Tool) { + const Eigen::Matrix3d rotation = pose.block<3, 3>(0, 0); + twist.head<3>() = rotation * twist.head<3>().eval(); + twist.tail<3>() = rotation * twist.tail<3>().eval(); + } else if (frame != FrameType::Base) { + return false; + } + reference.tcp_pose_base = common::math::matrixToPose(pose); + reference.target_base = common::math::vectorToVelocity(twist); + reference.valid = true; return true; } diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp new file mode 100644 index 00000000..2454b6c6 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp @@ -0,0 +1,213 @@ +#include + +#include +#include +#include +#include + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h" +#include "common/io/proto_file_io.h" +#include "common/math/transform_math.h" + +namespace cmvr::device { +namespace { +constexpr double kDt = .001; +using Twist = Eigen::Matrix; + +// Exercise the production planner and real URDF kinematics, without motor I/O. +class PinocchioSpeedLLimits : public ::testing::Test { +protected: + void SetUp() override { + const auto root = std::filesystem::path(CMVR_TEST_SOURCE_DIR); + config::ArmRootConfig arms; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (root / "cmvr-es/config/devices/arm/arm.pb.txt").string(), &arms)); + ASSERT_GT(arms.arm().robot_arms_size(), 0); + auto ik = arms.arm().robot_arms(0).kinematics().pinocchio_dls_ik_solver(); + ik.set_urdf_path((root / "model/xiaoyan_description/dual_arm.urdf").string()); + solver_ = std::make_shared(ik); + ASSERT_TRUE(solver_->init()); + planner_ = std::make_unique(solver_); + limits_.set_linear_velocity_max(.2); + limits_.set_linear_acceleration_max(.4); + limits_.set_linear_jerk_max(2); + limits_.set_angular_velocity_max(1); + limits_.set_angular_acceleration_max(.8); + limits_.set_angular_jerk_max(4); + limits_.set_enforce_joint_acceleration_limits(false); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + } + + struct Peaks { double velocity{0}, acceleration{0}, jerk{0}; }; + Peaks sample(const CartesianVelocity& target, const SpeedLOptions& options, + bool angular = false, int steps = 2200, bool legacy_api = false) { + Peaks peaks; + Twist previous = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase()); + Twist previous_acceleration = Twist::Zero(); + for (int i = 0; i < steps; ++i) { + const bool updated = legacy_api + ? planner_->updateSpeedLAcceleration(options.acceleration) + : planner_->updateSpeedLLimits(options); + if (!updated || !planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)) { + ADD_FAILURE() << "Planning failed at sample " << i; + break; + } + const Twist velocity = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase()); + const Twist acceleration = (velocity - previous) / kDt; + const Twist jerk = (acceleration - previous_acceleration) / kDt; + const int offset = angular ? 3 : 0; + peaks.velocity = std::max(peaks.velocity, velocity.segment<3>(offset).norm()); + peaks.acceleration = std::max(peaks.acceleration, acceleration.segment<3>(offset).norm()); + peaks.jerk = std::max(peaks.jerk, jerk.segment<3>(offset).norm()); + previous = velocity; + previous_acceleration = acceleration; + } + return peaks; + } + + void expectPeaks(const Peaks& p, double velocity, double acceleration, double jerk) { + // These trajectories contain plateaus: limits must be reached, as well + // as obeyed, so an unintended smaller cap cannot pass the test. + EXPECT_NEAR(p.velocity, velocity, 1e-8); + EXPECT_NEAR(p.acceleration, acceleration, 1e-8); + EXPECT_NEAR(p.jerk, jerk, 1e-6); + } + + std::shared_ptr solver_; + std::unique_ptr planner_; + config::SpeedLPlannerConfig limits_; + const std::vector q_{.25, 1, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; + const std::vector qd_ = std::vector(7, 0); + std::vector command_; +}; + +TEST_F(PinocchioSpeedLLimits, ExcessiveRequestsRespectAllLinearLimits) { + SpeedLOptions options; + options.acceleration = 60; + options.linear_jerk = 60; + CartesianVelocity target; + target.vx = .6; + target.vy = .8; + expectPeaks(sample(target, options), .2, .4, 2); +} + +TEST_F(PinocchioSpeedLLimits, LowerRequestsRemainEffective) { + SpeedLOptions options; + options.acceleration = .15; + options.linear_jerk = .8; + CartesianVelocity target; + target.vy = 1; + expectPeaks(sample(target, options), .2, .15, .8); +} + +TEST_F(PinocchioSpeedLLimits, OmittedJerkRestoresArmLimit) { + SpeedLOptions options; + options.acceleration = .4; + options.linear_jerk = .3; + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + options.linear_jerk.reset(); + CartesianVelocity target; + target.vy = 1; + expectPeaks(sample(target, options), .2, .4, 2); +} + +TEST_F(PinocchioSpeedLLimits, LegacyAccelerationAndStopAreCapped) { + SpeedLOptions options; + options.acceleration = 60; + CartesianVelocity target; + target.vy = 1; + expectPeaks(sample(target, options, false, 1200, true), .2, .4, 2); + const auto stop = sample({}, options, false, 1200, true); + EXPECT_LE(stop.velocity, .2); + EXPECT_NEAR(stop.acceleration, .4, 1e-8); + EXPECT_NEAR(stop.jerk, 2, 1e-6); + EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-12); +} + +TEST_F(PinocchioSpeedLLimits, AngularAccelerationHasItsOwnCapAndCache) { + limits_.set_linear_acceleration_max(.2); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + SpeedLOptions options; + options.acceleration = .3; + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + // Linear effective acceleration stays at .2, but angular must change. + options.acceleration = .6; + CartesianVelocity target; + target.wy = 2; + expectPeaks(sample(target, options, true), 1, .6, 4); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + options.acceleration = 60; + expectPeaks(sample(target, options, true), 1, .8, 4); +} + +TEST_F(PinocchioSpeedLLimits, InvalidRequestsAreRejectedBeforeClamping) { + for (double invalid : {0.0, -1.0, std::numeric_limits::infinity(), + std::numeric_limits::quiet_NaN()}) { + SpeedLOptions options; + options.acceleration = invalid; + EXPECT_FALSE(planner_->updateSpeedLLimits(options)); + EXPECT_FALSE(planner_->updateSpeedLAcceleration(invalid)); + options.acceleration = .3; + options.linear_jerk = invalid; + EXPECT_FALSE(planner_->updateSpeedLLimits(options)); + } +} + +TEST_F(PinocchioSpeedLLimits, StreamingAlignmentOverridesGlobalReversalPolicy) { + for (const double jerk : {10.0, 30.0}) { + SCOPED_TRACE(jerk); + limits_.set_continuous_linear_reversal(true); + limits_.set_linear_acceleration_max(5); + limits_.set_linear_jerk_max(jerk); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + SpeedLOptions options; + options.acceleration = .5; + CartesianVelocity target; + target.vy = .02; + sample(target, options, false, 300); + ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10); + + // Same acceleration/jerk: the policy override must bypass their cache. + options.continuous_linear_reversal = false; + double min_speed = .02; + double max_error = 0; + for (int i = 0; i < 2000; ++i) { + const double angle = (i / 20) * .01; + target.vx = .02 * std::sin(angle); + target.vy = .02 * std::cos(angle); + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)); + const Twist actual = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase()); + min_speed = std::min(min_speed, actual.head<3>().norm()); + max_error = std::max(max_error, (actual - common::math::velocityToVector(target)).norm()); + } + EXPECT_NEAR(min_speed, .02, 1e-10); + EXPECT_LT(max_error, 1e-10); + } +} + +TEST_F(PinocchioSpeedLLimits, ContactReversalStillCrossesZeroAfterStreamingAlignment) { + limits_.set_linear_acceleration_max(1); + limits_.set_linear_jerk_max(4); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + SpeedLOptions options; + options.acceleration = 1; + options.continuous_linear_reversal = false; + CartesianVelocity target; + target.vy = .02; + sample(target, options, false, 300); + ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10); + + options.continuous_linear_reversal = true; + target.vy = -.02; + for (int i = 0; i < 100; ++i) { + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)); + } + EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-10); + ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)); + EXPECT_LT(planner_->getSpeedLCommandTwistBase().vy, -.0003); +} +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h b/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h index 66ab7e88..97dde369 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h @@ -1,6 +1,8 @@ #ifndef CMVR_ES_TWIST_LIMITER_CONFIG_H #define CMVR_ES_TWIST_LIMITER_CONFIG_H +#include + #include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" #include "cmvr/config/arm_config/arm_config.pb.h" #include "common/config/config_files.h" @@ -28,6 +30,8 @@ inline void configureTwistLimiterFromSpeedLConfig( ? config.linear_reverse_cos_threshold() : -0.8660254037844386, positiveOr(config.linear_reverse_switch_speed_threshold(), 1e-3)); + limiter.setContinuousLinearReversal(!config.has_continuous_linear_reversal() || + config.continuous_linear_reversal()); limiter.initialize(Eigen::Matrix::Zero()); } @@ -39,10 +43,10 @@ inline void updateTwistLimiterAcceleration( using cmvr::common::config::positiveOr; limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55), - acceleration, + std::min(acceleration, positiveOr(config.linear_acceleration_max(), 5.0)), positiveOr(config.linear_jerk_max(), 10.0)); limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0), - acceleration, + std::min(acceleration, positiveOr(config.angular_acceleration_max(), 5.0)), positiveOr(config.angular_jerk_max(), 12.0)); } 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..d815332b 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -20,4 +20,30 @@ 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 +add_executable(cartesian_twist_limiter_reversal_test + cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp) +target_link_libraries(cartesian_twist_limiter_reversal_test PRIVATE + cmvr_es::base_motion gtest gtest_main) +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 +) + +add_executable(s_curve_velocity_planner_stop_test + motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp +) + +target_link_libraries(s_curve_velocity_planner_stop_test + PRIVATE + cmvr_es::base_motion + gtest + gtest_main +) diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h index 220389f0..864f1902 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h @@ -16,7 +16,8 @@ enum class CartesianFrame * @brief 6维末端 twist 在线限幅器（基于 SCurveVelocityPlanner1D） * * 线速度部分： - * - 模长使用 SCurveVelocityPlanner1D 做 jerk-limited 速度规划 + * - 固定轴上的有符号速度使用 SCurveVelocityPlanner1D 做 jerk-limited 规划 + * - 同轴反向连续过零，保留加速度；可显式选择旧的停止后换向策略 * - 运动中锁定当前方向，不做方向插值 * - 若目标方向与当前方向不共线，则采用“先减速到0，再切方向”的 switch policy * @@ -67,6 +68,11 @@ public: void setLinearReverseSwitchPolicy(double cos_threshold, double switch_speed_threshold); + // Same-axis reversal uses a signed velocity without resetting acceleration + // at zero. Disable for streaming direction tracking (e.g. visual alignment). + void setContinuousLinearReversal(bool enabled); + bool continuousLinearReversalEnabled() const { return continuous_linear_reversal_; } + /** * @brief 初始化当前 twist 状态 * @@ -120,7 +126,7 @@ public: * * 作用： * - 更新当前执行方向 - * - 用测得模长与模长加速度同步两个 planner + * - 线速度使用固定轴投影（保留正负号），角速度使用模长 * - keep_target=true 时： * - 若测量状态仍贴着当前 profile，则保持当前 profile * - 否则由 planner 内部从测量状态重规划到当前目标 @@ -178,6 +184,7 @@ private: double linear_reverse_switch_speed_threshold_; double angular_switch_speed_threshold_; bool emergency_stop_active_; + bool continuous_linear_reversal_{true}; // 目标/当前状态 Twist target_twist_input_; diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp index a4ecf10b..8167c59a 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp @@ -108,6 +108,25 @@ void CartesianTwistLimiter::setLinearReverseSwitchPolicy(double cos_threshold, linear_reverse_switch_speed_threshold_ = std::max(0.0, switch_speed_threshold); } +void CartesianTwistLimiter::setContinuousLinearReversal(bool enabled) +{ + if (continuous_linear_reversal_ == enabled) return; + + if (!enabled) { + const double velocity = linear_norm_planner_.getVelocity(); + const double acceleration = linear_norm_planner_.getAcceleration(); + // The streaming policy stores a speed magnitude and a physical direction. + // Convert a negative signed state without reversing its physical motion or + // dropping its acceleration when a command changes policy mid-retraction. + if (velocity < 0.0 || (std::abs(velocity) <= EPSILON && acceleration < 0.0)) { + current_linear_dir_base_ = -current_linear_dir_base_; + linear_norm_planner_.overwriteState(-velocity, -acceleration, false); + prev_measured_linear_norm_ = -prev_measured_linear_norm_; + } + } + continuous_linear_reversal_ = enabled; +} + void CartesianTwistLimiter::initialize(const Twist& initial_twist) { restoreNominalConstraints(); @@ -201,7 +220,12 @@ void CartesianTwistLimiter::setTargetTwist(const Twist& target_twist, CartesianF void CartesianTwistLimiter::stop() { + // target_twist_input_.setZero(); target_twist_input_.setZero(); + target_twist_base_.setZero(); + + linear_norm_planner_.setTargetVelocity(0.0); + angular_norm_planner_.setTargetVelocity(0.0); } void CartesianTwistLimiter::emergencyStop(double emergency_acceleration, @@ -244,7 +268,8 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base, const Eigen::Vector3d v_meas = measured_twist_base.head<3>(); const Eigen::Vector3d w_meas = measured_twist_base.tail<3>(); - const double v_norm = v_meas.norm(); + const bool signed_linear = continuous_linear_reversal_ && current_linear_dir_base_.norm() > EPSILON; + const double v_norm = signed_linear ? v_meas.dot(current_linear_dir_base_) : v_meas.norm(); const double w_norm = w_meas.norm(); double v_acc = 0.0; @@ -268,7 +293,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base, last_measured_twist_base_ = measured_twist_base; has_measured_sync_ = true; - // 用测得模长/模长加速度同步 planner 当前状态。 + // 线速度按固定轴投影保留正负号；角速度仍按模长同步 planner。 // keep_target=true: // - 若测量值仍贴着当前 profile，则继续沿旧 profile 走 // - 否则从测量状态重规划到当前目标 @@ -284,7 +309,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base, target_twist_base_.setZero(); } - if (v_norm > EPSILON) { + if (!signed_linear && v_norm > EPSILON) { current_linear_dir_base_ = v_meas / v_norm; last_target_linear_dir_base_ = current_linear_dir_base_; } @@ -459,7 +484,18 @@ CartesianTwistLimiter::update(double dt, const Eigen::Matrix3d& base_R_tool) const bool must_switch_axis = !same_axis || dir_dot < linear_reverse_cos_threshold_; - if (must_switch_axis && v_cur_norm > linear_reverse_switch_speed_threshold_) { + if (continuous_linear_reversal_ && same_axis) { + // Keep the axis fixed: the scalar profile carries the direction sign. + // Crossing zero is an interior point, with continuous acceleration. + linear_norm_planner_.setTargetVelocity(v_des.dot(current_linear_dir_base_)); + } else if (continuous_linear_reversal_ && + (std::abs(v_cur_norm) > EPSILON || + std::abs(linear_norm_planner_.getAcceleration()) > EPSILON || + linear_norm_planner_.hasActiveProfile())) { + // A different axis may only be adopted after the old profile settles. + linear_norm_planner_.setTargetVelocity(0.0); + } else if (!continuous_linear_reversal_ && must_switch_axis && + v_cur_norm > linear_reverse_switch_speed_threshold_) { linear_norm_planner_.setTargetVelocity(0.0); } else { current_linear_dir_base_ = v_target_dir; diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp new file mode 100644 index 00000000..18ba446f --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp @@ -0,0 +1,118 @@ +#include +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" + +namespace cmvr { +namespace { +using Twist = CartesianTwistLimiter::Twist; +constexpr double dt = .001; +Twist y(double v) { Twist t = Twist::Zero(); t.y() = v; return t; } +struct Motion { double peak{0}, reverse_time{-1}, return_time{-1}; }; +Motion reverse(bool continuous, int approach_ticks) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(.55, 5, 10); + limiter.setContinuousLinearReversal(continuous); + limiter.initialize(approach_ticks == 0 ? y(.08) : Twist::Zero()); + limiter.setTargetTwist(y(.08), CartesianFrame::Base); + for (int i = 0; i < approach_ticks; ++i) limiter.update(dt, Eigen::Matrix3d::Identity()); + limiter.setLinearConstraints(.55, 3, 10); + limiter.setTargetTwist(y(-.08), CartesianFrame::Base); + Motion result; + double position = 0; + for (int i = 1; i <= 1500; ++i) { + const auto v = limiter.update(dt, Eigen::Matrix3d::Identity()); + position += v.y() * dt; + result.peak = std::max(result.peak, position); + if (v.y() < 0 && result.reverse_time < 0) result.reverse_time = i * dt; + if (result.reverse_time > 0 && position <= 0 && result.return_time < 0) result.return_time = i * dt; + if (continuous) { + EXPECT_LE(limiter.getAccelerationBase().norm(), 3.0 + 1e-7); + EXPECT_LE(limiter.getJerkBase().norm(), 10.0 + 1e-6); + EXPECT_NEAR(v.x(), 0, 1e-12); + EXPECT_NEAR(v.z(), 0, 1e-12); + } + } + EXPECT_NEAR(limiter.getTwistBase().y(), -.08, 1e-9); + return result; +} +TEST(CartesianTwistReversal, ContinuousReversalReducesTimeAndForwardTravel) { + for (const int ticks : {0, 80, 120}) { + SCOPED_TRACE(ticks); + const auto legacy = reverse(false, ticks); + const auto continuous = reverse(true, ticks); + EXPECT_GT(continuous.reverse_time, 0); + EXPECT_LT(continuous.reverse_time, legacy.reverse_time); + EXPECT_LT(continuous.return_time, legacy.return_time); + EXPECT_LT(continuous.peak, legacy.peak); + } +} +TEST(CartesianTwistReversal, ExactZeroCrossingIsStillMovingAndKeepsAcceleration) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 1, 4); + limiter.initialize(y(.02)); + limiter.setTargetTwist(y(-.02), CartesianFrame::Base); + for (int i = 0; i < 100; ++i) limiter.update(dt, Eigen::Matrix3d::Identity()); + EXPECT_NEAR(limiter.getTwistBase().y(), 0, 1e-12); + EXPECT_TRUE(limiter.isMoving()); + EXPECT_LT(limiter.getAccelerationBase().y(), -.39); + EXPECT_LT(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.0003); +} +TEST(CartesianTwistReversal, StopFromEitherDirectionSettlesWithoutReversing) { + for (double v : {.08, -.08}) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 3, 10); + limiter.initialize(y(v)); + limiter.stop(); + for (int i = 0; i < 1000; ++i) { + EXPECT_GE(limiter.update(dt, Eigen::Matrix3d::Identity()).y() * v, -1e-12); + } + EXPECT_FALSE(limiter.isMoving()); + EXPECT_NEAR(limiter.getTwistBase().norm(), 0, 1e-12); + } +} +TEST(CartesianTwistReversal, FeedbackPreservesNegativeVelocityOnLockedAxis) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 3, 10); + limiter.initialize(y(.08)); + limiter.setTargetTwist(y(-.08), CartesianFrame::Base); + for (int i = 0; i < 400; ++i) limiter.update(dt, Eigen::Matrix3d::Identity()); + for (int i = 0; i < 10; ++i) { + limiter.synchronize(y(-.08), dt, true); + EXPECT_NEAR(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.08, 1e-9); + } +} +TEST(CartesianTwistReversal, NonCollinearChangeStopsBeforeSwitchingAxis) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 3, 10); + limiter.initialize(y(.08)); + Twist x = Twist::Zero(); x.x() = .08; + limiter.setTargetTwist(x, CartesianFrame::Base); + for (int i = 0; i < 1000; ++i) { + const auto v = limiter.update(dt, Eigen::Matrix3d::Identity()); + EXPECT_FALSE(v.x() > 1e-9 && std::abs(v.y()) > 1e-9); + EXPECT_LE(limiter.getJerkBase().norm(), 10 + 1e-6); + } + EXPECT_NEAR(limiter.getTwistBase().x(), .08, 1e-9); +} + +TEST(CartesianTwistReversal, SwitchingToStreamingPreservesNegativePhysicalVelocity) { + for (const int reverse_ticks : {150, 250, 500}) { + SCOPED_TRACE(reverse_ticks); + CartesianTwistLimiter reference; + reference.setLinearConstraints(1, 3, 10); + reference.initialize(y(.08)); + reference.setTargetTwist(y(-.08), CartesianFrame::Base); + for (int i = 0; i < reverse_ticks; ++i) reference.update(dt, Eigen::Matrix3d::Identity()); + ASSERT_LT(reference.getTwistBase().y(), 0); + auto streaming = reference; + streaming.setContinuousLinearReversal(false); + EXPECT_NEAR((streaming.getTwistBase() - reference.getTwistBase()).norm(), 0, 1e-12); + for (int i = 0; i < 500; ++i) { + const auto expected = reference.update(dt, Eigen::Matrix3d::Identity()); + const auto actual = streaming.update(dt, Eigen::Matrix3d::Identity()); + EXPECT_NEAR((actual - expected).norm(), 0, 1e-9); + EXPECT_LE(streaming.getJerkBase().norm(), 10 + 1e-6); + } + } +} +} +} 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/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp index b6748195..c058e673 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp @@ -114,13 +114,29 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, updateIsMovingFlag(); } - void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity, - double acceleration) +void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity, + double acceleration) { const double measured_velocity = clamp(velocity, -max_velocity_, max_velocity_); - const double measured_acceleration = + double measured_acceleration = clamp(acceleration, -max_acceleration_, max_acceleration_); + // When the target is zero this planner is also used for Cartesian speed + // magnitudes. A magnitude is non-negative, while the finite-difference + // derivative of the measured magnitude is signed. Feeding a large negative + // measured acceleration into a signed 1-D velocity planner can generate a + // profile that crosses through zero and becomes negative before returning to + // zero. The twist limiter then multiplies that negative "norm" by the + // current direction, which reverses and amplifies the Cartesian command. + // + // For feedback resynchronization during a stop, synchronize the measured + // speed only and restart the stop profile with zero scalar acceleration. + // This avoids noise-sensitive stop replans and preserves a non-overshooting + // deceleration profile for speed-magnitude users. + if (std::abs(state_.target_velocity) <= VELOCITY_THRESHOLD) { + measured_acceleration = 0.0; + } + if (!state_.has_active_profile && std::abs(measured_velocity - state_.target_velocity) <= VELOCITY_THRESHOLD) { state_.velocity = state_.target_velocity; @@ -128,7 +144,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, state_.jerk = 0.0; updateIsMovingFlag(); return; - } + } // 如果测量值已经基本落在当前采样状态上，就继续沿现有 profile 走。 // 否则每拍都从同一目标重规划，会把已经进入的 jerk phase 反复打断。 @@ -139,7 +155,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time); updateIsMovingFlag(); return; - } + } state_.velocity = measured_velocity; state_.acceleration = measured_acceleration; diff --git a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp new file mode 100644 index 00000000..12ba5e38 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp @@ -0,0 +1,77 @@ +#include + +#include + +#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h" + +namespace cmvr { +namespace { + +void finishActiveProfile(SCurveVelocityPlanner1D& planner, double dt) +{ + for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) { + planner.update(dt); + } + ASSERT_FALSE(planner.hasActiveProfile()); +} + +TEST(SCurveVelocityPlannerStopTest, FeedbackResyncDoesNotReverseSpeedMagnitude) +{ + SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0); + constexpr double kDt = 0.001; + constexpr double kInitialSpeed = 0.04; + + planner.initialize(kInitialSpeed, 0.0); + planner.setTargetVelocity(0.0); + finishActiveProfile(planner, kDt); + + // Reproduce the speedL stop feedback case: the command profile has already + // reached zero, but the measured TCP still has residual speed. A 1 kHz + // finite difference may report a large negative scalar acceleration. + planner.synchronizeAndReplan(kInitialSpeed, -3.0); + + double max_speed = planner.getVelocity(); + double min_speed = planner.getVelocity(); + for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) { + const double speed = planner.update(kDt); + max_speed = std::max(max_speed, speed); + min_speed = std::min(min_speed, speed); + } + + EXPECT_GE(min_speed, -1e-9); + EXPECT_LE(max_speed, kInitialSpeed + 1e-9); + EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9); +} + +TEST(SCurveVelocityPlannerStopTest, LargerMeasuredDecelerationDoesNotIncreaseStopSpeed) +{ + constexpr double kDt = 0.001; + constexpr double kInitialSpeed = 0.04; + const double measured_accelerations[] = {-0.5, -1.0, -2.0, -3.0}; + + for (const double measured_acceleration : measured_accelerations) { + SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0); + planner.initialize(kInitialSpeed, 0.0); + planner.setTargetVelocity(0.0); + finishActiveProfile(planner, kDt); + + planner.synchronizeAndReplan(kInitialSpeed, measured_acceleration); + + double max_speed = planner.getVelocity(); + double min_speed = planner.getVelocity(); + for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) { + const double speed = planner.update(kDt); + max_speed = std::max(max_speed, speed); + min_speed = std::min(min_speed, speed); + } + + EXPECT_GE(min_speed, -1e-9) << "measured_acceleration=" << measured_acceleration; + EXPECT_LE(max_speed, kInitialSpeed + 1e-9) + << "measured_acceleration=" << measured_acceleration; + EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9) + << "measured_acceleration=" << measured_acceleration; + } +} + +} // namespace +} // namespace cmvr diff --git a/cmvr-es/algorithms/perception/CMakeLists.txt b/cmvr-es/algorithms/perception/CMakeLists.txt index 98eddf21..d47a2906 100644 --- a/cmvr-es/algorithms/perception/CMakeLists.txt +++ b/cmvr-es/algorithms/perception/CMakeLists.txt @@ -4,6 +4,7 @@ find_package(OpenCV REQUIRED) add_library(perception SHARED apriltag/src/tag_relative_target_3d.cpp apriltag/src/apriltag_perception.cpp + apriltag/src/tag_relative_tcp_pose.cpp ) target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h b/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h index a4486e56..6f4e57cb 100644 --- a/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h +++ b/cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h @@ -84,6 +84,10 @@ public: // Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。 Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()}; + + // Semantic alias used by visualization and downstream consumers: + // `T_C_Tag` maps points in this tag frame into camera frame C. + const Eigen::Matrix4d& T_C_Tag() const { return T_c_t; } }; struct FrameCache { diff --git a/cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h b/cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h new file mode 100644 index 00000000..7e41574d --- /dev/null +++ b/cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h @@ -0,0 +1,289 @@ +#pragma once + +#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H +#define CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H + +#include + +#include + +#include "algorithms/perception/apriltag/include/apriltag_perception.h" + +namespace cmvr::perception { + +/** + * @brief 根据同一台相机同时观测到的 Screen Tag 和 Hand Tag， + * 计算 TCP 相对于 Screen Tag 的位姿。 + * + * 坐标系： + * + * C : 固定外部相机坐标系 + * G : Screen Tag 坐标系，同时作为屏幕参考坐标系 + * H : Hand Tag 坐标系 + * P : TCP / 触控点坐标系 + * + * 已知： + * + * T_C_G : Screen Tag -> Camera + * T_C_H : Hand Tag -> Camera + * T_H_P : TCP -> Hand Tag + * + * 其中 T_H_P 是外部传入的一次标定结果。 + * + * 计算： + * + * T_G_H = inverse(T_C_G) * T_C_H + * + * T_G_P = T_G_H * T_H_P + * + * 即： + * + * T_G_P = inverse(T_C_G) * T_C_H * T_H_P + * + * 最终： + * + * p_G_P = T_G_P.block<3, 1>(0, 3) + * + * 得到 TCP 原点在 Screen Tag 坐标系下的位置。 + * + * 注意： + * - 本类不主动抓相机图像； + * - 本类不主动调用 AprilTagPerception::update()； + * - 上层应保证当前 perception 缓存来自同一帧； + * - Screen Tag 和 Hand Tag 必须同时在当前帧可见。 + */ +class TagRelativeTcpPose { +public: + enum class Status { + OK = 0, + + NO_PERCEPTION, + + INVALID_SCREEN_TAG_ID, + + INVALID_HAND_TAG_ID, + + SAME_TAG_ID, + + SCREEN_TAG_NOT_FOUND, + + HAND_TAG_NOT_FOUND, + + INVALID_T_C_G, + + INVALID_T_C_H, + + INVALID_T_H_P, + + INVALID_T_G_H, + + INVALID_T_G_P, + + INVALID_TCP_POSITION + }; + +public: + explicit TagRelativeTcpPose( + const std::shared_ptr& perception = nullptr); + + /** + * @brief 设置 AprilTag 感知前端。 + * + * 本类只读取感知缓存，不主动 update。 + */ + void setPerception( + const std::shared_ptr& perception); + + const std::shared_ptr& perception() const { + return perception_; + } + + /** + * @brief 设置 Screen Tag ID。 + */ + void setScreenTagId(int id); + + /** + * @brief 设置 Hand Tag ID。 + */ + void setHandTagId(int id); + + int screenTagId() const { + return screen_tag_id_; + } + + int handTagId() const { + return hand_tag_id_; + } + + /** + * @brief 使用当前 AprilTagPerception 缓存计算 TCP 相对 Screen Tag 的位姿。 + * + * 核心公式： + * + * T_G_P = + * inverse(T_C_G) + * * T_C_H + * * T_H_P + * + * @param T_H_P + * TCP(P) 相对于 Hand Tag(H) 的固定齐次变换。 + * + * 坐标变换语义： + * + * p_H = T_H_P * p_P + * + * 即： + * + * ^H T_P + * + * @return 成功返回 true。 + */ + bool update( + const Eigen::Matrix4d& T_H_P); + + /** + * @brief 当前结果是否有效。 + */ + bool valid() const { + return valid_; + } + + /** + * @brief 最近一次 update() 的状态。 + */ + Status lastStatus() const { + return last_status_; + } + + static const char* statusToString(Status status); + + /** + * @brief 当前 Screen Tag 在 Camera 中的位姿。 + * + * ^C T_G + */ + const Eigen::Matrix4d& T_C_G() const { + return T_C_G_; + } + + /** + * @brief 当前 Hand Tag 在 Camera 中的位姿。 + * + * ^C T_H + */ + const Eigen::Matrix4d& T_C_H() const { + return T_C_H_; + } + + /** + * @brief Hand Tag 相对于 Screen Tag 的位姿。 + * + * ^G T_H + */ + const Eigen::Matrix4d& T_G_H() const { + return T_G_H_; + } + + /** + * @brief TCP 相对于 Screen Tag 的完整 6DoF 位姿。 + * + * ^G T_P + */ + const Eigen::Matrix4d& T_G_P() const { + return T_G_P_; + } + + /** + * @brief TCP 原点在 Screen Tag 坐标系中的位置。 + * + * P_P^G = + * + * [ x_P ] + * [ y_P ] + * [ z_P ] + */ + const Eigen::Vector3d& tcpPositionInScreenTag() const { + return p_G_P_; + } + + /** + * @brief TCP 相对于 Screen Tag 的旋转矩阵。 + * + * R_G_P + */ + Eigen::Matrix3d tcpRotationInScreenTag() const { + return T_G_P_.block<3, 3>(0, 0); + } + + /** + * @brief 清空当前结果。 + */ + void clear(); + +private: + /** + * @brief 检查 4x4 矩阵元素是否全部有限。 + */ + static bool isFiniteTransform( + const Eigen::Matrix4d& T); + + /** + * @brief 基础检查齐次矩阵最后一行。 + */ + static bool hasValidHomogeneousBottomRow( + const Eigen::Matrix4d& T, + double tolerance = 1e-6); + + /** + * @brief 判断一个矩阵是否可以作为基本齐次变换使用。 + * + * 当前只检查： + * - 所有元素 finite + * - 最后一行约等于 [0 0 0 1] + * + * 暂时不强制检查 rotation orthonormal， + * 避免视觉估计中的微小数值误差导致误判。 + */ + static bool isValidTransform( + const Eigen::Matrix4d& T); + +private: + std::shared_ptr perception_{nullptr}; + + int screen_tag_id_{-1}; + int hand_tag_id_{-1}; + + // 当前外部相机观测 + Eigen::Matrix4d T_C_G_{ + Eigen::Matrix4d::Identity() + }; + + Eigen::Matrix4d T_C_H_{ + Eigen::Matrix4d::Identity() + }; + + // 相对变换 + Eigen::Matrix4d T_G_H_{ + Eigen::Matrix4d::Identity() + }; + + Eigen::Matrix4d T_G_P_{ + Eigen::Matrix4d::Identity() + }; + + // TCP 原点在 Screen Tag 坐标系的位置 + Eigen::Vector3d p_G_P_{ + Eigen::Vector3d::Zero() + }; + + bool valid_{false}; + + Status last_status_{ + Status::NO_PERCEPTION + }; +}; + +} // namespace cmvr::perception + +#endif // CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H \ No newline at end of file diff --git a/cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp new file mode 100644 index 00000000..25501d8d --- /dev/null +++ b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp @@ -0,0 +1,315 @@ +#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h" + +#include + +namespace cmvr::perception { + +TagRelativeTcpPose::TagRelativeTcpPose( + const std::shared_ptr& perception) + : perception_(perception) +{ +} + +void TagRelativeTcpPose::setPerception( + const std::shared_ptr& perception) +{ + perception_ = perception; + clear(); + + if (!perception_) { + last_status_ = Status::NO_PERCEPTION; + } +} + +void TagRelativeTcpPose::setScreenTagId( + const int id) +{ + screen_tag_id_ = id; + valid_ = false; +} + +void TagRelativeTcpPose::setHandTagId( + const int id) +{ + hand_tag_id_ = id; + valid_ = false; +} + +bool TagRelativeTcpPose::update( + const Eigen::Matrix4d& T_H_P) +{ + valid_ = false; + + /* + * 1. 检查 perception + */ + if (!perception_) { + last_status_ = Status::NO_PERCEPTION; + return false; + } + + /* + * 2. 检查 Tag ID + */ + if (screen_tag_id_ < 0) { + last_status_ = Status::INVALID_SCREEN_TAG_ID; + return false; + } + + if (hand_tag_id_ < 0) { + last_status_ = Status::INVALID_HAND_TAG_ID; + return false; + } + + if (screen_tag_id_ == hand_tag_id_) { + last_status_ = Status::SAME_TAG_ID; + return false; + } + + /* + * 3. 检查外部传入的： + * + * ^H T_P + * + * Hand Tag -> TCP 固定标定矩阵。 + */ + if (!isValidTransform(T_H_P)) { + last_status_ = Status::INVALID_T_H_P; + return false; + } + + /* + * 4. 从 AprilTagPerception 当前缓存取 Screen Tag。 + * + * AprilTagPerception 已经通过 ViSP / PnP 得到： + * + * ^C T_G + */ + const auto* screen_tag = + perception_->findTag(screen_tag_id_); + + if (!screen_tag) { + last_status_ = + Status::SCREEN_TAG_NOT_FOUND; + return false; + } + + /* + * 5. 取 Hand Tag： + * + * ^C T_H + */ + const auto* hand_tag = + perception_->findTag(hand_tag_id_); + + if (!hand_tag) { + last_status_ = + Status::HAND_TAG_NOT_FOUND; + return false; + } + + /* + * 6. 保存当前帧两个原始视觉变换。 + * + * AprilTagPerception::Tag::T_c_t + * + * 定义是： + * + * Tag -> Camera + * + * 因此： + * + * screen tag: + * + * ^C T_G + * + * hand tag: + * + * ^C T_H + */ + T_C_G_ = screen_tag->T_c_t; + T_C_H_ = hand_tag->T_c_t; + + if (!isValidTransform(T_C_G_)) { + last_status_ = Status::INVALID_T_C_G; + return false; + } + + if (!isValidTransform(T_C_H_)) { + last_status_ = Status::INVALID_T_C_H; + return false; + } + + /* + * 7. 求 Hand Tag 相对于 Screen Tag 的位姿。 + * + * 已知： + * + * ^C T_G + * ^C T_H + * + * 因此： + * + * ^G T_H + * + * = (^C T_G)^-1 * ^C T_H + */ + T_G_H_ = + T_C_G_.inverse() * + T_C_H_; + + if (!isValidTransform(T_G_H_)) { + last_status_ = Status::INVALID_T_G_H; + return false; + } + + /* + * 8. 求 TCP 相对于 Screen Tag 的位姿。 + * + * 已知： + * + * ^G T_H + * ^H T_P + * + * 因此： + * + * ^G T_P + * + * = ^G T_H * ^H T_P + * + * = (^C T_G)^-1 + * * ^C T_H + * * ^H T_P + */ + T_G_P_ = + T_G_H_ * + T_H_P; + + if (!isValidTransform(T_G_P_)) { + last_status_ = Status::INVALID_T_G_P; + return false; + } + + /* + * 9. 提取 TCP 原点在 Screen Tag + * 坐标系 G 下的位置。 + * + * P_P^G = + * + * [ x_P ] + * [ y_P ] + * [ z_P ] + * + */ + p_G_P_ = + T_G_P_.block<3, 1>(0, 3); + + if (!p_G_P_.allFinite()) { + last_status_ = + Status::INVALID_TCP_POSITION; + return false; + } + + /* + * 10. 当前帧结果有效。 + */ + valid_ = true; + last_status_ = Status::OK; + + return true; +} + +void TagRelativeTcpPose::clear() +{ + T_C_G_.setIdentity(); + T_C_H_.setIdentity(); + + T_G_H_.setIdentity(); + T_G_P_.setIdentity(); + + p_G_P_.setZero(); + + valid_ = false; +} + +const char* TagRelativeTcpPose::statusToString( + const Status status) +{ + switch (status) { + + case Status::OK: + return "ok"; + + case Status::NO_PERCEPTION: + return "no_perception"; + + case Status::INVALID_SCREEN_TAG_ID: + return "invalid_screen_tag_id"; + + case Status::INVALID_HAND_TAG_ID: + return "invalid_hand_tag_id"; + + case Status::SAME_TAG_ID: + return "same_tag_id"; + + case Status::SCREEN_TAG_NOT_FOUND: + return "screen_tag_not_found"; + + case Status::HAND_TAG_NOT_FOUND: + return "hand_tag_not_found"; + + case Status::INVALID_T_C_G: + return "invalid_T_C_G"; + + case Status::INVALID_T_C_H: + return "invalid_T_C_H"; + + case Status::INVALID_T_H_P: + return "invalid_T_H_P"; + + case Status::INVALID_T_G_H: + return "invalid_T_G_H"; + + case Status::INVALID_T_G_P: + return "invalid_T_G_P"; + + case Status::INVALID_TCP_POSITION: + return "invalid_tcp_position"; + + default: + return "unknown"; + } +} + +bool TagRelativeTcpPose::isFiniteTransform( + const Eigen::Matrix4d& T) +{ + return T.allFinite(); +} + +bool TagRelativeTcpPose::hasValidHomogeneousBottomRow( + const Eigen::Matrix4d& T, + const double tolerance) +{ + return + std::abs(T(3, 0)) <= tolerance && + std::abs(T(3, 1)) <= tolerance && + std::abs(T(3, 2)) <= tolerance && + std::abs(T(3, 3) - 1.0) <= tolerance; +} + +bool TagRelativeTcpPose::isValidTransform( + const Eigen::Matrix4d& T) +{ + if (!isFiniteTransform(T)) { + return false; + } + + if (!hasValidHomogeneousBottomRow(T)) { + return false; + } + + return true; +} + +} // namespace cmvr::perception \ No newline at end of file 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/math/proto_geometry.h b/cmvr-es/common/math/proto_geometry.h index f065ef88..757e8616 100644 --- a/cmvr-es/common/math/proto_geometry.h +++ b/cmvr-es/common/math/proto_geometry.h @@ -27,6 +27,26 @@ inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src) return toEigenVec3(src, Eigen::Vector3d::Zero()); } +inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src, + Eigen::Vector3d defaults) +{ + if (src.has_rx()) { + defaults.x() = src.rx(); + } + if (src.has_ry()) { + defaults.y() = src.ry(); + } + if (src.has_rz()) { + defaults.z() = src.rz(); + } + return defaults; +} + +inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src) +{ + return toEigenEuler(src, Eigen::Vector3d::Zero()); +} + inline Eigen::Matrix toEigenVec6( const cmvr::common::Vec6& src, Eigen::Matrix defaults) @@ -102,6 +122,10 @@ inline bool hasVec3(const cmvr::common::Vec3& value) { return value.has_x() && value.has_y() && value.has_z(); } +inline bool hasEuler(const cmvr::common::Euler& value) { + return value.has_rx() && value.has_ry() && value.has_rz(); +} + inline bool hasVec6(const cmvr::common::Vec6& value) { return value.has_x() && value.has_y() && value.has_z() && value.has_rx() && value.has_ry() && value.has_rz(); 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/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index b6f8a0ed..74ffe854 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -65,6 +66,27 @@ struct CartesianVelocity { double wz{0.0}; }; +struct SpeedLOptions { + // Requested acceleration, capped separately by the arm's linear/angular maxima. + double acceleration{0.5}; + // Requested linear jerk (m/s^3), capped by the arm's configured maximum. + // Unset uses that maximum. + std::optional linear_jerk; + // Unset follows the arm config. False preserves the original streaming + // direction-following policy used by PBVS; true enables fixed-axis reversal. + std::optional continuous_linear_reversal; + // Capture the first applied command's measured TCP position and target + // direction in Base. Useful for distance-based motion without caller FK. + bool capture_reference{false}; +}; + +struct SpeedLReference { + bool valid{false}; + std::uint64_t command_version{0}; + CartesianPose tcp_pose_base{}; + CartesianVelocity target_base{}; +}; + struct CartesianWrench { double fx{0.0}; double fy{0.0}; @@ -113,6 +135,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/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index 714c5c9b..59b24597 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -3,8 +3,8 @@ arm { id: "right_arm" motor { - motor_system_id: "ti5_motors" - motor_group_ids: "right_arm_can" + motor_system_id: "right_arm_can_motors" + motor_group_ids: "right_arm_can_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" @@ -51,7 +51,6 @@ arm { gain: 0.2 margin_ratio: 0.15 max_push: 0.25 - weight: 0.05 } } } @@ -59,6 +58,14 @@ arm { motion { move_j { + # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。 + settle_timeout_s: 2.0 + # MoveJ 完成时允许的最大关节位置误差，单位为弧度。 + settle_position_tolerance_rad: 0.002 + # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。 + settle_velocity_tolerance_rad_s: 0.02 + # 位置和速度连续满足条件的采样次数。 + settle_stable_sample_count: 3 toppra_joint_motion_planner { path_type: TOPPRA_PATH_TYPE_QUINTIC sample_period_s: 0.001 @@ -144,9 +151,161 @@ arm { stop_command_velocity_norm: 1e-3 stop_measured_velocity_norm: 1e-2 stop_acceleration: 10 + # 等待 Cartesian 速度运动停止的最长时间，单位为秒。 + stop_timeout_s: 2.0 } } } } } + + robot_arms { + id: "left_arm" + + motor { + motor_system_id: "left_arm_can_motors" + motor_group_ids: "left_arm_can_motors" + dof: 7 + joint_names: "L_SHOULDER_P" + joint_names: "L_SHOULDER_R" + joint_names: "L_SHOULDER_Y" + joint_names: "L_ELBOW_R" + joint_names: "L_WRIST_P" + joint_names: "L_WRIST_Y" + joint_names: "L_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "L_WRIST_R_S" + tcp_frame_name: "L_FINGER_TIP_FIXED" + max_iters: 100 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-6 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } + joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.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 + } + } + } + } + + 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.01 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 45.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 45.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + } + + speed_l { + pinocchio_cartesian_motion_planner { + linear_velocity_max: 0.55 + linear_acceleration_max: 5.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.01 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 30.0 + max_joint_acceleration_rad_s2: 10000.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 5.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 5.0 + min_desired_linear_speed: 1e-4 + 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: 10 + } + } + } + } + } } 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..f5c8d8f5 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt @@ -0,0 +1,160 @@ +arm { + robot_arms { + id: "mujoco_right_arm" + + motor { + motor_system_id: "right_arm_mujoco_motors" + motor_group_ids: "right_arm_mujoco_motors" + 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 + } + } + } + } + + motion { + move_j { + # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。 + settle_timeout_s: 2.0 + # MoveJ 完成时允许的最大关节位置误差，单位为弧度。 + settle_position_tolerance_rad: 0.002 + # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。 + settle_velocity_tolerance_rad_s: 0.02 + # 位置和速度连续满足条件的采样次数。 + settle_stable_sample_count: 3 + 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 + # 等待 Cartesian 速度运动停止的最长时间，单位为秒。 + stop_timeout_s: 2.0 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt index 0d5604bd..6acc138b 100644 --- a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt +++ b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt @@ -3,8 +3,8 @@ arm { id: "mujoco_right_arm" motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "mujoco_right_arm" + motor_system_id: "right_arm_mujoco_motors" + motor_group_ids: "right_arm_mujoco_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" @@ -51,7 +51,6 @@ arm { gain: 0.2 margin_ratio: 0.15 max_push: 0.25 - weight: 2.0 } } } @@ -59,6 +58,14 @@ arm { motion { move_j { + # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。 + settle_timeout_s: 2.0 + # MoveJ 完成时允许的最大关节位置误差，单位为弧度。 + settle_position_tolerance_rad: 0.002 + # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。 + settle_velocity_tolerance_rad_s: 0.02 + # 位置和速度连续满足条件的采样次数。 + settle_stable_sample_count: 3 toppra_joint_motion_planner { path_type: TOPPRA_PATH_TYPE_QUINTIC sample_period_s: 0.001 @@ -144,6 +151,8 @@ arm { stop_command_velocity_norm: 1e-3 stop_measured_velocity_norm: 1e-2 stop_acceleration: 10 + # 等待 Cartesian 速度运动停止的最长时间，单位为秒。 + stop_timeout_s: 2.0 } } } diff --git a/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt index ef729d58..26b59ef0 100644 --- a/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt +++ b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt @@ -3,8 +3,8 @@ arm { id: "mujoco_right_arm" motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "mujoco_right_arm" + motor_system_id: "right_arm_mujoco_motors" + motor_group_ids: "right_arm_mujoco_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" @@ -52,7 +52,6 @@ arm { gain: 0.2 margin_ratio: 0.01 max_push: 0.02 - weight: 0.05 } } } @@ -60,6 +59,14 @@ arm { motion { move_j { + # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。 + settle_timeout_s: 2.0 + # MoveJ 完成时允许的最大关节位置误差，单位为弧度。 + settle_position_tolerance_rad: 0.002 + # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。 + settle_velocity_tolerance_rad_s: 0.02 + # 位置和速度连续满足条件的采样次数。 + settle_stable_sample_count: 3 toppra_joint_motion_planner { path_type: TOPPRA_PATH_TYPE_QUINTIC sample_period_s: 0.001 @@ -145,6 +152,8 @@ arm { stop_command_velocity_norm: 1e-3 stop_measured_velocity_norm: 1e-2 stop_acceleration: 5 + # 等待 Cartesian 速度运动停止的最长时间，单位为秒。 + stop_timeout_s: 2.0 } } } diff --git a/cmvr-es/config/devices/arm/arm_qp.pb.txt b/cmvr-es/config/devices/arm/arm_qp.pb.txt index 9e25391f..6ad6d6d7 100644 --- a/cmvr-es/config/devices/arm/arm_qp.pb.txt +++ b/cmvr-es/config/devices/arm/arm_qp.pb.txt @@ -3,8 +3,8 @@ arm { id: "right_arm" motor { - motor_system_id: "ti5_motors" - motor_group_ids: "right_arm_can" + motor_system_id: "right_arm_can_motors" + motor_group_ids: "right_arm_can_motors" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" @@ -52,7 +52,6 @@ arm { gain: 0.2 margin_ratio: 0.01 max_push: 0.02 - weight: 0.05 } } } @@ -60,6 +59,14 @@ arm { motion { move_j { + # MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。 + settle_timeout_s: 2.0 + # MoveJ 完成时允许的最大关节位置误差，单位为弧度。 + settle_position_tolerance_rad: 0.01 + # MoveJ 完成时允许的最大关节速度，单位为弧度/秒。 + settle_velocity_tolerance_rad_s: 0.02 + # 位置和速度连续满足条件的采样次数。 + settle_stable_sample_count: 3 toppra_joint_motion_planner { path_type: TOPPRA_PATH_TYPE_QUINTIC sample_period_s: 0.001 @@ -102,9 +109,9 @@ arm { speed_l { pinocchio_cartesian_motion_planner { - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 + linear_velocity_max: 0.8 + linear_acceleration_max: 10.0 + linear_jerk_max: 60.0 angular_velocity_max: 1.0 angular_acceleration_max: 5.0 angular_jerk_max: 12.0 @@ -130,9 +137,9 @@ arm { cartesian_velocity_feasibility_check { enable: true min_linear_speed_ratio: 0.2 - max_linear_direction_deviation_deg: 5.0 + max_linear_direction_deviation_deg: 70 min_angular_speed_ratio: 0.2 - max_angular_direction_deviation_deg: 5.0 + max_angular_direction_deviation_deg: 70 min_desired_linear_speed: 1e-4 min_desired_angular_speed: 1e-4 } @@ -144,7 +151,9 @@ arm { stop_twist_norm: 1e-9 stop_command_velocity_norm: 1e-3 stop_measured_velocity_norm: 1e-2 - stop_acceleration: 0.5 + stop_acceleration: 5 + # 等待 Cartesian 速度运动停止的最长时间，单位为秒。 + stop_timeout_s: 2.0 } } } diff --git a/cmvr-es/config/devices/camera/camera.pb.txt b/cmvr-es/config/devices/camera/camera.pb.txt index f21959ce..6d3e83c8 100644 --- a/cmvr-es/config/devices/camera/camera.pb.txt +++ b/cmvr-es/config/devices/camera/camera.pb.txt @@ -35,8 +35,8 @@ camera { stream_mode: STREAM_MODE_RGB } encoder { - width: 640 - height: 360 + width: 480 + height: 320 fps: 30 codec: "H264" enable_stream_timestamp: true @@ -48,21 +48,21 @@ camera { } cameras { - id: "cam3" + id: "left_hand_cam" realsense { serialNumber: "243122075614" camera_mode: CAMERA_MODE_VIDEO capture { - width: 640 - height: 480 + width: 1280 + height: 720 fps: 30 - stream_mode: STREAM_MODE_RGBD + stream_mode: STREAM_MODE_RGB } encoder { - width: 640 - height: 480 + width: 480 + height: 320 fps: 30 - codec: "H265" + codec: "H264" enable_stream_timestamp: true buffer_size: 30 } @@ -83,8 +83,8 @@ camera { stream_mode: STREAM_MODE_RGBD } encoder { - width: 1280 - height: 720 + width: 480 + height: 320 fps: 30 codec: "H264" enable_stream_timestamp: true @@ -92,7 +92,7 @@ camera { } consume_new_frame_only: false viewer_pip { - enable: true + enable: false left: -10 bottom: 10 width: 320 @@ -101,6 +101,36 @@ camera { } } + cameras { + id: "mujoco_external_touch_cam" + mujoco { + world_id: "mujoco_world" + camera_name: "external_touch_cam" + render { + width: 1280 + height: 720 + fps: 30 + stream_mode: STREAM_MODE_RGBD + } + encoder { + width: 480 + height: 320 + fps: 30 + codec: "H264" + enable_stream_timestamp: true + buffer_size: 30 + } + consume_new_frame_only: false + viewer_pip { + enable: false + left: 10 + bottom: 10 + width: 320 + height: 180 + } + } + } + cameras { id: "left_eye_cam" uvc { diff --git a/cmvr-es/config/devices/dexhand/dexhand.pb.txt b/cmvr-es/config/devices/dexhand/dexhand.pb.txt index 10e2f9f6..c1730107 100644 --- a/cmvr-es/config/devices/dexhand/dexhand.pb.txt +++ b/cmvr-es/config/devices/dexhand/dexhand.pb.txt @@ -2,7 +2,7 @@ dexhand { dexhands { id: "hand1" rh56dftp { - ip: "192.168.1.213" + ip: "192.168.1.223" port: 6000 poll_interval_ms: 10 } @@ -28,6 +28,8 @@ dexhand { resultant_length: 3 poll_interval_ms: 5 response_timeout_ms: 200 + # 触觉数据最大有效期；超过此时间报不可用，不继续返回旧力值。 + max_sample_age_ms: 50 response_header_bytes: 14 tactile_rows: 1 tactile_cols: 51 @@ -38,4 +40,9 @@ dexhand { auto_calibrate: false } } + + dexhands { + id: "mujoco_zero_touch_dexhand" + zero_sim_touch {} + } } 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..0d9432ec --- /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_motors" + 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..4f649ab1 --- /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_motors" + 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..448270e3 100644 --- a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt +++ b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt @@ -2,11 +2,10 @@ motor { id: "mujoco_motors" motor_groups { - id: "mujoco_right_arm" + id: "right_arm_mujoco_motors" 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..a45bafae --- /dev/null +++ b/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt @@ -0,0 +1,35 @@ +motor { + id: "mujoco_motors" + + motor_groups { + id: "right_arm_mujoco_motors" + 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..173d7b1c 100644 --- a/cmvr-es/config/devices/motor/ti5_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ti5_motors.pb.txt @@ -2,11 +2,10 @@ motor { id: "ti5_motors" motor_groups { - id: "left_arm_can" + id: "left_arm_can_motors" bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "L_FINGER_TIP" can { channel_id: 0 } @@ -16,22 +15,21 @@ 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 } } } motor_groups { - id: "right_arm_can" + id: "right_arm_can_motors" bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "R_FINGER_TIP" can { channel_id: 1 } @@ -47,18 +45,18 @@ 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 } } } motor_groups { - id: "head_can" + id: "head_can_motors" bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN @@ -73,14 +71,14 @@ 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 } } } motor_groups { - id: "waist_can" + id: "waist_can_motors" bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN @@ -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/devices/mujoco/right_arm_eye_to_hand_world.pb.txt b/cmvr-es/config/devices/mujoco/right_arm_eye_to_hand_world.pb.txt new file mode 100644 index 00000000..230ad18b --- /dev/null +++ b/cmvr-es/config/devices/mujoco/right_arm_eye_to_hand_world.pb.txt @@ -0,0 +1,7 @@ +worlds { + id: "mujoco_world" + model_path: "model/xiaoyan_description/right_arm_eye_to_hand.xml" + timestep_s: 0.001 + realtime_factor: 1.0 + require_actuator: true +} diff --git a/cmvr-es/config/logger/logger.pb.txt b/cmvr-es/config/logger/logger.pb.txt index 85f8aac0..5b624d49 100644 --- a/cmvr-es/config/logger/logger.pb.txt +++ b/cmvr-es/config/logger/logger.pb.txt @@ -30,7 +30,7 @@ logger { max_file_size_mb: 100 flush_interval_seconds: 1 format { - show_time: false + show_time: true show_level: true show_thread_id: false show_source_location: true diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 484f2c7a..c387cc85 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -2,16 +2,15 @@ device_manager { name: "cmvr_es" version: "0.1" description: "cmvr edge system version 0.1" - devices { id: "mujoco_world" type: DEVICE_TYPE_MUJOCO_WORLD - config_file: "devices/mujoco/mujoco_world.pb.txt" + config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt" enable: false } devices { - id: "mujoco_motors" + id: "right_arm_mujoco_motors" type: DEVICE_TYPE_MOTOR_SYSTEM config_file: "devices/motor/mujoco_motors.pb.txt" enable: false @@ -39,52 +38,95 @@ device_manager { } devices { - id: "right_hand_cam" + id: "mujoco_external_touch_cam" type: DEVICE_TYPE_CAMERA config_file: "devices/camera/camera.pb.txt" enable: false } devices { - id: "cam5" + id: "right_hand_cam" type: DEVICE_TYPE_CAMERA config_file: "devices/camera/camera.pb.txt" - enable: false + enable: true } + devices { + id: "left_hand_cam" + type: DEVICE_TYPE_CAMERA + config_file: "devices/camera/camera.pb.txt" + enable: true + } + + devices { id: "hand2" type: DEVICE_TYPE_DEXHAND config_file: "devices/dexhand/dexhand.pb.txt" - enable: false + enable: true } devices { id: "paxini_tip_1" type: DEVICE_TYPE_DEXHAND config_file: "devices/dexhand/dexhand.pb.txt" + enable: true + } + + devices { + id: "mujoco_zero_touch_dexhand" + type: DEVICE_TYPE_DEXHAND + config_file: "devices/dexhand/dexhand.pb.txt" enable: false } devices { - id: "ti5_motors" + id: "left_arm_can_motors" type: DEVICE_TYPE_MOTOR_SYSTEM config_file: "devices/motor/ti5_motors.pb.txt" enable: false } + devices { + id: "right_arm_can_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ti5_motors.pb.txt" + enable: true + } + + devices { + id: "head_can_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ti5_motors.pb.txt" + enable: false + } + + devices { + id: "waist_can_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ti5_motors.pb.txt" + enable: false + } + + devices { + id: "right_arm_ethercat_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ethercat_motors.pb.txt" + enable: false + } + devices { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/arm.pb.txt" - enable: false + config_file: "devices/arm/arm_qp.pb.txt" + enable: true } devices { id: "aubo_arm" type: DEVICE_TYPE_ROBOT_ARM config_file: "devices/arm/aubo_arm.pb.txt" - enable: true + enable: false } devices { @@ -107,12 +149,4 @@ device_manager { config_file: "devices/agv/agv.pb.txt" enable: false } - - devices { - # 仙工控制器实例；与 agv.pb.txt 中的设备 ID 保持一致。 - id: "src1100" - type: DEVICE_TYPE_AGV - config_file: "devices/agv/agv.pb.txt" - enable: true - } } diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index db13dfa9..21293504 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -5,7 +5,7 @@ task_manager { run_mode: TASK_RUN_MODE_PERIODIC_STEP control_period_s: 0.001 config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt" - enable: false + enable: true } tasks { id: "grpc_server" @@ -14,4 +14,12 @@ 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 + } } 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/config/tasks/touch_screen_task/touch_screen_task.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt index a9e8ad72..69820cdd 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt @@ -1,90 +1,109 @@ touch_screen_task { id: "touch_screen" + # 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。 + debug_draw_coordinate_frames: true + # G/H 坐标轴长度，单位为米。 + debug_coordinate_axis_length_m: 0.02 devices { arm_id: "right_arm" dexhand_id: "paxini_tip_1" + # 手部相机和外部相机的 DeviceManager ID。 camera_id: "right_hand_cam" + external_camera_id: "left_hand_cam" } initialization { - before_start: true + before_start: false after_finish: true - joint_positions { joint_name: "R_SHOULDER_P" rad: -0.3678 } - joint_positions { joint_name: "R_SHOULDER_R" rad: 1.1127 } - joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.6084 } - joint_positions { joint_name: "R_ELBOW_R" rad: 1.61 } - joint_positions { joint_name: "R_WRIST_P" rad: -2.5718 } - joint_positions { joint_name: "R_WRIST_Y" rad: 0.1276 } - joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 } + joint_positions { joint_name: "R_SHOULDER_P" rad: -0.338732 } + joint_positions { joint_name: "R_SHOULDER_R" rad: 1.265943 } + joint_positions { joint_name: "R_SHOULDER_Y" rad: 1.572396 } + joint_positions { joint_name: "R_ELBOW_R" rad: 1.5464 } + joint_positions { joint_name: "R_WRIST_P" rad: -2.8179 } + joint_positions { joint_name: "R_WRIST_Y" rad: 0.1105 } + joint_positions { joint_name: "R_WRIST_R" rad: 0.1347 } velocity: 1.0 acceleration: 2.0 + # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。 + skip_position_tolerance_rad: 0.01 + # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。 + skip_velocity_tolerance_rad_s: 0.1 } perception { - apriltag { - tag_size_m: 0.012 + # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。 + tags { + screen { + id: 14 + size_m: 0.016 + } + hand { + id: 16 + size_m: 0.016 + } + } + # 手部相机：用于点击目标点和手部目标跟踪。 + hand_camera { depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE } } alignment { - ibvs { - camera_link: "R_CAM" - lambda: 0.4 - mu: 0.1 - qdot_max: 1.0 - vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 } - amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 } + calibration { + # TCP P 相对于 Hand Tag H 的目标姿态 + hand_tag_to_tcp { + m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.035 + m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.09 + m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.18 + m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0 + } + } + pbvs { + position_gain { x: 1.0 y: 2.0 z: 1.0 } + rotation_gain { x: 1.0 y: 1.5 z: 1.0 } + vmax6 { x: 0.04 y: 0.04 z: 0.04 rx: 0.050 ry: 0.050 rz: 0.050 } + amax6 { x: 0.80 y: 0.80 z: 0.80 rx: 2.0 ry: 2.0 rz: 2.0 } twist_filter_alpha: 1.0 - r_camera_to_visp { - m00: 1.0 m01: 0.0 m02: 0.0 - m10: 0.0 m11: 1.0 m12: 0.0 - m20: 0.0 m21: 0.0 m22: 1.0 - } - r_camera_to_urdf { - m00: 1.0 m01: 0.0 m02: 0.0 - m10: 0.0 m11: 1.0 m12: 0.0 - m20: 0.0 m21: 0.0 m22: 1.0 - } - control_joint_names: "R_SHOULDER_P" - control_joint_names: "R_SHOULDER_R" - control_joint_names: "R_SHOULDER_Y" - control_joint_names: "R_ELBOW_R" - control_joint_names: "R_WRIST_P" - control_joint_names: "R_WRIST_Y" - control_joint_names: "R_WRIST_R" } target { - position_in_camera { x: -0.001 y: 0.08 z: 0.15 } - rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 } + # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。 + # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。 + # 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。 + # PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。 + hand_orientation_G { rx: 0.0 ry: 0.0 rz: -1.57 } + # 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。 + position_offset_G { x: 0.0 y: 0.0 z: 0.00 } mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION } error_threshold { x: 0.005 y: 0.005 - z: 0.01 - rx: 0.1026646259971647 - ry: 0.1026646259971647 + z: 0.005 + rx: 0.0126646259971647 + ry: 0.0126646259971647 rz: 0.1026646259971647 } stable_frames: 2 + # 对齐及到位暂停各自的超时时间（秒）；暂停从对齐成功时重新计时。 timeout_s: 20.0 + # 到位后暂停；暂停超时结束任务，after_finish 为 true 时尝试回初始位置。 pause_when_reached: false } touch { speed_l { - twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } - acceleration: 6.0 - max_distance_m: 0.035 + twist_tool { x: 0.0 y: -0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } + acceleration: 5.0 + linear_jerk: 10.0 + max_distance_m: 0.03 } tactile { finger: TOUCH_SCREEN_FINGER_TYPE_INDEX region: TOUCH_SCREEN_TACTILE_REGION_TIP criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ - force_threshold: 1.0 + force_threshold: 0.1 # N } dwell_time_s: 0.0 } @@ -92,6 +111,8 @@ touch_screen_task { retract { twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } acceleration: 8.0 - duration_s: 0.45 + linear_jerk: 40.0 # m/s^3，约束接触后的减速和连续换向。 + # TCP 后退目标距离，单位为米。 + distance_m: 0.01 } } diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt index d607f7e4..5bacf38f 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt @@ -1,10 +1,16 @@ touch_screen_task { id: "touch_screen" + # 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。 + debug_draw_coordinate_frames: true + # G/H 坐标轴长度，单位为米。 + debug_coordinate_axis_length_m: 0.02 devices { - arm_id: "right_arm_mujoco" + arm_id: "mujoco_right_arm" dexhand_id: "mujoco_zero_touch_dexhand" - camera_id: "hand_cam" + # 手部相机和外部相机的 DeviceManager ID。 + camera_id: "mujoco_hand_cam" + external_camera_id: "mujoco_external_touch_cam" } initialization { @@ -17,60 +23,81 @@ touch_screen_task { joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 } joint_positions { joint_name: "R_WRIST_Y" rad: 0.1150 } joint_positions { joint_name: "R_WRIST_R" rad: -0.08 } - velocity: 2.8 - acceleration: 20.0 + velocity: 2.0 + acceleration: 3.0 + # 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。 + skip_position_tolerance_rad: 0.001 + # 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。 + skip_velocity_tolerance_rad_s: 0.01 } perception { - apriltag { - tag_size_m: 0.12 + # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。 + tags { + screen { + id: 1 + size_m: 0.03 + } + hand { + id: 0 + size_m: 0.03 + } + } + # 手部相机：用于点击目标点和手部目标跟踪。 + hand_camera { + depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE } } alignment { - ibvs { - camera_link: "R_CAM" - lambda: 0.4 - mu: 0.1 - qdot_max: 0.8 - vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 } - amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 } + calibration { + # TCP P 相对于屏幕 Hand Tag H 的目标姿态 + hand_tag_to_tcp { + m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0 + m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0 + m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03 + m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0 + } + } + pbvs { + position_gain { x: 2.0 y: 2.0 z: 1.5 } + rotation_gain { x: 1.5 y: 1.5 z: 1.5 } + vmax6 { x: 0.10 y: 0.10 z: 0.05 rx: 0.50 ry: 0.50 rz: 0.50 } + amax6 { x: 0.50 y: 0.50 z: 0.30 rx: 2.0 ry: 2.0 rz: 2.0 } twist_filter_alpha: 1.0 - r_camera_to_visp { - m00: 1.0 m01: 0.0 m02: 0.0 - m10: 0.0 m11: -1.0 m12: 0.0 - m20: 0.0 m21: 0.0 m22: -1.0 - } - r_camera_to_urdf { - m00: 1.0 m01: 0.0 m02: 0.0 - m10: 0.0 m11: -1.0 m12: 0.0 - m20: 0.0 m21: 0.0 m22: -1.0 - } - control_joint_names: "R_SHOULDER_P" - control_joint_names: "R_SHOULDER_R" - control_joint_names: "R_SHOULDER_Y" - control_joint_names: "R_ELBOW_R" - control_joint_names: "R_WRIST_P" - control_joint_names: "R_WRIST_Y" - control_joint_names: "R_WRIST_R" } target { - position_in_camera { x: 0.0 y: 0.0 z: 0.30 } - rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 } - mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY + # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。 + # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。 + # 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。 + # PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。 + hand_orientation_G { + rx: 0.0 + ry: 0.0 + rz: 3.141592653589793 + } + # 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。 + position_offset_G { + x: 0.0 + y: 0.0 + z: 0.05 + } + mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION } error_threshold { x: 0.005 y: 0.005 - z: 0.010 + z: 0.005 rx: 0.08726646259971647 ry: 0.08726646259971647 rz: 0.08726646259971647 } stable_frames: 5 + # 对齐及到位暂停各自的超时时间（秒）；暂停从对齐成功时重新计时。 timeout_s: 20.0 + # 到位后暂停；暂停超时结束任务，after_finish 为 true 时尝试回初始位置。 pause_when_reached: false } @@ -78,20 +105,23 @@ touch_screen_task { speed_l { twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } acceleration: 6.0 - max_distance_m: 0.12 + linear_jerk: 60.0 # m/s^3，接近阶段请求值，受机械臂 linear_jerk_max 限制。 + max_distance_m: 0.02 } tactile { finger: TOUCH_SCREEN_FINGER_TYPE_INDEX region: TOUCH_SCREEN_TACTILE_REGION_TIP criterion: TOUCH_SCREEN_TACTILE_CRITERION_FZ - force_threshold: 1.0 + force_threshold: 0.1 # N } dwell_time_s: 0.0 } retract { - twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } - acceleration: 8.0 - duration_s: 5.0 + twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } + acceleration: 4.0 + linear_jerk: 60.0 # m/s^3，保持仿真机械臂原有的 jerk 上限。 + # TCP 后退目标距离，单位为米。 + distance_m: 0.05 } } diff --git a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h index da392f79..de31b00d 100644 --- a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h @@ -52,6 +52,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 AuboArm"); + } Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } bool isProtectiveStopped() const override { return false; } diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 1ea0da0e..850f22b9 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -48,6 +48,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; diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt index c0b7d99b..a3270cd1 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -19,6 +19,11 @@ target_link_libraries(motor_robot_arm add_library(cmvr_es::device::motor_robot_arm ALIAS motor_robot_arm) install(TARGETS motor_robot_arm LIBRARY DESTINATION lib) +add_executable(speedl_reversal_mujoco_test src/speedl_reversal_mujoco_test.cpp) +target_link_libraries(speedl_reversal_mujoco_test PRIVATE + cmvr_es::device::motor_robot_arm cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver cmvr_es::proto pthread) + add_executable(motor_robot_arm_mujoco_test src/motor_robot_arm_mujoco_test.cpp ) @@ -34,3 +39,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/SPEEDL_REVERSAL.md b/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md new file mode 100644 index 00000000..561872b9 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md @@ -0,0 +1,79 @@ +# speedL 连续换向与 MuJoCo 验证 + +触控接近和回撤的同轴线速度换向使用固定轴上的有符号 S 曲线，从当前速度和加速度直接规划到反向目标。过零时保留加速度，不重置规划器。该模式下非同轴变向先停止再换轴；角速度仍使用原有策略。 + +`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true；`SpeedLOptions.continuous_linear_reversal` 可按命令覆盖。触控任务的 PBVS 对齐明确设为 false，保持原有的小角度方向跟随和低速换轴行为；接近及回撤设为 true。固定轴模式不适合方向不断变化的视觉对齐：小方向变化会累积到换轴阈值，导致反复停走。设为 false 也可用于与旧换向策略做同条件比较。运动中退出固定轴模式时会保留实际运动方向和加速度。 + +`MotorRobotArm::speedL(velocity, SpeedLOptions, duration, frame)` 可以为本条命令指定 `linear_jerk`（m/s³）。未指定时使用机器人配置的 jerk；普通 `speedL(velocity, acceleration, duration, frame)` 仍可直接使用。控制线程将速度、加速度、jerk 作为同一命令读取。正常停止的收尾操作检查命令版本，避免清除之后提交的运动命令。 + +机械臂的 `SpeedLPlannerConfig` 提供规划上限：速度保持按配置限幅；线加速度、角加速度分别取请求 `acceleration` 与各自配置上限的较小值；线性 jerk 取请求值与 `linear_jerk_max` 的较小值。省略 jerk 时使用配置上限。普通 `stopL` 的加速度也遵循此规则。未配置或无效的上限继续使用规划器默认值（线性 0.55/5/10，角向 1/5/12）。 + +例如机械臂配置加速度 5、jerk 10 时，回撤请求 60/60 实际按 5/10 规划，请求 3/6 则按 3/6 规划。任务参数不能提高机械臂上限。 + +接近阶段也可在 `touch.speed_l` 内配置 `linear_jerk`（m/s³），与 `retract.linear_jerk` 独立设置。两者都必须为有限正数，省略时使用机械臂上限；执行时都取请求值与上限的较小值。配置示例： + +```protobuf +touch { + speed_l { + twist_tool { x: 0 y: -0.08 z: 0 rx: 0 ry: 0 rz: 0 } + acceleration: 5.0 + linear_jerk: 10.0 + max_distance_m: 0.03 + } + # tactile 和 dwell_time_s 等其他必填项仍按任务配置填写。 +} +``` + +`capture_reference=true` 时，控制线程从第一拍关节状态计算 TCP 位置及 Base 下的目标方向，成功发送速度后发布 `SpeedLReference`；Tool 目标随后固定到这一 Base 方向。新命令提交时旧参考立即失效。尚不支持这些选项的机械臂后端明确返回不支持。 + +触屏任务在 `dwell_time_s=0` 时检测到阈值就提交回撤，随后才记录日志，跳过停止/停留中间目标。回撤距离为沿回撤方向的有符号位移，继续前压不会算成回撤。`retract.linear_jerk` 控制整个减速、过零及反向加速过程，并受机械臂上限限制。实机触控任务当前接近和回撤都请求 10 m/s³；DLS 机械臂配置上限为 10，QP 配置上限为 30，MuJoCo 配置上限为 60 m/s³，执行时取所加载配置与任务请求的较小值。 + +触屏任务的 `[RETRACT]`、`[RETRACT_DONE]` 日志包含 `max_forward_after_retract_mm`：以回撤首个控制周期锁存的实测 TCP 为起点，统计沿回撤反方向的最大正位移，单位毫米。每次回撤清零，后续后退不抵消已记录的峰值。统计复用任务每次回撤步骤的 FK 位置采样，不额外阻塞触觉触发后的命令提交。它是采样到的最大值，不包括触发到首个回撤控制周期之前的移动，也不包括非零 dwell 阶段的移动；短暂峰值可能落在两次任务采样之间。已取得回撤参考后发生失败时，`[RETRACT_FAILED]` 也会打印已记录的最大值。 + +## 实际仿真测试 + +测试创建 MuJoCo 世界、MuJoCo 电机组和 MotorRobotArm，不创建真实硬件或视觉任务。使用 `dual_arm.xml` 的位置执行器驱动动力学；没有通过直接修改关节状态模拟运动。 + +先 MoveJ 到已有运动测试姿态，然后调用 `speedL(vy=+0.08)`，运动过程中直接调用 `speedL(vy=-0.08)`。默认 Base 坐标系，`--tool-frame true` 可验证 Tool 坐标系。测量来自 MuJoCo `R_FINGER_TIP_SITE` 的位置和雅可比乘实际 qvel；不是规划速度的积分。 + +在仓库根目录运行（需要已配置 `cmake-build-debug`；可用 `CMVR_BUILD_DIR` 覆盖）： + +```bash +# 匀速阶段换向：接近加速度 5，回撤加速度 3，jerk 均为 10。 +./script/test_speedl_reversal_mujoco.sh --csv /tmp/reversal-new.csv + +# 同一新版本中选择旧换向策略，比较算法本身。 +./script/test_speedl_reversal_mujoco.sh --legacy true --csv /tmp/reversal-legacy.csv + +# 仍在加速时，命令 vy 首次达到 0.032 m/s 即反向。 +./script/test_speedl_reversal_mujoco.sh --trigger-speed 0.032 --csv /tmp/reversal-accelerating.csv + +# 上限仍为 10，回撤请求 60 将被限制为 10。 +./script/test_speedl_reversal_mujoco.sh --reverse-jerk 60 --reverse-acceleration 60 \ + --capture-reference true --csv /tmp/reversal-capped.csv + +# 将仿真机械臂 jerk 上限设为 60；接近请求 10，回撤请求 60。 +./script/test_speedl_reversal_mujoco.sh --jerk 60 --approach-jerk 10 \ + --reverse-jerk 60 --capture-reference true \ + --csv /tmp/reversal-jerk60.csv + +# Tool 坐标系和触屏任务使用的参数接口。 +./script/test_speedl_reversal_mujoco.sh --tool-frame true --jerk 60 --approach-jerk 10 --reverse-jerk 60 \ + --capture-reference true --csv /tmp/reversal-tool.csv +``` + +默认前进 500 ms 后换向；`--approach-ms` 可修改，`--trigger-speed` 非零时优先按命令速度触发。默认速度为 0.08 m/s，可用 `--speed` 修改；`--jerk` 设置仿真机械臂 jerk 上限，`--approach-jerk` 和 `--reverse-jerk` 分别设置接近、回撤请求（省略时使用上限）。`--reverse-acceleration` 设置回撤加速度请求，默认 3，机械臂上限为 5。输出同时标注请求值和限幅值。测试采样目标周期和仿真步长均为 1 ms；线程由操作系统调度，CSV 同时记录 wall time 和 simulation time。 + +`max_forward_mm` 是反向调用前采样点之后，实际 TCP 沿前进轴的最大正位移；`peak_at_ms` 是到达该最远点的时间；`actual_reverse_ms` 要求至少连续 5 次采样的轴向速度小于 -0.0001 m/s；`return_to_origin_ms` 是确认反向后返回调用时位置的时间。Tool 模式的 CSV 速度字段也表示沿锁定前进轴的投影。 + +测试要求换向前实际速度为正、之后产生反向速度并退回起点，且控制器未提前退出；使用起点锁存时还要求参考有效。失败返回非零。该自由空间实验不包含屏幕接触、触觉延迟或硅胶形变，不能把返回起点时间直接当作屏幕抬起事件时间。 + +## 回归测试 + +构建并运行 `cartesian_twist_limiter_reversal_test`、`s_curve_velocity_planner_stop_test` 和 `cartesian_velocity_controller_test`。覆盖匀速/加速中换向、速度与 jerk 连续性、恰好过零、普通停止、非同轴转向、负速度反馈、停止/回撤竞争、成功发送后才发布回撤参考及非法参数拒绝。 + +`pinocchio_speedl_limits_test` 使用真实 URDF 和生产规划器，采样输出速度并差分验证实际规划的速度、加速度、jerk 上限；覆盖超限请求、较小请求、省略 jerk、旧接口及停止、独立角加速度限幅和非法请求。测试不初始化电机。 + +对齐回归覆盖每 20 ms 小幅更新速度方向：显式关闭固定轴模式后，20 mm/s 的命令不会因方向变化反复降到零；同时验证对齐后启用连续换向仍能平滑过零，以及反向运动中退出固定轴模式不会翻转实际速度。 + +运行时应优先加载当前构建的项目动态库，测试脚本已处理；不要把新可执行文件与 `output/lib` 的旧项目库混用。 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..bd3f5c8d 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; @@ -59,6 +62,9 @@ public: double duration, FrameType frame = FrameType::Base) override; Result stopL(std::optional acceleration = std::nullopt) override; + Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options, + double duration, FrameType frame = FrameType::Base) override; + SpeedLReference getSpeedLReference() const override; Result stopMotion() override; Result startServoMode(const ServoOptions& options) override; @@ -73,10 +79,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,11 +100,17 @@ 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; bool readArmState_(std::vector& q_now, std::vector& qd_now) const; std::vector readJointPosition_() const; + Result stopCartesianMotionAndWait_(); + Result waitForJointTarget_(const std::vector& target) const; bool configureAlgorithms_(); bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory); @@ -123,10 +135,20 @@ private: std::shared_ptr cartesian_planner_{nullptr}; std::unique_ptr cartesian_velocity_controller_{nullptr}; + // MoveJ post-trajectory settling criteria. These defaults preserve the + // historical behavior when the optional arm configuration fields are absent. + double move_j_settle_timeout_s_{2.0}; + double move_j_position_tolerance_rad_{2e-3}; + double move_j_velocity_tolerance_rad_s_{2e-2}; + int move_j_stable_sample_count_{3}; + 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..f19328b2 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,7 +1,10 @@ #include "arm/motor_robot_arm/include/motor_robot_arm.h" +#include #include +#include #include +#include #include #include #include @@ -28,6 +31,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 +140,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 +197,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 +213,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 +229,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 +272,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 +507,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); @@ -262,13 +517,23 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti if (!joint_planner_) { return Result::failure(ArmErrorCode::RobotNotReady, "joint planner is not initialized"); } + + // speedL runs in a worker thread and sends speedJ commands. A moveJ + // trajectory writes cyclic-position commands directly, so allowing both + // paths to run concurrently can overwrite the motor mode/target and cause + // a short surge at the transition. Finish the Cartesian worker before + // reading the planning start state. + const auto cartesian_stop = stopCartesianMotionAndWait_(); + if (!cartesian_stop.ok()) { + return cartesian_stop; + } if (busy_.exchange(true)) { return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_); } 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,29 +549,66 @@ 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); + // A MoveJ is a rest-to-rest command. Do not let a non-zero numerical + // endpoint velocity from an alternate planner keep the drive moving + // while the position-mode trajectory is being handed back to the arm. + if (k + 1 < samples.size()) { + 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))); } } + + // Repeat the final position with zero velocity before checking feedback. + // This seeds the position-mode target after a velocity-to-position switch + // and prevents a stale final velocity command from producing a short + // motion spike at the destination. + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, target.position, std::vector(motors.size(), 0.0))) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to hold final moveJ target"); + } + // Allow one servo cycle to consume the explicit hold command before + // evaluating feedback-based settling criteria. + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + const auto settle_result = waitForJointTarget_(target.position); + if (!settle_result.ok()) { + return settle_result; + } return Result::success(); } @@ -315,6 +617,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 +634,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 +657,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,8 +669,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame) { - if (cartesian_velocity_controller_) { - cartesian_velocity_controller_->shutdown(); + if (const auto stopped = safetyStopResult_("moveL")) { + return *stopped; + } + const auto cartesian_stop = stopCartesianMotionAndWait_(); + if (!cartesian_stop.ok()) { + return cartesian_stop; } if (options.asynchronous) { return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported"); @@ -394,8 +714,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 +728,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"); } @@ -420,9 +748,27 @@ Result MotorRobotArm::stopL(const std::optional acceleration) return cartesian_velocity_controller_->stop(acceleration); } +Result MotorRobotArm::speedL(const CartesianVelocity& velocity, const SpeedLOptions& options, + const double duration, const FrameType frame) +{ + if (const auto stopped = safetyStopResult_("speedL")) return *stopped; + if (busy_.load() || !cartesian_velocity_controller_) { + return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy or velocity controller is unavailable"); + } + return cartesian_velocity_controller_->speedL(velocity, options, duration, frame); +} + +SpeedLReference MotorRobotArm::getSpeedLReference() const +{ + return cartesian_velocity_controller_ ? cartesian_velocity_controller_->getReference() : SpeedLReference{}; +} + Result MotorRobotArm::stopMotion() { - stopL(0.0); + const auto cartesian_stop = stopCartesianMotionAndWait_(); + if (!cartesian_stop.ok()) { + return cartesian_stop; + } return stopJ(0.0); } @@ -442,12 +788,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 +806,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(); } @@ -656,8 +1016,181 @@ std::vector MotorRobotArm::readJointPosition_() const return q_start; } +Result MotorRobotArm::stopCartesianMotionAndWait_() +{ + if (!cartesian_velocity_controller_) { + return Result::success(); + } + + if (cartesian_velocity_controller_->busy()) { + const auto stop_result = cartesian_velocity_controller_->stop(); + if (!stop_result.ok()) { + return stop_result; + } + + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::duration( + cartesian_velocity_controller_->stopTimeoutS()); + while (cartesian_velocity_controller_->busy() && + std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (cartesian_velocity_controller_->busy()) { + // Do not start a position trajectory while the worker can still + // issue velocity commands. Shutdown joins it and sends one final + // zero-velocity command before reporting the timeout. + cartesian_velocity_controller_->shutdown(); + return Result::failure( + ArmErrorCode::Timeout, + "timed out waiting for Cartesian velocity motion to stop"); + } + } + + // The worker may be idle but still joinable. Joining it here removes any + // last command/worker race before the next motion mode is selected. + cartesian_velocity_controller_->shutdown(); + return Result::success(); +} + +Result MotorRobotArm::waitForJointTarget_(const std::vector& target) const +{ + if (target.size() != joint_names_.size()) { + return Result::failure(ArmErrorCode::InvalidArgument, + "moveJ target size mismatch while settling"); + } + + struct JointFeedback { + double position{0.0}; + double velocity{0.0}; + }; + std::vector last_feedback(joint_names_.size()); + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::duration(move_j_settle_timeout_s_); + std::size_t sample_count = 0; + int stable_samples = 0; + int max_stable_samples = 0; + while (std::chrono::steady_clock::now() < deadline) { + if (const auto stopped = safetyStopResult_("moveJ", true)) { + return *stopped; + } + + bool settled = true; + for (std::size_t i = 0; i < joint_names_.size(); ++i) { + const auto motor = getMotor_(joint_names_[i]); + if (!motor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "motor not found while waiting for moveJ target: " + joint_names_[i]); + } + const double position = motor->getQ(); + const double velocity = motor->getQd(); + last_feedback[i] = {position, velocity}; + if (!std::isfinite(position) || !std::isfinite(velocity) || + std::abs(position - target[i]) > move_j_position_tolerance_rad_ || + std::abs(velocity) > move_j_velocity_tolerance_rad_s_) { + settled = false; + } + } + + ++sample_count; + stable_samples = settled ? stable_samples + 1 : 0; + max_stable_samples = std::max(max_stable_samples, stable_samples); + if (stable_samples >= move_j_stable_sample_count_) { + return Result::success(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + CMVR_LOG(ERROR) << "[MotorRobotArm] moveJ target did not settle before timeout: " << id_ + << ", timeout_s=" << move_j_settle_timeout_s_ + << ", sample_count=" << sample_count + << ", stable_samples=" << stable_samples + << ", required_stable_samples=" << move_j_stable_sample_count_ + << ", max_stable_samples=" << max_stable_samples + << ", reason=" << (sample_count == 0 ? "no_feedback_samples" + : stable_samples > 0 ? "insufficient_stable_samples" + : "joint_feedback_not_settled"); + // Report the feedback from the final check, rather than reading newer + // values that may no longer explain the timeout. Include every joint so + // valid axes and non-finite feedback are distinguishable in the same sample. + for (std::size_t i = 0; sample_count > 0 && i < joint_names_.size(); ++i) { + const auto& feedback = last_feedback[i]; + const double position_error = std::abs(feedback.position - target[i]); + const bool position_ok = std::isfinite(feedback.position) && + position_error <= move_j_position_tolerance_rad_; + const bool velocity_ok = std::isfinite(feedback.velocity) && + std::abs(feedback.velocity) <= move_j_velocity_tolerance_rad_s_; + CMVR_LOG(ERROR) << std::setprecision(12) + << "[MotorRobotArm][MOVEJ_SETTLE_CHECK] arm=" << id_ + << ", joint=" << joint_names_[i] + << ", target_rad=" << target[i] + << ", actual_rad=" << feedback.position + << ", abs_position_error_rad=" << position_error + << ", position_tolerance_rad=" << move_j_position_tolerance_rad_ + << ", position_ok=" << (position_ok ? "true" : "false") + << ", velocity_rad_s=" << feedback.velocity + << ", velocity_tolerance_rad_s=" << move_j_velocity_tolerance_rad_s_ + << ", velocity_ok=" << (velocity_ok ? "true" : "false"); + } + return Result::failure(ArmErrorCode::Timeout, + "timed out waiting for moveJ target to settle"); +} + bool MotorRobotArm::configureAlgorithms_() { + // Optional settling fields are read once during initialization so every + // MoveJ command uses one consistent set of safety criteria. + move_j_settle_timeout_s_ = 2.0; + move_j_position_tolerance_rad_ = 2e-3; + move_j_velocity_tolerance_rad_s_ = 2e-2; + move_j_stable_sample_count_ = 3; + const auto& move_j_config = cfg_.motion().move_j(); + if (move_j_config.has_settle_timeout_s()) { + const double value = move_j_config.settle_timeout_s(); + if (!std::isfinite(value) || value <= 0.0) { + CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_timeout_s: " << value; + return false; + } + move_j_settle_timeout_s_ = value; + } + if (move_j_config.has_settle_position_tolerance_rad()) { + const double value = move_j_config.settle_position_tolerance_rad(); + if (!std::isfinite(value) || value < 0.0) { + CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_position_tolerance_rad: " + << value; + return false; + } + move_j_position_tolerance_rad_ = value; + } + if (move_j_config.has_settle_velocity_tolerance_rad_s()) { + const double value = move_j_config.settle_velocity_tolerance_rad_s(); + if (!std::isfinite(value) || value < 0.0) { + CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_velocity_tolerance_rad_s: " + << value; + return false; + } + move_j_velocity_tolerance_rad_s_ = value; + } + if (move_j_config.has_settle_stable_sample_count()) { + const int value = move_j_config.settle_stable_sample_count(); + if (value < 1) { + CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_stable_sample_count: " + << value; + return false; + } + move_j_stable_sample_count_ = value; + } + + const auto& cartesian_controller_config = + cfg_.motion().speed_l().speed_l_controller().cartesian_velocity_controller(); + if (cartesian_controller_config.has_stop_timeout_s() && + (!std::isfinite(cartesian_controller_config.stop_timeout_s()) || + cartesian_controller_config.stop_timeout_s() <= 0.0)) { + CMVR_LOG(ERROR) << "[MotorRobotArm] invalid Cartesian stop_timeout_s: " + << cartesian_controller_config.stop_timeout_s(); + return false; + } + joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j()); if (!joint_planner_) { return false; @@ -725,7 +1258,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 +1268,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)); @@ -765,6 +1303,10 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController result.stop_acceleration = config.stop_acceleration() > 0.0 ? config.stop_acceleration() : result.stop_acceleration; + if (config.has_stop_timeout_s() && std::isfinite(config.stop_timeout_s()) && + config.stop_timeout_s() > 0.0) { + result.stop_timeout_s = config.stop_timeout_s(); + } return result; } 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..9feebb00 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp @@ -0,0 +1,871 @@ +#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 "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)); + + motor_system_ = std::make_shared( + "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors"); + ASSERT_TRUE(motor_system_->init()); + world_ = MotorManager::mujocoWorldFor("right_arm_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(); + } + + 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/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp index 5ec14ee0..82e473f9 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include @@ -147,17 +146,10 @@ protected: (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.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()); + motor_system_ = std::make_shared( + "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors"); ASSERT_NO_THROW(motor_system_->init()); - world_ = MotorManager::mujocoWorldFor("mujoco_motors"); + world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors"); ASSERT_TRUE(world_); ASSERT_TRUE(world_->isLoaded()); @@ -187,7 +179,6 @@ protected: if (world_device_) { world_device_->stop(); } - MotorManager::clearActiveJoints(); } std::filesystem::path project_root_; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_speedl_stop_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_speedl_stop_test.cpp new file mode 100644 index 00000000..49b61bcf --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_speedl_stop_test.cpp @@ -0,0 +1,352 @@ +#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 "algorithms/kinematics/ik_solver/ik_solver_factory.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "common/io/proto_file_io.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" + +namespace cmvr::device { +namespace { + +constexpr std::size_t kDof = 7; +constexpr std::array kJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", + "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; +constexpr std::array kInitialJointPosition = { + -0.2423, 1.2929, 1.61, 1.58, -2.8792, 0.1150, -0.08}; +constexpr std::array kAccelerationSweep = { + 0.1, 0.3, 0.5, 1.0, 2.0, 3.0, 5.0}; +constexpr double kCommandVelocity = 0.04; +constexpr double kCommandDurationS = 2.0; +constexpr double kSamplePeriodS = 0.001; + +std::filesystem::path findProjectRoot() +{ + const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml"; + 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()); +} + +void setKinematicsUrdfPath(config::RobotArmConfig& arm_config, + const std::string& urdf_path) +{ + switch (arm_config.kinematics().algorithm_case()) { + case config::ArmKinematicsConfig::kPinocchioDlsIkSolver: + arm_config.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path(urdf_path); + break; + case config::ArmKinematicsConfig::kPinocchioQpIkSolver: + arm_config.mutable_kinematics()->mutable_pinocchio_qp_ik_solver()->set_urdf_path(urdf_path); + break; + default: + break; + } +} + +struct Sample { + double time_s{0.0}; + std::vector q; + std::vector qd; + CartesianVelocity commanded_twist; +}; + +struct Metrics { + double max_qd_before_stop{0.0}; + double max_qd_after_stop{0.0}; + double max_qdd{0.0}; + double max_tcp_linear_speed_before_stop{0.0}; + double max_tcp_linear_speed_after_stop{0.0}; + double max_tcp_linear_acceleration{0.0}; +}; + +double vectorNorm(const std::vector& value) +{ + double sum = 0.0; + for (const double item : value) { + sum += item * item; + } + return std::sqrt(sum); +} + +double linearSpeed(const Eigen::Matrix& twist) +{ + return twist.head<3>().norm(); +} + +std::string numberForFile(const double value) +{ + std::ostringstream stream; + stream << std::fixed << std::setprecision(3) << value; + auto result = stream.str(); + std::replace(result.begin(), result.end(), '.', '_'); + return result; +} + +void writeCsv(const std::filesystem::path& path, + const std::vector& samples, + const double stop_time_s, + const cmvr::PinocchioIKBase& solver, + Metrics& metrics) +{ + std::ofstream output(path); + ASSERT_TRUE(output.is_open()) << "failed to open CSV: " << path; + + output << "time_s,phase"; + for (const auto* name : kJointNames) { + output << "," << name << "_q"; + } + for (const auto* name : kJointNames) { + output << "," << name << "_qd"; + } + output << ",command_vx,command_vy,command_vz,command_wx,command_wy,command_wz" + << ",tcp_vx,tcp_vy,tcp_vz,tcp_wx,tcp_wy,tcp_wz,tcp_linear_speed,tcp_linear_acceleration"; + output << '\n'; + + Eigen::Matrix previous_tcp_twist = Eigen::Matrix::Zero(); + bool have_previous_tcp = false; + for (const auto& sample : samples) { + Eigen::Matrix tcp_twist = Eigen::Matrix::Zero(); + ASSERT_TRUE(solver.computeTwistBaseAtQ(sample.q, sample.qd, true, tcp_twist)); + const bool after_stop = stop_time_s >= 0.0 && sample.time_s >= stop_time_s; + const double qd_norm = vectorNorm(sample.qd); + const double tcp_speed = linearSpeed(tcp_twist); + double tcp_acceleration = 0.0; + if (have_previous_tcp) { + tcp_acceleration = (tcp_twist.head<3>() - previous_tcp_twist.head<3>()).norm() / + std::max(kSamplePeriodS, sample.time_s - + (samples[&sample - samples.data() - 1].time_s)); + } + previous_tcp_twist = tcp_twist; + have_previous_tcp = true; + + if (after_stop) { + metrics.max_qd_after_stop = std::max(metrics.max_qd_after_stop, qd_norm); + metrics.max_tcp_linear_speed_after_stop = + std::max(metrics.max_tcp_linear_speed_after_stop, tcp_speed); + } else { + metrics.max_qd_before_stop = std::max(metrics.max_qd_before_stop, qd_norm); + metrics.max_tcp_linear_speed_before_stop = + std::max(metrics.max_tcp_linear_speed_before_stop, tcp_speed); + } + metrics.max_tcp_linear_acceleration = + std::max(metrics.max_tcp_linear_acceleration, tcp_acceleration); + + if (&sample != samples.data()) { + const auto& previous = samples[&sample - samples.data() - 1]; + const double dt = std::max(kSamplePeriodS, sample.time_s - previous.time_s); + for (std::size_t i = 0; i < kDof; ++i) { + metrics.max_qdd = std::max(metrics.max_qdd, + std::abs(sample.qd[i] - previous.qd[i]) / dt); + } + } + + output << std::setprecision(9) << sample.time_s << ',' << (after_stop ? "stop" : "run"); + for (const double value : sample.q) { + output << ',' << value; + } + for (const double value : sample.qd) { + output << ',' << value; + } + output << ',' << sample.commanded_twist.vx + << ',' << sample.commanded_twist.vy + << ',' << sample.commanded_twist.vz + << ',' << sample.commanded_twist.wx + << ',' << sample.commanded_twist.wy + << ',' << sample.commanded_twist.wz; + for (Eigen::Index i = 0; i < 6; ++i) { + output << ',' << tcp_twist[i]; + } + output << ',' << tcp_speed << ',' << tcp_acceleration << '\n'; + } +} + +class MotorRobotArmSpeedLStopTest : public ::testing::Test { +protected: + void SetUp() override + { + 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/xiaoyan_description/dual_arm.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.pb.txt").string(), + &motor_root_config)); + motor_system_ = std::make_shared( + "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors"); + ASSERT_TRUE(motor_system_->init()); + + config::ArmRootConfig root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt").string(), + &root_config)); + ASSERT_GT(root_config.arm().robot_arms_size(), 0); + arm_config_ = root_config.arm().robot_arms(0); + setKinematicsUrdfPath( + arm_config_, (project_root_ / "model/xiaoyan_description/dual_arm.urdf").string()); + arm_ = std::make_unique(arm_config_); + ASSERT_TRUE(arm_->init()); + + analysis_solver_ = IKSolverFactory::create(arm_config_.kinematics()); + ASSERT_TRUE(analysis_solver_); + ASSERT_TRUE(analysis_solver_->init()); + analysis_pinocchio_solver_ = std::dynamic_pointer_cast(analysis_solver_); + ASSERT_TRUE(analysis_pinocchio_solver_); + } + + void TearDown() override + { + if (arm_) { + arm_->stop(); + } + if (motor_system_) { + motor_system_->stop(); + } + if (world_device_) { + world_device_->stop(); + } + } + + std::filesystem::path project_root_; + config::RobotArmConfig arm_config_; + std::shared_ptr world_device_; + std::shared_ptr motor_system_; + std::unique_ptr arm_; + std::shared_ptr analysis_solver_; + std::shared_ptr analysis_pinocchio_solver_; +}; + +TEST_F(MotorRobotArmSpeedLStopTest, SweepAccelerationAndDirection) +{ + ASSERT_TRUE(world_device_); + ASSERT_TRUE(world_device_->world()); + + const std::vector initial(kInitialJointPosition.begin(), kInitialJointPosition.end()); + MotionOptions move_options; + move_options.velocity = 2.0; + move_options.acceleration = 3.0; + ASSERT_TRUE(arm_->moveJ(JointPositionCommand{initial}, move_options).ok()); + + for (const double acceleration : kAccelerationSweep) { + for (const double direction : {1.0, -1.0}) { + ASSERT_FALSE(arm_->busy()); + std::vector samples; + samples.reserve(5000); + std::atomic stop_time_s{-1.0}; + Result command_result = Result::failure(ArmErrorCode::UnknownError, "not run"); + const auto start_time = std::chrono::steady_clock::now(); + + std::thread command_thread([&] { + CartesianVelocity velocity; + velocity.vx = direction * kCommandVelocity; + command_result = arm_->speedL(velocity, + acceleration, + kCommandDurationS, + FrameType::Base); + stop_time_s.store(std::chrono::duration( + std::chrono::steady_clock::now() - start_time).count()); + }); + + while (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() < 5.0) { + const double time_s = std::chrono::duration( + std::chrono::steady_clock::now() - start_time).count(); + const auto state = arm_->getJointState(); + ASSERT_EQ(state.position.size(), kDof); + ASSERT_EQ(state.velocity.size(), kDof); + samples.push_back(Sample{time_s, + state.position, + state.velocity, + arm_->getSpeedLCommandTwistBase()}); + + const double stop = stop_time_s.load(); + if (stop >= 0.0 && time_s > stop + 0.8 && !arm_->busy()) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + command_thread.join(); + + ASSERT_TRUE(command_result.ok()) << command_result.message; + const double stop = stop_time_s.load(); + ASSERT_GT(stop, 1.8); + ASSERT_LT(stop, 2.5); + ASSERT_FALSE(arm_->busy()); + + const auto csv_path = std::filesystem::path("/tmp") / + ("speedl_stop_acc_" + numberForFile(acceleration) + + "_" + (direction > 0.0 ? "pos" : "neg") + ".csv"); + Metrics metrics; + writeCsv(csv_path, samples, stop, *analysis_pinocchio_solver_, metrics); + std::cout << "[SpeedLStopTest] acceleration=" << acceleration + << ", direction=" << direction + << ", csv=" << csv_path + << ", max_qd_before=" << metrics.max_qd_before_stop + << ", max_qd_after=" << metrics.max_qd_after_stop + << ", max_qdd=" << metrics.max_qdd + << ", max_tcp_speed_before=" << metrics.max_tcp_linear_speed_before_stop + << ", max_tcp_speed_after=" << metrics.max_tcp_linear_speed_after_stop + << ", max_tcp_acceleration=" << metrics.max_tcp_linear_acceleration + << std::endl; + + // Stopping must not create a new velocity peak. A small tolerance + // allows one 1 ms feedback sample of transport jitter. + EXPECT_LE(metrics.max_qd_after_stop, + metrics.max_qd_before_stop + 0.25) + << "post-stop joint velocity peak for acceleration=" << acceleration + << ", direction=" << direction; + EXPECT_LE(metrics.max_tcp_linear_speed_after_stop, + metrics.max_tcp_linear_speed_before_stop + 0.01) + << "post-stop TCP velocity peak for acceleration=" << acceleration + << ", direction=" << direction; + } + } +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/speedl_reversal_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/speedl_reversal_mujoco_test.cpp new file mode 100644 index 00000000..abc459b5 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/speedl_reversal_mujoco_test.cpp @@ -0,0 +1,196 @@ +#include "arm/motor_robot_arm/include/motor_robot_arm.h" +#include "common/io/proto_file_io.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Headless, actuator-driven MuJoCo experiment. Never creates real motor drivers. +namespace { +using namespace cmvr; +using namespace cmvr::device; +using Clock = std::chrono::steady_clock; + +template T readConfig(const std::filesystem::path& path) { + T config; + if (!ProtoMessageIo::getProtoFromAsciiFile(path.string(), &config)) { + throw std::runtime_error("Cannot read " + path.string()); + } + return config; +} +void require(const Result& result) { + if (!result.ok()) throw std::runtime_error(result.message); +} +struct Sample { + double sim_time; + Eigen::Vector3d position; + Eigen::Vector3d velocity; + Eigen::Matrix3d rotation; +}; +Sample sample(const std::shared_ptr& world, int site) { + std::lock_guard lock(world->mutex()); + const auto* model = world->model(); + const auto* data = world->data(); + std::vector jac(3 * model->nv); + mj_jacSite(model, data, jac.data(), nullptr, site); + Sample s{data->time, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity()}; + for (int axis = 0; axis < 3; ++axis) { + s.position[axis] = data->site_xpos[site * 3 + axis]; + for (int j = 0; j < 3; ++j) s.rotation(axis, j) = data->site_xmat[site * 9 + axis * 3 + j]; + for (int j = 0; j < model->nv; ++j) s.velocity[axis] += jac[axis * model->nv + j] * data->qvel[j]; + } + return s; +} +} + +int main(int argc, char** argv) { + try { + const auto root = std::filesystem::current_path(); + double approach_ms = 500.0, jerk = 10.0, speed = 0.08, reverse_jerk = 0.0, trigger_speed = 0.0; + double approach_jerk = 0.0, reverse_acceleration = 3.0; + bool legacy = false, capture = false, tool_frame = false; + std::string csv_path = "/tmp/speedl-reversal.csv"; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (++i >= argc) throw std::runtime_error("Missing value for " + arg); + if (arg == "--approach-ms") approach_ms = std::stod(argv[i]); + else if (arg == "--jerk") jerk = std::stod(argv[i]); + else if (arg == "--speed") speed = std::stod(argv[i]); + else if (arg == "--reverse-jerk") reverse_jerk = std::stod(argv[i]); + else if (arg == "--approach-jerk") approach_jerk = std::stod(argv[i]); + else if (arg == "--reverse-acceleration") reverse_acceleration = std::stod(argv[i]); + else if (arg == "--trigger-speed") trigger_speed = std::stod(argv[i]); + else if (arg == "--legacy") legacy = std::string(argv[i]) == "true"; + else if (arg == "--capture-reference") capture = std::string(argv[i]) == "true"; + else if (arg == "--tool-frame") tool_frame = std::string(argv[i]) == "true"; + else if (arg == "--csv") csv_path = argv[i]; + else throw std::runtime_error("Unknown option " + arg); + } + if (!(approach_ms > 0 && approach_ms <= 1000 && jerk > 0 && speed > 0 && speed <= .1 && + reverse_jerk >= 0 && trigger_speed >= 0 && trigger_speed <= speed && + approach_jerk >= 0 && reverse_acceleration > 0 && std::isfinite(jerk) && + std::isfinite(reverse_jerk) && std::isfinite(approach_jerk) && std::isfinite(reverse_acceleration))) { + throw std::runtime_error("Invalid experiment parameters"); + } + auto worlds = readConfig(root / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt"); + auto wc = worlds.worlds(0); + wc.set_model_path((root / "model/xiaoyan_description/dual_arm.xml").string()); + simulate::MujocoWorldDevice world_device(wc); + if (!world_device.init() || !world_device.start()) throw std::runtime_error("MuJoCo start failed"); + auto motors = readConfig(root / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt"); + auto manager = std::make_shared("right_arm_mujoco_motors", motors.motor(), "right_arm_mujoco_motors"); + manager->init(); + const auto world = world_device.world(); + const int site = mj_name2id(world->model(), mjOBJ_SITE, "R_FINGER_TIP_SITE"); + if (site < 0) throw std::runtime_error("TCP site not found"); + auto arms = readConfig(root / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt"); + auto ac = arms.arm().robot_arms(0); + ac.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path( + (root / "model/xiaoyan_description/dual_arm.urdf").string()); + auto* planner_config = ac.mutable_motion()->mutable_speed_l()->mutable_pinocchio_cartesian_motion_planner(); + // Match physical-arm limits instead of the simulation's faster defaults. + planner_config->set_linear_velocity_max(.55); + planner_config->set_linear_acceleration_max(5.0); + planner_config->set_linear_jerk_max(jerk); + planner_config->set_continuous_linear_reversal(!legacy); + MotorRobotArm arm(ac); + if (!arm.init()) throw std::runtime_error("Simulated arm initialization failed"); + MotionOptions move; + move.velocity = 1.0; + move.acceleration = 3.0; + require(arm.moveJ(JointPositionCommand{{.25, 1.0, M_PI/2, M_PI/2, -M_PI/2, 0, 0}}, move)); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + std::ofstream csv(csv_path); + if (!csv) throw std::runtime_error("Cannot open CSV"); + csv << "wall_ms,sim_ms,after_reverse,x_m,y_m,z_m,vy_m_s,forward_displacement_m,command_vy_m_s\n" << std::setprecision(12); + CartesianVelocity forward; + forward.vy = speed; + const auto frame = tool_frame ? FrameType::Tool : FrameType::Base; + const Eigen::Vector3d axis = tool_frame ? Eigen::Vector3d(sample(world, site).rotation.col(1)) + : Eigen::Vector3d::UnitY(); + SpeedLOptions forward_options; + forward_options.acceleration = 5.0; + if (approach_jerk > 0) forward_options.linear_jerk = approach_jerk; + require(arm.speedL(forward, forward_options, 0.0, frame)); + const auto begin = Clock::now(); + auto next = begin; + auto reverse_time = begin; + Sample origin{}; + bool reversed = false; + bool reference_valid = !capture; + double peak = 0, peak_ms = 0, return_ms = -1, reverse_ms = -1; + int negative_samples = 0; + while (Clock::now() - begin < std::chrono::milliseconds(static_cast(approach_ms) + 1000)) { + auto now = Clock::now(); + auto s = sample(world, site); + const auto command = arm.getSpeedLCommandTwistBase(); + const double command_vy = Eigen::Vector3d(command.vx, command.vy, command.vz).dot(axis); + const double actual_vy = s.velocity.dot(axis); + const double wall_ms = std::chrono::duration(now - begin).count(); + if (!reversed && (trigger_speed > 0 ? command_vy >= trigger_speed : wall_ms >= approach_ms)) { + origin = s; + reverse_time = Clock::now(); + CartesianVelocity backward; + backward.vy = -speed; + SpeedLOptions options; + options.acceleration = reverse_acceleration; + options.capture_reference = capture; + if (reverse_jerk > 0) options.linear_jerk = reverse_jerk; + require(arm.speedL(backward, options, 0.0, frame)); + reversed = true; + } + const double elapsed = std::chrono::duration(now - reverse_time).count(); + const double displacement = reversed ? (s.position - origin.position).dot(axis) : 0.0; + if (reversed) { + if (capture) { + const auto ref = arm.getSpeedLReference(); + reference_valid |= ref.valid && Eigen::Vector3d(ref.target_base.vx, + ref.target_base.vy, ref.target_base.vz).dot(axis) < 0 && + std::isfinite(ref.tcp_pose_base.y) && ref.command_version != 0; + } + if (displacement > peak) { peak = displacement; peak_ms = elapsed; } + negative_samples = actual_vy < -1e-4 ? negative_samples + 1 : 0; + if (negative_samples >= 5 && reverse_ms < 0) reverse_ms = elapsed; + if (reverse_ms >= 0 && displacement <= 0 && return_ms < 0) return_ms = elapsed; + } + csv << wall_ms << ',' << s.sim_time * 1000 << ',' << reversed << ',' + << s.position.x() << ',' << s.position.y() << ',' << s.position.z() << ',' + << actual_vy << ',' << displacement << ',' << command_vy << '\n'; + next += std::chrono::milliseconds(1); + std::this_thread::sleep_until(next); + } + const bool active = arm.busy(); + require(arm.stopL(3.0)); + const auto stop_deadline = Clock::now() + std::chrono::seconds(3); + while (arm.busy() && Clock::now() < stop_deadline) std::this_thread::sleep_for(std::chrono::milliseconds(5)); + std::cout << std::fixed << std::setprecision(3) + << "REVERSAL legacy=" << legacy << " approach_ms=" << approach_ms + << " frame=" << (tool_frame ? "Tool" : "Base") + << " trigger_speed=" << trigger_speed << " speed_m_s=" << speed << " jerk_m_s3=" << jerk + << " approach_jerk_m_s3=" << std::min(approach_jerk > 0 ? approach_jerk : jerk, jerk) + << " requested_reverse_acceleration_m_s2=" << reverse_acceleration + << " reverse_acceleration_m_s2=" << std::min(reverse_acceleration, 5.0) + << " requested_reverse_jerk_m_s3=" << (reverse_jerk > 0 ? reverse_jerk : jerk) + << " reverse_jerk_m_s3=" << std::min(reverse_jerk > 0 ? reverse_jerk : jerk, jerk) + << " actual_vy_at_reverse=" << origin.velocity.dot(axis) + << " max_forward_mm=" << peak * 1000 << " peak_at_ms=" << peak_ms + << " actual_reverse_ms=" << reverse_ms << " return_to_origin_ms=" << return_ms + << " remained_active=" << active << " reference_valid=" << reference_valid << " csv=" << csv_path << '\n'; + arm.stop(); + manager->stop(); + world_device.stop(); + return active && reference_valid && reversed && origin.velocity.dot(axis) > .001 && reverse_ms > 0 && return_ms > 0 ? 0 : 1; + } catch (const std::exception& e) { + std::cerr << "Experiment failed: " << e.what() << '\n'; + return 1; + } +} diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 48dab718..932facec 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -79,6 +79,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; @@ -113,6 +116,17 @@ public: double duration, FrameType frame = FrameType::Base) = 0; virtual Result stopL(std::optional acceleration = std::nullopt) = 0; + virtual Result speedL(const CartesianVelocity& velocity, + const SpeedLOptions& options, + double duration, + FrameType frame = FrameType::Base) { + if (options.linear_jerk || options.capture_reference || + options.continuous_linear_reversal.value_or(false)) { + return Result::failure(ArmErrorCode::UnsupportedCommand, "speedL options are not supported by this arm"); + } + return speedL(velocity, options.acceleration, duration, frame); + } + virtual SpeedLReference getSpeedLReference() const { return {}; } virtual Result stopMotion() = 0; virtual Result moveP(const CartesianPose& target, diff --git a/cmvr-es/devices/camera/abstract_camera.h b/cmvr-es/devices/camera/abstract_camera.h index a05281dc..525414a7 100644 --- a/cmvr-es/devices/camera/abstract_camera.h +++ b/cmvr-es/devices/camera/abstract_camera.h @@ -3,20 +3,22 @@ #pragma once #include +#include #include "../abstract_device.h" #include #include "cmvr/config/camera_config/camera_config.pb.h" +#include "devices/camera/common/include/camera_stream_overlay.h" namespace cmvr::device { enum CameraMode {PHOTO_MODE, VIDEO_MODE}; struct Rs2Intrinsics { - float cx; - float cy; - float fx; - float fy; - float coeffs[5]; + float cx{0.0F}; + float cy{0.0F}; + float fx{0.0F}; + float fy{0.0F}; + float coeffs[5]{}; }; struct StreamFrameData @@ -64,11 +66,26 @@ namespace cmvr::device { return false; } + // Video overlay is a presentation-only snapshot. It is deliberately + // kept on the camera so the encoding thread can consume it without + // coupling the camera to AprilTag or task code. + void setStreamOverlay(const CameraStreamOverlay& overlay) { + std::lock_guard lock(stream_overlay_mutex_); + stream_overlay_ = overlay; + } + + CameraStreamOverlay streamOverlay() const { + std::lock_guard lock(stream_overlay_mutex_); + return stream_overlay_; + } + virtual bool startStreaming() {return true;} virtual void stopStreaming() {} virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};} protected: CameraState state_{}; + mutable std::mutex stream_overlay_mutex_; + CameraStreamOverlay stream_overlay_{}; void clear_error_() { this->state_.is_error = false; this->state_.error_message.clear(); diff --git a/cmvr-es/devices/camera/common/include/camera_stream_encoder.h b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h index 5001d23d..53222545 100644 --- a/cmvr-es/devices/camera/common/include/camera_stream_encoder.h +++ b/cmvr-es/devices/camera/common/include/camera_stream_encoder.h @@ -7,9 +7,12 @@ #include #include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h" +#include "devices/camera/common/include/camera_stream_overlay.h" namespace cmvr::device { +struct Rs2Intrinsics; + struct FfmpegEncoderInfo { std::string codec_name; int width = 0; @@ -27,8 +30,23 @@ struct FfmpegEncoderInfo { struct CameraStreamEncodeOptions { bool draw_timestamp = false; + CameraStreamOverlay overlay; }; +// Draw a frame whose pose is expressed as ^C T_Frame onto a BGR/BGRA image. +// The image is modified in place and no camera/perception state is touched. +void drawCoordinateFrame(cv::Mat& image, + const Eigen::Matrix4d& T_C_Frame, + const Rs2Intrinsics& intrinsics, + double axis_length_m, + const std::string& label); + +Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics, + int source_width, + int source_height, + int target_width, + int target_height); + class CameraStreamEncoder { public: static bool init(std::shared_ptr& encoder, @@ -37,6 +55,14 @@ public: int height, int fps); + static bool encode(std::shared_ptr& encoder, + const cv::Mat& frame, + std::vector& encoded_frame, + bool& is_key, + const Rs2Intrinsics& intrinsics, + const CameraStreamEncodeOptions& options = {}); + + // Compatibility overload for callers that only need timestamp drawing. static bool encode(std::shared_ptr& encoder, const cv::Mat& frame, std::vector& encoded_frame, diff --git a/cmvr-es/devices/camera/common/include/camera_stream_overlay.h b/cmvr-es/devices/camera/common/include/camera_stream_overlay.h new file mode 100644 index 00000000..7ecd6cfe --- /dev/null +++ b/cmvr-es/devices/camera/common/include/camera_stream_overlay.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +#include + +namespace cmvr::device { + +// A pose snapshot used only by the encoded video overlay. The transform is +// ^C T_Frame: it maps points in the named frame into the camera frame. +struct CoordinateFrameOverlay { + Eigen::Matrix4d T_C_Frame{Eigen::Matrix4d::Identity()}; + std::string label; + int tag_id{-1}; + bool valid{false}; +}; + +struct CameraStreamOverlay { + bool draw_coordinate_frames{false}; + double coordinate_axis_length_m{0.02}; + std::vector coordinate_frames; +}; + +} // namespace cmvr::device diff --git a/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp index c08f9f37..8efd1fb6 100644 --- a/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp +++ b/cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp @@ -1,6 +1,7 @@ #include "devices/camera/common/include/camera_stream_encoder.h" #include +#include #include #include #include @@ -9,6 +10,7 @@ #include #include "common/base/logging/logger.h" +#include "devices/camera/abstract_camera.h" namespace cmvr::device { namespace { @@ -46,6 +48,41 @@ void drawTimeStamp(cv::Mat& image) cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness); } +bool projectPoint(const Eigen::Vector3d& point, + const Rs2Intrinsics& intrinsics, + cv::Point& pixel) +{ + if (!point.allFinite() || point.z() <= 1e-9 || + !std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) || + !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) || + intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) { + return false; + } + const double u = static_cast(intrinsics.fx) * point.x() / point.z() + + static_cast(intrinsics.cx); + const double v = static_cast(intrinsics.fy) * point.y() / point.z() + + static_cast(intrinsics.cy); + if (!std::isfinite(u) || !std::isfinite(v)) { + return false; + } + pixel = cv::Point(cvRound(u), cvRound(v)); + return true; +} + +void drawOutlinedText(cv::Mat& image, + const std::string& text, + const cv::Point& origin, + const cv::Scalar& color) +{ + constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX; + constexpr double font_scale = 0.55; + constexpr int thickness = 1; + cv::putText(image, text, origin, font_face, font_scale, + cv::Scalar(0, 0, 0), thickness + 2, cv::LINE_AA); + cv::putText(image, text, origin, font_face, font_scale, + color, thickness, cv::LINE_AA); +} + const AVCodec* findEncoder(const std::string& codec_name) { if (codec_name == "h264" || codec_name == "H264") { @@ -75,6 +112,77 @@ AVPixelFormat sourcePixelFormat(const cv::Mat& frame) } // namespace +void drawCoordinateFrame(cv::Mat& image, + const Eigen::Matrix4d& T_C_Frame, + const Rs2Intrinsics& intrinsics, + const double axis_length_m, + const std::string& label) +{ + if (image.empty() || image.channels() < 3 || !T_C_Frame.allFinite() || + !std::isfinite(axis_length_m) || axis_length_m <= 0.0) { + return; + } + + const Eigen::Vector4d origin_h(0.0, 0.0, 0.0, 1.0); + const Eigen::Vector4d x_h(axis_length_m, 0.0, 0.0, 1.0); + const Eigen::Vector4d y_h(0.0, axis_length_m, 0.0, 1.0); + const Eigen::Vector4d z_h(0.0, 0.0, axis_length_m, 1.0); + const Eigen::Vector3d origin = (T_C_Frame * origin_h).head<3>(); + const Eigen::Vector3d x = (T_C_Frame * x_h).head<3>(); + const Eigen::Vector3d y = (T_C_Frame * y_h).head<3>(); + const Eigen::Vector3d z = (T_C_Frame * z_h).head<3>(); + + cv::Point origin_px; + if (!projectPoint(origin, intrinsics, origin_px)) { + return; + } + + cv::Point x_px; + cv::Point y_px; + cv::Point z_px; + constexpr int thickness = 2; + if (projectPoint(x, intrinsics, x_px)) { + cv::arrowedLine(image, origin_px, x_px, cv::Scalar(0, 0, 255), thickness, + cv::LINE_AA, 0, 0.15); + drawOutlinedText(image, "X", x_px + cv::Point(4, -4), cv::Scalar(0, 0, 255)); + } + if (projectPoint(y, intrinsics, y_px)) { + cv::arrowedLine(image, origin_px, y_px, cv::Scalar(0, 255, 0), thickness, + cv::LINE_AA, 0, 0.15); + drawOutlinedText(image, "Y", y_px + cv::Point(4, -4), cv::Scalar(0, 255, 0)); + } + if (projectPoint(z, intrinsics, z_px)) { + cv::arrowedLine(image, origin_px, z_px, cv::Scalar(255, 0, 0), thickness, + cv::LINE_AA, 0, 0.15); + drawOutlinedText(image, "Z", z_px + cv::Point(4, -4), cv::Scalar(255, 0, 0)); + } + + cv::drawMarker(image, origin_px, cv::Scalar(255, 255, 255), cv::MARKER_CROSS, 9, 1, + cv::LINE_AA); + if (!label.empty()) { + drawOutlinedText(image, label, origin_px + cv::Point(7, -7), + cv::Scalar(255, 255, 255)); + } +} + +Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics, + const int source_width, + const int source_height, + const int target_width, + const int target_height) +{ + Rs2Intrinsics scaled = intrinsics; + if (source_width > 0 && source_height > 0 && target_width > 0 && target_height > 0) { + const float sx = static_cast(target_width) / static_cast(source_width); + const float sy = static_cast(target_height) / static_cast(source_height); + scaled.fx *= sx; + scaled.cx *= sx; + scaled.fy *= sy; + scaled.cy *= sy; + } + return scaled; +} + FfmpegEncoderInfo::~FfmpegEncoderInfo() { if (frame) { @@ -189,6 +297,7 @@ bool CameraStreamEncoder::encode(std::shared_ptr& encoder, const cv::Mat& frame, std::vector& encoded_frame, bool& is_key, + const Rs2Intrinsics& intrinsics, const CameraStreamEncodeOptions& options) { encoded_frame.clear(); @@ -205,9 +314,27 @@ bool CameraStreamEncoder::encode(std::shared_ptr& encoder, } cv::Mat frame_to_encode = frame; - if (options.draw_timestamp) { + if (options.draw_timestamp || options.overlay.draw_coordinate_frames) { frame_to_encode = frame.clone(); - drawTimeStamp(frame_to_encode); + if (options.draw_timestamp) { + drawTimeStamp(frame_to_encode); + } + if (options.overlay.draw_coordinate_frames) { + for (const auto& coordinate_frame : options.overlay.coordinate_frames) { + if (!coordinate_frame.valid) { + continue; + } + std::string label = coordinate_frame.label; + if (coordinate_frame.tag_id >= 0) { + label += " #" + std::to_string(coordinate_frame.tag_id); + } + drawCoordinateFrame(frame_to_encode, + coordinate_frame.T_C_Frame, + intrinsics, + options.overlay.coordinate_axis_length_m, + label); + } + } } const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode); @@ -294,4 +421,14 @@ bool CameraStreamEncoder::encode(std::shared_ptr& encoder, return true; } +bool CameraStreamEncoder::encode(std::shared_ptr& encoder, + const cv::Mat& frame, + std::vector& encoded_frame, + bool& is_key, + const CameraStreamEncodeOptions& options) +{ + Rs2Intrinsics intrinsics{}; + return encode(encoder, frame, encoded_frame, is_key, intrinsics, options); +} + } // namespace cmvr::device diff --git a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h index 78419aa1..11cbaff3 100644 --- a/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h +++ b/cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h @@ -5,10 +5,13 @@ #pragma once #include +#include +#include #include #include #include #include +#include #include #include @@ -57,6 +60,11 @@ private: bool initOffscreen_(); void destroyOffscreen_(); bool renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics); + void renderLoop_(); + bool fetchCached_(cv::Mat& color, + cv::Mat& depth, + Rs2Intrinsics& intrinsics, + bool consume_new_frame_only); bool ensureEncoder_(int width, int height, int fps); void setError_(const std::string& error); static void flipRgbAndDepth_(std::vector& rgb, @@ -65,6 +73,14 @@ private: int height); static void linearizeDepth_(const mjModel* model, std::vector& depth); + struct CachedFrame { + cv::Mat color; + cv::Mat depth; + Rs2Intrinsics intrinsics{}; + uint64_t frame_id{0}; + bool valid{false}; + }; + private: FetchRgbdFn fetch_rgbd_fn_; mutable std::mutex mtx_; @@ -78,6 +94,7 @@ private: mjrContext context_{}; bool scene_initialized_{false}; bool context_initialized_{false}; + mjData* render_data_{nullptr}; int camera_id_{-1}; int width_{640}; int height_{480}; @@ -92,6 +109,13 @@ private: size_t stream_frame_index_{0}; bool streaming_{false}; std::shared_ptr rgb_encoder_; + + mutable std::mutex cache_mtx_; + std::condition_variable cache_cv_; + CachedFrame latest_frame_; + std::thread render_thread_; + bool render_thread_running_{false}; + bool render_stop_requested_{false}; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp index 3b02335b..abf9f499 100644 --- a/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp +++ b/cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp @@ -19,6 +19,13 @@ constexpr int kDefaultWidth = 640; constexpr int kDefaultHeight = 480; constexpr int kMaxGeom = 100000; +// GLFW keeps process-global initialization state. MuJoCo cameras render on +// independent threads, so serialize the one-time init/window creation path. +std::mutex& glfwInitMutex() { + static std::mutex mutex; + return mutex; +} + int positiveOrDefault(const int value, const int fallback) { return value > 0 ? value : fallback; @@ -106,10 +113,6 @@ bool MujocoCamera::init() fovy_deg_ = model->cam_fovy[camera_id_]; } - if (!initOffscreen_()) { - return false; - } - state_.is_initialized = true; state_.is_opened = true; state_.fps = positiveOrDefault(config_.render().fps(), 30); @@ -125,37 +128,132 @@ bool MujocoCamera::init() bool MujocoCamera::start() { - if (!state_.is_initialized) { + bool initialized = false; + { + std::lock_guard lock(mtx_); + initialized = state_.is_initialized; + } + if (!initialized) { if (!init()) { return false; } } - std::lock_guard lock(mtx_); + auto world = world_.lock(); if (world && !world->isRunning() && !world->start()) { setError_("[MujocoCamera] failed to start MuJoCo world: " + world->lastError()); return false; } - state_.is_streaming = true; - state_.is_opened = true; + + bool use_external_frames = false; + { + std::lock_guard lock(mtx_); + state_.is_streaming = true; + state_.is_opened = true; + use_external_frames = static_cast(fetch_rgbd_fn_); + } + std::thread stale_thread; + { + std::lock_guard lock(cache_mtx_); + if (render_thread_running_) { + return true; + } + latest_frame_ = CachedFrame{}; + last_frame_id_ = 0; + has_last_frame_id_ = false; + render_stop_requested_ = false; + render_thread_running_ = true; + } + + { + std::lock_guard lock(mtx_); + stale_thread = std::move(render_thread_); + } + if (stale_thread.joinable()) { + stale_thread.join(); + } + + try { + std::lock_guard lock(mtx_); + render_thread_ = std::thread(&MujocoCamera::renderLoop_, this); + } catch (const std::exception& e) { + { + std::lock_guard lock(cache_mtx_); + render_thread_running_ = false; + render_stop_requested_ = true; + } + cache_cv_.notify_all(); + setError_("[MujocoCamera] failed to start render thread: " + std::string(e.what())); + return false; + } + + // External PiP callbacks are not ready until the viewer enters its render + // loop, so let their polling thread warm up asynchronously. + if (use_external_frames) { + return true; + } + + std::unique_lock cache_lock(cache_mtx_); + const bool ready = cache_cv_.wait_for( + cache_lock, + std::chrono::seconds(5), + [this] { return latest_frame_.valid || !render_thread_running_ || render_stop_requested_; }); + const bool has_frame = latest_frame_.valid; + cache_lock.unlock(); + if (!ready || !has_frame) { + stop(); + if (ready) { + setError_("[MujocoCamera] render thread stopped before producing a frame"); + } else { + setError_("[MujocoCamera] timed out waiting for the first rendered frame"); + } + return false; + } return true; } bool MujocoCamera::stop() { - std::lock_guard lock(mtx_); - state_.is_streaming = false; - state_.is_opened = false; - destroyOffscreen_(); + { + std::lock_guard lock(cache_mtx_); + render_stop_requested_ = true; + } + cache_cv_.notify_all(); + + std::thread thread_to_join; + { + std::lock_guard lock(mtx_); + state_.is_streaming = false; + state_.is_opened = false; + thread_to_join = std::move(render_thread_); + } + if (thread_to_join.joinable()) { + thread_to_join.join(); + } + + { + std::lock_guard lock(cache_mtx_); + render_thread_running_ = false; + latest_frame_ = CachedFrame{}; + } + cache_cv_.notify_all(); return true; } void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn) { + bool render_thread_active = false; + { + std::lock_guard lock(cache_mtx_); + render_thread_active = render_thread_running_; + } + if (render_thread_active) { + stop(); + } + std::lock_guard lock(mtx_); fetch_rgbd_fn_ = std::move(fetch_rgbd_fn); if (fetch_rgbd_fn_) { - destroyOffscreen_(); state_.is_initialized = true; state_.is_opened = true; state_.fps = positiveOrDefault(config_.render().fps(), 30); @@ -203,9 +301,10 @@ void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& bool MujocoCamera::startStreaming() { - if (!state_.is_initialized && !init()) { + if (!start()) { return false; } + std::lock_guard lock(mtx_); streaming_ = true; state_.is_streaming = true; @@ -246,12 +345,17 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne } frame_data.rgbImage = color.clone(); + frame_data.intrinsics = intrinsics; CameraStreamEncodeOptions encode_options; encode_options.draw_timestamp = enable_stream_timestamp_; + encode_options.overlay = streamOverlay(); + const Rs2Intrinsics encode_intrinsics = scaleIntrinsics( + intrinsics, color.cols, color.rows, color_to_encode.cols, color_to_encode.rows); if (!CameraStreamEncoder::encode(rgb_encoder_, color_to_encode, frame_data.rgbFrame, frame_data.bKey, + encode_intrinsics, encode_options)) { return false; } @@ -261,7 +365,6 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne const auto* depth_end = depth_begin + depth.total() * depth.elemSize(); frame_data.depthFrame.assign(depth_begin, depth_end); } - frame_data.intrinsics = intrinsics; frame_data.width = color_to_encode.cols; frame_data.height = color_to_encode.rows; frame_data.fps = fps; @@ -273,14 +376,29 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) { - std::lock_guard lock(mtx_); - if (fetch_rgbd_fn_) { + FetchRgbdFn fetch_rgbd_fn; + bool consume_new_frame_only = false; + bool render_thread_active = false; + { + std::lock_guard lock(mtx_); + fetch_rgbd_fn = fetch_rgbd_fn_; + consume_new_frame_only = consume_new_frame_only_; + } + + { + std::lock_guard lock(cache_mtx_); + render_thread_active = render_thread_running_; + } + + // Keep compatibility with callback-only cameras that have not been + // started. Once start() owns a polling thread, reads are cache-only. + if (fetch_rgbd_fn && !render_thread_active) { std::vector rgb_raw; std::vector depth_raw; int width = 0; int height = 0; std::uint64_t frame_id = 0; - if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) { + if (!fetch_rgbd_fn(rgb_raw, depth_raw, width, height, frame_id)) { return false; } if (width <= 0 || height <= 0) { @@ -292,11 +410,14 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi if (!depth_raw.empty() && static_cast(depth_raw.size()) != width * height) { return false; } - if (consume_new_frame_only_ && has_last_frame_id_ && frame_id == last_frame_id_) { - return false; + { + std::lock_guard lock(cache_mtx_); + if (consume_new_frame_only && has_last_frame_id_ && frame_id == last_frame_id_) { + return false; + } + last_frame_id_ = frame_id; + has_last_frame_id_ = true; } - last_frame_id_ = frame_id; - has_last_frame_id_ = true; cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data()); cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR); @@ -310,7 +431,31 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi return true; } - return renderOffscreen_(color, depth, intrinsics); + return fetchCached_(color, depth, intrinsics, consume_new_frame_only); +} + +bool MujocoCamera::fetchCached_(cv::Mat& color, + cv::Mat& depth, + Rs2Intrinsics& intrinsics, + const bool consume_new_frame_only) +{ + std::lock_guard lock(cache_mtx_); + if (!latest_frame_.valid || latest_frame_.color.empty()) { + return false; + } + if (consume_new_frame_only && has_last_frame_id_ && + latest_frame_.frame_id == last_frame_id_) { + return false; + } + + // cv::Mat copies are reference-counted; keep the cache immutable while the + // consumer reads the published frame and avoid a full image copy per poll. + color = latest_frame_.color; + depth = latest_frame_.depth; + intrinsics = latest_frame_.intrinsics; + last_frame_id_ = latest_frame_.frame_id; + has_last_frame_id_ = true; + return !color.empty(); } bool MujocoCamera::initOffscreen_() @@ -319,6 +464,7 @@ bool MujocoCamera::initOffscreen_() return true; } + std::lock_guard glfw_lock(glfwInitMutex()); if (!glfwInit()) { setError_("[MujocoCamera] glfwInit failed"); return false; @@ -345,10 +491,25 @@ bool MujocoCamera::initOffscreen_() return false; } - std::lock_guard world_lock(world->mutex()); - const mjModel* model = world->model(); - if (model == nullptr) { - setError_("[MujocoCamera] world model is null"); + mjModel* model = nullptr; + { + std::lock_guard world_lock(world->mutex()); + model = world->model(); + if (model == nullptr) { + setError_("[MujocoCamera] world model is null"); + return false; + } + + // MuJoCo clips rendering to the model's offscreen buffer. Make sure + // the buffer is large enough before creating this camera's context; + // otherwise a larger requested frame is only partially populated. + model->vis.global.offwidth = std::max(model->vis.global.offwidth, width_); + model->vis.global.offheight = std::max(model->vis.global.offheight, height_); + } + + render_data_ = mj_makeData(model); + if (render_data_ == nullptr) { + setError_("[MujocoCamera] failed to allocate render data"); return false; } mjv_makeScene(model, &scene_, kMaxGeom); @@ -360,9 +521,116 @@ bool MujocoCamera::initOffscreen_() setError_("[MujocoCamera] MuJoCo offscreen buffer is not available"); return false; } + if (context_.offWidth < width_ || context_.offHeight < height_) { + setError_("[MujocoCamera] offscreen buffer is smaller than requested frame: " + + std::to_string(context_.offWidth) + "x" + + std::to_string(context_.offHeight) + " < " + + std::to_string(width_) + "x" + std::to_string(height_)); + return false; + } return true; } +void MujocoCamera::renderLoop_() +{ + FetchRgbdFn external_fetch; + { + std::lock_guard lock(mtx_); + external_fetch = fetch_rgbd_fn_; + } + + const bool use_external_frames = static_cast(external_fetch); + if (!use_external_frames && !initOffscreen_()) { + destroyOffscreen_(); + { + std::lock_guard lock(cache_mtx_); + render_thread_running_ = false; + } + cache_cv_.notify_all(); + return; + } + + const int fps = positiveOrDefault(config_.render().fps(), 30); + const auto period = std::chrono::duration(1.0 / static_cast(fps)); + const auto period_ticks = std::chrono::duration_cast(period); + auto next_tick = std::chrono::steady_clock::now(); + + while (true) { + { + std::lock_guard lock(cache_mtx_); + if (render_stop_requested_) { + break; + } + } + + cv::Mat color; + cv::Mat depth; + Rs2Intrinsics intrinsics{}; + uint64_t external_frame_id = 0; + bool got_frame = false; + + if (use_external_frames) { + std::vector rgb_raw; + std::vector depth_raw; + int width = 0; + int height = 0; + if (external_fetch(rgb_raw, depth_raw, width, height, external_frame_id) && + width > 0 && height > 0 && + static_cast(rgb_raw.size()) == width * height * 3 && + (depth_raw.empty() || static_cast(depth_raw.size()) == width * height)) { + cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data()); + cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR); + if (!depth_raw.empty()) { + cv::Mat dep(height, width, CV_32FC1, depth_raw.data()); + depth = dep.clone(); + } + fillIntrinsics(width, height, intrinsics); + got_frame = !color.empty(); + } + } else { + got_frame = renderOffscreen_(color, depth, intrinsics); + } + + if (got_frame) { + { + std::lock_guard lock(cache_mtx_); + latest_frame_.color = std::move(color); + latest_frame_.depth = std::move(depth); + latest_frame_.intrinsics = intrinsics; + latest_frame_.frame_id = use_external_frames && external_frame_id != 0 + ? external_frame_id + : latest_frame_.frame_id + 1; + latest_frame_.valid = true; + } + { + std::lock_guard lock(mtx_); + clear_error_(); + } + cache_cv_.notify_all(); + } + + next_tick += period_ticks; + std::unique_lock lock(cache_mtx_); + if (cache_cv_.wait_until(lock, next_tick, [this] { return render_stop_requested_; })) { + break; + } + + const auto now = std::chrono::steady_clock::now(); + if (next_tick < now) { + next_tick = now + period_ticks; + } + } + + if (!use_external_frames) { + destroyOffscreen_(); + } + { + std::lock_guard lock(cache_mtx_); + render_thread_running_ = false; + } + cache_cv_.notify_all(); +} + void MujocoCamera::destroyOffscreen_() { if (window_ != nullptr) { @@ -376,6 +644,10 @@ void MujocoCamera::destroyOffscreen_() mjv_freeScene(&scene_); scene_initialized_ = false; } + if (render_data_ != nullptr) { + mj_deleteData(render_data_); + render_data_ = nullptr; + } if (window_ != nullptr) { glfwDestroyWindow(window_); window_ = nullptr; @@ -388,7 +660,8 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic setError_("[MujocoCamera] camera is not initialized: " + id_); return false; } - if (!initOffscreen_()) { + if (window_ == nullptr || !context_initialized_ || !scene_initialized_) { + setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_); return false; } @@ -402,15 +675,27 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic std::vector rgb(static_cast(width_) * height_ * 3); std::vector depth_raw(static_cast(width_) * height_); + mjModel* model = nullptr; { - std::lock_guard world_lock(world->mutex()); - mjModel* model = world->model(); - mjData* data = world->data(); - if (model == nullptr || data == nullptr) { + std::unique_lock world_lock(world->mutex(), std::try_to_lock); + if (!world_lock.owns_lock()) { + // Never make the simulation wait for a camera frame. The next + // scheduled capture will use a newer state if this one is busy. + return false; + } + model = world->model(); + const mjData* data = world->data(); + if (model == nullptr || data == nullptr || render_data_ == nullptr) { setError_("[MujocoCamera] world model/data is null"); return false; } + // Keep the world lock limited to the state copy. GPU rendering runs on + // the camera thread using its private data snapshot. + mjv_copyData(render_data_, model, data); + } + + { camera_.type = mjCAMERA_FIXED; camera_.fixedcamid = camera_id_; camera_.trackbodyid = -1; @@ -421,7 +706,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic viewport.width = width_; viewport.height = height_; - mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_); + mjv_updateScene(model, render_data_, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_); mjr_render(viewport, &scene_, &context_); mjr_readPixels(rgb.data(), depth_raw.data(), viewport, &context_); flipRgbAndDepth_(rgb, depth_raw, width_, height_); @@ -429,13 +714,12 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic } cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data()); - color = rgb_mat.clone(); + // mjr_readPixels returns RGB, while the rest of the camera API exposes + // OpenCV-compatible BGR frames (as UVC and RealSense do). + cv::cvtColor(rgb_mat, color, cv::COLOR_RGB2BGR); cv::Mat depth_mat(height_, width_, CV_32FC1, depth_raw.data()); depth = depth_mat.clone(); fillIntrinsics(width_, height_, intrinsics); - ++last_frame_id_; - has_last_frame_id_ = true; - clear_error_(); return true; } diff --git a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp index 6aad3be4..71565a78 100644 --- a/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp +++ b/cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp @@ -772,10 +772,18 @@ void RealsenseCamera::streaming_worker_() { } CameraStreamEncodeOptions encode_options; encode_options.draw_timestamp = enable_stream_timestamp_; + encode_options.overlay = streamOverlay(); + const Rs2Intrinsics encode_intrinsics = scaleIntrinsics( + frame_data.intrinsics, + frame_data.rgbImage.cols, + frame_data.rgbImage.rows, + rgb_to_encode.cols, + rgb_to_encode.rows); success = CameraStreamEncoder::encode(rgbEncoder_, rgb_to_encode, frame_data.rgbFrame, frame_data.bKey, + encode_intrinsics, encode_options); // 深度图编码 // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey); diff --git a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp index bccc0204..8e8be98d 100644 --- a/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp +++ b/cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp @@ -504,10 +504,18 @@ void UVCCamera::streaming_worker_() { } CameraStreamEncodeOptions encode_options; encode_options.draw_timestamp = enable_stream_timestamp_; + encode_options.overlay = streamOverlay(); + const Rs2Intrinsics encode_intrinsics = scaleIntrinsics( + frame_data.intrinsics, + frame_data.rgbImage.cols, + frame_data.rgbImage.rows, + rgb_to_encode.cols, + rgb_to_encode.rows); success = CameraStreamEncoder::encode(rgbEncoder_, rgb_to_encode, frame_data.rgbFrame, frame_data.bKey, + encode_intrinsics, encode_options); if (success) { frame_data.fps = fps_; diff --git a/cmvr-es/devices/canbus/can_comm/can_sender.h b/cmvr-es/devices/canbus/can_comm/can_sender.h index 63e4b42c..34272241 100644 --- a/cmvr-es/devices/canbus/can_comm/can_sender.h +++ b/cmvr-es/devices/canbus/can_comm/can_sender.h @@ -247,6 +247,9 @@ namespace cmvr { curr_period_ = period_; Update(); + if (send_with_once_) { + has_sent_ = true; + } } template diff --git a/cmvr-es/devices/canbus/can_comm/can_sender_test.cc b/cmvr-es/devices/canbus/can_comm/can_sender_test.cc index 39e941ad..5bad78fe 100644 --- a/cmvr-es/devices/canbus/can_comm/can_sender_test.cc +++ b/cmvr-es/devices/canbus/can_comm/can_sender_test.cc @@ -77,6 +77,19 @@ namespace cmvr { sensor_data->enable = (bytes[2] != 0); } + TEST(CanSenderTest, OneShotMessageWaitsForExplicitUpdate) { + MyProtocol protocol; + SenderMessage message(MyProtocol::ID, &protocol, true); + + EXPECT_TRUE(message.has_sent()); + + protocol.SetSpeed(12.3); + protocol.SetEnable(true); + message.Update(); + + EXPECT_FALSE(message.has_sent()); + } + TEST(CanSenderTest, OneRunCase) { cmvr::config::SocketCanConfig cfg; 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/dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/CMakeLists.txt index 41130532..f162ecd8 100644 --- a/cmvr-es/devices/dexhand/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/CMakeLists.txt @@ -1,5 +1,6 @@ add_subdirectory(rh56dftp_dexhand) add_subdirectory(px_6ax_gen3) +add_subdirectory(zero_sim_touch_dexhand) add_library(dexhand INTERFACE) @@ -9,6 +10,7 @@ target_link_libraries(dexhand INTERFACE cmvr_es::device::rh56dftp_dexhand cmvr_es::device::px_6ax_gen3 + cmvr_es::device::zero_sim_touch_dexhand cmvr_es::proto ) diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 1cf8b9c7..3a21be43 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,17 @@ namespace cmvr::device { using ResultantForce = TactilePoint; + // Physical force, separate from device-specific integer tactile data. + struct ForceNewtons { + double fx{0.0}; + double fy{0.0}; + double fz{0.0}; + + double magnitude() const { + return std::hypot(fx, fy, fz); + } + }; + enum class FingerType { PINKY, RING, @@ -147,6 +159,11 @@ namespace cmvr::device { virtual std::vector getSensorData() = 0; virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0; virtual ResultantForce getResultantForce(FingerType finger, TactileRegion region) = 0; + // Backends must provide a documented conversion; raw pressure counts + // cannot be assumed to represent newtons. + virtual ForceNewtons getResultantForceNewtons(FingerType, TactileRegion) { + throw std::runtime_error(typeName() + " does not provide force in newtons."); + } virtual void setPositions(const std::vector&) { CMVR_LOG(ERROR) << "[AbstractDexHand] setPositions is not supported by this dexhand abstraction."; diff --git a/cmvr-es/devices/dexhand/dexhand_factory.h b/cmvr-es/devices/dexhand/dexhand_factory.h index 0bba4dfb..de7ea7d9 100644 --- a/cmvr-es/devices/dexhand/dexhand_factory.h +++ b/cmvr-es/devices/dexhand/dexhand_factory.h @@ -10,6 +10,7 @@ #include "devices/dexhand/abstract_dexhand.h" #include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h" #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h" +#include "devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h" namespace cmvr::device { @@ -32,6 +33,10 @@ public: return std::make_shared( backendWithId_(cfg.id(), cfg.px_6ax_gen3())); + case config::DexHandDeviceConfig::kZeroSimTouch: + return std::make_shared( + backendWithId_(cfg.id(), cfg.zero_sim_touch())); + case config::DexHandDeviceConfig::BACKEND_NOT_SET: default: { diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt b/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt index b6a30201..0b592756 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt @@ -12,3 +12,12 @@ target_link_libraries(px_6ax_gen3 ) install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib) + +# Sensor-only executable: no DeviceManager, arm initialization or calibration. +add_executable(px_6ax_gen3_real_test src/px_6ax_gen3_real_test.cpp) +target_link_libraries(px_6ax_gen3_real_test PRIVATE + px_6ax_gen3 cmvr_es::proto cmvr_es::logging pthread) + +add_executable(px_6ax_gen3_test src/px_6ax_gen3_test.cpp) +target_link_libraries(px_6ax_gen3_test PRIVATE + px_6ax_gen3 cmvr_es::proto cmvr_es::logging gtest gtest_main pthread) diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/README.md b/cmvr-es/devices/dexhand/px_6ax_gen3/README.md new file mode 100644 index 00000000..6b861cdf --- /dev/null +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/README.md @@ -0,0 +1,51 @@ +# PX-6AX GEN3 传感器读取与真实 USB 测试 + +真实测试只创建 PX6AXGen3 和 POSIX 串口，不初始化机械臂或 DeviceManager；禁用自动标定，传输层只允许 `0xFB` 读取命令。 + +在仓库根目录运行： + +```bash +# 每次读取都发起一个真实请求：检查串口应答耗时和按压力值。 +./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \ + --mode sync --duration-s 30 --csv /tmp/paxini-sync.csv --raw-log /tmp/paxini-frames.log + +# 与触屏任务相同：后台轮询，前台读取最新有效缓存。 +./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --module-id 2 \ + --mode stream --duration-s 30 --csv /tmp/paxini-stream.csv +``` + +也可把 `--port` 指定为 `/dev/serial/by-id/` 下的稳定设备链接。设备端口取决于连接顺序；测试参数不会更改机器人部署配置里的串口。`module-id=2` 对应协议设备地址 3。 + +脚本会构建真实测试并优先加载本次构建的驱动和 protobuf。默认构建目录为 `cmake-build-debug`，可用 `CMVR_BUILD_DIR` 覆盖；构建目录须已完成 CMake 配置。串口应可读写，运行测试前退出占用同一串口的程序。 + +测试期间可用手轻按、松开传感器，终端每 100 ms 显示一次力值，CSV 记录每次 getter 调用。按 Ctrl-C 可结束。CSV 的无效读数留空，不能当作零力。进程在初始化失败、没有有效数据或存在读取失败时返回非零退出码。 + +- `sync` 的 `read_ms` 是一次驱动请求/应答调用的耗时；默认每 5 ms 请求一次。 +- `stream` 的 `read_ms` 是读取缓存耗时，成功次数包含重复快照，不能据此推算传感器实际更新频率。 +- `force_N`、`fz_N` 和 CSV 中的三个力分量均由驱动直接返回，单位为 N；例如原始值 1 对应 0.1 N、108 对应 10.8 N。传感器标称输出频率 83.3 Hz；请求频率可以高于内部测量更新频率。 +- 串口往返时间不包含“物理接触到传感器产生非零输出”的全部时间。需要按压试验或外部同步信号才能测量接触检测延迟。 +- `--raw-log` 会记录原始 TX/RX 和单调时钟时间，用于对照手册定位帧问题；写日志会给时间测量带来少量开销。 + +## 驱动行为 + +`getResultantForceNewtons()` 将合力寄存器的三个原始分量各乘以 0.1，返回以 N 为单位的浮点力值；原有 `getResultantForce()` 保留原始整数。触屏任务和 USB 测试使用牛顿接口，不再额外换算。触屏任务的 `force_threshold`、力值日志和 `lastTouchPressureSum()` 均使用 N；阈值 `0.1` 与旧版原始值阈值 `1.0` 对应相同力度。FZ 判据比较法向力，MAGNITUDE 判据比较三轴合力大小，多区域按原逻辑累加。 + +MuJoCo 零值触觉后端也实现牛顿接口。尚无确定换算系数的其他后端（如 RH56DFTP）调用该接口会明确报错，不会把原始压力计数当成 N。 + +合力应答按 `14 字节头部 + 3 字节数据 + 1 字节 LRC` 完整读取。验证帧长度、设备地址、预留位、功能码、寄存器地址、字节数及 LRC，并支持分片、请求回显和噪声后的重新定位。 + +读状态字节属于内部调试信息（手册 5.3.5）；实测正常读应答为 `0x01`，不能套用写应答 `0x00=成功` 的规则。自动标定的写应答要求完整 15 字节并且状态为 0。 + +`max_sample_age_ms` 默认 50 ms，可在设备配置中调整。每类数据分别记录请求开始时间：较晚返回的旧请求不能被重新标记为新数据。此值独立于 `response_timeout_ms`（默认 200 ms）。后台读取接口遇到过期或失效快照会抛出异常，不等待串口补读、不返回旧力值或伪造零值；触屏任务现有的异常捕获会将其识别为触觉不可用。 + +通信失败会使快照失效，后台继续尝试恢复，只有通过验证的新应答才能恢复有效数据。`stop()` 清除缓存；停止或故障状态下的 getter 报错，显式 `init()` / `start()` 后才能恢复。初始化重试失败会返回 false。 + +## 自动回归测试 + +真实设备无法稳定制造的坏帧和超时，用可注入的串口实现验证： + +```bash +cmake --build cmake-build-debug --target px_6ax_gen3_test -j 4 +LD_LIBRARY_PATH="$PWD/cmake-build-debug:$PWD/cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3:$PWD/cmake-build-debug/cmvr-es/hardware:$PWD/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \ + ./cmake-build-debug/cmvr-es/devices/dexhand/px_6ax_gen3/px_6ax_gen3_test +``` diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h index 163f12c8..7b6b51d9 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h @@ -39,6 +39,8 @@ namespace cmvr::device { }; explicit PX6AXGen3(const config::PX6AXGen3& cfg); + PX6AXGen3(const config::PX6AXGen3& cfg, + std::unique_ptr<::cmvr::AbstractSerialTransport> serial); ~PX6AXGen3() override; std::string typeName() const override { return "PX6AXGen3"; } @@ -54,7 +56,10 @@ namespace cmvr::device { void setTactilePollingRegions(const std::vector& regions) override; std::vector getSensorData() override; TactileRegionData getSensorData(FingerType finger, TactileRegion region) override; + // Throws when no fresh, validated sample is available. A failed read is + // never represented as a zero force (which would mean no contact). ResultantForce getResultantForce(FingerType finger, TactileRegion region) override; + ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override; private: struct SensorSnapshot { @@ -64,6 +69,8 @@ namespace cmvr::device { int cols{0}; bool tactile_valid{false}; bool resultant_valid{false}; + std::chrono::steady_clock::time_point tactile_request_time{}; + std::chrono::steady_clock::time_point resultant_request_time{}; }; static PollingReadMode parsePollingReadMode(config::PX6AXGen3PollingReadMode mode); @@ -81,17 +88,19 @@ namespace cmvr::device { bool isSupportedRegion(FingerType finger, TactileRegion region) const; bool isSnapshotReady(bool require_tactile, bool require_resultant) const; + bool isSampleFresh(std::chrono::steady_clock::time_point request_time) const; TactileRegionData buildSupportedRegionSnapshot() const; std::pair resolvePollingReadSelection() const; void clearOperationalError(); - void handleRefreshFailure(const std::string& error, bool had_valid_snapshot); + void handleRefreshFailure(const std::string& error); void transitionTo(Status next_state); void enterFault(const std::string& error); bool isOperationalState(Status lifecycle) const; std::unique_ptr<::cmvr::AbstractSerialTransport> serial_; config::PX6AXGen3 config_; + bool config_valid_{false}; mutable std::mutex lifecycle_mutex_; Status lifecycle_state_{Status::CREATED}; @@ -109,6 +118,7 @@ namespace cmvr::device { int tactile_rows_{1}; int tactile_cols_{0}; int response_timeout_ms_{200}; + std::chrono::milliseconds max_sample_age_{50}; FingerType tactile_finger_{FingerType::INDEX}; TactileRegion tactile_region_{TactileRegion::TIP}; PollingReadMode polling_read_mode_{PollingReadMode::DISTRIBUTED_AND_RESULTANT_FORCE}; diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp index d15d4cf6..588b38b5 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3.cpp @@ -125,32 +125,13 @@ namespace { return frame; } - std::vector extractPayload(const std::vector& response, - const size_t frame_offset, - const size_t response_header_bytes, - const size_t payload_length) { - if (response.size() < frame_offset + response_header_bytes + payload_length) { - CMVR_LOG(ERROR) << "PX-6AX GEN3 response shorter than expected payload window."; - return {}; - } - return std::vector( - response.begin() + static_cast(frame_offset + response_header_bytes), - response.begin() + static_cast(frame_offset + response_header_bytes + payload_length)); - } + // UART read reply: 14-byte header, N payload bytes, one LRC byte. + constexpr size_t kResponseHeaderBytes = 14U; + constexpr size_t kResponseOverheadBytes = kResponseHeaderBytes + 1U; - size_t findResponseFrameOffset(const std::vector& response, - const size_t expected_frame_bytes) { - if (response.size() < expected_frame_bytes) { - return std::string::npos; - } - - for (size_t offset = 0; offset + expected_frame_bytes <= response.size(); ++offset) { - if (response[offset] == 0xAA && response[offset + 1] == 0x55) { - return offset; - } - } - - return std::string::npos; + uint16_t readLe16(const std::vector& bytes, const size_t offset) { + return static_cast(bytes[offset]) | + static_cast(static_cast(bytes[offset + 1U]) << 8U); } std::string previewBytesHex(const std::vector& data, const size_t max_bytes = 32U) { @@ -171,39 +152,83 @@ namespace { return stream.str(); } - std::vector readFramedResponse(cmvr::AbstractSerialTransport& serial, - const size_t expected_frame_bytes, - const size_t max_prefix_bytes, - const std::chrono::milliseconds timeout, - const std::string& response_name) { + std::vector transact(cmvr::AbstractSerialTransport& serial, + const std::vector& request, + const size_t payload_length, + const std::chrono::milliseconds timeout) { + if (!serial.flushInput()) { + throw std::runtime_error("Failed to flush PX6AXGen3 input: " + serial.lastError()); + } + if (!serial.write(request)) { + throw std::runtime_error("Failed to send PX6AXGen3 request: " + serial.lastError()); + } + + const size_t expected_size = kResponseOverheadBytes + payload_length; + // Permit a request echo/noisy prefix, but bound resynchronization work. + const size_t max_response_bytes = expected_size + 4096U; const auto deadline = std::chrono::steady_clock::now() + timeout; std::vector response; - const bool read_ok = serial.read(expected_frame_bytes, timeout, response); - auto frame_offset = findResponseFrameOffset(response, expected_frame_bytes); - - while (frame_offset == std::string::npos && - response.size() < expected_frame_bytes + max_prefix_bytes) { + size_t offset = 0; + std::string error = "Incomplete PX6AXGen3 response"; + while (response.size() < max_response_bytes) { const auto now = std::chrono::steady_clock::now(); if (now >= deadline) { break; } - - std::vector extra_bytes; - const auto remaining_timeout = std::chrono::duration_cast(deadline - now); - if (!serial.read(1U, remaining_timeout, extra_bytes)) { + std::vector chunk; + const auto remaining = std::max(std::chrono::milliseconds(1), + std::chrono::duration_cast(deadline - now)); + // Read available bytes; never wait for a guessed length before + // identifying the header. This also handles split headers/echoes. + const bool read_ok = serial.read(1U, remaining, chunk); + if (chunk.size() > max_response_bytes - response.size()) { + throw std::runtime_error("PX6AXGen3 response exceeds resynchronization limit"); + } + response.insert(response.end(), chunk.begin(), chunk.end()); + while (offset + kResponseHeaderBytes <= response.size()) { + if (response[offset] != 0xAA || response[offset + 1U] != 0x55) { + ++offset; + continue; + } + // Frame length counts data[4] through data[N+13], excluding LRC. + if (readLe16(response, offset + 2U) != payload_length + 10U || + response[offset + 4U] != request[4] || + response[offset + 5U] != 0x00 || + !std::equal(request.begin() + 6, request.begin() + 13, + response.begin() + static_cast(offset + 6U))) { + error = "PX6AXGen3 response does not match request (length/address/function)"; + ++offset; + continue; + } + if (response.size() - offset < expected_size) { + break; + } + uint8_t sum = 0; + for (size_t i = offset; i < offset + expected_size; ++i) { + sum = static_cast(sum + response[i]); + } + if (sum != 0U) { + error = "Invalid PX6AXGen3 response LRC"; + ++offset; + continue; + } + // Manual 5.3.5: read status is internal/debug information; + // hardware returns 0x01 on normal force reads. Only write ACKs + // define 0x00 as success (5.4.2), so do not apply it to reads. + if (request[6] == 0x79 && response[offset + 13U] != 0x00) { + throw std::runtime_error("PX6AXGen3 returned status " + + std::to_string(response[offset + 13U])); + } + return std::vector( + response.begin() + static_cast(offset + kResponseHeaderBytes), + response.begin() + static_cast(offset + kResponseHeaderBytes + payload_length)); + } + if (!read_ok || chunk.empty()) { break; } - - response.insert(response.end(), extra_bytes.begin(), extra_bytes.end()); - frame_offset = findResponseFrameOffset(response, expected_frame_bytes); } - - if (!read_ok && frame_offset == std::string::npos) { - CMVR_LOG(ERROR) << "Failed to read " << response_name << ": " << serial.lastError() - << ", raw=" << previewBytesHex(response); - } - - return response; + throw std::runtime_error(error + "; " + serial.lastError() + + ", raw=" + previewBytesHex(response)); } std::array parseResultantPayload(const std::vector& payload) { @@ -303,21 +328,28 @@ PX6AXGen3::PollingReadMode PX6AXGen3::parsePollingReadModeName(std::string value } PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) - : serial_(std::make_unique<::cmvr::PosixSerialTransport>()), - config_(cfg) { + : PX6AXGen3(cfg, std::make_unique<::cmvr::PosixSerialTransport>()) {} + +PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg, + std::unique_ptr<::cmvr::AbstractSerialTransport> serial) + : serial_(std::move(serial)), config_(cfg) { id_ = config_.id(); port_name_ = config_.serial_port(); if (!config_.sensor_model().empty()) { sensor_model_ = config_.sensor_model(); } - module_id_ = std::max(0, config_.module_id()); + module_id_ = config_.module_id(); + if (!serial_ || module_id_ < 0 || module_id_ > 254) { + enterFault("PX6AXGen3 requires a serial transport and module_id in [0, 254]."); + return; + } device_address_ = module_id_ + 1; if (config_.baud_rate() > 0) { baud_rate_ = config_.baud_rate(); } distributed_length_ = config_.distributed_length(); - if (distributed_length_ <= 0) { - enterFault("PX6AXGen3 requires config.distributed_length to be explicitly configured."); + if (distributed_length_ <= 0 || distributed_length_ > 65525 || distributed_length_ % 3 != 0) { + enterFault("PX6AXGen3 distributed_length must be a positive multiple of 3 fitting the UART frame."); return; } if (config_.resultant_length() > 0) { @@ -326,6 +358,15 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) if (config_.response_header_bytes() > 0) { response_header_bytes_ = config_.response_header_bytes(); } + if (resultant_length_ != 3 || response_header_bytes_ != kResponseHeaderBytes || + config_.resultant_length() < 0 || config_.response_header_bytes() < 0 || + config_.max_sample_age_ms() < 0) { + enterFault("PX6AXGen3 requires resultant_length=3, response_header_bytes=14 and a nonnegative max_sample_age_ms."); + return; + } + if (config_.max_sample_age_ms() > 0) { + max_sample_age_ = std::chrono::milliseconds(config_.max_sample_age_ms()); + } if (config_.tactile_rows() > 0) { tactile_rows_ = config_.tactile_rows(); } @@ -350,6 +391,7 @@ PX6AXGen3::PX6AXGen3(const config::PX6AXGen3& cfg) poll_interval_ = std::chrono::milliseconds(config_.poll_interval_ms()); } initializeSnapshot(); + config_valid_ = true; } PX6AXGen3::~PX6AXGen3() { @@ -357,15 +399,15 @@ PX6AXGen3::~PX6AXGen3() { } bool PX6AXGen3::init() { + if (!config_valid_) { + return false; + } try { - ensureConnected(); - if (!isOperationalState(state())) { - return false; - } refreshSensorDataWithRetry( 5, std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2))); - return isOperationalState(state()); + const auto [tactile, resultant] = resolvePollingReadSelection(); + return isOperationalState(state()) && isSnapshotReady(tactile, resultant); } catch (const std::exception& e) { enterFault("[PX6AXGen3](init): " + std::string(e.what())); return false; @@ -373,8 +415,10 @@ bool PX6AXGen3::init() { } bool PX6AXGen3::start() { + if (!config_valid_) { + return false; + } if (polling_thread_running_.exchange(true, std::memory_order_acq_rel)) { - transitionTo(Status::STREAMING); return true; } @@ -383,19 +427,22 @@ bool PX6AXGen3::start() { polling_thread_.join(); } - ensureConnected(); - if (!isOperationalState(state())) { - polling_thread_running_.store(false, std::memory_order_release); - return false; + bool requested_polling; + { + std::lock_guard lock(polling_mutex_); + requested_polling = requested_polling_; } - if (requested_polling_) { + if (requested_polling) { refreshSensorDataWithRetry( 5, std::chrono::milliseconds(std::max(10, response_timeout_ms_ / 2))); + } else { + std::lock_guard lock(refresh_mutex_); + ensureConnected(); } - polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this); transitionTo(Status::STREAMING); + polling_thread_ = std::thread(&PX6AXGen3::pollingLoop, this); polling_cv_.notify_all(); return true; } catch (const std::exception& e) { @@ -413,6 +460,8 @@ bool PX6AXGen3::stop() { polling_thread_.join(); } + std::lock_guard lock(refresh_mutex_); + initializeSnapshot(); closeConnection(); if (state() != Status::FAULT) { @@ -434,10 +483,13 @@ std::string PX6AXGen3::lastError() const { void PX6AXGen3::getState(DexHandState& state_out) { DexHandState next_state{}; next_state.is_initialized = isOperationalState(state()); + const auto [tactile, resultant] = resolvePollingReadSelection(); + const bool ready = isSnapshotReady(tactile, resultant); + next_state.is_initialized = next_state.is_initialized && ready; { std::lock_guard lock(snapshot_mutex_); - if (latest_snapshot_.resultant_valid) { + if (latest_snapshot_.resultant_valid && isSampleFresh(latest_snapshot_.resultant_request_time)) { next_state.hands[0].force = latest_snapshot_.resultant_force_tenths[2]; } } @@ -445,6 +497,8 @@ void PX6AXGen3::getState(DexHandState& state_out) { const auto error = lastError(); if (!error.empty()) { next_state.hands[0].error_message.push_back(error); + } else if (!ready) { + next_state.hands[0].error_message.push_back("PX6AXGen3 sample is unavailable or stale."); } state_out = std::move(next_state); @@ -468,7 +522,8 @@ void PX6AXGen3::setTactilePollingRegions(const std::vector& re } polling_cv_.notify_all(); - if (!regions.empty() && isOperationalState(state())) { + if (!regions.empty() && isOperationalState(state()) && + !polling_thread_running_.load(std::memory_order_acquire)) { refreshSensorData(); } } @@ -482,8 +537,7 @@ std::vector PX6AXGen3::getSensorData() { TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion region) { if (!isSupportedRegion(finger, region)) { - CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data."; - return {}; + throw std::invalid_argument("PX6AXGen3 requested tactile region is not configured."); } ensureSensorReady(true, true, false); @@ -492,16 +546,14 @@ TactileRegionData PX6AXGen3::getSensorData(FingerType finger, TactileRegion regi PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, TactileRegion region) { if (!isSupportedRegion(finger, region)) { - CMVR_LOG(ERROR) << "PX6AXGen3 only supports INDEX/TIP tactile data."; - return {}; + throw std::invalid_argument("PX6AXGen3 requested resultant-force region is not configured."); } ensureSensorReady(true, false, true); std::lock_guard lock(snapshot_mutex_); - if (!latest_snapshot_.resultant_valid) { - CMVR_LOG(ERROR) << "PX6AXGen3 resultant-force snapshot is not ready."; - return {}; + if (!latest_snapshot_.resultant_valid || !isSampleFresh(latest_snapshot_.resultant_request_time)) { + throw std::runtime_error("PX6AXGen3 resultant-force sample is unavailable or stale."); } return ResultantForce{ @@ -511,29 +563,28 @@ PX6AXGen3::ResultantForce PX6AXGen3::getResultantForce(FingerType finger, Tactil }; } +PX6AXGen3::ForceNewtons PX6AXGen3::getResultantForceNewtons(FingerType finger, TactileRegion region) { + const auto raw = getResultantForce(finger, region); + // PX-6AX GEN3 manual 5.6.2: one resultant-force LSB is 0.1 N. + constexpr double kNewtonsPerCount = 0.1; + return {raw.fx * kNewtonsPerCount, raw.fy * kNewtonsPerCount, raw.fz * kNewtonsPerCount}; +} + void PX6AXGen3::initializeSnapshot() { std::lock_guard lock(snapshot_mutex_); latest_snapshot_ = SensorSnapshot{}; } void PX6AXGen3::ensureConnected() { - if (port_name_.empty()) { - enterFault("PX6AXGen3 serial port is not configured."); - return; + if (!config_valid_ || port_name_.empty()) { + throw std::runtime_error("PX6AXGen3 serial/configuration is invalid."); } - - if (!serial_) { - serial_ = std::make_unique<::cmvr::PosixSerialTransport>(); - } - if (!serial_->isOpen()) { if (!serial_->open(::cmvr::AbstractSerialTransport::Config{port_name_, baud_rate_})) { - enterFault("Failed to open PX-6AX GEN3 serial transport: " + serial_->lastError()); - return; + throw std::runtime_error("Failed to open PX6AXGen3 serial transport: " + serial_->lastError()); } calibration_performed_ = false; } - calibrateIfRequested(); if (!isOperationalState(state())) { transitionTo(Status::INITIALIZED); @@ -553,27 +604,9 @@ void PX6AXGen3::calibrateIfRequested() { if (!auto_calibrate_ || calibration_performed_) { return; } - const auto frame = buildCommandFrame(CommandType::CALIBRATION, device_address_, distributed_length_); - if (!serial_->flushInput()) { - enterFault("Failed to flush serial input before calibration: " + serial_->lastError()); - return; - } - if (!serial_->write(frame)) { - enterFault("Failed to send calibration command: " + serial_->lastError()); - return; - } - const auto response = readFramedResponse( - *serial_, - 2U, - frame.size(), - std::chrono::milliseconds(response_timeout_ms_), - "calibration response"); - if (findResponseFrameOffset(response, 2U) == std::string::npos) { - enterFault("PX-6AX GEN3 calibration command did not receive a valid acknowledgment. raw=" + - previewBytesHex(response)); - return; - } + // Write acknowledgment has a complete 14-byte header and LRC, no payload. + transact(*serial_, frame, 0U, std::chrono::milliseconds(response_timeout_ms_)); calibration_performed_ = true; } @@ -584,117 +617,51 @@ void PX6AXGen3::refreshSensorData() { void PX6AXGen3::refreshSensorData(const bool read_distributed, const bool read_resultant) { if (!read_distributed && !read_resultant) { - CMVR_LOG(ERROR) << "PX6AXGen3 refreshSensorData requires at least one data type to read."; - return; + throw std::invalid_argument("PX6AXGen3 refresh requires at least one data type."); } std::lock_guard refresh_lock(refresh_mutex_); - const bool had_valid_snapshot = isSnapshotReady(read_distributed, read_resultant); - try { ensureConnected(); - if (!isOperationalState(state())) { - return; - } - - std::vector tactile_points; - int rows = 0; - int cols = 0; - bool tactile_valid = false; - - if (read_distributed) { - const auto distributed_frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_); - if (!serial_->flushInput()) { - handleRefreshFailure("Failed to flush serial input before distributed-force read: " + serial_->lastError(), had_valid_snapshot); - return; - } - if (!serial_->write(distributed_frame)) { - handleRefreshFailure("Failed to send distributed-force command: " + serial_->lastError(), had_valid_snapshot); - return; - } - const size_t distributed_frame_bytes = - static_cast(response_header_bytes_) + static_cast(distributed_length_); - const auto distributed_response = readFramedResponse( - *serial_, - distributed_frame_bytes, - distributed_frame.size(), - std::chrono::milliseconds(response_timeout_ms_), - "distributed tactile response"); - const auto distributed_frame_offset = - findResponseFrameOffset(distributed_response, distributed_frame_bytes); - if (distributed_frame_offset == std::string::npos) { - handleRefreshFailure("Invalid distributed tactile response header. raw=" + previewBytesHex(distributed_response), had_valid_snapshot); - return; - } - - tactile_points = parseDistributedPayload(extractPayload( - distributed_response, - distributed_frame_offset, - static_cast(response_header_bytes_), - static_cast(distributed_length_))); - std::tie(rows, cols) = resolveMatrixShape(tactile_rows_, tactile_cols_, tactile_points.size()); - tactile_valid = !tactile_points.empty(); - } - - std::array resultant_force_tenths{}; - bool resultant_valid = false; + // Publish force first: a slower distributed read must not postpone a + // contact measurement. Each channel has its own request timestamp. if (read_resultant) { - try { - const auto resultant_frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_); - if (!serial_->flushInput()) { - handleRefreshFailure("Failed to flush serial input before resultant-force read: " + serial_->lastError(), had_valid_snapshot); - return; - } - if (!serial_->write(resultant_frame)) { - handleRefreshFailure("Failed to send resultant-force command: " + serial_->lastError(), had_valid_snapshot); - return; - } - const size_t resultant_frame_bytes = - static_cast(response_header_bytes_) + static_cast(resultant_length_); - const auto resultant_response = readFramedResponse( - *serial_, - resultant_frame_bytes, - resultant_frame.size(), - std::chrono::milliseconds(response_timeout_ms_), - "resultant-force response"); - const auto resultant_frame_offset = - findResponseFrameOffset(resultant_response, resultant_frame_bytes); - if (resultant_frame_offset != std::string::npos) { - resultant_force_tenths = parseResultantPayload(extractPayload( - resultant_response, - resultant_frame_offset, - static_cast(response_header_bytes_), - static_cast(resultant_length_))); - resultant_valid = true; - } else { - handleRefreshFailure("Invalid resultant-force response header. raw=" + previewBytesHex(resultant_response), had_valid_snapshot); - return; - } - } catch (const std::exception& e) { - if (!read_distributed) { - handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot); - return; - } - CMVR_LOG(WARNING) << "[PX6AXGen3] Failed to refresh resultant force: " << e.what(); + const auto frame = buildCommandFrame(CommandType::RESULTANT_FORCE, device_address_, distributed_length_); + const auto request_time = std::chrono::steady_clock::now(); + const auto payload = transact(*serial_, frame, resultant_length_, + std::chrono::milliseconds(response_timeout_ms_)); + if (!isSampleFresh(request_time)) { + throw std::runtime_error("PX6AXGen3 resultant-force response arrived too late."); } + const auto force = parseResultantPayload(payload); + std::lock_guard lock(snapshot_mutex_); + latest_snapshot_.resultant_force_tenths = force; + latest_snapshot_.resultant_request_time = request_time; + latest_snapshot_.resultant_valid = true; } - - std::lock_guard lock(snapshot_mutex_); if (read_distributed) { - latest_snapshot_.tactile_points = std::move(tactile_points); + const auto frame = buildCommandFrame(CommandType::DISTRIBUTED_FORCE, device_address_, distributed_length_); + const auto request_time = std::chrono::steady_clock::now(); + const auto payload = transact(*serial_, frame, distributed_length_, + std::chrono::milliseconds(response_timeout_ms_)); + if (!isSampleFresh(request_time)) { + throw std::runtime_error("PX6AXGen3 distributed-force response arrived too late."); + } + auto points = parseDistributedPayload(payload); + const auto [rows, cols] = resolveMatrixShape(tactile_rows_, tactile_cols_, points.size()); + std::lock_guard lock(snapshot_mutex_); + latest_snapshot_.tactile_points = std::move(points); latest_snapshot_.rows = rows; latest_snapshot_.cols = cols; - latest_snapshot_.tactile_valid = tactile_valid; + latest_snapshot_.tactile_request_time = request_time; + latest_snapshot_.tactile_valid = true; } - if (read_resultant) { - latest_snapshot_.resultant_force_tenths = resultant_force_tenths; - latest_snapshot_.resultant_valid = resultant_valid; - } - clearOperationalError(); } catch (const std::exception& e) { - handleRefreshFailure("[PX6AXGen3](refreshSensorData): " + std::string(e.what()), had_valid_snapshot); - return; + handleRefreshFailure(e.what()); + // Let initialization retries and synchronous callers see the failure. + // The polling thread catches it and continues reconnecting in background. + throw; } } @@ -715,7 +682,7 @@ void PX6AXGen3::refreshSensorDataWithRetry(const int max_attempts, } } - handleRefreshFailure(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error, isSnapshotReady(true, true)); + throw std::runtime_error(last_error.empty() ? "PX6AXGen3 refresh retries exhausted." : last_error); } void PX6AXGen3::pollingLoop() { @@ -754,18 +721,21 @@ void PX6AXGen3::pollingLoop() { void PX6AXGen3::ensureSensorReady(const bool allow_background, const bool require_tactile, const bool require_resultant) { + const auto lifecycle = state(); + if (!config_valid_ || lifecycle == Status::STOPPED || lifecycle == Status::FAULT) { + throw std::runtime_error("PX6AXGen3 is not operational: " + lastError()); + } const auto [polls_tactile, polls_resultant] = resolvePollingReadSelection(); const bool background_covers_request = (!require_tactile || polls_tactile) && (!require_resultant || polls_resultant); - const bool background_ready = allow_background && - background_covers_request && - polling_thread_running_.load(std::memory_order_acquire) && - isSnapshotReady(require_tactile, require_resultant); - - if (!background_ready) { - refreshSensorData(require_tactile, require_resultant); + if (allow_background && background_covers_request && + polling_thread_running_.load(std::memory_order_acquire)) { + // The getter checks freshness while copying under snapshot_mutex_. + // Never block the control loop on serial I/O to replace a stale sample. + return; } + refreshSensorData(require_tactile, require_resultant); } bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion region) const { @@ -774,8 +744,15 @@ bool PX6AXGen3::isSupportedRegion(const FingerType finger, const TactileRegion r bool PX6AXGen3::isSnapshotReady(const bool require_tactile, const bool require_resultant) const { std::lock_guard lock(snapshot_mutex_); - return (!require_tactile || latest_snapshot_.tactile_valid) && - (!require_resultant || latest_snapshot_.resultant_valid); + return (!require_tactile || (latest_snapshot_.tactile_valid && + isSampleFresh(latest_snapshot_.tactile_request_time))) && + (!require_resultant || (latest_snapshot_.resultant_valid && + isSampleFresh(latest_snapshot_.resultant_request_time))); +} + +bool PX6AXGen3::isSampleFresh(const std::chrono::steady_clock::time_point request_time) const { + return request_time != std::chrono::steady_clock::time_point{} && + std::chrono::steady_clock::now() - request_time <= max_sample_age_; } TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const { @@ -785,9 +762,8 @@ TactileRegionData PX6AXGen3::buildSupportedRegionSnapshot() const { { std::lock_guard lock(snapshot_mutex_); - if (!latest_snapshot_.tactile_valid) { - CMVR_LOG(ERROR) << "PX6AXGen3 tactile snapshot is not ready."; - return {}; + if (!latest_snapshot_.tactile_valid || !isSampleFresh(latest_snapshot_.tactile_request_time)) { + throw std::runtime_error("PX6AXGen3 tactile sample is unavailable or stale."); } *snapshot = latest_snapshot_.tactile_points; rows = latest_snapshot_.rows; @@ -823,21 +799,15 @@ void PX6AXGen3::clearOperationalError() { } } -void PX6AXGen3::handleRefreshFailure(const std::string& error, const bool had_valid_snapshot) { - closeConnection(); - - if (had_valid_snapshot) { - { - std::lock_guard lock(lifecycle_mutex_); - if (lifecycle_state_ == Status::INITIALIZED || lifecycle_state_ == Status::STREAMING) { - last_error_ = error; - } - } - CMVR_LOG(WARNING) << error; - return; +void PX6AXGen3::handleRefreshFailure(const std::string& error) { + // Invalidate before any close/reconnect work. Keep the worker alive so a + // subsequent valid transaction can restore service, but never expose old data. + initializeSnapshot(); + { + std::lock_guard lock(lifecycle_mutex_); + last_error_ = error; } - - enterFault(error); + closeConnection(); } void PX6AXGen3::transitionTo(const Status next_state) { @@ -857,6 +827,7 @@ void PX6AXGen3::enterFault(const std::string& error) { polling_thread_running_.store(false, std::memory_order_release); polling_cv_.notify_all(); + initializeSnapshot(); closeConnection(); CMVR_LOG(ERROR) << error; diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp new file mode 100644 index 00000000..4459a289 --- /dev/null +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_real_test.cpp @@ -0,0 +1,201 @@ +#include "../include/px_6ax_gen3.h" +#include "hardware/include/posix_serial_transport.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { +using Clock = std::chrono::steady_clock; +volatile std::sig_atomic_t interrupted = 0; +void interrupt(int) { interrupted = 1; } + +// Wrap the actual POSIX serial transport; optionally capture every transmitted +// and received byte to diagnose framing/USB latency without a second reader. +class TraceTransport final : public cmvr::AbstractSerialTransport { +public: + explicit TraceTransport(const std::string& path) { + if (!path.empty()) { + trace_.open(path); + if (!trace_) throw std::runtime_error("Cannot open raw log: " + path); + } + } + bool open(const Config& cfg) override { return serial_.open(cfg); } + bool close() override { return serial_.close(); } + bool isOpen() const override { return serial_.isOpen(); } + bool flushInput() override { return serial_.flushInput(); } + bool write(const std::vector& data) override { + // Only permit sensor read requests, even if driver defaults change. + if (data.size() != 14 || data[6] != 0xFB) { + throw std::runtime_error("Real test only permits force read commands."); + } + const bool ok = serial_.write(data); + record(ok ? "TX" : "TX_FAILED", data); + return ok; + } + bool read(size_t n, std::chrono::milliseconds timeout, std::vector& data) override { + const bool ok = serial_.read(n, timeout, data); + record(ok ? "RX" : "RX_FAILED", data); + return ok; + } + std::string lastError() const override { return serial_.lastError(); } +private: + void record(const char* direction, const std::vector& data) { + if (!trace_.is_open()) return; + trace_ << std::fixed << std::setprecision(3) + << std::chrono::duration(Clock::now() - begin_).count() + << " ms " << direction; + for (const auto byte : data) { + trace_ << ' ' << std::hex << std::setw(2) << std::setfill('0') << static_cast(byte); + } + trace_ << std::dec << std::setfill(' ') << '\n'; + trace_.flush(); + } + cmvr::PosixSerialTransport serial_; + std::ofstream trace_; + Clock::time_point begin_{Clock::now()}; +}; + +struct Options { + std::string port{"/dev/ttyACM0"}; + std::string mode{"sync"}; + std::string csv; + std::string raw_log; + int module_id{2}; + int poll_ms{5}; + int max_age_ms{50}; + int timeout_ms{200}; + double duration_s{10.0}; +}; + +void usage() { + std::cout << "Usage: px_6ax_gen3_real_test [--port /dev/ttyACM0] [--module-id 2]\n" + " [--duration-s 10] [--mode sync|stream] [--poll-ms 5]\n" + " [--max-age-ms 50] [--timeout-ms 200] [--csv samples.csv]\n" + " [--raw-log frames.log]\n" + "sync: each read sends a new force request; latency is request/response time.\n" + "stream: exercise background polling and cached getters used by the task;\n" + " getter counts include repeated samples, not sensor update frequency.\n" + "Only reads the sensor. No calibration or robot commands. Ctrl-C stops.\n"; +} + +Options parseOptions(int argc, char** argv) { + Options o; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (arg == "--help") { usage(); std::exit(0); } + if (++i >= argc) throw std::invalid_argument("Missing value for " + arg); + const std::string value = argv[i]; + if (arg == "--port") o.port = value; + else if (arg == "--mode") o.mode = value; + else if (arg == "--module-id") o.module_id = std::stoi(value); + else if (arg == "--duration-s") o.duration_s = std::stod(value); + else if (arg == "--poll-ms") o.poll_ms = std::stoi(value); + else if (arg == "--max-age-ms") o.max_age_ms = std::stoi(value); + else if (arg == "--timeout-ms") o.timeout_ms = std::stoi(value); + else if (arg == "--csv") o.csv = value; + else if (arg == "--raw-log") o.raw_log = value; + else throw std::invalid_argument("Unknown option: " + arg); + } + if ((o.mode != "sync" && o.mode != "stream") || !std::isfinite(o.duration_s) || + o.duration_s <= 0 || o.poll_ms <= 0 || o.max_age_ms <= 0 || o.timeout_ms <= 0) { + throw std::invalid_argument("Invalid mode, duration or timing option"); + } + return o; +} + +double percentile(const std::vector& sorted, double fraction) { + return sorted.empty() ? 0.0 : sorted[static_cast((sorted.size() - 1) * fraction)]; +} +} + +int main(int argc, char** argv) { + try { + const auto options = parseOptions(argc, argv); + std::signal(SIGINT, interrupt); + std::signal(SIGTERM, interrupt); + cmvr::config::PX6AXGen3 cfg; + cfg.set_serial_port(options.port); + cfg.set_module_id(options.module_id); + cfg.set_baud_rate(921600); + cfg.set_distributed_length(153); + cfg.set_resultant_length(3); + cfg.set_poll_interval_ms(options.poll_ms); + cfg.set_response_timeout_ms(options.timeout_ms); + cfg.set_max_sample_age_ms(options.max_age_ms); + cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE); + cfg.set_auto_calibrate(false); + cmvr::device::PX6AXGen3 sensor(cfg, std::make_unique(options.raw_log)); + std::ofstream csv; + if (!options.csv.empty()) { + csv.open(options.csv); + if (!csv) throw std::runtime_error("Cannot open CSV: " + options.csv); + csv << "elapsed_ms,read_ms,valid,fx_N,fy_N,fz_N\n"; + } + std::cout << "port=" << options.port << " module_id=" << options.module_id + << " device_address=" << options.module_id + 1 << " mode=" << options.mode + << " max_sample_age_ms=" << options.max_age_ms << '\n'; + if (!sensor.init()) throw std::runtime_error("Sensor init failed: " + sensor.lastError()); + if (options.mode == "stream" && !sensor.start()) { + throw std::runtime_error("Sensor start failed: " + sensor.lastError()); + } + const auto begin = Clock::now(); + auto next = begin; + auto next_print = begin; + std::vector durations; + size_t failures = 0; + double max_fz = 0.0; + while (!interrupted && std::chrono::duration(Clock::now() - begin).count() < options.duration_s) { + const auto read_begin = Clock::now(); + cmvr::device::AbstractDexHand::ForceNewtons force; + bool valid = true; + std::string error; + try { + force = sensor.getResultantForceNewtons(cmvr::device::AbstractDexHand::FingerType::INDEX, + cmvr::device::AbstractDexHand::TactileRegion::TIP); + } catch (const std::exception& e) { + valid = false; + error = e.what(); + ++failures; + } + const auto now = Clock::now(); + const double read_ms = std::chrono::duration(now - read_begin).count(); + const double elapsed_ms = std::chrono::duration(now - begin).count(); + if (valid) { durations.push_back(read_ms); max_fz = std::max(max_fz, force.fz); } + if (csv.is_open()) { + csv << std::fixed << std::setprecision(3) << elapsed_ms << ',' << read_ms << ',' << valid; + if (valid) csv << ',' << force.fx << ',' << force.fy << ',' << force.fz; + else csv << ",,,"; + csv << '\n'; + } + if (now >= next_print) { + std::cout << std::fixed << std::setprecision(3) << "t_ms=" << elapsed_ms + << " read_ms=" << read_ms; + if (valid) std::cout << " force_N=[" << force.fx << ',' << force.fy << ',' << force.fz + << "] fz_N=" << force.fz; + else std::cout << " UNAVAILABLE: " << error; + std::cout << std::endl; + next_print = now + std::chrono::milliseconds(100); + } + next += std::chrono::milliseconds(options.poll_ms); + if (next < now) next = now; + std::this_thread::sleep_until(next); + } + const double elapsed_s = std::chrono::duration(Clock::now() - begin).count(); + sensor.stop(); + std::sort(durations.begin(), durations.end()); + std::cout << "SUMMARY mode=" << options.mode << " valid_reads=" << durations.size() + << " unavailable_reads=" << failures << " valid_reads_per_s=" << durations.size() / elapsed_s + << " p50_ms=" << percentile(durations, .5) << " p95_ms=" << percentile(durations, .95) + << " max_ms=" << percentile(durations, 1) << " max_fz_N=" << max_fz << '\n'; + return durations.empty() || failures != 0 ? 1 : 0; + } catch (const std::exception& e) { + std::cerr << e.what() << '\n'; + return 1; + } +} diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp new file mode 100644 index 00000000..fee9564a --- /dev/null +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp @@ -0,0 +1,318 @@ +#include "../include/px_6ax_gen3.h" + +#include +#include +#include +#include +#include + +namespace { +using Sensor = cmvr::device::PX6AXGen3; +using Bytes = std::vector; +using namespace std::chrono_literals; + +void checksum(Bytes& frame) { + unsigned sum = 0; + for (size_t i = 0; i + 1 < frame.size(); ++i) sum += frame[i]; + frame.back() = static_cast(-sum); +} + +Bytes reply(const Bytes& request) { + const bool is_write = request[6] == 0x79; + const size_t n = is_write ? 0U : request[11] | (size_t(request[12]) << 8U); + Bytes frame{0xAA, 0x55, static_cast((n + 10U) & 0xffU), + static_cast((n + 10U) >> 8U)}; + frame.insert(frame.end(), request.begin() + 4, request.begin() + 13); + // Real device returns status=1 on reads; zero is only the write ACK status. + frame.push_back(is_write ? 0 : 1); + for (size_t i = 0; i < n; ++i) { + frame.push_back(i % 3U == 0 ? 0x80 : (i % 3U == 1 ? 0x7f : 10)); + } + frame.push_back(0); + checksum(frame); + return frame; +} + +class ScriptedTransport final : public cmvr::AbstractSerialTransport { +public: + bool open(const Config&) override { opened = true; return true; } + bool close() override { opened = false; return true; } + bool isOpen() const override { return opened; } + bool flushInput() override { pending.clear(); return true; } + bool write(const Bytes& request) override { + ++writes; + unsigned sum = 0; + for (auto byte : request) sum += byte; + EXPECT_EQ(sum % 256, 0U); + pending = dropping.load() ? Bytes{} : make_reply(request); + return true; + } + bool read(size_t, std::chrono::milliseconds timeout, Bytes& out) override { + out.clear(); + if (pending.empty()) { + waiting.store(true); + std::this_thread::sleep_for(timeout); + waiting.store(false); + return false; + } + const size_t n = std::min(chunk_size, pending.size()); + out.assign(pending.begin(), pending.begin() + n); + pending.erase(pending.begin(), pending.begin() + n); + return true; + } + std::string lastError() const override { return "scripted timeout"; } + std::function make_reply{reply}; + size_t chunk_size{256}; + std::atomic writes{0}; + std::atomic dropping{false}; + std::atomic waiting{false}; +private: + bool opened{false}; + Bytes pending; +}; + +cmvr::config::PX6AXGen3 config() { + cmvr::config::PX6AXGen3 cfg; + cfg.set_serial_port("scripted"); + cfg.set_module_id(2); + cfg.set_distributed_length(153); + cfg.set_poll_interval_ms(5); + cfg.set_response_timeout_ms(5); + cfg.set_max_sample_age_ms(50); + cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_RESULTANT_FORCE); + return cfg; +} + +Sensor::ResultantForce force(Sensor& sensor) { + return sensor.getResultantForce(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP); +} + +Sensor::ForceNewtons forceNewtons(cmvr::device::AbstractDexHand& sensor) { + return sensor.getResultantForceNewtons(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP); +} + +bool waitFor(const std::function& predicate) { + const auto deadline = std::chrono::steady_clock::now() + 1s; + do { + if (predicate()) return true; + std::this_thread::sleep_for(1ms); + } while (std::chrono::steady_clock::now() < deadline); + return predicate(); +} + +TEST(PX6AXGen3, ParsesRealReadStatusAndSignedForcesAcrossByteFragments) { + auto serial = std::make_unique(); + serial->chunk_size = 1; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + const auto f = force(sensor); + EXPECT_EQ(f.fx, -128); + EXPECT_EQ(f.fy, 127); + EXPECT_EQ(f.fz, 10); +} + +TEST(PX6AXGen3, NewtonInterfacePreservesSignsAndComputesPhysicalMagnitude) { + auto serial = std::make_unique(); + auto* transport = serial.get(); + serial->make_reply = [](const Bytes& request) { + auto frame = reply(request); + frame[14] = 0xfd; // -3 raw = -0.3 N + frame[15] = 4; + frame[16] = 12; + checksum(frame); + return frame; + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + const auto writes_before = transport->writes.load(); + const auto f = forceNewtons(sensor); + EXPECT_EQ(transport->writes.load(), writes_before + 1); + EXPECT_DOUBLE_EQ(f.fx, -0.3); + EXPECT_DOUBLE_EQ(f.fy, 0.4); + EXPECT_DOUBLE_EQ(f.fz, 1.2); + EXPECT_DOUBLE_EQ(f.magnitude(), 1.3); + EXPECT_EQ(force(sensor).fz, 12); // Raw interface remains unchanged. +} + +class NewtonForce : public testing::TestWithParam {}; +TEST_P(NewtonForce, ConvertsRawFzOnceAndPreservesPointOneNewtonTrigger) { + auto serial = std::make_unique(); + const int raw_fz = GetParam(); + serial->make_reply = [raw_fz](const Bytes& request) { + auto frame = reply(request); + frame[14] = 0; + frame[15] = 0; + frame[16] = static_cast(raw_fz); + checksum(frame); + return frame; + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + const auto f = forceNewtons(sensor); + EXPECT_DOUBLE_EQ(f.fz, static_cast(raw_fz) / 10.0); + EXPECT_DOUBLE_EQ(f.magnitude(), f.fz); + EXPECT_EQ(f.fz >= 0.1, raw_fz >= 1); + EXPECT_EQ(force(sensor).fz, raw_fz); +} +INSTANTIATE_TEST_SUITE_P(PhysicalUnits, NewtonForce, testing::Values(0, 1, 10, 108, 255)); + +TEST(PX6AXGen3, ResynchronizesAfterEchoNoiseAndInvalidFrame) { + auto serial = std::make_unique(); + serial->chunk_size = 7; + serial->make_reply = [](const Bytes& request) { + auto bad = reply(request); + bad.back() ^= 1; + Bytes frames{0xAA, 0x00}; + frames.insert(frames.end(), request.begin(), request.end()); + frames.insert(frames.end(), bad.begin(), bad.end()); + const auto valid = reply(request); + frames.insert(frames.end(), valid.begin(), valid.end()); + return frames; + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + EXPECT_EQ(force(sensor).fz, 10); +} + +class InvalidFrame : public testing::TestWithParam {}; +TEST_P(InvalidFrame, RejectsResponseAndFailsInitializationAfterRetries) { + auto serial = std::make_unique(); + auto* transport = serial.get(); + const int kind = GetParam(); + serial->make_reply = [kind](const Bytes& request) { + auto frame = reply(request); + if (kind == 0) { frame.pop_back(); return frame; } // missing LRC + if (kind == 1) { frame.back() ^= 1; return frame; } // bad LRC + // Frame size, device, reserved, function, register, returned byte count. + const int offsets[]{2, 4, 5, 6, 7, 11}; + frame[offsets[kind - 2]] ^= 1; + checksum(frame); // Valid LRC must not bypass request matching. + return frame; + }; + Sensor sensor(config(), std::move(serial)); + EXPECT_FALSE(sensor.init()); + EXPECT_EQ(transport->writes.load(), 5); + EXPECT_EQ(sensor.state(), Sensor::Status::FAULT); + EXPECT_FALSE(sensor.lastError().empty()); + EXPECT_THROW(force(sensor), std::runtime_error); +} +INSTANTIATE_TEST_SUITE_P(ProtocolValidation, InvalidFrame, testing::Range(0, 8)); + +TEST(PX6AXGen3, RetriesTransientStartupFailure) { + auto serial = std::make_unique(); + int count = 0; + serial->make_reply = [&count](const Bytes& request) { + ++count; + return count < 3 ? Bytes{} : reply(request); + }; + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + EXPECT_EQ(count, 3); + EXPECT_TRUE(sensor.lastError().empty()); +} + +TEST(PX6AXGen3, StaleGetterFailsWithoutWaitingForSerialAndRecovers) { + auto cfg = config(); + cfg.set_response_timeout_ms(200); + cfg.set_max_sample_age_ms(30); + auto serial = std::make_unique(); + auto* transport = serial.get(); + Sensor sensor(cfg, std::move(serial)); + ASSERT_TRUE(sensor.init()); + ASSERT_TRUE(sensor.start()); + ASSERT_EQ(force(sensor).fz, 10); + transport->dropping.store(true); + ASSERT_TRUE(waitFor([&] { return transport->waiting.load(); })); + std::this_thread::sleep_for(40ms); + const auto begin = std::chrono::steady_clock::now(); + EXPECT_THROW(force(sensor), std::runtime_error); + EXPECT_THROW(forceNewtons(sensor), std::runtime_error); + EXPECT_LT(std::chrono::steady_clock::now() - begin, 50ms); + cmvr::device::DexHandState state; + sensor.getState(state); + EXPECT_FALSE(state.is_initialized); + EXPECT_FALSE(state.hands[0].error_message.empty()); + transport->dropping.store(false); + ASSERT_TRUE(waitFor([&] { + try { return force(sensor).fz == 10; } + catch (const std::exception&) { return false; } + })); + sensor.stop(); + const auto writes = transport->writes.load(); + EXPECT_THROW(force(sensor), std::runtime_error); + EXPECT_THROW(forceNewtons(sensor), std::runtime_error); + EXPECT_EQ(transport->writes.load(), writes); +} + +TEST(PX6AXGen3, ReadFailureInvalidatesPreviouslyValidSample) { + auto serial = std::make_unique(); + auto* transport = serial.get(); + Sensor sensor(config(), std::move(serial)); + ASSERT_TRUE(sensor.init()); + transport->dropping.store(true); + EXPECT_THROW(force(sensor), std::runtime_error); + cmvr::device::DexHandState state; + sensor.getState(state); + EXPECT_FALSE(state.is_initialized); + transport->dropping.store(false); + EXPECT_EQ(force(sensor).fz, 10); +} + +TEST(PX6AXGen3, RejectsLateResponseEvenWithValidChecksum) { + auto cfg = config(); + cfg.set_max_sample_age_ms(2); + auto serial = std::make_unique(); + serial->make_reply = [](const Bytes& request) { + std::this_thread::sleep_for(5ms); + return reply(request); + }; + Sensor sensor(cfg, std::move(serial)); + EXPECT_FALSE(sensor.init()); + EXPECT_NE(sensor.lastError().find("too late"), std::string::npos); +} + +TEST(PX6AXGen3, DistributedDataIsValidatedAndParsed) { + auto cfg = config(); + cfg.set_polling_read_mode(cmvr::config::PX_6AX_GEN3_POLLING_READ_MODE_DISTRIBUTED_FORCE); + auto serial = std::make_unique(); + serial->chunk_size = 5; + Sensor sensor(cfg, std::move(serial)); + ASSERT_TRUE(sensor.init()); + auto data = sensor.getSensorData(Sensor::FingerType::INDEX, Sensor::TactileRegion::TIP); + ASSERT_TRUE(data.valid()); + ASSERT_EQ(data.view.pointCount(), 51); + EXPECT_EQ(data.view.at(0, 50).fx, -128); + EXPECT_EQ(data.view.at(0, 50).fz, 10); +} + +TEST(PX6AXGen3, CalibrationRequiresFullSuccessfulWriteAcknowledgment) { + auto cfg = config(); + cfg.set_auto_calibrate(true); + auto serial = std::make_unique(); + serial->make_reply = [](const Bytes& request) { + if (request[6] == 0x79) return Bytes{0xAA, 0x55}; + return reply(request); + }; + Sensor sensor(cfg, std::move(serial)); + EXPECT_FALSE(sensor.init()); +} + +TEST(PX6AXGen3, CalibrationAcceptsFullWriteAcknowledgment) { + auto cfg = config(); + cfg.set_auto_calibrate(true); + Sensor sensor(cfg, std::make_unique()); + ASSERT_TRUE(sensor.init()); + EXPECT_EQ(force(sensor).fz, 10); +} + +TEST(PX6AXGen3, InvalidConfigurationCannotBeResurrectedByInitOrStart) { + auto cfg = config(); + cfg.set_response_header_bytes(15); + auto serial = std::make_unique(); + auto* transport = serial.get(); + Sensor sensor(cfg, std::move(serial)); + EXPECT_FALSE(sensor.init()); + EXPECT_FALSE(sensor.start()); + EXPECT_EQ(transport->writes.load(), 0); +} +} diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt new file mode 100644 index 00000000..429666eb --- /dev/null +++ b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt @@ -0,0 +1,13 @@ +add_library(zero_sim_touch_dexhand SHARED src/zero_sim_touch_dexhand.cpp) + +target_include_directories(zero_sim_touch_dexhand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +add_library(cmvr_es::device::zero_sim_touch_dexhand ALIAS zero_sim_touch_dexhand) + +target_link_libraries(zero_sim_touch_dexhand + PRIVATE + cmvr_es::proto + glog +) + +install(TARGETS zero_sim_touch_dexhand LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h new file mode 100644 index 00000000..05f96794 --- /dev/null +++ b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h @@ -0,0 +1,58 @@ +#ifndef CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H +#define CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H + +#include +#include +#include +#include + +#include "cmvr/config/dexhand_config/dexhand_config.pb.h" +#include "../../abstract_dexhand.h" + +namespace cmvr::device { + +class ZeroSimTouchDexHand final : public AbstractDexHand { +public: + using FingerType = AbstractDexHand::FingerType; + using ResultantForce = AbstractDexHand::ResultantForce; + using TactilePoint = AbstractDexHand::TactilePoint; + using TactileRegion = AbstractDexHand::TactileRegion; + using TactileRegionKey = AbstractDexHand::TactileRegionKey; + using TactileRegionData = AbstractDexHand::TactileRegionData; + using Status = AbstractDexHand::Status; + + explicit ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg); + ~ZeroSimTouchDexHand() override = default; + + std::string typeName() const override { return "ZeroSimTouchDexHand"; } + bool init() override; + bool start() override; + bool stop() override; + + Status state() const override; + std::string lastError() const override; + void getState(DexHandState& state) override; + + void setAngles(const std::vector& finger_joint_angles) override; + void setTactilePollingRegions(const std::vector& regions) override; + std::vector getSensorData() override; + TactileRegionData getSensorData(FingerType finger, TactileRegion region) override; + ResultantForce getResultantForce(FingerType finger, TactileRegion region) override; + ForceNewtons getResultantForceNewtons(FingerType finger, TactileRegion region) override; + +private: + TactileRegionData makeRegionData(FingerType finger, TactileRegion region); + + mutable std::mutex mutex_; + std::vector polling_regions_; + std::array tactile_points_{}; + Status lifecycle_state_{Status::CREATED}; + std::string last_error_; +}; + +// Keep the historical spelling available to callers that used the test double. +using ZeroSImTouchDexHand = ZeroSimTouchDexHand; + +} // namespace cmvr::device + +#endif // CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp new file mode 100644 index 00000000..46e95a76 --- /dev/null +++ b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp @@ -0,0 +1,101 @@ +#include "../include/zero_sim_touch_dexhand.h" + +#include + +namespace cmvr::device { + +ZeroSimTouchDexHand::ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg) { + id_ = cfg.id(); +} + +bool ZeroSimTouchDexHand::init() { + std::lock_guard lock(mutex_); + lifecycle_state_ = Status::STREAMING; + last_error_.clear(); + return true; +} + +bool ZeroSimTouchDexHand::start() { + std::lock_guard lock(mutex_); + lifecycle_state_ = Status::STREAMING; + last_error_.clear(); + return true; +} + +bool ZeroSimTouchDexHand::stop() { + std::lock_guard lock(mutex_); + lifecycle_state_ = Status::STOPPED; + return true; +} + +ZeroSimTouchDexHand::Status ZeroSimTouchDexHand::state() const { + std::lock_guard lock(mutex_); + return lifecycle_state_; +} + +std::string ZeroSimTouchDexHand::lastError() const { + std::lock_guard lock(mutex_); + return last_error_; +} + +void ZeroSimTouchDexHand::getState(DexHandState& state) { + std::lock_guard lock(mutex_); + state = DexHandState{}; + state.is_initialized = lifecycle_state_ == Status::INITIALIZED || + lifecycle_state_ == Status::STREAMING; + if (!last_error_.empty()) { + state.hands[0].error_message.push_back(last_error_); + } +} + +void ZeroSimTouchDexHand::setAngles(const std::vector&) { + // The simulation has no finger actuators; commands are intentionally ignored. +} + +void ZeroSimTouchDexHand::setTactilePollingRegions( + const std::vector& regions) { + std::lock_guard lock(mutex_); + polling_regions_ = regions; +} + +std::vector ZeroSimTouchDexHand::getSensorData() { + std::lock_guard lock(mutex_); + if (polling_regions_.empty()) { + polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP}); + } + + std::vector data; + data.reserve(polling_regions_.size()); + for (const auto& region : polling_regions_) { + data.push_back(makeRegionData(region.first, region.second)); + } + return data; +} + +ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::getSensorData( + const FingerType finger, const TactileRegion region) { + std::lock_guard lock(mutex_); + return makeRegionData(finger, region); +} + +ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce( + const FingerType, const TactileRegion) { + return TactilePoint::fromFz(0); +} + +ZeroSimTouchDexHand::ForceNewtons ZeroSimTouchDexHand::getResultantForceNewtons( + const FingerType, const TactileRegion) { + return {}; +} + +ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData( + const FingerType finger, const TactileRegion region) { + tactile_points_[0] = TactilePoint::fromFz(0); + TactileMatrixView view; + view.data = tactile_points_.data(); + view.rows = 1; + view.cols = 1; + return {finger, region, view, "mujoco_touch_tip"}; +} + +} // namespace cmvr::device 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..e3ab5788 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; @@ -189,14 +214,14 @@ namespace cmvr::device{ // 使用的通讯协议 - virtual void setProtocol(std::shared_ptr protocol) { + virtual bool setProtocol(std::shared_ptr protocol) { std::scoped_lock lock(mtx_); protocol_ = std::move(protocol); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->initNode(node_id_); + return protocol_->initNode(node_id_); } uint8_t id() const { 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/can/src/can_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp index 09c1284c..637978e6 100644 --- a/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp @@ -68,16 +68,16 @@ bool CanMotorBusRuntime::start() return false; } - auto ret = sender_->Start(); + auto ret = receiver_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; stop(); return false; } - ret = receiver_->Start(); + ret = sender_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; stop(); return false; } 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..140f7c15 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp @@ -0,0 +1,380 @@ +#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 = "right_arm_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"); + 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..d10a4db2 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_); @@ -44,9 +59,14 @@ namespace cmvr { // canopen_protocol->configProfile(node_id_,4000,8000,8000); 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); + if (!canopen_protocol->setMode( + node_id_, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + CMVR_LOG(ERROR) << "[Ti5Motor] failed to initialize operation mode: " + << info_.joint_name; + return false; + } + // 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 +75,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..cb3951df 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); @@ -61,23 +74,24 @@ namespace cmvr { return data_ptr; } - msgs::RunMode getMode(uint8_t node_id) override { - return GetRobotDetail()->motors().at(node_id).run_mode(); - // cur_mode_[node_id] = feed_mode; - // return feed_mode; - // return cur_mode_[node_id]; - } + msgs::RunMode getMode(uint8_t node_id) override; 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 +108,8 @@ 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); + bool getMotorStatus(uint8_t node_id, msgs::MotorStatus* status) const; + void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos); void configTPDO1(uint8_t node_id); @@ -107,6 +118,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..47fd04c4 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" @@ -66,7 +67,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { if (sdo_commands_[node_id] == nullptr) { CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; + return false; } can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true); @@ -77,7 +78,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { if (rpdo1_commands_[node_id] == nullptr) { CMVR_LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; + return false; } can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); @@ -87,52 +88,187 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { if (rpdo2_commands_[node_id] == nullptr) { CMVR_LOG(ERROR) << "Ti5 Motor RPDO2 Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; + return false; } can_sender_->AddMessage(rpdo2_commands_[node_id]->ID(), rpdo2_commands_[node_id], true); - return ErrorCode::OK; + return true; +} + +bool Ti5MotorCanopenProtocol::getMotorStatus( + const uint8_t node_id, + msgs::MotorStatus* const status) const { + if (!message_manager_ || !status) { + return false; + } + + msgs::RobotDetail robot_detail; + if (message_manager_->GetSensorData(&robot_detail) != ErrorCode::OK) { + return false; + } + const auto it = robot_detail.motors().find(node_id); + if (it == robot_detail.motors().end()) { + return false; + } + status->CopyFrom(it->second); + return true; +} + +cmvr::msgs::RunMode Ti5MotorCanopenProtocol::getMode(const uint8_t node_id) { + msgs::MotorStatus status; + if (!getMotorStatus(node_id, &status)) { + return msgs::RUN_MODE_UNSPECIFIED; + } + return status.run_mode(); +} + +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}; +} + +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, ObIndex index, ObSubIndex sub_index, +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 +277,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,56 +299,72 @@ 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); + + int32_t current_position = 0; + if (mode == RUN_MODE_PROFILE_POSITION || mode == RUN_MODE_CYCLIC_SYNC_POSITION) { + seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); + msgs::MotorStatus status; + if (!waitUntil([&]() { + return getMotorStatus(node_id, &status) && + status.has_sdo_response() && + status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064; + }, 500)) { + CMVR_LOG(ERROR) << "motor " << static_cast(node_id) + << ": actual-position feedback timed out"; + return false; + } + current_position = status.position(); + } 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, current_position); // 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; } case RUN_MODE_CYCLIC_SYNC_POSITION: { // 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, current_position); //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 +372,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 +397,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 +407,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,140 +540,166 @@ 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; + msgs::MotorStatus status; + return getMotorStatus(node_id, &status) && + status.has_sdo_response() && + status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 && + status.position_offset() == 0; }, 1000)) { CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed"; return false; } // 3: 读取当前位置 0x6064 - seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); - auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); + seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); + msgs::MotorStatus status; + if (!waitUntil([&]() { + return getMotorStatus(node_id, &status) && + status.has_sdo_response() && + status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064; + }, 500)) { + CMVR_LOG(ERROR) << "motor " << static_cast(node_id) + << ": actual-position feedback timed out during calibration"; + return false; + } + const auto cur_pos = status.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; + msgs::MotorStatus latest_status; + return getMotorStatus(node_id, &latest_status) && + latest_status.has_sdo_response() && + latest_status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 && + latest_status.position_offset() == cur_pos; }, 500)) { - return false; CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed"; + return false; } 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"; + torqueOff(node_id); + return true; +} + +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) { + msgs::MotorStatus motor_status; + if (!getMotorStatus(node_id, &motor_status)) { + return false; + } statusword_t st{}; - st.value = GetRobotDetail()->motors().at(node_id).status_word(); + st.value = motor_status.status_word(); 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) { - 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); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } + msgs::MotorStatus motor_status; + if (!getMotorStatus(node_id, &motor_status)) { + return 0.0; + } + const auto cnt = motor_status.position(); + return countsToRad(cnt, *conversion); } double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) { - 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); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } + msgs::MotorStatus motor_status; + if (!getMotorStatus(node_id, &motor_status)) { + return 0.0; + } + const auto cnt = motor_status.speed(); + 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..45e8ab3e 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -5,7 +5,6 @@ #include #include #include -#include #include #include @@ -32,7 +31,9 @@ class AbstractMotorBusRuntime; class MotorManager final : public AbstractDevice, public std::enable_shared_from_this { public: - MotorManager(std::string id, const config::MotorConfig& cfg); + MotorManager(std::string id, + const config::MotorConfig& cfg, + std::string selected_group_id); ~MotorManager() override; DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; } @@ -45,19 +46,18 @@ 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); - static void setActiveJoints(const std::string& motor_manager_id, - std::unordered_map> group_joints); - static void clearActiveJoints(); - private: - using ActiveJointSelection = std::unordered_map>; - - bool selectActiveMotors_(const std::string& group_name, - const google::protobuf::RepeatedPtrField& source, - std::vector& selected) const; bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, std::vector& selected) const; std::shared_ptr createBusRuntime_( @@ -81,6 +81,7 @@ private: private: config::MotorConfig cfg_; + std::string selected_group_id_; std::vector> bus_runtimes_; mutable std::mutex motors_mutex_; std::unordered_map> motors_by_id_; @@ -90,7 +91,6 @@ private: static std::mutex registry_mutex_; static std::unordered_map> managers_; static std::unordered_map> mujoco_world_registry_; - static std::unordered_map active_joints_; }; } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index d1e6affe..1d1b262e 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -1,38 +1,41 @@ -#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 { std::mutex MotorManager::registry_mutex_; std::unordered_map> MotorManager::managers_; std::unordered_map> MotorManager::mujoco_world_registry_; -std::unordered_map MotorManager::active_joints_; -MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg) - : cfg_(cfg) +MotorManager::MotorManager(std::string id, + const config::MotorConfig& cfg, + std::string selected_group_id) + : cfg_(cfg), + selected_group_id_(std::move(selected_group_id)) { id_ = std::move(id); - if (!cfg_.id().empty() && cfg_.id() != id_) { - CMVR_LOG(ERROR) << "[MotorManager] config id '" << cfg_.id() - << "' does not match device id '" << id_ << "'"; - id_.clear(); - } } MotorManager::~MotorManager() = default; @@ -46,6 +49,14 @@ bool MotorManager::init() CMVR_LOG(ERROR) << "[MotorManager] id is empty"; return false; } + if (selected_group_id_.empty()) { + CMVR_LOG(ERROR) << "[MotorManager] selected motor group id is empty: " << id_; + return false; + } + if (cfg_.motor_groups_size() == 0) { + CMVR_LOG(ERROR) << "[MotorManager] no motor groups configured: " << id_; + return false; + } bus_runtimes_.clear(); bus_runtimes_.reserve(static_cast(cfg_.motor_groups_size())); @@ -56,25 +67,27 @@ bool MotorManager::init() } bool all_ok = true; + std::size_t selected_group_count = 0; for (const auto& motor_group_cfg : cfg_.motor_groups()) { const auto& group_name = motor_group_cfg.id(); - if (group_name.empty()) { - CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_; - all_ok = false; + if (group_name != selected_group_id_) { continue; } + ++selected_group_count; if (!motor_group_cfg.has_motors()) { CMVR_LOG(ERROR) << "[MotorManager] motor group missing motors: " << group_name; all_ok = false; continue; } - std::vector selected_motor_cfgs; - const bool selected_active_group = selectActiveMotors_( - group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs); - if (!selected_active_group) { + if (motor_group_cfg.motors().motors_size() == 0) { + CMVR_LOG(ERROR) << "[MotorManager] motor group has no motors: " << group_name; + all_ok = false; continue; } + std::vector selected_motor_cfgs( + motor_group_cfg.motors().motors().begin(), + motor_group_cfg.motors().motors().end()); if (!applyConfiguredJointLimits_(motor_group_cfg, selected_motor_cfgs)) { all_ok = false; @@ -91,11 +104,17 @@ bool MotorManager::init() all_ok = false; continue; } - if (!bus_runtime->start()) { + + const bool start_before_motor_creation = + motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT; + if (start_before_motor_creation && !bus_runtime->start()) { bus_runtime->stop(); all_ok = false; continue; } + if (start_before_motor_creation) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime); if (motors.empty()) { @@ -104,7 +123,29 @@ bool MotorManager::init() all_ok = false; continue; } + if (!start_before_motor_creation && !bus_runtime->start()) { + bus_runtime->stop(); + all_ok = false; + continue; + } + bool group_ok = true; + if (motor_group_cfg.bus_type() == config::MOTOR_BUS_CAN) { + for (auto& motor : motors) { + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorManager] failed to initialize CAN motor: " + << motor->jointName(); + group_ok = false; + break; + } + } + } + if (!group_ok) { + bus_runtime->stop(); + all_ok = false; + continue; + } + for (auto& motor : motors) { if (!addMotor(motor)) { group_ok = false; @@ -120,6 +161,12 @@ bool MotorManager::init() bus_runtimes_.push_back(std::move(bus_runtime)); } + if (selected_group_count != 1) { + CMVR_LOG(ERROR) << "[MotorManager] selected motor group '" << selected_group_id_ + << "' must occur exactly once in config: " << id_; + all_ok = false; + } + if (!all_ok) { for (auto& bus_runtime : bus_runtimes_) { if (bus_runtime) { @@ -222,6 +269,66 @@ 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); + } + if (std::dynamic_pointer_cast(motors.front())) { + // TI5 currently exposes only a per-motor RPDO command. Keep the + // fallback here so callers retain one manager-level entry point. + static std::once_flag ti5_non_atomic_warning; + std::call_once(ti5_non_atomic_warning, []() { + CMVR_LOG(WARNING) + << "[MotorManager] TI5 motors use non-atomic cyclic position commands"; + }); + for (std::size_t i = 0; i < motors.size(); ++i) { + if (!std::dynamic_pointer_cast(motors[i])) { + CMVR_LOG(ERROR) << "[MotorManager] mixed motor types in cyclic position command"; + return false; + } + if (!motors[i]->commandCyclicPosition(positions[i], velocities[i])) { + CMVR_LOG(ERROR) << "[MotorManager] failed to command TI5 motor at index " << i; + return false; + } + } + return true; + } + + 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_); @@ -242,68 +349,6 @@ std::shared_ptr MotorManager::mujocoWorldFor(const std::s return it->second.lock(); } -void MotorManager::setActiveJoints(const std::string& motor_manager_id, - ActiveJointSelection group_joints) -{ - std::lock_guard lock(registry_mutex_); - active_joints_[motor_manager_id] = std::move(group_joints); -} - -void MotorManager::clearActiveJoints() -{ - std::lock_guard lock(registry_mutex_); - active_joints_.clear(); -} - -bool MotorManager::selectActiveMotors_( - const std::string& group_name, - const google::protobuf::RepeatedPtrField& source, - std::vector& selected) const -{ - selected.clear(); - - ActiveJointSelection selection; - { - std::lock_guard lock(registry_mutex_); - const auto it = active_joints_.find(id_); - if (it != active_joints_.end()) { - selection = it->second; - } - } - - if (selection.empty()) { - CMVR_LOG(WARNING) << "[MotorManager] No active joints selected for motor manager " << id_ - << ", motor group " << group_name << " will not initialize motors."; - return false; - } - - const auto group_it = selection.find(group_name); - if (group_it == selection.end() || group_it->second.empty()) { - return false; - } - - for (const auto& motor_cfg : source) { - if (group_it->second.count(motor_cfg.joint_name()) > 0) { - selected.push_back(motor_cfg); - } - } - - if (selected.size() != group_it->second.size()) { - std::unordered_set found; - for (const auto& motor_cfg : selected) { - found.insert(motor_cfg.joint_name()); - } - for (const auto& joint_name : group_it->second) { - if (found.count(joint_name) == 0) { - CMVR_LOG(ERROR) << "[MotorManager] active joint '" << joint_name - << "' not found in motor group '" << group_name << "'"; - return false; - } - } - } - return !selected.empty(); -} - bool MotorManager::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg, std::vector& selected) const { @@ -398,8 +443,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()) @@ -464,9 +520,8 @@ std::vector> MotorManager::createCanMotors_( motors.reserve(motor_cfgs.size()); for (const auto& cfg : motor_cfgs) { auto motor = std::make_shared(cfg); - motor->setProtocol(protocol); - if (!motor->init()) { - CMVR_LOG(ERROR) << "[MotorManager] failed to init TI5 motor: " + if (!motor->setProtocol(protocol)) { + CMVR_LOG(ERROR) << "[MotorManager] failed to register TI5 motor protocols: " << cfg.joint_name(); return {}; } @@ -534,6 +589,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 +605,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/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index d680dded..58bedc74 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -8,7 +8,6 @@ #include #include #include -#include #include #include "device_factory.h" #include "cmvr/config/device_manager_config/device_manager_config.pb.h" @@ -49,7 +48,6 @@ namespace cmvr::device { explicit DeviceManager(const config::DeviceManagerConfig &cfg); void log_device_plan_() const; - void pre_scan_robot_arm_dependencies_() const; void init_devices_(); void configure_mujoco_viewer_pip_(); }; diff --git a/cmvr-es/manager/device_manager/src/device_factory.cpp b/cmvr-es/manager/device_manager/src/device_factory.cpp index b1bfe45e..853df6d5 100644 --- a/cmvr-es/manager/device_manager/src/device_factory.cpp +++ b/cmvr-es/manager/device_manager/src/device_factory.cpp @@ -226,22 +226,41 @@ DeviceFactory::DeviceFactory() }); registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM, - [](const auto& entry) { - if (entry.id().empty()) { - CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required"; - return DeviceRecord{}; - } - if (entry.config_file().empty()) { - CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id(); - return DeviceRecord{}; - } - config::MotorRootConfig root_cfg; - if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { - CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; - return DeviceRecord{}; - } + [](const auto& entry) { + if (entry.id().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id is required"; + return DeviceRecord{}; + } + if (entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for motor group ID: " << entry.id(); + return DeviceRecord{}; + } + config::MotorRootConfig root_cfg; + if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail"; + return DeviceRecord{}; + } + + const config::MotorGroupConfig* selected_group = nullptr; + for (const auto& group_cfg : root_cfg.motor().motor_groups()) { + if (group_cfg.id() != entry.id()) { + continue; + } + if (selected_group != nullptr) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Duplicate motor group id '" + << entry.id() << "' in config: " << entry.config_file(); + return DeviceRecord{}; + } + selected_group = &group_cfg; + } + if (selected_group == nullptr) { + CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id '" << entry.id() + << "' not found in config: " << entry.config_file(); + return DeviceRecord{}; + } CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success"; - auto device = std::make_shared(entry.id(), root_cfg.motor()); + auto device = std::make_shared( + entry.id(), root_cfg.motor(), entry.id()); DeviceRecord record; record.id = entry.id(); record.kind = device->kind(); diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index d957afb4..d8d8ac0c 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -18,8 +18,6 @@ #include "devices/speaker/abstract_speaker.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" #include "common/config/config_files.h" -#include "cmvr/config/arm_config/arm_config.pb.h" -#include "cmvr/config/motor_config/motor_config.pb.h" using namespace std; using namespace cmvr::device; @@ -60,17 +58,6 @@ const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType } } -bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group, - const std::string& joint_name) -{ - for (const auto& motor : motor_group.motors().motors()) { - if (motor.joint_name() == joint_name) { - return true; - } - } - return false; -} - } // namespace template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -96,7 +83,6 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { dev_factory_ = std::make_unique(); logSection("Device Plan"); log_device_plan_(); - pre_scan_robot_arm_dependencies_(); logSection("Initialize Devices"); init_devices_(); configure_mujoco_viewer_pip_(); @@ -121,7 +107,6 @@ DeviceManager& DeviceManager::getInstance() { void DeviceManager::destroyInstance() { std::lock_guard lock(init_mutex_); instance_.reset(); - MotorManager::clearActiveJoints(); } void DeviceManager::start(){ @@ -254,145 +239,6 @@ void DeviceManager::log_device_plan_() const CMVR_LOG(INFO) << "[DeviceManager]: Device plan end"; } -void DeviceManager::pre_scan_robot_arm_dependencies_() const -{ - using GroupJointSelection = std::unordered_map>; - std::unordered_map selections; - std::unordered_map motor_roots; - - for (const auto& entry : cfg_.devices()) { - if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) { - continue; - } - if (entry.id().empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty"; - return; - } - if (entry.config_file().empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id(); - return; - } - - config::MotorRootConfig root_cfg; - if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { - CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file(); - return; - } - if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) { - CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id() - << "' does not match config id '" << root_cfg.motor().id() << "'"; - return; - } - motor_roots.emplace(entry.id(), std::move(root_cfg)); - } - - for (const auto& entry : cfg_.devices()) { - if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM) { - continue; - } - if (entry.id().empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty"; - return; - } - if (entry.config_file().empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id(); - return; - } - - config::ArmRootConfig root_cfg; - if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { - CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file(); - return; - } - - const config::RobotArmConfig* arm_cfg = nullptr; - for (const auto& candidate : root_cfg.arm().robot_arms()) { - if (candidate.id() == entry.id()) { - arm_cfg = &candidate; - break; - } - } - if (!arm_cfg) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id() - << "' not found in config: " << entry.config_file(); - return; - } - - if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) { - continue; - } - if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id(); - return; - } - - const auto& motor_config = arm_cfg->motor(); - if (motor_config.motor_system_id().empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id(); - return; - } - if (motor_config.motor_group_ids_size() == 0) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id(); - return; - } - if (motor_config.joint_names_size() == 0) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id(); - return; - } - - const auto motor_root_it = motor_roots.find(motor_config.motor_system_id()); - if (motor_root_it == motor_roots.end()) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() - << "' depends on disabled or missing MotorManager: " - << motor_config.motor_system_id(); - return; - } - - std::unordered_set allowed_groups; - allowed_groups.reserve(static_cast(motor_config.motor_group_ids_size())); - for (const auto& group_id : motor_config.motor_group_ids()) { - if (group_id.empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id(); - return; - } - allowed_groups.insert(group_id); - } - - auto& group_selection = selections[motor_config.motor_system_id()]; - for (const auto& joint_name : motor_config.joint_names()) { - if (joint_name.empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id(); - return; - } - - std::string matched_group; - for (const auto& motor_group : motor_root_it->second.motor().motor_groups()) { - const auto& group_id = motor_group.id(); - if (allowed_groups.count(group_id) == 0) { - continue; - } - if (motorGroupHasJoint(motor_group, joint_name)) { - matched_group = group_id; - break; - } - } - - if (matched_group.empty()) { - CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() - << "' joint '" << joint_name - << "' not found in configured motor_group_ids"; - return; - } - group_selection[matched_group].insert(joint_name); - } - } - - MotorManager::clearActiveJoints(); - for (auto& [motor_system_id, group_selection] : selections) { - MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); - } -} - void DeviceManager::init_devices_() { for (const auto& entry : cfg_.devices()) { if (!entry.enable()) { @@ -462,12 +308,14 @@ void DeviceManager::configure_mujoco_viewer_pip_() auto viewer = getDevice(viewer_id); if (viewer && viewer->setPiPCameraConfig(camera_config)) { - camera->setFetchRgbdFn([viewer](std::vector& rgb, - std::vector& depth, - int& width, - int& height, - uint64_t& frame_id) { - return viewer->getPiPCameraRGBD(rgb, depth, width, height, frame_id); + const std::string camera_name = camera_config.camera_name(); + camera->setFetchRgbdFn([viewer, camera_name](std::vector& rgb, + std::vector& depth, + int& width, + int& height, + uint64_t& frame_id) { + return viewer->getPiPCameraRGBD( + camera_name, rgb, depth, width, height, frame_id); }); break; } diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index 05410231..ac8567ab 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -36,6 +36,8 @@ 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_UNKNOWN: default: return "TASK_TYPE_UNKNOWN"; diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt b/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt index 54ce1b93..2902d956 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt @@ -35,4 +35,9 @@ target_link_libraries(mujoco_viewer_test gtest_main cmvr_es::mujoco_viewer cmvr_es::mujoco_world + cmvr_es::device_manager + cmvr_es::device::camera + cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver + cmvr_es::device::motor_robot_arm ) diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h b/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h index 9af82f7a..2a36b4b0 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h @@ -5,8 +5,11 @@ #pragma once #include +#include +#include #include #include +#include #include #include @@ -52,6 +55,14 @@ namespace cmvr { int display_height, int render_width, int render_height); + // Append another fixed camera view to the same viewer window. + void addPiPCamera(const char *camera_name, + int left, + int bottom, + int display_width, + int display_height, + int render_width, + int render_height); void disablePiPCamera(); // 获取 PiP 相机 RGB+Depth（Depth 已线性化为米） // depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口） @@ -60,9 +71,16 @@ namespace cmvr { int &width, int &height, uint64_t &frame_id) const; + bool getPiPCameraRGBD(const std::string &camera_name, + std::vector &rgb, + std::vector &depth, + int &width, + int &height, + uint64_t &frame_id) const; // 只拿 frame_id，便于 physics 线程判断是否新帧 uint64_t getPiPCameraFrameId() const; + uint64_t getPiPCameraFrameId(const std::string &camera_name) const; public: void setupCamera(double distance = 3.0, @@ -79,9 +97,34 @@ namespace cmvr { void printCameraState() const; private: + struct PiPCameraState { + std::string name; + int camera_id = -1; + int width = 320; + int height = 240; + int render_width = 320; + int render_height = 240; + bool render_size_warning_logged = false; + int margin = 10; + bool custom_pos = false; + int left = 0; + int bottom = 0; + mjvCamera camera{}; + mjvScene scene{}; + bool scene_inited = false; + mjModel *scene_model = nullptr; + std::vector rgb; + std::vector depth; + int rgb_width = 0; + int rgb_height = 0; + bool rgb_valid = false; + uint64_t frame_id = 0; + }; + void renderPiP(); void initSim(); void syncThreadFunc(); + void clearPiPCameras(); private: std::shared_ptr world_; @@ -93,29 +136,10 @@ namespace cmvr { std::unique_ptr sim_; std::thread sync_thread_; - bool pip_enabled_ = false; - std::string pip_camera_name_; - int pip_camera_id_ = -1; - int pip_width_ = 320; - int pip_height_ = 240; - int pip_render_width_ = 320; - int pip_render_height_ = 240; - bool pip_render_size_warning_logged_ = false; - int pip_margin_ = 10; - bool pip_custom_pos_ = false; - int pip_left_ = 0; - int pip_bottom_ = 0; - mjvCamera pip_cam_; - mjvScene pip_scene_; - bool pip_scene_inited_ = false; - mjModel *pip_scene_model_ = nullptr; + std::deque pip_cameras_; + mjData *pip_render_data_ = nullptr; + mjModel *pip_render_data_model_ = nullptr; mutable std::mutex pip_rgb_mtx_; - std::vector pip_rgb_; - std::vector pip_depth_; // 新增：z-buffer - int pip_rgb_width_ = 0; - int pip_rgb_height_ = 0; - bool pip_rgb_valid_ = false; - uint64_t pip_frame_id_ = 0; // 新增：帧序号 }; @@ -137,15 +161,20 @@ namespace cmvr { int& width, int& height, uint64_t& frame_id) const; + bool getPiPCameraRGBD(const std::string& camera_name, + std::vector& rgb, + std::vector& depth, + int& width, + int& height, + uint64_t& frame_id) const; private: config::MujocoViewerConfig config_; - config::MujocoCameraConfig pip_camera_config_; + std::vector pip_camera_configs_; std::shared_ptr world_; std::unique_ptr viewer_; std::thread viewer_thread_; mutable std::mutex mtx_; - bool has_pip_camera_config_ = false; bool running_ = false; bool stop_requested_ = false; }; diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp index 300456dd..236ebed5 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp @@ -53,8 +53,6 @@ namespace cmvr { mjv_defaultCamera(&cam_); mjv_defaultOption(&opt_); mjv_defaultPerturb(&pert_); - mjv_defaultCamera(&pip_cam_); - mjv_defaultScene(&pip_scene_); auto platform_ui = std::make_unique(this); sim_ = std::make_unique( @@ -72,9 +70,11 @@ namespace cmvr { sync_thread_.join(); } - if (pip_scene_inited_) { - mjv_freeScene(&pip_scene_); - pip_scene_inited_ = false; + clearPiPCameras(); + if (pip_render_data_ != nullptr) { + mj_deleteData(pip_render_data_); + pip_render_data_ = nullptr; + pip_render_data_model_ = nullptr; } } @@ -115,14 +115,12 @@ namespace cmvr { } void MuJocoViewer::enablePiPCamera(const char *camera_name) { - pip_enabled_ = true; - pip_camera_name_ = camera_name ? camera_name : ""; - pip_camera_id_ = -1; - pip_width_ = 320; - pip_height_ = 240; - pip_render_width_ = pip_width_; - pip_render_height_ = pip_height_; - pip_custom_pos_ = false; + clearPiPCameras(); + PiPCameraState state; + state.name = camera_name ? camera_name : ""; + mjv_defaultCamera(&state.camera); + mjv_defaultScene(&state.scene); + pip_cameras_.push_back(std::move(state)); } void MuJocoViewer::enablePiPCamera(const char *camera_name, @@ -140,183 +138,241 @@ namespace cmvr { int display_height, int render_width, int render_height) { - pip_enabled_ = true; - pip_camera_name_ = camera_name ? camera_name : ""; - pip_camera_id_ = -1; - pip_left_ = left; - pip_bottom_ = bottom; - pip_width_ = display_width > 0 ? display_width : 320; - pip_height_ = display_height > 0 ? display_height : 240; - pip_render_width_ = render_width > 0 ? render_width : pip_width_; - pip_render_height_ = render_height > 0 ? render_height : pip_height_; - pip_custom_pos_ = true; + clearPiPCameras(); + addPiPCamera(camera_name, + left, + bottom, + display_width, + display_height, + render_width, + render_height); + } + + void MuJocoViewer::addPiPCamera(const char *camera_name, + int left, + int bottom, + int display_width, + int display_height, + int render_width, + int render_height) { + PiPCameraState state; + state.name = camera_name ? camera_name : ""; + state.left = left; + state.bottom = bottom; + state.width = display_width > 0 ? display_width : 320; + state.height = display_height > 0 ? display_height : 240; + state.render_width = render_width > 0 ? render_width : state.width; + state.render_height = render_height > 0 ? render_height : state.height; + state.custom_pos = true; + mjv_defaultCamera(&state.camera); + mjv_defaultScene(&state.scene); + pip_cameras_.push_back(std::move(state)); } void MuJocoViewer::disablePiPCamera() { - pip_enabled_ = false; + clearPiPCameras(); + } + + void MuJocoViewer::clearPiPCameras() { + for (auto &pip : pip_cameras_) { + if (pip.scene_inited) { + mjv_freeScene(&pip.scene); + pip.scene_inited = false; + pip.scene_model = nullptr; + } + } + pip_cameras_.clear(); } void MuJocoViewer::renderPiP() { - if (!pip_enabled_ || !sim_) return; - if (pip_camera_name_.empty()) return; + if (!sim_ || pip_cameras_.empty()) return; mjModel* render_model = sim_->m_passive_ ? sim_->m_passive_ : sim_->m_; mjData* render_data = sim_->d_passive_ ? sim_->d_passive_ : sim_->d_; if (!render_model || !render_data) return; - if (pip_camera_id_ < 0) { - pip_camera_id_ = mj_name2id(render_model, mjOBJ_CAMERA, pip_camera_name_.c_str()); - if (pip_camera_id_ < 0) { - return; - } - } - auto [fb_width, fb_height] = sim_->platform_ui->GetFramebufferSize(); if (fb_width <= 0 || fb_height <= 0) return; - int left = 0; - int bottom = 0; - int width = 0; - int height = 0; - - if (pip_custom_pos_) { - width = std::min(pip_width_, fb_width); - height = std::min(pip_height_, fb_height); - const int max_left = fb_width - width; - const int max_bottom = fb_height - height; - left = pip_left_ >= 0 - ? std::max(0, std::min(pip_left_, max_left)) - : std::max(0, std::min(fb_width - width + pip_left_, max_left)); - bottom = pip_bottom_ >= 0 - ? std::max(0, std::min(pip_bottom_, max_bottom)) - : std::max(0, std::min(fb_height - height + pip_bottom_, max_bottom)); - } else { - width = std::min(pip_width_, fb_width - 2 * pip_margin_); - height = std::min(pip_height_, fb_height - 2 * pip_margin_); - left = fb_width - pip_margin_ - width; - bottom = pip_margin_; - } - - if (width <= 0 || height <= 0) return; - - mjrRect display_rect; - display_rect.width = width; - display_rect.height = height; - display_rect.left = left; - display_rect.bottom = bottom; - - const std::unique_lock lock(sim_->mtx); - - if (!pip_scene_inited_ || pip_scene_model_ != render_model) { - if (pip_scene_inited_) { - mjv_freeScene(&pip_scene_); + // Copy one visualization snapshot for all PiP cameras. Keep the + // simulation lock out of scene updates, GPU rendering, and readback. + { + std::unique_lock lock(sim_->mtx, std::try_to_lock); + if (lock.owns_lock()) { + if (pip_render_data_model_ != render_model) { + if (pip_render_data_ != nullptr) { + mj_deleteData(pip_render_data_); + pip_render_data_ = nullptr; + } + pip_render_data_ = mj_makeData(render_model); + pip_render_data_model_ = render_model; + } + if (pip_render_data_ != nullptr) { + mjv_copyData(pip_render_data_, render_model, render_data); + } } - mjv_makeScene(render_model, &pip_scene_, kPiPMaxGeom); - pip_scene_inited_ = true; - pip_scene_model_ = render_model; } - pip_cam_.type = mjCAMERA_FIXED; - pip_cam_.fixedcamid = pip_camera_id_; - pip_cam_.trackbodyid = -1; - - mjv_updateScene(render_model, render_data, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_); + // If the sync thread owns the lock, use the last complete snapshot. + if (pip_render_data_ == nullptr || pip_render_data_model_ != render_model) { + return; + } auto& context = sim_->platform_ui->mjr_context(); - const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width; - const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height; - const int render_width = std::min(std::max(1, pip_render_width_), offscreen_width); - const int render_height = std::min(std::max(1, pip_render_height_), offscreen_height); - if (!pip_render_size_warning_logged_ && - (render_width != pip_render_width_ || render_height != pip_render_height_)) { - CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped" - << ", requested=" << pip_render_width_ << "x" << pip_render_height_ - << ", actual=" << render_width << "x" << render_height - << ", offscreen=" << offscreen_width << "x" << offscreen_height; - pip_render_size_warning_logged_ = true; - } - const bool use_offscreen = render_width != display_rect.width || render_height != display_rect.height; + for (size_t pip_index = 0; pip_index < pip_cameras_.size(); ++pip_index) { + auto& pip = pip_cameras_[pip_index]; + if (pip.name.empty()) continue; - mjrRect render_rect; - render_rect.left = 0; - render_rect.bottom = 0; - render_rect.width = render_width; - render_rect.height = render_height; + if (pip.camera_id < 0) { + pip.camera_id = mj_name2id(render_model, mjOBJ_CAMERA, pip.name.c_str()); + if (pip.camera_id < 0) { + CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera not found: " << pip.name; + continue; + } + } - bool rendered_offscreen = false; - if (use_offscreen) { - mjr_setBuffer(mjFB_OFFSCREEN, &context); - if (context.currentBuffer == mjFB_OFFSCREEN) { - mjr_render(render_rect, &pip_scene_, &context); - rendered_offscreen = true; + int left = 0; + int bottom = 0; + int width = 0; + int height = 0; + if (pip.custom_pos) { + width = std::min(pip.width, fb_width); + height = std::min(pip.height, fb_height); + const int max_left = fb_width - width; + const int max_bottom = fb_height - height; + left = pip.left >= 0 + ? std::max(0, std::min(pip.left, max_left)) + : std::max(0, std::min(fb_width - width + pip.left, max_left)); + bottom = pip.bottom >= 0 + ? std::max(0, std::min(pip.bottom, max_bottom)) + : std::max(0, std::min(fb_height - height + pip.bottom, max_bottom)); } else { - mjr_render(display_rect, &pip_scene_, &context); + width = std::min(pip.width, fb_width - 2 * pip.margin); + height = std::min(pip.height, fb_height - 2 * pip.margin); + left = fb_width - pip.margin - width; + bottom = pip.margin; + } + if (width <= 0 || height <= 0) continue; + + mjrRect display_rect; + display_rect.width = width; + display_rect.height = height; + display_rect.left = left; + display_rect.bottom = bottom; + + if (!pip.scene_inited || pip.scene_model != render_model) { + if (pip.scene_inited) { + mjv_freeScene(&pip.scene); + } + mjv_makeScene(render_model, &pip.scene, kPiPMaxGeom); + pip.scene_inited = true; + pip.scene_model = render_model; + } + + pip.camera.type = mjCAMERA_FIXED; + pip.camera.fixedcamid = pip.camera_id; + pip.camera.trackbodyid = -1; + mjv_updateScene(render_model, + pip_render_data_, + &opt_, + &pert_, + &pip.camera, + mjCAT_ALL, + &pip.scene); + + const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width; + const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height; + const int render_width = std::min(std::max(1, pip.render_width), offscreen_width); + const int render_height = std::min(std::max(1, pip.render_height), offscreen_height); + if (!pip.render_size_warning_logged && + (render_width != pip.render_width || render_height != pip.render_height)) { + CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped" + << ", camera=" << pip.name + << ", requested=" << pip.render_width << "x" << pip.render_height + << ", actual=" << render_width << "x" << render_height + << ", offscreen=" << offscreen_width << "x" << offscreen_height; + pip.render_size_warning_logged = true; + } + const bool use_offscreen = + render_width != display_rect.width || render_height != display_rect.height; + + mjrRect render_rect; + render_rect.left = 0; + render_rect.bottom = 0; + render_rect.width = render_width; + render_rect.height = render_height; + + bool rendered_offscreen = false; + if (use_offscreen) { + mjr_setBuffer(mjFB_OFFSCREEN, &context); + if (context.currentBuffer == mjFB_OFFSCREEN) { + mjr_render(render_rect, &pip.scene, &context); + rendered_offscreen = true; + } else { + mjr_render(display_rect, &pip.scene, &context); + render_rect = display_rect; + } + } else { + mjr_render(display_rect, &pip.scene, &context); render_rect = display_rect; } - } else { - mjr_render(display_rect, &pip_scene_, &context); - render_rect = display_rect; - } - { - std::lock_guard lock(pip_rgb_mtx_); - const int w = render_rect.width; - const int h = render_rect.height; - if (w > 0 && h > 0) { - pip_rgb_.resize(static_cast(3 * w * h)); - pip_depth_.resize(static_cast(w * h)); + { + std::lock_guard lock(pip_rgb_mtx_); + const int w = render_rect.width; + const int h = render_rect.height; + if (w > 0 && h > 0) { + pip.rgb.resize(static_cast(3 * w * h)); + pip.depth.resize(static_cast(w * h)); + mjr_readPixels(pip.rgb.data(), pip.depth.data(), render_rect, &context); - // 同时读 RGB 和 depth(z-buffer 0..1) - mjr_readPixels(pip_rgb_.data(), pip_depth_.data(), - render_rect, &context); - if (rendered_offscreen) { - mjr_setBuffer(mjFB_WINDOW, &context); - mjr_render(display_rect, &pip_scene_, &context); - } + // OpenGL's pixel origin is bottom-left; expose top-left images. + for (int r = 0; r < h / 2; ++r) { + unsigned char *top_row = pip.rgb.data() + 3 * w * r; + unsigned char *bottom_row = pip.rgb.data() + 3 * w * (h - 1 - r); + std::swap_ranges(top_row, top_row + 3 * w, bottom_row); - // OpenGL 像素原点在左下，需要竖直翻转 RGB 和 depth - for (int r = 0; r < h / 2; ++r) { - // flip rgb row - unsigned char *top_row = pip_rgb_.data() + 3 * w * r; - unsigned char *bottom_row = pip_rgb_.data() + 3 * w * (h - 1 - r); - std::swap_ranges(top_row, top_row + 3 * w, bottom_row); - - // flip depth row - float *top_d = pip_depth_.data() + w * r; - float *bot_d = pip_depth_.data() + w * (h - 1 - r); - std::swap_ranges(top_d, top_d + w, bot_d); - } - - // 将 OpenGL depth buffer(0..1) 线性化为相机前向距离（米）。 - const double znear = static_cast(render_model->vis.map.znear) * - static_cast(render_model->stat.extent); - const double zfar = static_cast(render_model->vis.map.zfar) * - static_cast(render_model->stat.extent); - if (znear > 0.0 && zfar > znear) { - const double two_nf = 2.0 * znear * zfar; - const double f_plus_n = zfar + znear; - const double f_minus_n = zfar - znear; - for (float &d : pip_depth_) { - if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) { - d = std::numeric_limits::infinity(); - continue; - } - const double z_ndc = 2.0 * static_cast(d) - 1.0; // [-1,1] - const double denom = f_plus_n - z_ndc * f_minus_n; - if (denom <= 1e-12) { - d = std::numeric_limits::infinity(); - continue; - } - d = static_cast(two_nf / denom); + float *top_d = pip.depth.data() + w * r; + float *bot_d = pip.depth.data() + w * (h - 1 - r); + std::swap_ranges(top_d, top_d + w, bot_d); } - } - pip_rgb_width_ = w; - pip_rgb_height_ = h; - pip_rgb_valid_ = true; - ++pip_frame_id_; // 新帧 + // Linearize the OpenGL depth buffer to camera-forward meters. + const double znear = static_cast(render_model->vis.map.znear) * + static_cast(render_model->stat.extent); + const double zfar = static_cast(render_model->vis.map.zfar) * + static_cast(render_model->stat.extent); + if (znear > 0.0 && zfar > znear) { + const double two_nf = 2.0 * znear * zfar; + const double f_plus_n = zfar + znear; + const double f_minus_n = zfar - znear; + for (float &d : pip.depth) { + if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) { + d = std::numeric_limits::infinity(); + continue; + } + const double z_ndc = 2.0 * static_cast(d) - 1.0; + const double denom = f_plus_n - z_ndc * f_minus_n; + if (denom <= 1e-12) { + d = std::numeric_limits::infinity(); + continue; + } + d = static_cast(two_nf / denom); + } + } + + pip.rgb_width = w; + pip.rgb_height = h; + pip.rgb_valid = true; + ++pip.frame_id; + } + } + + if (rendered_offscreen) { + mjr_setBuffer(mjFB_WINDOW, &context); + mjr_render(display_rect, &pip.scene, &context); } } } @@ -331,12 +387,18 @@ namespace cmvr { if (!world_->model() || !world_->data()) { mju_error("MuJocoViewer world has null model/data"); } - if (pip_enabled_ && pip_render_width_ > 0 && pip_render_height_ > 0) { + int max_pip_render_width = 0; + int max_pip_render_height = 0; + for (const auto &pip : pip_cameras_) { + max_pip_render_width = std::max(max_pip_render_width, pip.render_width); + max_pip_render_height = std::max(max_pip_render_height, pip.render_height); + } + if (!pip_cameras_.empty() && max_pip_render_width > 0 && max_pip_render_height > 0) { mjModel* model = world_->model(); const int old_width = model->vis.global.offwidth; const int old_height = model->vis.global.offheight; - model->vis.global.offwidth = std::max(model->vis.global.offwidth, pip_render_width_); - model->vis.global.offheight = std::max(model->vis.global.offheight, pip_render_height_); + model->vis.global.offwidth = std::max(model->vis.global.offwidth, max_pip_render_width); + model->vis.global.offheight = std::max(model->vis.global.offheight, max_pip_render_height); if (model->vis.global.offwidth != old_width || model->vis.global.offheight != old_height) { CMVR_LOG(INFO) << "[MuJocoViewer] resize offscreen buffer before context creation" << ", old=" << old_width << "x" << old_height @@ -386,7 +448,15 @@ namespace cmvr { uint64_t MuJocoViewer::getPiPCameraFrameId() const { std::lock_guard lock(pip_rgb_mtx_); - return pip_frame_id_; + return pip_cameras_.empty() ? 0 : pip_cameras_.front().frame_id; + } + + uint64_t MuJocoViewer::getPiPCameraFrameId(const std::string &camera_name) const { + std::lock_guard lock(pip_rgb_mtx_); + const auto it = std::find_if( + pip_cameras_.begin(), pip_cameras_.end(), + [&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; }); + return it == pip_cameras_.end() ? 0 : it->frame_id; } bool MuJocoViewer::getPiPCameraRGBD(std::vector &rgb, @@ -395,13 +465,35 @@ namespace cmvr { int &height, uint64_t &frame_id) const { std::lock_guard lock(pip_rgb_mtx_); - if (!pip_rgb_valid_ || pip_rgb_.empty()) return false; + if (pip_cameras_.empty()) return false; + const auto &pip = pip_cameras_.front(); + if (!pip.rgb_valid || pip.rgb.empty()) return false; - rgb = pip_rgb_; - depth = pip_depth_; - width = pip_rgb_width_; - height = pip_rgb_height_; - frame_id = pip_frame_id_; + rgb = pip.rgb; + depth = pip.depth; + width = pip.rgb_width; + height = pip.rgb_height; + frame_id = pip.frame_id; + return true; + } + + bool MuJocoViewer::getPiPCameraRGBD(const std::string &camera_name, + std::vector &rgb, + std::vector &depth, + int &width, + int &height, + uint64_t &frame_id) const { + std::lock_guard lock(pip_rgb_mtx_); + const auto it = std::find_if( + pip_cameras_.begin(), pip_cameras_.end(), + [&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; }); + if (it == pip_cameras_.end() || !it->rgb_valid || it->rgb.empty()) return false; + + rgb = it->rgb; + depth = it->depth; + width = it->rgb_width; + height = it->rgb_height; + frame_id = it->frame_id; return true; } @@ -456,6 +548,7 @@ namespace cmvr { } std::shared_ptr world; + std::vector pip_camera_configs; { std::lock_guard lock(mtx_); if (running_) { @@ -466,6 +559,7 @@ namespace cmvr { return false; } world = world_; + pip_camera_configs = pip_camera_configs_; stop_requested_ = false; running_ = true; } @@ -486,19 +580,30 @@ namespace cmvr { config_.camera_azimuth(), config_.camera_elevation()); - if (has_pip_camera_config_ && !pip_camera_config_.camera_name().empty()) { - const auto& pip = pip_camera_config_.viewer_pip(); - const auto& render = pip_camera_config_.render(); - if (pip.width() > 0 && pip.height() > 0) { - viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str(), + bool first_pip_camera = true; + for (const auto& camera_config : pip_camera_configs) { + if (camera_config.camera_name().empty()) { + continue; + } + const auto& pip = camera_config.viewer_pip(); + const auto& render = camera_config.render(); + if (first_pip_camera) { + viewer->enablePiPCamera(camera_config.camera_name().c_str(), pip.left(), pip.bottom(), pip.width(), pip.height(), render.width(), render.height()); + first_pip_camera = false; } else { - viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str()); + viewer->addPiPCamera(camera_config.camera_name().c_str(), + pip.left(), + pip.bottom(), + pip.width(), + pip.height(), + render.width(), + render.height()); } } @@ -525,19 +630,31 @@ namespace cmvr { if (camera_config.world_id() != config_.world_id()) { return false; } - - std::lock_guard lock(mtx_); - if (has_pip_camera_config_) { - CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured, keep first" - << ", viewer_id=" << id_ - << ", current_camera=" << pip_camera_config_.camera_name() - << ", ignored_camera=" << camera_config.camera_name(); + if (camera_config.camera_name().empty()) { return false; } - pip_camera_config_ = camera_config; - has_pip_camera_config_ = true; - CMVR_LOG(INFO) << "[MujocoViewerDevice] set PiP camera" + std::lock_guard lock(mtx_); + if (running_) { + CMVR_LOG(WARNING) << "[MujocoViewerDevice] cannot add PiP camera while viewer is running" + << ", viewer_id=" << id_ + << ", camera=" << camera_config.camera_name(); + return false; + } + const auto duplicate = std::find_if( + pip_camera_configs_.begin(), pip_camera_configs_.end(), + [&camera_config](const config::MujocoCameraConfig& configured) { + return configured.camera_name() == camera_config.camera_name(); + }); + if (duplicate != pip_camera_configs_.end()) { + CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured" + << ", viewer_id=" << id_ + << ", camera=" << camera_config.camera_name(); + return false; + } + + pip_camera_configs_.push_back(camera_config); + CMVR_LOG(INFO) << "[MujocoViewerDevice] add PiP camera" << ", viewer_id=" << id_ << ", camera=" << camera_config.camera_name() << ", world_id=" << camera_config.world_id(); @@ -556,6 +673,20 @@ namespace cmvr { return viewer_->getPiPCameraRGBD(rgb, depth, width, height, frame_id); } + bool MujocoViewerDevice::getPiPCameraRGBD(const std::string& camera_name, + std::vector& rgb, + std::vector& depth, + int& width, + int& height, + uint64_t& frame_id) const { + std::lock_guard lock(mtx_); + if (!viewer_) { + return false; + } + return viewer_->getPiPCameraRGBD( + camera_name, rgb, depth, width, height, frame_id); + } + bool MujocoViewerDevice::stop() { std::thread thread_to_join; { diff --git a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp index 4cf3364e..6c27fe1f 100644 --- a/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp +++ b/cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp @@ -1,11 +1,21 @@ +#include +#include #include #include #include #include #include +#include #include +#include "common/config/config_files.h" +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "devices/arm/robot_arm.h" +#include "devices/camera/abstract_camera.h" +#include "devices/camera/mujoco_camera/include/mujoco_camera.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "manager/device_manager/include/device_manager.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" #include "simulate/mujoco/mujoco_world/include/mujoco_world.h" @@ -37,6 +47,63 @@ std::string defaultModelPath() return (root / "model/xiaoyan_description/dual_arm.xml").string(); } +cmvr::device::DeviceManager& createEyeToHandDeviceManager( + const std::filesystem::path& project_root) +{ + cmvr::device::DeviceManager::destroyInstance(); + cmvr::ConfigHelper::setConfigRootFromFile( + (project_root / "cmvr-es/config/cmvr_es.pb.txt").string()); + + cmvr::config::DeviceManagerConfig config; + config.set_name("mujoco_viewer_eye_to_hand_test"); + config.set_version("test"); + + auto* world = config.add_devices(); + world->set_id("mujoco_world"); + world->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD); + world->set_config_file("devices/mujoco/right_arm_eye_to_hand_world.pb.txt"); + world->set_enable(true); + + auto* motors = config.add_devices(); + motors->set_id("right_arm_mujoco_motors"); + motors->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); + motors->set_config_file("devices/motor/mujoco_motors.pb.txt"); + motors->set_enable(true); + + auto* arm = config.add_devices(); + arm->set_id("mujoco_right_arm"); + arm->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM); + arm->set_config_file("devices/arm/arm_mujoco.pb.txt"); + arm->set_enable(true); + + for (const auto* camera_id : {"mujoco_hand_cam", "mujoco_external_touch_cam"}) { + auto* camera = config.add_devices(); + camera->set_id(camera_id); + camera->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA); + camera->set_config_file("devices/camera/camera.pb.txt"); + camera->set_enable(true); + } + + return cmvr::device::DeviceManager::getInstance(config); +} + +cmvr::device::Result moveArmToInitialization(cmvr::device::RobotArm& arm) +{ + const std::vector positions = { + -0.2423, + 1.2929, + 1.61, + 1.58, + -2.8792, + 0.1150, + -0.08, + }; + cmvr::device::MotionOptions options; + options.velocity = 2.8; + options.acceleration = 20.0; + return arm.moveJ(cmvr::device::JointPositionCommand{positions}, options); +} + } // namespace TEST(MujocoViewerTest, ShowsUiWithMujocoWorld) @@ -63,3 +130,67 @@ TEST(MujocoViewerTest, ShowsUiWithMujocoWorld) world->stop(); } + +TEST(MujocoViewerTest, ShowsRightArmEyeToHandCamera) +{ + const auto project_root = findProjectRoot(); + ASSERT_FALSE(project_root.empty()); + const auto model_path = project_root / "model/xiaoyan_description/right_arm_eye_to_hand.xml"; + + auto& device_manager = createEyeToHandDeviceManager(project_root); + auto arm = device_manager.getDevice("mujoco_right_arm"); + ASSERT_NE(arm, nullptr); + + auto hand_camera_base = + device_manager.getDevice("mujoco_hand_cam"); + auto external_camera_base = + device_manager.getDevice("mujoco_external_touch_cam"); + auto hand_camera = std::dynamic_pointer_cast(hand_camera_base); + auto external_camera = + std::dynamic_pointer_cast(external_camera_base); + ASSERT_NE(hand_camera, nullptr); + ASSERT_NE(external_camera, nullptr); + + auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors"); + ASSERT_NE(world, nullptr); + ASSERT_TRUE(world->isLoaded()); + ASSERT_TRUE(world->isRunning()); + + const auto move_result = moveArmToInitialization(*arm); + ASSERT_TRUE(move_result.ok()) << move_result.message; + + cmvr::MuJocoViewer viewer(world); + ASSERT_NE(viewer.model(), nullptr); + ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "hand_cam"), 0) + << "right_arm_eye_to_hand.xml does not contain hand_cam"; + ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "external_touch_cam"), 0) + << "right_arm_eye_to_hand.xml does not contain external_touch_cam"; + + viewer.setupCamera(2.5, -160.0, -25.0); + const auto& external_config = external_camera->config(); + const auto& hand_config = hand_camera->config(); + ASSERT_TRUE(external_config.viewer_pip().enable()); + ASSERT_TRUE(hand_config.viewer_pip().enable()); + viewer.enablePiPCamera(external_config.camera_name().c_str(), + external_config.viewer_pip().left(), + external_config.viewer_pip().bottom(), + external_config.viewer_pip().width(), + external_config.viewer_pip().height(), + external_config.encoder().width(), + external_config.encoder().height()); + viewer.addPiPCamera(hand_config.camera_name().c_str(), + hand_config.viewer_pip().left(), + hand_config.viewer_pip().bottom(), + hand_config.viewer_pip().width(), + hand_config.viewer_pip().height(), + hand_config.encoder().width(), + hand_config.encoder().height()); + + std::cout << "MujocoWorld loaded: " << model_path.string() << std::endl; + std::cout << "Showing hand_cam and external_touch_cam in PiP. " + "Close the MuJoCo window to exit." + << std::endl; + viewer.run(); + + world->stop(); +} diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index c8a2960f..b10b0978 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 ) @@ -17,22 +19,22 @@ target_link_libraries(task add_library(cmvr_es::task ALIAS task) install(TARGETS task LIBRARY DESTINATION lib) -#add_executable(touch_screen_task_test -# touch_screen_task/src/touch_screen_task_test.cpp -#) -# -#target_link_libraries(touch_screen_task_test PRIVATE -# cmvr_es::task -# cmvr_es::device::arm -# cmvr_es::device::motor_manager -# cmvr_es::device::mujoco_motor_driver -# cmvr_es::device::mujoco_camera -# cmvr_es::mujoco_viewer -# cmvr_es::proto -# cmvr_es::device_manager -# cmvr_es::service -# gtest -# gtest_main -# pthread -# glog -#) +add_executable(touch_screen_task_test + touch_screen_task/src/touch_screen_task_test.cpp +) + +target_link_libraries(touch_screen_task_test PRIVATE + cmvr_es::task + cmvr_es::device::arm + cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver + cmvr_es::device::mujoco_camera + cmvr_es::mujoco_viewer + cmvr_es::proto + cmvr_es::device_manager + cmvr_es::service + gtest + gtest_main + pthread + glog +) 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/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h index 6e284d7e..4e8decfc 100644 --- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -5,21 +5,24 @@ #include #include +#include #include #include #include +#include #include #include #include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h" -#include "algorithms/controllers/ibvs/include/ibvs_controller.h" +#include "algorithms/controllers/pbvs/include/pbvs_controller.h" +#include "algorithms/perception/apriltag/include/apriltag_perception.h" +#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" +#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h" #include "devices/camera/abstract_camera.h" #include "devices/dexhand/abstract_dexhand.h" #include "devices/arm/robot_arm.h" #include "task/task.h" -#include "algorithms/perception/apriltag/include/apriltag_perception.h" -#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" namespace cmvr::task { @@ -27,7 +30,7 @@ class TouchScreenTask : public Task { public: enum class Phase { IDLE = 0, // 空闲，尚未开始任务。 - ALIGNING, // 视觉对准阶段：持续 IBVS 对齐目标点。 + ALIGNING, // 视觉对准阶段：持续 PBVS 对齐目标点。 ALIGN_REACHED, // 视觉对准已达到阈值，等待进入下一阶段。 TOUCHING, // 前进触控阶段：沿设定方向向屏幕推进。 DWELLING, // 已检测到接触，保持当前位置短暂停留。 @@ -40,12 +43,11 @@ public: IDLE = 0, // 空闲状态。 NOT_INITIALIZED, // 尚未调用 init() 完成初始化。 INVALID_CONFIG, // 配置非法，无法启动或应用参数。 - CONTROL_JOINT_MISMATCH, // 控制关节顺序与 IK 链不一致。 ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。 ALIGN_WAITING_TRACK, // 对准阶段等待目标点跟踪恢复成功。 - ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。 - ALIGN_COMPUTE_FAILED, // 对准阶段 IBVS 或 IK 计算失败。 - ALIGN_TIMEOUT, // 对准阶段超时仍未收敛。 + ALIGN_TARGET_SETUP_FAILED,// PBVS 目标位姿设置失败。 + ALIGN_COMPUTE_FAILED, // 对准阶段 PBVS 计算失败。 + ALIGN_TIMEOUT, // 对准阶段或对准完成后的暂停超时。 ALIGNING, // 正在执行视觉对准。 ALIGN_REACHED, // 视觉对准完成。 TOUCHING, // 正在向前触控。 @@ -61,12 +63,16 @@ public: }; explicit TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg); - ~TouchScreenTask() = default; + ~TouchScreenTask() override; bool init() override; bool init(const std::shared_ptr& arm, const std::shared_ptr& dexhand, const std::shared_ptr& camera); + bool init(const std::shared_ptr& arm, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera, + const std::shared_ptr& external_camera); const std::string& id() const override { return id_; } @@ -89,14 +95,19 @@ public: int targetU() const; int targetV() const; + // Selected force criterion summed over requested tactile regions, in N. double lastTouchPressureSum() const; int lastTouchNonzeroCount() const; int lastActiveTagId() const; - Eigen::Vector3d lastAlignErrorCamera() const; + Eigen::Vector3d lastAlignErrorScreenTag() const; const std::shared_ptr& perception() const { return perception_; } + const std::shared_ptr& externalPerception() const { + return external_perception_; + } const perception::TagRelativeTarget3D& tracker() const { return tracker_; } - const IbvsController& ibvs() const { return ibvs_; } + const perception::TagRelativeTcpPose& tcpPoseTracker() const { return tcp_pose_tracker_; } + const PbvsController& pbvs() const { return pbvs_; } private: using Clock = std::chrono::steady_clock; @@ -106,18 +117,14 @@ private: bool startFromPixelUnlocked(int u, int v); void stopUnlocked(); bool applyConfig(); - bool validateControlJointNames() const; bool stepAligning(double dt); bool stepTouching(); bool stepDwelling(); bool stepRetracting(); - bool readControlledJointPositions(std::vector& q_out) const; - bool sendJointVelocity(const std::vector& qdot) const; - bool sendZeroJointVelocity() const; - void hardStopIbvsMotion(); - bool holdCurrentControlledPosition() const; + void stopPbvsMotion(); bool buildInitJointPositions(std::vector& positions_out) const; + bool isAtInitPosition(const std::vector& positions) const; bool moveToInitPositionBeforeStartIfEnabled(); bool moveToInitPositionIfEnabled() const; bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; @@ -129,17 +136,34 @@ private: void enterFailed(Status status); bool updateTouchPressure(); + void logTouchPressure(bool force = false); + device::CameraStreamOverlay buildCoordinateOverlay( + const perception::AprilTagPerception* overlay_perception) const; + void publishCoordinateOverlay(); + void startCoordinateOverlayWorker(); + void stopCoordinateOverlayWorker(); + void setCoordinateOverlayEnabled(bool enabled); + void coordinateOverlayLoop(std::shared_ptr overlay_perception); private: mutable std::mutex mutex_; + // The display worker never takes mutex_ or uses the PBVS perception state. + std::mutex coordinate_overlay_mutex_; + std::condition_variable coordinate_overlay_cv_; + std::thread coordinate_overlay_thread_; + bool coordinate_overlay_stop_{false}; + bool coordinate_overlay_enabled_{true}; std::string id_; std::shared_ptr arm_{nullptr}; std::shared_ptr dexhand_{nullptr}; std::shared_ptr camera_{nullptr}; + std::shared_ptr external_camera_{nullptr}; std::shared_ptr perception_{nullptr}; + std::shared_ptr external_perception_{nullptr}; perception::TagRelativeTarget3D tracker_; - IbvsController ibvs_; + perception::TagRelativeTcpPose tcp_pose_tracker_; + PbvsController pbvs_; cmvr::config::TouchScreenTaskConfig config_{}; bool config_valid_{false}; @@ -149,7 +173,7 @@ private: bool initialized_{false}; bool target_locked_{false}; - bool ibvs_target_initialized_{false}; + bool pbvs_target_initialized_{false}; bool touch_command_started_{false}; bool retract_command_started_{false}; @@ -157,19 +181,36 @@ private: int target_v_{-1}; int align_stable_count_{0}; int align_debug_count_{0}; + int pbvs_debug_count_{0}; int last_active_tag_id_{-1}; - double last_touch_pressure_sum_{0.0}; + double last_touch_pressure_sum_{0.0}; // N + double last_touch_resultant_fz_{0.0}; // N int last_touch_nonzero_count_{0}; - Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()}; + Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()}; + Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()}; + double pbvs_command_acceleration_{0.25}; + // Initialization MoveJ is skipped only when both position and velocity + // are within these configured limits. + double init_skip_position_tolerance_rad_{1e-3}; + double init_skip_velocity_tolerance_rad_s_{1e-2}; bool locked_target_rotation_valid_{false}; Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()}; bool touch_start_position_valid_{false}; Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()}; bool retract_start_position_valid_{false}; Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()}; + Eigen::Vector3d retract_direction_base_{Eigen::Vector3d::Zero()}; + // Sampled maximum TCP displacement opposite to the retract direction, + // relative to the first applied retract command's measured TCP position. + double max_forward_after_retract_m_{0.0}; + bool have_last_T_B_G_{false}; + Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()}; + double max_T_B_G_translation_delta_m_{0.0}; + double max_T_B_G_rotation_delta_rad_{0.0}; Clock::time_point phase_start_time_{}; + Clock::time_point last_touch_pressure_log_time_{}; Clock::time_point last_retract_log_time_{}; Status final_status_after_retract_{Status::DONE}; }; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 7640cc33..98374c3b 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -6,18 +6,24 @@ #include #include #include +#include + +#include #include "common/base/logging/logger.h" +#include "common/math/cartesian_motion_math.h" #include "common/math/proto_geometry.h" -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "common/math/transform_math.h" #include "cmvr/config/touch_screen_algorithm_config.pb.h" #include "manager/device_manager/include/device_manager.h" -#include namespace cmvr::task { namespace { +// The initialization move is a safety repositioning step. Avoid replanning +// an already settled arm, while still requiring a stopped joint velocity so a +// moving arm is never mistaken for being at the initial pose. device::CartesianVelocity toCartesianVelocity(const Eigen::Matrix& twist) { return {twist[0], twist[1], twist[2], twist[3], twist[4], twist[5]}; } @@ -54,15 +60,8 @@ using TouchScreenTaskConfig = cmvr::config::TouchScreenTaskConfig; std::array toArray6(const Eigen::Matrix& value) { return {{value[0], value[1], value[2], value[3], value[4], value[5]}}; } - - - -std::vector controlJointNames(const TouchScreenTaskConfig& config) { - const auto& names = config.alignment().ibvs().control_joint_names(); - return {names.begin(), names.end()}; -} - bool buildInitJointPositionsFromConfig(const TouchScreenTaskConfig& config, + const std::vector& joint_names, std::vector& positions_out) { std::unordered_map q_map; q_map.reserve(static_cast(config.initialization().joint_positions_size())); @@ -71,13 +70,14 @@ bool buildInitJointPositionsFromConfig(const TouchScreenTaskConfig& config, !std::isfinite(joint.rad())) { return false; } - q_map[joint.joint_name()] = joint.rad(); + if (!q_map.emplace(joint.joint_name(), joint.rad()).second) { + return false; + } } positions_out.clear(); - positions_out.reserve(static_cast( - config.alignment().ibvs().control_joint_names_size())); - for (const auto& name : config.alignment().ibvs().control_joint_names()) { + positions_out.reserve(joint_names.size()); + for (const auto& name : joint_names) { const auto it = q_map.find(name); if (it == q_map.end()) { return false; @@ -87,52 +87,59 @@ bool buildInitJointPositionsFromConfig(const TouchScreenTaskConfig& config, return !positions_out.empty(); } +bool hasMat4(const cmvr::common::Mat4& value) { + return value.has_m00() && value.has_m01() && value.has_m02() && value.has_m03() && + value.has_m10() && value.has_m11() && value.has_m12() && value.has_m13() && + value.has_m20() && value.has_m21() && value.has_m22() && value.has_m23() && + value.has_m30() && value.has_m31() && value.has_m32() && value.has_m33(); +} + +Eigen::Matrix4d toEigenMat4(const cmvr::common::Mat4& value) { + Eigen::Matrix4d transform; + transform << value.m00(), value.m01(), value.m02(), value.m03(), + value.m10(), value.m11(), value.m12(), value.m13(), + value.m20(), value.m21(), value.m22(), value.m23(), + value.m30(), value.m31(), value.m32(), value.m33(); + return transform; +} + +bool isHomogeneousTransform(const Eigen::Matrix4d& transform) { + return transform.allFinite() && + std::abs(transform(3, 0)) <= 1e-6 && + std::abs(transform(3, 1)) <= 1e-6 && + std::abs(transform(3, 2)) <= 1e-6 && + std::abs(transform(3, 3) - 1.0) <= 1e-6; +} + bool isTouchTriggered(const TouchScreenTaskConfig& config, const double resultant_force_value) { return resultant_force_value >= config.touch().tactile().force_threshold(); } -double tactileForceValue(const device::AbstractDexHand::TactilePoint& point, +double tactileForceValue(const device::AbstractDexHand::ForceNewtons& point, const cmvr::config::TouchScreenTactileCriterion criterion) { switch (criterion) { case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ: - return static_cast(point.fz); + return point.fz; case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE: return point.magnitude(); } - return static_cast(point.fz); + return point.fz; } -Eigen::Matrix3d rotationFromTargetRotvec(double rx, double ry, double rz) { - vpRotationMatrix R_visp; - R_visp.buildFrom(rx, ry, rz); - - Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - R(r, c) = R_visp[r][c]; - } - } - return R; -} - -double rotationErrorRad(const Eigen::Matrix3d& R_current, - const Eigen::Matrix3d& R_target) { - if (!R_current.allFinite() || !R_target.allFinite()) { - return std::numeric_limits::infinity(); - } - - const Eigen::Matrix3d R_err = R_current * R_target.transpose(); - const double cos_angle = std::clamp(0.5 * (R_err.trace() - 1.0), -1.0, 1.0); - return std::acos(cos_angle); -} - -Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) { - const Eigen::AngleAxisd aa(R); - if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) { - return Eigen::Vector3d::Zero(); - } - return aa.axis() * aa.angle(); +Eigen::Matrix3d rotationFromTargetEuler(const double rx, + const double ry, + const double rz) { + // The configured rotations are extrinsic rotations about the fixed G axes, + // applied in X -> Y -> Z order. With column-vector transforms this is + // represented by left multiplication in reverse order. + const Eigen::Matrix3d R_x = + Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).toRotationMatrix(); + const Eigen::Matrix3d R_y = + Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()).toRotationMatrix(); + const Eigen::Matrix3d R_z = + Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()).toRotationMatrix(); + return R_z * R_y * R_x; } bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target, @@ -276,6 +283,10 @@ TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg) } } +TouchScreenTask::~TouchScreenTask() { + stopCoordinateOverlayWorker(); +} + bool TouchScreenTask::init() { if (!config_valid_) { last_status_ = Status::INVALID_CONFIG; @@ -283,9 +294,14 @@ bool TouchScreenTask::init() { } const auto& devices = config_.devices(); + const auto& perception_config = config_.perception(); if (!devices.has_arm_id() || devices.arm_id().empty() || + !devices.has_dexhand_id() || devices.dexhand_id().empty() || !devices.has_camera_id() || devices.camera_id().empty() || - !devices.has_dexhand_id() || devices.dexhand_id().empty()) { + !devices.has_external_camera_id() || devices.external_camera_id().empty() || + !perception_config.has_hand_camera() || + !perception_config.hand_camera().has_depth_policy() || + !perception_config.hand_camera().has_target_point_method()) { last_status_ = Status::INVALID_CONFIG; return false; } @@ -293,24 +309,47 @@ bool TouchScreenTask::init() { auto& dm = device::DeviceManager::getInstance(); auto arm = dm.getDevice(devices.arm_id()); auto dexhand = dm.getDevice(devices.dexhand_id()); + const auto& hand_camera_config = perception_config.hand_camera(); auto camera = dm.getDevice(devices.camera_id()); + auto external_camera = + dm.getDevice(devices.external_camera_id()); if (!camera || !camera->start()) { CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id(); last_status_ = Status::NOT_INITIALIZED; return false; } - return init(arm, dexhand, camera); + if (!external_camera || !external_camera->start()) { + CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start external camera: " + << devices.external_camera_id(); + last_status_ = Status::NOT_INITIALIZED; + return false; + } + return init(arm, dexhand, camera, external_camera); } bool TouchScreenTask::init(const std::shared_ptr& arm, const std::shared_ptr& dexhand, const std::shared_ptr& camera) { + std::shared_ptr external_camera; + if (config_.has_devices() && config_.devices().has_external_camera_id()) { + external_camera = device::DeviceManager::getInstance().getDevice( + config_.devices().external_camera_id()); + } + return init(arm, dexhand, camera, external_camera); +} + +bool TouchScreenTask::init(const std::shared_ptr& arm, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera, + const std::shared_ptr& external_camera) { std::lock_guard lock(mutex_); + stopCoordinateOverlayWorker(); arm_ = arm; dexhand_ = dexhand; camera_ = camera; + external_camera_ = external_camera; - if (!config_valid_ || !arm_ || !camera_) { + if (!config_valid_ || !arm_ || !camera_ || !external_camera_) { initialized_ = false; last_status_ = Status::INVALID_CONFIG; return false; @@ -323,59 +362,57 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, return false; } - if (config_.alignment().ibvs().camera_link().empty() || - config_.alignment().ibvs().control_joint_names().empty()) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } - + const auto& perception_config = config_.perception(); + const auto& tags = perception_config.tags(); + const auto& hand_camera_config = perception_config.hand_camera(); perception_ = std::make_shared(camera_); - perception_->setTagSize(config_.perception().apriltag().tag_size_m()); + perception_->setTagSize(tags.screen().size_m()); + external_perception_ = + std::make_shared(external_camera_); + external_perception_->setTagSize(tags.screen().size_m()); tracker_.setPerception(perception_); tracker_.setTargetPointMethod(toTargetPointMethod( - config_.perception().apriltag().target_point_method())); + hand_camera_config.target_point_method())); - auto pinocchio_solver = - std::dynamic_pointer_cast(arm_->kinematicsSolver()); - if (!pinocchio_solver || - !ibvs_.init(pinocchio_solver, - config_.alignment().ibvs().camera_link())) { - initialized_ = false; - last_status_ = Status::INVALID_CONFIG; - return false; - } + tcp_pose_tracker_.setPerception(external_perception_); + tcp_pose_tracker_.setScreenTagId(tags.screen().id()); + tcp_pose_tracker_.setHandTagId(tags.hand().id()); - ibvs_.setPerception(perception_); - if (!validateControlJointNames()) { - initialized_ = false; - last_status_ = Status::CONTROL_JOINT_MISMATCH; - return false; - } + // Publish the presentation settings immediately so a gRPC stream started + // before the first periodic task tick uses the configured behavior. + publishCoordinateOverlay(); initialized_ = applyConfig(); if (initialized_) { tracker_.clear(); tracker_.resetActiveTagTracking(); - ibvs_.reset(); + tcp_pose_tracker_.clear(); + pbvs_.reset(); phase_ = Phase::IDLE; phase_after_retract_ = Phase::DONE; final_status_after_retract_ = Status::DONE; target_locked_ = false; - ibvs_target_initialized_ = false; + pbvs_target_initialized_ = false; touch_command_started_ = false; retract_command_started_ = false; align_stable_count_ = 0; last_active_tag_id_ = -1; last_touch_pressure_sum_ = 0.0; + last_touch_resultant_fz_ = 0.0; last_touch_nonzero_count_ = 0; - last_align_error_camera_.setZero(); + last_align_error_screen_tag_.setZero(); touch_start_position_valid_ = false; touch_start_position_base_.setZero(); retract_start_position_valid_ = false; retract_start_position_base_.setZero(); + have_last_T_B_G_ = false; + last_T_B_G_.setIdentity(); + max_T_B_G_translation_delta_m_ = 0.0; + max_T_B_G_rotation_delta_rad_ = 0.0; + last_touch_pressure_log_time_ = Clock::time_point{}; last_status_ = Status::IDLE; + startCoordinateOverlayWorker(); } else { last_status_ = Status::INVALID_CONFIG; } @@ -385,6 +422,7 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, bool TouchScreenTask::touch(const int u, const int v) { std::lock_guard lock(mutex_); if (isBusyUnlocked()) { + last_status_ = Status::TASK_BUSY; return false; } return startFromPixelUnlocked(u, v); @@ -412,27 +450,36 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { tracker_.clear(); tracker_.resetActiveTagTracking(); - ibvs_.reset(); + tcp_pose_tracker_.clear(); + pbvs_.reset(); target_u_ = u; target_v_ = v; target_locked_ = false; - ibvs_target_initialized_ = false; + pbvs_target_initialized_ = false; touch_command_started_ = false; retract_command_started_ = false; align_stable_count_ = 0; align_debug_count_ = 0; + pbvs_debug_count_ = 0; last_touch_pressure_sum_ = 0.0; + last_touch_resultant_fz_ = 0.0; last_touch_nonzero_count_ = 0; last_active_tag_id_ = -1; - last_align_error_camera_.setZero(); + last_align_error_screen_tag_.setZero(); locked_target_rotation_valid_ = false; locked_target_rotation_.setIdentity(); touch_start_position_valid_ = false; touch_start_position_base_.setZero(); retract_start_position_valid_ = false; retract_start_position_base_.setZero(); + have_last_T_B_G_ = false; + last_T_B_G_.setIdentity(); + max_T_B_G_translation_delta_m_ = 0.0; + max_T_B_G_rotation_delta_rad_ = 0.0; phase_ = Phase::ALIGNING; + setCoordinateOverlayEnabled(false); + startCoordinateOverlayWorker(); phase_after_retract_ = Phase::DONE; final_status_after_retract_ = Status::DONE; phase_start_time_ = Clock::now(); @@ -451,15 +498,9 @@ bool TouchScreenTask::step(const double dt) { return false; } - if (dexhand_) { - const bool tactile_ok = updateTouchPressure(); - // std::cout << "[TouchScreenTask][TACTILE] finger=" - // << fingerTypeToString(toFingerType(config_.touch().tactile().finger())) - // << ", region=" << tactileRegionToString(toTactileRegion(config_.touch().tactile().region())) - // << ", ok=" << (tactile_ok ? 1 : 0) - // << ", nonzero_count=" << last_touch_nonzero_count_ - // << ", pressure_sum=" << last_touch_pressure_sum_ - // << std::endl; + // TOUCHING reads and checks the same tactile sample together in stepTouching(). + if (dexhand_ && phase_ != Phase::TOUCHING) { + updateTouchPressure(); } switch (phase_) { @@ -471,6 +512,18 @@ bool TouchScreenTask::step(const double dt) { case Phase::ALIGN_REACHED: last_status_ = Status::ALIGN_REACHED; if (config_.alignment().pause_when_reached()) { + // Entering ALIGN_REACHED resets phase_start_time_, so the + // pause has its own timeout measured from successful alignment. + const double paused_s = + std::chrono::duration(Clock::now() - phase_start_time_).count(); + if (paused_s > config_.alignment().timeout_s()) { + CMVR_LOG(WARNING) << "[TouchScreenTask][ALIGN_PAUSE_TIMEOUT]" + << " elapsed_s=" << paused_s + << ", timeout_s=" << config_.alignment().timeout_s() + << ", move_to_init=" << config_.initialization().after_finish(); + enterFailed(Status::ALIGN_TIMEOUT); + return false; + } return true; } if (!startTouchPhase()) { @@ -498,9 +551,153 @@ bool TouchScreenTask::step(const double dt) { return false; } +device::CameraStreamOverlay TouchScreenTask::buildCoordinateOverlay( + const perception::AprilTagPerception* overlay_perception) const +{ + device::CameraStreamOverlay overlay; + overlay.draw_coordinate_frames = + !config_.has_debug_draw_coordinate_frames() || + config_.debug_draw_coordinate_frames(); + overlay.coordinate_axis_length_m = 0.02; + if (config_.has_debug_coordinate_axis_length_m() && + std::isfinite(config_.debug_coordinate_axis_length_m()) && + config_.debug_coordinate_axis_length_m() > 0.0) { + overlay.coordinate_axis_length_m = config_.debug_coordinate_axis_length_m(); + } + + if (overlay.draw_coordinate_frames && overlay_perception) { + const auto& tags = config_.perception().tags(); + const int screen_tag_id = tags.screen().id(); + const int hand_tag_id = tags.hand().id(); + const auto* screen_tag = overlay_perception->findTag(screen_tag_id); + + if (screen_tag) { + device::CoordinateFrameOverlay frame; + frame.T_C_Frame = screen_tag->T_C_Tag(); + frame.label = "G"; + frame.tag_id = screen_tag->id; + frame.valid = true; + overlay.coordinate_frames.push_back(std::move(frame)); + } + + if (const auto* hand_tag = overlay_perception->findTag(hand_tag_id)) { + device::CoordinateFrameOverlay frame; + frame.T_C_Frame = hand_tag->T_C_Tag(); + frame.label = "H"; + frame.tag_id = hand_tag->id; + frame.valid = true; + overlay.coordinate_frames.push_back(std::move(frame)); + } + } + + return overlay; +} + +void TouchScreenTask::publishCoordinateOverlay() +{ + if (external_camera_) { + external_camera_->setStreamOverlay(buildCoordinateOverlay(external_perception_.get())); + } +} + +void TouchScreenTask::startCoordinateOverlayWorker() +{ + if (coordinate_overlay_thread_.joinable() || !external_camera_ || + (config_.has_debug_draw_coordinate_frames() && + !config_.debug_draw_coordinate_frames())) { + return; + } + + try { + // AprilTagPerception owns mutable frame/detector state. Never share + // the PBVS instance with the display worker. + auto overlay_perception = + std::make_shared(external_camera_); + overlay_perception->setTagSize(config_.perception().tags().screen().size_m()); + { + std::lock_guard lock(coordinate_overlay_mutex_); + coordinate_overlay_stop_ = false; + coordinate_overlay_enabled_ = phase_ != Phase::ALIGNING; + } + coordinate_overlay_thread_ = std::thread( + &TouchScreenTask::coordinateOverlayLoop, this, std::move(overlay_perception)); + } catch (const std::exception& e) { + CMVR_LOG(WARNING) << "[TouchScreenTask] Failed to start coordinate overlay worker: " + << e.what(); + } +} + +void TouchScreenTask::stopCoordinateOverlayWorker() +{ + { + std::lock_guard lock(coordinate_overlay_mutex_); + coordinate_overlay_stop_ = true; + } + coordinate_overlay_cv_.notify_all(); + if (coordinate_overlay_thread_.joinable()) { + coordinate_overlay_thread_.join(); + } +} + +void TouchScreenTask::setCoordinateOverlayEnabled(const bool enabled) +{ + { + std::lock_guard lock(coordinate_overlay_mutex_); + coordinate_overlay_enabled_ = enabled; + } + coordinate_overlay_cv_.notify_all(); +} + +void TouchScreenTask::coordinateOverlayLoop( + std::shared_ptr overlay_perception) +{ + perception::AprilTagPerception::Options options; + options.depth_policy = perception::AprilTagPerception::DepthPolicy::NONE; + options.detect_tags = true; + options.fetch_encoded = false; + const auto refresh_period = + std::chrono::duration_cast(std::chrono::duration(1.0 / 30.0)); + + std::unique_lock lock(coordinate_overlay_mutex_); + while (true) { + coordinate_overlay_cv_.wait(lock, [this] { + return coordinate_overlay_stop_ || coordinate_overlay_enabled_; + }); + if (coordinate_overlay_stop_) { + break; + } + const auto next_refresh = Clock::now() + refresh_period; + lock.unlock(); + + // No task/worker lock is held during camera acquisition or tag detection. + try { + overlay_perception->update(options); + const auto overlay = buildCoordinateOverlay(overlay_perception.get()); + std::lock_guard publish_lock(coordinate_overlay_mutex_); + // ALIGNING publishes its own perception snapshot. Discard an + // in-flight display frame if alignment or shutdown has started. + if (!coordinate_overlay_stop_ && coordinate_overlay_enabled_) { + external_camera_->setStreamOverlay(overlay); + } + } catch (const std::exception& e) { + CMVR_LOG_EVERY_N(WARNING, 30) + << "[TouchScreenTask] Coordinate overlay refresh failed: " << e.what(); + } catch (...) { + CMVR_LOG_EVERY_N(WARNING, 30) + << "[TouchScreenTask] Coordinate overlay refresh failed: unknown exception"; + } + + lock.lock(); + coordinate_overlay_cv_.wait_until(lock, next_refresh, [this] { + return coordinate_overlay_stop_ || !coordinate_overlay_enabled_; + }); + } +} + void TouchScreenTask::stop() { std::lock_guard lock(mutex_); stopUnlocked(); + stopCoordinateOverlayWorker(); } void TouchScreenTask::stopUnlocked() { @@ -511,28 +708,32 @@ void TouchScreenTask::stopUnlocked() { } } - sendZeroJointVelocity(); - ibvs_.resetTwistCommandState(); - holdCurrentControlledPosition(); + pbvs_.resetTwistCommandState(); phase_ = Phase::IDLE; + setCoordinateOverlayEnabled(true); phase_after_retract_ = Phase::DONE; final_status_after_retract_ = Status::DONE; target_locked_ = false; - ibvs_target_initialized_ = false; + pbvs_target_initialized_ = false; touch_command_started_ = false; retract_command_started_ = false; align_stable_count_ = 0; last_active_tag_id_ = -1; last_touch_pressure_sum_ = 0.0; + last_touch_resultant_fz_ = 0.0; last_touch_nonzero_count_ = 0; - last_align_error_camera_.setZero(); + last_align_error_screen_tag_.setZero(); locked_target_rotation_valid_ = false; locked_target_rotation_.setIdentity(); touch_start_position_valid_ = false; touch_start_position_base_.setZero(); retract_start_position_valid_ = false; retract_start_position_base_.setZero(); + have_last_T_B_G_ = false; + last_T_B_G_.setIdentity(); + max_T_B_G_translation_delta_m_ = 0.0; + max_T_B_G_rotation_delta_rad_ = 0.0; last_status_ = Status::STOPPED; } @@ -612,9 +813,9 @@ int TouchScreenTask::lastActiveTagId() const { return last_active_tag_id_; } -Eigen::Vector3d TouchScreenTask::lastAlignErrorCamera() const { +Eigen::Vector3d TouchScreenTask::lastAlignErrorScreenTag() const { std::lock_guard lock(mutex_); - return last_align_error_camera_; + return last_align_error_screen_tag_; } std::string TouchScreenTask::stateString() const { @@ -645,7 +846,6 @@ const char* TouchScreenTask::statusToString(const Status status) { case Status::IDLE: return "IDLE"; case Status::NOT_INITIALIZED: return "NOT_INITIALIZED"; case Status::INVALID_CONFIG: return "INVALID_CONFIG"; - case Status::CONTROL_JOINT_MISMATCH: return "CONTROL_JOINT_MISMATCH"; case Status::ALIGN_WAITING_PERCEPTION: return "ALIGN_WAITING_PERCEPTION"; case Status::ALIGN_WAITING_TRACK: return "ALIGN_WAITING_TRACK"; case Status::ALIGN_TARGET_SETUP_FAILED: return "ALIGN_TARGET_SETUP_FAILED"; @@ -673,15 +873,19 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& !config.devices().has_arm_id() || config.devices().arm_id().empty() || !config.devices().has_dexhand_id() || config.devices().dexhand_id().empty() || !config.devices().has_camera_id() || config.devices().camera_id().empty() || + !config.devices().has_external_camera_id() || + config.devices().external_camera_id().empty() || !config.has_initialization() || !config.initialization().has_before_start() || !config.initialization().has_after_finish() || !config.initialization().has_velocity() || !config.initialization().has_acceleration() || !config.has_perception() || - !config.perception().has_apriltag() || + !config.perception().has_tags() || + !config.perception().has_hand_camera() || !config.has_alignment() || - !config.alignment().has_ibvs() || + !config.alignment().has_calibration() || + !config.alignment().has_pbvs() || !config.alignment().has_target() || !config.alignment().has_error_threshold() || !config.alignment().has_stable_frames() || @@ -693,40 +897,44 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& !config.has_retract() || !config.retract().has_twist_tool() || !config.retract().has_acceleration() || - !config.retract().has_duration_s()) { + !config.retract().has_distance_m()) { return false; } - const auto& apriltag = config.perception().apriltag(); + const auto& perception = config.perception(); + const auto& tags = perception.tags(); + const auto& hand_tag = tags.hand(); + const auto& screen_tag = tags.screen(); + const auto& hand_camera = perception.hand_camera(); const auto& alignment = config.alignment(); - const auto& ibvs = alignment.ibvs(); + const auto& calibration = alignment.calibration(); + const auto& pbvs = alignment.pbvs(); const auto& target = alignment.target(); const auto& touch = config.touch(); const auto& tactile = touch.tactile(); const auto& retract = config.retract(); - if (!apriltag.has_tag_size_m() || - !apriltag.has_depth_policy() || - !apriltag.has_target_point_method() || - !target.has_position_in_camera() || - !hasVec3(target.position_in_camera()) || - !target.has_rotation_vector() || - !hasVec3(target.rotation_vector()) || + if (!screen_tag.has_id() || + !screen_tag.has_size_m() || + !hand_tag.has_id() || + !hand_tag.has_size_m() || + !hand_camera.has_depth_policy() || + !hand_camera.has_target_point_method() || + !calibration.has_hand_tag_to_tcp() || + !hasMat4(calibration.hand_tag_to_tcp()) || + !target.has_hand_orientation_g() || + !hasEuler(target.hand_orientation_g()) || !target.has_mode() || !hasVec6(alignment.error_threshold()) || - !ibvs.has_camera_link() || - !ibvs.has_lambda() || - !ibvs.has_mu() || - !ibvs.has_qdot_max() || - !ibvs.has_vmax6() || - !hasVec6(ibvs.vmax6()) || - !ibvs.has_amax6() || - !hasVec6(ibvs.amax6()) || - !ibvs.has_twist_filter_alpha() || - !ibvs.has_r_camera_to_visp() || - !hasMat3(ibvs.r_camera_to_visp()) || - !ibvs.has_r_camera_to_urdf() || - !hasMat3(ibvs.r_camera_to_urdf()) || + !pbvs.has_position_gain() || + !hasVec3(pbvs.position_gain()) || + !pbvs.has_rotation_gain() || + !hasVec3(pbvs.rotation_gain()) || + !pbvs.has_vmax6() || + !hasVec6(pbvs.vmax6()) || + !pbvs.has_amax6() || + !hasVec6(pbvs.amax6()) || + !pbvs.has_twist_filter_alpha() || !tactile.has_finger() || !tactile.has_region() || !tactile.has_criterion() || @@ -735,37 +943,65 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& return false; } - if (ibvs.camera_link().empty() || - ibvs.control_joint_names().empty()) { - return false; - } - - const Eigen::Vector3d target_position = - cmvr::common::math::toEigenVec3(alignment.target().position_in_camera()); - const Eigen::Vector3d target_rotation = - cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); + const Eigen::Vector3d target_orientation = + cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g()); + const Eigen::Vector3d position_offset_G = + target.has_position_offset_g() + ? cmvr::common::math::toEigenVec3(target.position_offset_g()) + : Eigen::Vector3d::Zero(); + const Eigen::Vector3d rotation_offset_G = + target.has_rotation_offset_g() + ? cmvr::common::math::toEigenVec3(target.rotation_offset_g()) + : Eigen::Vector3d::Zero(); const Eigen::Matrix error_threshold = cmvr::common::math::toEigenVec6(alignment.error_threshold()); - const Eigen::Matrix3d camera_to_visp = - cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp()); - const Eigen::Matrix3d camera_to_urdf = - cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf()); + const Eigen::Vector3d position_gain = + cmvr::common::math::toEigenVec3(pbvs.position_gain()); + const Eigen::Vector3d rotation_gain = + cmvr::common::math::toEigenVec3(pbvs.rotation_gain()); + const Eigen::Matrix vmax6 = + cmvr::common::math::toEigenVec6(pbvs.vmax6()); + const Eigen::Matrix amax6 = + cmvr::common::math::toEigenVec6(pbvs.amax6()); + const Eigen::Matrix4d T_H_P = toEigenMat4(calibration.hand_tag_to_tcp()); - if (!std::isfinite(apriltag.tag_size_m()) || apriltag.tag_size_m() <= 0.0 || - !target_position.allFinite() || - !target_rotation.allFinite() || + if (screen_tag.id() < 0 || hand_tag.id() < 0 || + screen_tag.id() == hand_tag.id() || + !std::isfinite(screen_tag.size_m()) || screen_tag.size_m() <= 0.0 || + !std::isfinite(hand_tag.size_m()) || hand_tag.size_m() <= 0.0 || + std::abs(screen_tag.size_m() - hand_tag.size_m()) > 1e-12 || + config.devices().camera_id().empty() || + config.devices().external_camera_id().empty() || + !isHomogeneousTransform(T_H_P) || + !target_orientation.allFinite() || + !position_offset_G.allFinite() || + !rotation_offset_G.allFinite() || + !position_gain.allFinite() || + !rotation_gain.allFinite() || + !vmax6.allFinite() || + !amax6.allFinite() || + !std::isfinite(pbvs.twist_filter_alpha()) || + pbvs.twist_filter_alpha() < 0.0 || pbvs.twist_filter_alpha() > 1.0 || alignment.stable_frames() <= 0 || !std::isfinite(alignment.timeout_s()) || alignment.timeout_s() <= 0.0 || !std::isfinite(tactile.force_threshold()) || tactile.force_threshold() < 0.0 || !std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 || - !camera_to_visp.allFinite() || !camera_to_urdf.allFinite() || !cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() || !std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 || - !std::isfinite(retract.duration_s()) || retract.duration_s() < 0.0) { + (retract.has_linear_jerk() && + (!std::isfinite(retract.linear_jerk()) || retract.linear_jerk() <= 0.0)) || + cmvr::common::math::toEigenVec6(retract.twist_tool()).head<3>().norm() <= 1e-9 || + !std::isfinite(retract.distance_m()) || retract.distance_m() <= 0.0) { return false; } - for (int i = 0; i < error_threshold.size(); ++i) { - if (!std::isfinite(error_threshold[i]) || error_threshold[i] < 0.0) { + for (int i = 0; i < 3; ++i) { + if (position_gain[i] < 0.0 || rotation_gain[i] < 0.0) { + return false; + } + } + for (int i = 0; i < 6; ++i) { + if (!std::isfinite(error_threshold[i]) || error_threshold[i] < 0.0 || + vmax6[i] <= 0.0 || amax6[i] < 0.0) { return false; } } @@ -778,12 +1014,32 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& } if (config.initialization().before_start() || config.initialization().after_finish()) { + std::vector init_joint_names; + init_joint_names.reserve(static_cast( + config.initialization().joint_positions_size())); + for (const auto& joint : config.initialization().joint_positions()) { + init_joint_names.push_back(joint.joint_name()); + } std::vector init_positions; - if (!buildInitJointPositionsFromConfig(config, init_positions)) { + if (!buildInitJointPositionsFromConfig(config, init_joint_names, init_positions) || + static_cast(init_positions.size()) != + config.initialization().joint_positions_size()) { return false; } } + const auto& initialization = config.initialization(); + if (initialization.has_skip_position_tolerance_rad() && + (!std::isfinite(initialization.skip_position_tolerance_rad()) || + initialization.skip_position_tolerance_rad() < 0.0)) { + return false; + } + if (initialization.has_skip_velocity_tolerance_rad_s() && + (!std::isfinite(initialization.skip_velocity_tolerance_rad_s()) || + initialization.skip_velocity_tolerance_rad_s() < 0.0)) { + return false; + } + std::vector tactile_regions; if (!appendRequestedTactileRegions(toFingerType(tactile.finger()), toTactileRegion(tactile.region()), @@ -805,6 +1061,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& return twist.allFinite() && std::isfinite(speed_l.acceleration()) && speed_l.acceleration() > 0.0 && + (!speed_l.has_linear_jerk() || + (std::isfinite(speed_l.linear_jerk()) && speed_l.linear_jerk() > 0.0)) && std::isfinite(speed_l.max_distance_m()) && speed_l.max_distance_m() >= 0.0; } @@ -825,7 +1083,7 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& !std::isfinite(move_l.velocity()) || move_l.velocity() <= 0.0 || !std::isfinite(move_l.acceleration()) || move_l.acceleration() <= 0.0 || !std::isfinite(move_l.jerk()) || move_l.jerk() <= 0.0 || - move_l.joint_velocity_limits_size() != ibvs.control_joint_names_size()) { + move_l.joint_velocity_limits().empty()) { return false; } for (const double qd_max_i : move_l.joint_velocity_limits()) { @@ -842,13 +1100,22 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& } bool TouchScreenTask::applyConfig() { - if (!perception_) { + if (!perception_ || !external_perception_) { return false; } if (!validateConfig(config_)) { return false; } + init_skip_position_tolerance_rad_ = + config_.initialization().has_skip_position_tolerance_rad() + ? config_.initialization().skip_position_tolerance_rad() + : 1e-3; + init_skip_velocity_tolerance_rad_s_ = + config_.initialization().has_skip_velocity_tolerance_rad_s() + ? config_.initialization().skip_velocity_tolerance_rad_s() + : 1e-2; + if (dexhand_) { const auto& tactile = config_.touch().tactile(); std::vector tactile_regions; @@ -864,85 +1131,65 @@ bool TouchScreenTask::applyConfig() { } } - const auto& apriltag = config_.perception().apriltag(); - const auto& ibvs = config_.alignment().ibvs(); - const auto target_position_in_camera = - cmvr::common::math::toEigenVec3(config_.alignment().target().position_in_camera()); - const Eigen::Matrix3d r_camera_to_visp = - cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp()); - const Eigen::Matrix3d r_camera_to_urdf = - cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf()); - perception_->setTagSize(apriltag.tag_size_m()); - tracker_.setTargetPointMethod(toTargetPointMethod(apriltag.target_point_method())); - ibvs_.setLambda(ibvs.lambda()); - ibvs_.setMu(ibvs.mu()); - ibvs_.setQdotMax(ibvs.qdot_max()); - ibvs_.setVelocityLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.vmax6()))); - ibvs_.setAccelerationLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.amax6()))); - ibvs_.setTwistFilterAlpha(ibvs.twist_filter_alpha()); - ibvs_.setAlignCameraToVisp(r_camera_to_visp); - ibvs_.setAlignCameraToUrdf(r_camera_to_urdf); + const auto& perception = config_.perception(); + const auto& tags = perception.tags(); + const auto& hand_camera_config = perception.hand_camera(); + const auto& calibration = config_.alignment().calibration(); + const auto& pbvs = config_.alignment().pbvs(); + perception_->setTagSize(tags.screen().size_m()); + external_perception_->setTagSize(tags.screen().size_m()); + tracker_.setTargetPointMethod(toTargetPointMethod(hand_camera_config.target_point_method())); + tcp_pose_tracker_.setPerception(external_perception_); + tcp_pose_tracker_.setScreenTagId(tags.screen().id()); + tcp_pose_tracker_.setHandTagId(tags.hand().id()); + T_H_P_ = toEigenMat4(calibration.hand_tag_to_tcp()); + + pbvs_.setPositionGain(cmvr::common::math::toEigenVec3(pbvs.position_gain())); + pbvs_.setRotationGain(cmvr::common::math::toEigenVec3(pbvs.rotation_gain())); + pbvs_.setVelocityLimit6(toArray6(cmvr::common::math::toEigenVec6(pbvs.vmax6()))); + pbvs_.setAccelerationLimit6(toArray6(cmvr::common::math::toEigenVec6(pbvs.amax6()))); + pbvs_.setTolerance6(toArray6( + cmvr::common::math::toEigenVec6(config_.alignment().error_threshold()))); + pbvs_.setTwistFilterAlpha(pbvs.twist_filter_alpha()); + + std::array enabled{{true, true, true, true, true, true}}; + if (config_.alignment().target().mode() == + cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION) { + enabled[5] = false; + } else if (config_.alignment().target().mode() == + cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY) { + enabled[3] = false; + enabled[4] = false; + enabled[5] = false; + } + pbvs_.setAxisEnabled(enabled); + + const auto amax = cmvr::common::math::toEigenVec6(pbvs.amax6()); + pbvs_command_acceleration_ = std::max(0.01, amax.head<3>().maxCoeff()); CMVR_LOG(DEBUG) << "[TouchScreenTask] Apply config id=" << config_.id() << ", arm_id=" << config_.devices().arm_id() - << ", camera_id=" << config_.devices().camera_id() - << ", tag_size_m=" << apriltag.tag_size_m() - << ", target_position_in_camera=[" << target_position_in_camera.x() - << ", " << target_position_in_camera.y() - << ", " << target_position_in_camera.z() << "]" - << ", r_camera_to_visp=[" << r_camera_to_visp(0, 0) - << ", " << r_camera_to_visp(0, 1) - << ", " << r_camera_to_visp(0, 2) - << "; " << r_camera_to_visp(1, 0) - << ", " << r_camera_to_visp(1, 1) - << ", " << r_camera_to_visp(1, 2) - << "; " << r_camera_to_visp(2, 0) - << ", " << r_camera_to_visp(2, 1) - << ", " << r_camera_to_visp(2, 2) << "]" - << ", r_camera_to_urdf=[" << r_camera_to_urdf(0, 0) - << ", " << r_camera_to_urdf(0, 1) - << ", " << r_camera_to_urdf(0, 2) - << "; " << r_camera_to_urdf(1, 0) - << ", " << r_camera_to_urdf(1, 1) - << ", " << r_camera_to_urdf(1, 2) - << "; " << r_camera_to_urdf(2, 0) - << ", " << r_camera_to_urdf(2, 1) - << ", " << r_camera_to_urdf(2, 2) << "]"; + << ", hand_camera_id=" << config_.devices().camera_id() + << ", external_camera_id=" << config_.devices().external_camera_id() + << ", hand_tag_size_m=" << tags.hand().size_m() + << ", screen_tag_size_m=" << tags.screen().size_m() + << ", hand_tag_id=" << tags.hand().id() + << ", screen_tag_id=" << tags.screen().id(); return true; } -bool TouchScreenTask::validateControlJointNames() const { - std::vector solver_joint_names; - if (!ibvs_.getChainJointNames(solver_joint_names)) { - return false; - } - const std::vector task_joint_names = controlJointNames(config_); - if (solver_joint_names == task_joint_names) { - return true; - } - - std::ostringstream mismatch; - mismatch << "[TouchScreenTask] control_joint_names mismatch with IbvsController IK chain" - << ", task joints=["; - for (const auto& name : task_joint_names) { - mismatch << name << ' '; - } - mismatch << "], solver joints=["; - for (const auto& name : solver_joint_names) { - mismatch << name << ' '; - } - mismatch << ']'; - CMVR_LOG(ERROR) << mismatch.str(); - return false; -} - bool TouchScreenTask::stepAligning(const double dt) { const auto& alignment = config_.alignment(); - const auto target_position_in_camera = - cmvr::common::math::toEigenVec3(alignment.target().position_in_camera()); - const auto target_rotation_vector = - cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); - const auto error_threshold = - cmvr::common::math::toEigenVec6(alignment.error_threshold()); + const auto target_orientation = + cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g()); + const auto& target = alignment.target(); + const Eigen::Vector3d position_offset_G = + target.has_position_offset_g() + ? cmvr::common::math::toEigenVec3(target.position_offset_g()) + : Eigen::Vector3d::Zero(); + const Eigen::Vector3d rotation_offset_G = + target.has_rotation_offset_g() + ? cmvr::common::math::toEigenVec3(target.rotation_offset_g()) + : Eigen::Vector3d::Zero(); const auto now = Clock::now(); const double elapsed = std::chrono::duration(now - phase_start_time_).count(); @@ -950,17 +1197,42 @@ bool TouchScreenTask::stepAligning(const double dt) { enterFailed(Status::ALIGN_TIMEOUT); return false; } - const double ibvs_dt = std::clamp(dt, 0.005, 0.05); + const double pbvs_dt = std::clamp(dt, 0.0001, 0.05); - perception::AprilTagPerception::Options perception_options; - perception_options.depth_policy = - toDepthPolicy(config_.perception().apriltag().depth_policy()); - perception_options.detect_tags = true; - perception_options.fetch_encoded = false; - if (!perception_->update(perception_options)) { - hardStopIbvsMotion(); - last_status_ = Status::ALIGN_WAITING_PERCEPTION; - return true; + // The hand camera is only needed to establish the target anchor once. + // After the first successful PBVS target initialization, tracker_ keeps + // that anchor fixed and the external camera supplies the live TCP pose. + const bool hand_camera_needed = !target_locked_ || !pbvs_target_initialized_; + bool hand_perception_ok = true; + if (hand_camera_needed) { + perception::AprilTagPerception::Options hand_options; + hand_options.depth_policy = + toDepthPolicy(config_.perception().hand_camera().depth_policy()); + hand_options.detect_tags = true; + hand_options.fetch_encoded = false; + hand_perception_ok = perception_->update(hand_options); + if (!hand_perception_ok) { + if (align_debug_count_ == 0 && !perception_->color().empty()) { + cv::imwrite("/tmp/cmvr_hand_camera_perception.png", perception_->color()); + } + if ((align_debug_count_++ % 200) == 0) { + const auto& image = perception_->color(); + const auto& intrinsics = perception_->intrinsics(); + CMVR_LOG(INFO) << "[TouchScreenTask][ALIGN_PERCEPTION_DEBUG]" + << " status=" + << perception::AprilTagPerception::statusToString( + perception_->lastStatus()) + << ", image=" << image.cols << "x" << image.rows + << ", channels=" << image.channels() + << ", fx=" << intrinsics.fx + << ", fy=" << intrinsics.fy + << ", cx=" << intrinsics.cx + << ", cy=" << intrinsics.cy; + } + stopPbvsMotion(); + last_status_ = Status::ALIGN_WAITING_PERCEPTION; + return true; + } } bool tracking_ok = false; @@ -968,15 +1240,28 @@ bool TouchScreenTask::stepAligning(const double dt) { tracking_ok = tracker_.startTrackingFromPixel(target_u_, target_v_); if (tracking_ok) { target_locked_ = true; - ibvs_target_initialized_ = false; + pbvs_target_initialized_ = false; align_stable_count_ = 0; } - } else { + } else if (hand_camera_needed && hand_perception_ok) { tracking_ok = tracker_.track(); } + // The touch pixel is converted into a fixed anchor in the selected tag + // frame on the first successful observation. Once locked, the external + // camera provides the live TCP pose, so the hand camera is intentionally + // skipped and cannot interrupt the PBVS motion or invalidate that target. + if (!tracking_ok && target_locked_ && pbvs_target_initialized_) { + const int locked_tag_id = tracker_.activeTagId(); + tracking_ok = locked_tag_id >= 0 && tracker_.hasAnchorForTag(locked_tag_id); + if (tracking_ok && (pbvs_debug_count_ % 20) == 0) { + CMVR_LOG(DEBUG) << "[TouchScreenTask][ALIGNING] target anchor locked; " + << "skip hand camera sampling, tag_id=" << locked_tag_id; + } + } + if (!tracking_ok) { - hardStopIbvsMotion(); + stopPbvsMotion(); align_stable_count_ = 0; last_status_ = Status::ALIGN_WAITING_TRACK; return true; @@ -984,34 +1269,83 @@ bool TouchScreenTask::stepAligning(const double dt) { const int tag_id = tracker_.activeTagId(); last_active_tag_id_ = tag_id; - if (tag_id < 0) { - hardStopIbvsMotion(); + const int screen_tag_id = config_.perception().tags().screen().id(); + if (tag_id != screen_tag_id) { + stopPbvsMotion(); align_stable_count_ = 0; last_status_ = Status::ALIGN_WAITING_TRACK; return true; } - Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero(); - if (!tracker_.getAnchorInTag(tag_id, p_t_target)) { - hardStopIbvsMotion(); + Eigen::Vector3d p_target_G = Eigen::Vector3d::Zero(); + if (!tracker_.getAnchorInTag(tag_id, p_target_G)) { + stopPbvsMotion(); align_stable_count_ = 0; last_status_ = Status::ALIGN_WAITING_TRACK; return true; } - const auto* current_tag = perception_->findTag(tag_id); - if (!current_tag) { - hardStopIbvsMotion(); + perception::AprilTagPerception::Options external_options; + external_options.depth_policy = perception::AprilTagPerception::DepthPolicy::NONE; + external_options.detect_tags = true; + external_options.fetch_encoded = false; + if (!external_perception_->update(external_options)) { + publishCoordinateOverlay(); + stopPbvsMotion(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_PERCEPTION; + return true; + } + + publishCoordinateOverlay(); + + tcp_pose_tracker_.setScreenTagId(screen_tag_id); + if (!tcp_pose_tracker_.update(T_H_P_)) { + if (align_debug_count_ == 0 && !external_perception_->color().empty()) { + cv::imwrite("/tmp/cmvr_external_camera_perception.png", + external_perception_->color()); + } + if ((align_debug_count_++ % 200) == 0) { + std::ostringstream detected_ids; + for (const auto& detected_tag : external_perception_->tags()) { + if (detected_ids.tellp() > 0) { + detected_ids << ','; + } + detected_ids << detected_tag.id; + } + CMVR_LOG(INFO) << "[TouchScreenTask][EXTERNAL_PERCEPTION_DEBUG]" + << " status=" + << perception::AprilTagPerception::statusToString( + external_perception_->lastStatus()) + << ", detected_ids=[" << detected_ids.str() << ']' + << ", tcp_status=" + << perception::TagRelativeTcpPose::statusToString( + tcp_pose_tracker_.lastStatus()); + } + stopPbvsMotion(); align_stable_count_ = 0; last_status_ = Status::ALIGN_WAITING_TRACK; return true; } - Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(), - target_rotation_vector.y(), - target_rotation_vector.z()); - const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0); - switch (alignment.target().mode()) { + const Eigen::Matrix4d& T_G_P = tcp_pose_tracker_.T_G_P(); + const Eigen::Matrix3d R_G_H_target = + rotationFromTargetEuler(target_orientation.x(), + target_orientation.y(), + target_orientation.z()); + // PBVS controls P, while hand_orientation_G configures H. Convert the + // configured target through the fixed hand-tag-to-TCP calibration. + Eigen::Matrix3d R_target = + R_G_H_target * T_H_P_.block<3, 3>(0, 0); + if (rotation_offset_G.norm() > 1e-12) { + const double offset_angle = rotation_offset_G.norm(); + R_target = Eigen::AngleAxisd( + offset_angle, rotation_offset_G / offset_angle) + .toRotationMatrix() * + R_target; + } + const Eigen::Matrix3d R_current = T_G_P.block<3, 3>(0, 0); + switch (target.mode()) { case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: break; case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: @@ -1029,128 +1363,146 @@ bool TouchScreenTask::stepAligning(const double dt) { R_target = locked_target_rotation_; break; case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY: - // True position-only mode: keep the desired orientation equal to the - // current tag orientation every frame so IBVS does not actively try to - // correct rotational error. R_target = R_current; break; } - const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target); - ibvs_.setTrackedTagId(tag_id); - const bool refresh_target = !ibvs_target_initialized_ || tracker_.lastSwitched() || - alignment.target().mode() == - cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY; + const bool refresh_target = !pbvs_target_initialized_ || tracker_.lastSwitched(); if (refresh_target) { - if (!ibvs_.setTargetFromPointInTag(p_t_target, - target_position_in_camera, - target_rotvec.x(), - target_rotvec.y(), - target_rotvec.z())) { + if (tracker_.lastSwitched()) { + pbvs_.resetTwistCommandState(); + } + if (!pbvs_.setTargetPose(p_target_G + position_offset_G, R_target)) { enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); return false; } - ibvs_target_initialized_ = true; + pbvs_target_initialized_ = true; } - std::vector q_now; - if (!readControlledJointPositions(q_now)) { + PbvsController::Output output; + if (!pbvs_.compute(T_G_P, pbvs_dt, output)) { + enterFailed(Status::ALIGN_COMPUTE_FAILED); + return false; + } + + Eigen::Matrix4d T_B_P = Eigen::Matrix4d::Identity(); + try { + T_B_P = cmvr::common::math::poseToMatrix(arm_->fk(true)); + } catch (...) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + if (!isHomogeneousTransform(T_B_P)) { enterFailed(Status::ROBOT_STATE_FAILED); return false; } - std::vector qdot_cmd; - if (!ibvs_.computeQdot(q_now, ibvs_dt, qdot_cmd)) { - switch (ibvs_.lastComputeStatus()) { - case IbvsController::ComputeStatus::NO_NEW_FRAME: - case IbvsController::ComputeStatus::NO_TAG: - case IbvsController::ComputeStatus::TAG_MISMATCH: - case IbvsController::ComputeStatus::NO_DEPTH: - hardStopIbvsMotion(); - align_stable_count_ = 0; - last_status_ = Status::ALIGN_WAITING_TRACK; - return true; - case IbvsController::ComputeStatus::OK: - case IbvsController::ComputeStatus::NOT_READY: - case IbvsController::ComputeStatus::BAD_IMAGE: - case IbvsController::ComputeStatus::INVALID_INPUT: - case IbvsController::ComputeStatus::IK_FAILED: - default: - enterFailed(Status::ALIGN_COMPUTE_FAILED); - return false; - } + const Eigen::Matrix4d T_B_G = T_B_P * T_G_P.inverse(); + if (!isHomogeneousTransform(T_B_G)) { + enterFailed(Status::ALIGN_COMPUTE_FAILED); + return false; } - - if ((align_debug_count_++ % 20) == 0) { - Eigen::Matrix achieved_twist_base = - Eigen::Matrix::Zero(); - bool achieved_ok = false; - auto pinocchio_solver = - arm_ ? std::dynamic_pointer_cast(arm_->kinematicsSolver()) : nullptr; - if (pinocchio_solver) { - achieved_ok = pinocchio_solver->computeTwistBaseAtQ( - q_now, - qdot_cmd, - config_.alignment().ibvs().camera_link(), - achieved_twist_base); - } - - const auto& target_c = tracker_.lastTargetInCamera(); - const Eigen::Vector3d err_c = target_c - target_position_in_camera; - const auto& v_visp = ibvs_.lastCameraTwistVisp(); - const double qdot_norm = - qdot_cmd.empty() - ? 0.0 - : Eigen::Map( - qdot_cmd.data(), - static_cast(qdot_cmd.size())).norm(); - CMVR_LOG(DEBUG) << "[TouchScreenTask][ALIGN_DEBUG]" - << " target_c=[" << target_c.x() << ", " << target_c.y() - << ", " << target_c.z() << "]" - << ", err_c=[" << err_c.x() << ", " << err_c.y() - << ", " << err_c.z() << "]" - << ", v_visp=[" << v_visp[0] << ", " << v_visp[1] - << ", " << v_visp[2] << ", " << v_visp[3] - << ", " << v_visp[4] << ", " << v_visp[5] << "]" - << ", qdot0=" << (qdot_cmd.empty() ? 0.0 : qdot_cmd.front()) - << ", qdot_norm=" << qdot_norm - << ", achieved_ok=" << achieved_ok - << ", achieved_twist_base=[" << achieved_twist_base[0] - << ", " << achieved_twist_base[1] - << ", " << achieved_twist_base[2] - << ", " << achieved_twist_base[3] - << ", " << achieved_twist_base[4] - << ", " << achieved_twist_base[5] << "]"; + const Eigen::Matrix3d R_B_G = T_B_G.block<3, 3>(0, 0); + double T_B_G_delta_translation_m = 0.0; + double T_B_G_delta_rotation_rad = 0.0; + if (have_last_T_B_G_) { + T_B_G_delta_translation_m = + (T_B_G.block<3, 1>(0, 3) - last_T_B_G_.block<3, 1>(0, 3)).norm(); + const Eigen::Matrix3d R_delta = + last_T_B_G_.block<3, 3>(0, 0).transpose() * R_B_G; + T_B_G_delta_rotation_rad = + cmvr::device::cartesian_motion::rotationVector(R_delta).norm(); + max_T_B_G_translation_delta_m_ = + std::max(max_T_B_G_translation_delta_m_, T_B_G_delta_translation_m); + max_T_B_G_rotation_delta_rad_ = + std::max(max_T_B_G_rotation_delta_rad_, T_B_G_delta_rotation_rad); } - - if (!sendJointVelocity(qdot_cmd)) { - enterFailed(Status::ROBOT_COMMAND_FAILED); + last_T_B_G_ = T_B_G; + have_last_T_B_G_ = true; + const Eigen::Vector3d linear_B = R_B_G * output.linear_velocity_G; + const Eigen::Vector3d angular_B = R_B_G * output.angular_velocity_G; + if (!linear_B.allFinite() || !angular_B.allFinite()) { + enterFailed(Status::ALIGN_COMPUTE_FAILED); return false; } - last_align_error_camera_ = tracker_.lastTargetInCamera() - target_position_in_camera; - const Eigen::Vector3d rot_error_vec = - rotvecFromRotationMatrix(R_target.transpose() * R_current); - bool align_ok = - std::abs(last_align_error_camera_.x()) <= error_threshold[0] && - std::abs(last_align_error_camera_.y()) <= error_threshold[1] && - std::abs(last_align_error_camera_.z()) <= error_threshold[2]; - switch (alignment.target().mode()) { - case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: - align_ok = align_ok && - std::abs(rot_error_vec.x()) <= error_threshold[3] && - std::abs(rot_error_vec.y()) <= error_threshold[4] && - std::abs(rot_error_vec.z()) <= error_threshold[5]; - break; - case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: - align_ok = align_ok && - std::abs(rot_error_vec.x()) <= error_threshold[3] && - std::abs(rot_error_vec.y()) <= error_threshold[4]; - break; - case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY: - break; + const device::CartesianVelocity twist_B{ + linear_B.x(), linear_B.y(), linear_B.z(), + angular_B.x(), angular_B.y(), angular_B.z()}; + + if ((pbvs_debug_count_++ % 20) == 0) { + constexpr double kMetersToMillimeters = 1000.0; + constexpr double kRadiansToDegrees = 180.0 / 3.14159265358979323846; + const Eigen::Vector3d p_target_used_G = + pbvs_.targetPose().block<3, 1>(0, 3); + const Eigen::Vector3d p_tip_G = T_G_P.block<3, 1>(0, 3); + const Eigen::Vector3d p_B_G = T_B_G.block<3, 1>(0, 3); + const Eigen::Vector3d rotvec_B_G = + cmvr::device::cartesian_motion::rotationVector(R_B_G); + const Eigen::Matrix4d& T_C_H = tcp_pose_tracker_.T_C_H(); + const Eigen::Vector3d hand_tag_position_C = T_C_H.block<3, 1>(0, 3); + const Eigen::Vector3d hand_tag_normal_C = T_C_H.block<3, 3>(0, 0).col(2); + const double hand_tag_front_dot_camera = + hand_tag_position_C.norm() > 1e-9 + ? hand_tag_normal_C.dot(-hand_tag_position_C.normalized()) + : 0.0; + + CMVR_LOG(DEBUG) << "[PBVS_DEBUG]" + << " target_G_mm=[" + << p_target_used_G.x() * kMetersToMillimeters << ' ' + << p_target_used_G.y() * kMetersToMillimeters << ' ' + << p_target_used_G.z() * kMetersToMillimeters << ']' + << " tip_G_mm=[" + << p_tip_G.x() * kMetersToMillimeters << ' ' + << p_tip_G.y() * kMetersToMillimeters << ' ' + << p_tip_G.z() * kMetersToMillimeters << ']' + << " err_G_mm=[" + << output.position_error_G.x() * kMetersToMillimeters << ' ' + << output.position_error_G.y() * kMetersToMillimeters << ' ' + << output.position_error_G.z() * kMetersToMillimeters << ']' + << " rot_err_deg=[" + << output.rotation_error_G.x() * kRadiansToDegrees << ' ' + << output.rotation_error_G.y() * kRadiansToDegrees << ' ' + << output.rotation_error_G.z() * kRadiansToDegrees << ']' + << " raw_G=[" + << output.raw_linear_velocity_G.x() << ' ' + << output.raw_linear_velocity_G.y() << ' ' + << output.raw_linear_velocity_G.z() << ' ' + << output.raw_angular_velocity_G.x() << ' ' + << output.raw_angular_velocity_G.y() << ' ' + << output.raw_angular_velocity_G.z() << ']' + << " cmd_G=[" + << output.linear_velocity_G.x() << ' ' + << output.linear_velocity_G.y() << ' ' + << output.linear_velocity_G.z() << ' ' + << output.angular_velocity_G.x() << ' ' + << output.angular_velocity_G.y() << ' ' + << output.angular_velocity_G.z() << ']' + << " T_B_G_pos=[" + << p_B_G.x() * kMetersToMillimeters << ' ' + << p_B_G.y() * kMetersToMillimeters << ' ' + << p_B_G.z() * kMetersToMillimeters << ']' + << " T_B_G_rot=[" + << rotvec_B_G.x() * kRadiansToDegrees << ' ' + << rotvec_B_G.y() * kRadiansToDegrees << ' ' + << rotvec_B_G.z() * kRadiansToDegrees << ']' + << " T_B_G_delta_mm=" + << T_B_G_delta_translation_m * kMetersToMillimeters + << " T_B_G_delta_rot_deg=" + << T_B_G_delta_rotation_rad * kRadiansToDegrees + << " T_B_G_max_delta_mm=" + << max_T_B_G_translation_delta_m_ * kMetersToMillimeters + << " T_B_G_max_delta_rot_deg=" + << max_T_B_G_rotation_delta_rad_ * kRadiansToDegrees + << " hand_tag_front_dot_camera=" + << hand_tag_front_dot_camera + << " cmd_B=[" + << twist_B.vx << ' ' << twist_B.vy << ' ' << twist_B.vz << ' ' + << twist_B.wx << ' ' << twist_B.wy << ' ' << twist_B.wz << ']'; } - if (align_ok) { + + last_align_error_screen_tag_ = output.position_error_G; + if (output.reached) { ++align_stable_count_; } else { align_stable_count_ = 0; @@ -1158,20 +1510,37 @@ bool TouchScreenTask::stepAligning(const double dt) { if (align_stable_count_ >= alignment.stable_frames()) { CMVR_LOG(INFO) << "[TouchScreenTask][ALIGN_REACHED] tag_id=" << tag_id - << ", err_xyz=[" << last_align_error_camera_.x() << ", " - << last_align_error_camera_.y() << ", " - << last_align_error_camera_.z() << "]" - << ", err_rxyz=[" << rot_error_vec.x() << ", " - << rot_error_vec.y() << ", " - << rot_error_vec.z() << "]"; - hardStopIbvsMotion(); + << ", err_xyz_G=[" << output.position_error_G.x() << ", " + << output.position_error_G.y() << ", " + << output.position_error_G.z() << "]" + << ", err_rxyz_G=[" << output.rotation_error_G.x() << ", " + << output.rotation_error_G.y() << ", " + << output.rotation_error_G.z() << "]"; + stopPbvsMotion(); phase_ = Phase::ALIGN_REACHED; + setCoordinateOverlayEnabled(true); phase_start_time_ = Clock::now(); touch_command_started_ = false; last_status_ = Status::ALIGN_REACHED; return true; } + device::SpeedLOptions alignment_options; + alignment_options.acceleration = pbvs_command_acceleration_; + // PBVS changes direction on each visual update. Keep the original + // direction-following policy instead of repeatedly braking to change axes. + alignment_options.continuous_linear_reversal = false; + const auto speed_result = arm_->speedL(twist_B, + alignment_options, + 0.0, + device::FrameType::Base); + if (!speed_result.ok()) { + CMVR_LOG(ERROR) << "[TouchScreenTask][ALIGNING] speedL failed: " + << speed_result.message; + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + last_status_ = Status::ALIGNING; return true; } @@ -1188,15 +1557,13 @@ bool TouchScreenTask::stepTouching() { } } - if (!dexhand_) { + if (!updateTouchPressure()) { enterFailed(Status::TACTILE_UNAVAILABLE); return false; } + // Decide from the freshly read sample before logging, FK, or display work. if (isTouchTriggered(config_, last_touch_pressure_sum_)) { - if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) { - logTouchingSpeedLState(); - } if (!handleTouchTriggered(true)) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; @@ -1205,10 +1572,27 @@ bool TouchScreenTask::stepTouching() { } if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kMoveL) { + logTouchPressure(); last_status_ = Status::TOUCHING; return true; } + // A successful speedL submission does not report later worker failures. + // With duration=0 the controller must stay active until we request a stop + // or retract; otherwise waiting for contact/distance can continue forever. + if (!arm_->busy()) { + const double elapsed = + std::chrono::duration(Clock::now() - phase_start_time_).count(); + CMVR_LOG(ERROR) << "[TouchScreenTask][TOUCHING] speedL controller became idle before contact" + << ", elapsed_s=" << elapsed + << ", fz_N=" << last_touch_resultant_fz_ + << ", criterion_value_N=" << last_touch_pressure_sum_ + << ", threshold_N=" << config_.touch().tactile().force_threshold() + << "; check preceding CartesianVelocityController/planner errors"; + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + const double speed_l_max_distance_m = config_.touch().speed_l().max_distance_m(); if (speed_l_max_distance_m > 0.0) { if (!touch_start_position_valid_) { @@ -1225,7 +1609,6 @@ bool TouchScreenTask::stepTouching() { const double traveled_distance = (current_position_base - touch_start_position_base_).norm(); if (traveled_distance >= speed_l_max_distance_m) { - logTouchingSpeedLState(); if (!updateTouchPressure()) { enterFailed(Status::TACTILE_UNAVAILABLE); return false; @@ -1237,6 +1620,8 @@ bool TouchScreenTask::stepTouching() { } return true; } + logTouchPressure(); + logTouchingSpeedLState(); if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; @@ -1245,6 +1630,7 @@ bool TouchScreenTask::stepTouching() { } } + logTouchPressure(); last_status_ = Status::TOUCHING; return true; } @@ -1273,37 +1659,65 @@ bool TouchScreenTask::stepRetracting() { const auto now = Clock::now(); const double elapsed = std::chrono::duration(now - phase_start_time_).count(); - const double retract_duration_s = config_.retract().duration_s(); - if (elapsed < retract_duration_s) { + const double retract_distance_m = config_.retract().distance_m(); + if (!retract_start_position_valid_) { + const auto reference = arm_->getSpeedLReference(); + if (!reference.valid) { + if (!arm_->busy() || elapsed > 1.0) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + return true; // First controller tick has not captured the origin yet. + } + retract_start_position_base_ << reference.tcp_pose_base.x, + reference.tcp_pose_base.y, + reference.tcp_pose_base.z; + retract_direction_base_ << reference.target_base.vx, + reference.target_base.vy, reference.target_base.vz; + if (!retract_start_position_base_.allFinite() || !retract_direction_base_.allFinite() || + retract_direction_base_.norm() <= 1e-9) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + retract_direction_base_.normalize(); + retract_start_position_valid_ = true; + } + Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); + if (!retract_start_position_valid_ || + !readCurrentTouchPointPositionBase(current_position_base)) { + try { + arm_->stopL(); + } catch (...) { + } + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + + const Eigen::Vector3d delta_base = + current_position_base - retract_start_position_base_; + const double traveled_distance_m = delta_base.dot(retract_direction_base_); + // Keep the forward peak: subsequent backward travel must not cancel it. + // Reuse the existing measured-position sample, without delaying reversal. + max_forward_after_retract_m_ = + std::max(max_forward_after_retract_m_, -traveled_distance_m); + if (traveled_distance_m < retract_distance_m) { + if (!arm_->busy()) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } if (std::chrono::duration(now - last_retract_log_time_).count() >= 0.2) { last_retract_log_time_ = now; const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{}; - Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); - const bool have_current = readCurrentTouchPointPositionBase(current_position_base); - const bool have_delta = retract_start_position_valid_ && have_current; - Eigen::Vector3d delta_base = Eigen::Vector3d::Zero(); - if (have_delta) { - delta_base = current_position_base - retract_start_position_base_; - } - if (have_delta) { - CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed - << "/" << retract_duration_s - << ", cmd_base=[" << cmd_base.vx << ", " - << cmd_base.vy << ", " << cmd_base.vz << ", " - << cmd_base.wx << ", " << cmd_base.wy << ", " - << cmd_base.wz << "]" - << ", tcp_delta_base=[" << delta_base.x() << ", " - << delta_base.y() << ", " << delta_base.z() - << "], tcp_dist=" << delta_base.norm(); - } else { - CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed - << "/" << retract_duration_s - << ", cmd_base=[" << cmd_base.vx << ", " - << cmd_base.vy << ", " << cmd_base.vz << ", " - << cmd_base.wx << ", " << cmd_base.wy << ", " - << cmd_base.wz << "]" - << ", tcp_delta_base=unavailable"; - } + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed + << ", distance=" << traveled_distance_m + << "/" << retract_distance_m + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0 + << ", cmd_base=[" << cmd_base.vx << ", " + << cmd_base.vy << ", " << cmd_base.vz << ", " + << cmd_base.wx << ", " << cmd_base.wy << ", " + << cmd_base.wz << "]" + << ", tcp_delta_base=[" << delta_base.x() << ", " + << delta_base.y() << ", " << delta_base.z() << "]"; } last_status_ = Status::RETRACTING; return true; @@ -1315,26 +1729,33 @@ bool TouchScreenTask::stepRetracting() { const bool have_final_delta = retract_start_position_valid_ && have_final_position; if (have_final_delta) { final_delta_base = final_position_base - retract_start_position_base_; + max_forward_after_retract_m_ = std::max(max_forward_after_retract_m_, + -final_delta_base.dot(retract_direction_base_)); } if (have_final_delta) { CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed + << ", target_distance_m=" << retract_distance_m + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0 << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0) << ", final_tcp_delta_base=[" << final_delta_base.x() << ", " << final_delta_base.y() << ", " << final_delta_base.z() << "], final_tcp_dist=" << final_delta_base.norm(); } else { CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed + << ", target_distance_m=" << retract_distance_m + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0 << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0) << ", final_tcp_delta_base=unavailable"; } - try { - arm_->stopL(); - } catch (...) { + // Stop the retract worker synchronously before switching to the final + // joint-position trajectory. stopL() only requests deceleration and can + // return while the worker still owns the velocity-control mode. + const auto stop_result = arm_->stopMotion(); + if (!stop_result.ok()) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; } - holdCurrentControlledPosition(); if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) && !moveToInitPositionIfEnabled()) { phase_ = Phase::FAILED; @@ -1346,55 +1767,14 @@ bool TouchScreenTask::stepRetracting() { return phase_ != Phase::FAILED; } -bool TouchScreenTask::readControlledJointPositions(std::vector& q_out) const { - if (!arm_) { - return false; - } - - const auto state = arm_->getJointState(); - const auto model = arm_->getRobotModel(); - std::unordered_map q_map; - q_map.reserve(model.joint_names.size()); - for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) { - q_map[model.joint_names[i]] = state.position[i]; - } - - const auto& control_joint_names = config_.alignment().ibvs().control_joint_names(); - q_out.resize(static_cast(control_joint_names.size())); - for (int i = 0; i < control_joint_names.size(); ++i) { - const auto it = q_map.find(control_joint_names[i]); - if (it == q_map.end()) { - return false; +void TouchScreenTask::stopPbvsMotion() { + if (arm_) { + try { + arm_->stopL(); + } catch (...) { } - q_out[static_cast(i)] = it->second; } - return true; -} - -bool TouchScreenTask::sendJointVelocity(const std::vector& qdot) const { - if (!arm_ || qdot.size() != static_cast( - config_.alignment().ibvs().control_joint_names_size())) { - return false; - } - - device::JointVelocityCommand cmd; - cmd.velocity = qdot; - const auto result = arm_->speedJ(cmd, 0.0, 0.0); - if (!result.ok()) { - return false; - } - return true; -} - -bool TouchScreenTask::sendZeroJointVelocity() const { - std::vector zero( - static_cast(config_.alignment().ibvs().control_joint_names_size()), 0.0); - return sendJointVelocity(zero); -} - -void TouchScreenTask::hardStopIbvsMotion() { - sendZeroJointVelocity(); - ibvs_.resetTwistCommandState(); + pbvs_.resetTwistCommandState(); } bool TouchScreenTask::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const { @@ -1446,39 +1826,33 @@ void TouchScreenTask::logTouchingSpeedLState() const { CMVR_LOG(DEBUG) << state_log.str(); } -bool TouchScreenTask::holdCurrentControlledPosition() const { +bool TouchScreenTask::buildInitJointPositions(std::vector& positions_out) const { if (!arm_) { return false; } + return buildInitJointPositionsFromConfig( + config_, arm_->getRobotModel().joint_names, positions_out); +} + +bool TouchScreenTask::isAtInitPosition(const std::vector& positions) const { + if (!arm_ || arm_->busy()) { + return false; + } const auto state = arm_->getJointState(); - const auto model = arm_->getRobotModel(); - std::unordered_map q_map; - q_map.reserve(model.joint_names.size()); - for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) { - q_map[model.joint_names[i]] = state.position[i]; - } - - device::JointPositionCommand joints; - joints.position.reserve(static_cast( - config_.alignment().ibvs().control_joint_names_size())); - for (const auto& name : config_.alignment().ibvs().control_joint_names()) { - const auto it = q_map.find(name); - if (it == q_map.end()) { - return false; - } - joints.position.push_back(it->second); - } - - const auto result = arm_->servoJ(joints); - if (!result.ok()) { + if (state.position.size() != positions.size() || + state.velocity.size() != positions.size()) { return false; } - return true; -} -bool TouchScreenTask::buildInitJointPositions(std::vector& positions_out) const { - return buildInitJointPositionsFromConfig(config_, positions_out); + for (std::size_t i = 0; i < positions.size(); ++i) { + if (!std::isfinite(state.position[i]) || !std::isfinite(state.velocity[i]) || + std::abs(state.position[i] - positions[i]) > init_skip_position_tolerance_rad_ || + std::abs(state.velocity[i]) > init_skip_velocity_tolerance_rad_s_) { + return false; + } + } + return true; } bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() { @@ -1496,6 +1870,12 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() { return false; } + if (isAtInitPosition(init_positions)) { + CMVR_LOG(INFO) << "[TouchScreenTask] initial position already reached; skipping moveJ" + << ", arm=" << id_; + return true; + } + device::JointPositionCommand init_cmd{init_positions}; device::MotionOptions options; options.velocity = config_.initialization().velocity(); @@ -1517,6 +1897,12 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const { return false; } + if (isAtInitPosition(init_positions)) { + CMVR_LOG(INFO) << "[TouchScreenTask] final initial position already reached; skipping moveJ" + << ", arm=" << id_; + return true; + } + device::JointPositionCommand init_cmd{init_positions}; device::MotionOptions options; options.velocity = config_.initialization().velocity(); @@ -1529,6 +1915,12 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const { } bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) { + if (config_.touch().dwell_time_s() <= 0.0) { + // Submit reversal in this tick, before any synchronous FK or logging. + if (!startRetractPhase(Phase::DONE, Status::DONE)) return false; + logTouchPressure(true); + return true; + } if (stop_forward_motion) { const auto result = arm_->stopL(); if (!result.ok()) { @@ -1536,8 +1928,12 @@ bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) { } } + // Stop first: synchronous logging/FK must not delay the contact response. + const auto triggered_time = Clock::now(); + logTouchPressure(true); + logTouchingSpeedLState(); phase_ = Phase::DWELLING; - phase_start_time_ = Clock::now(); + phase_start_time_ = triggered_time; last_status_ = Status::TOUCH_TRIGGERED; return true; } @@ -1550,6 +1946,7 @@ bool TouchScreenTask::startTouchPhase() { phase_ = Phase::TOUCHING; phase_start_time_ = Clock::now(); + last_touch_pressure_log_time_ = Clock::time_point{}; touch_command_started_ = true; retract_command_started_ = false; retract_start_position_valid_ = false; @@ -1565,9 +1962,13 @@ bool TouchScreenTask::startTouchPhase() { last_status_ = Status::ROBOT_STATE_FAILED; return false; } + device::SpeedLOptions options; + options.acceleration = speed_l.acceleration(); + if (speed_l.has_linear_jerk()) options.linear_jerk = speed_l.linear_jerk(); + options.continuous_linear_reversal = true; const auto result = arm_->speedL(toCartesianVelocity( cmvr::common::math::toEigenVec6(speed_l.twist_tool())), - speed_l.acceleration(), + options, 0.0, device::FrameType::Tool); if (!result.ok()) { @@ -1623,29 +2024,18 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, } const auto& retract = config_.retract(); - retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_); + retract_start_position_valid_ = false; + retract_direction_base_.setZero(); + max_forward_after_retract_m_ = 0.0; const auto retract_cmd = toCartesianVelocity( cmvr::common::math::toEigenVec6(retract.twist_tool())); - if (retract_start_position_valid_) { - CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=[" - << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz - << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz - << "], acceleration=" << retract.acceleration() - << ", duration_s=" << retract.duration_s() - << ", start_tcp_base=[" << retract_start_position_base_.x() << ", " - << retract_start_position_base_.y() << ", " - << retract_start_position_base_.z() << "]"; - } else { - CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=[" - << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz - << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz - << "], acceleration=" << retract.acceleration() - << ", duration_s=" << retract.duration_s() - << ", start_tcp_base=unavailable"; - } - + device::SpeedLOptions options; + options.acceleration = retract.acceleration(); + if (retract.has_linear_jerk()) options.linear_jerk = retract.linear_jerk(); + options.continuous_linear_reversal = true; + options.capture_reference = true; const auto result = arm_->speedL(retract_cmd, - retract.acceleration(), + options, 0.0, device::FrameType::Tool); if (!result.ok()) { @@ -1661,19 +2051,22 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, last_retract_log_time_ = phase_start_time_; retract_command_started_ = true; last_status_ = Status::RETRACTING; + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] submitted twist_tool=[" + << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz + << "], acceleration=" << retract.acceleration() + << ", linear_jerk=" << (retract.has_linear_jerk() ? retract.linear_jerk() : 0.0) + << ", distance_m=" << retract.distance_m(); return true; } void TouchScreenTask::enterFailed(const Status status) { - try { - if (arm_) { - arm_->stopL(); - } - } catch (...) { + stopPbvsMotion(); + if (phase_ == Phase::RETRACTING && retract_start_position_valid_) { + CMVR_LOG(WARNING) << "[TouchScreenTask][RETRACT_FAILED] status=" << statusToString(status) + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0; } - hardStopIbvsMotion(); - holdCurrentControlledPosition(); phase_ = Phase::FAILED; + setCoordinateOverlayEnabled(true); touch_command_started_ = false; retract_command_started_ = false; last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED; @@ -1681,6 +2074,7 @@ void TouchScreenTask::enterFailed(const Status status) { bool TouchScreenTask::updateTouchPressure() { last_touch_pressure_sum_ = 0.0; + last_touch_resultant_fz_ = 0.0; last_touch_nonzero_count_ = 0; if (!dexhand_) { return false; @@ -1698,9 +2092,10 @@ bool TouchScreenTask::updateTouchPressure() { double resultant_fz = 0.0; try { for (const auto& tactile_region : tactile_regions) { - const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second); + const auto resultant_force = dexhand_->getResultantForceNewtons( + tactile_region.first, tactile_region.second); resultant_value += tactileForceValue(resultant_force, tactile.criterion()); - resultant_fz += static_cast(resultant_force.fz); + resultant_fz += resultant_force.fz; } } catch (...) { return false; @@ -1708,13 +2103,21 @@ bool TouchScreenTask::updateTouchPressure() { last_touch_nonzero_count_ = std::abs(resultant_value) > 1e-9 ? 1 : 0; last_touch_pressure_sum_ = resultant_value; - if (phase_ == Phase::TOUCHING) { - CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << resultant_fz - << ", criterion_value=" << resultant_value - << ", threshold=" << tactile.force_threshold() - ; - } + last_touch_resultant_fz_ = resultant_fz; return true; } +void TouchScreenTask::logTouchPressure(const bool force) { + const auto now = Clock::now(); + if (!force && last_touch_pressure_log_time_.time_since_epoch().count() != 0 && + now - last_touch_pressure_log_time_ < std::chrono::milliseconds(100)) { + return; + } + last_touch_pressure_log_time_ = now; + CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz_N=" << last_touch_resultant_fz_ + << ", criterion_value_N=" << last_touch_pressure_sum_ + << ", threshold_N=" << config_.touch().tactile().force_threshold() + << ", triggered=" << isTouchTriggered(config_, last_touch_pressure_sum_); +} + } // namespace cmvr::task diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp index 55a45b27..7098140c 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -1,10 +1,11 @@ #include "gtest/gtest.h" +#include #include +#include #include #include #include -#include #include #include #include @@ -23,15 +24,17 @@ #include "common/vision/image_projection.h" #include "common/io/proto_file_io.h" #include "cmvr/config/arm_config/arm_config.pb.h" +#include "cmvr/config/logger_config/logger_config.pb.h" #include "cmvr/config/motor_config/motor_config.pb.h" #include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h" #include "devices/arm/robot_arm_factory.h" +#include "devices/camera/common/include/camera_stream_encoder.h" #include "devices/camera/mujoco_camera/include/mujoco_camera.h" #include "devices/motor/manager/include/motor_manager.h" -#include "devices/motor/drivers/mujoco/include/mujoco_motor.h" #include "manager/device_manager/include/device_manager.h" #include "manager/task_manager/include/task_manager.h" #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" namespace { @@ -68,178 +71,6 @@ std::filesystem::path findProjectRoot() return search(std::filesystem::path(__FILE__).parent_path()); } -class TouchMujocoViewer final : public cmvr::MuJocoViewer { -public: - TouchMujocoViewer(const std::string& model_path, - std::shared_ptr bridge) - : MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge)) - { - position_actuator_ids_.fill(-1); - qpos_ids_.fill(-1); - qvel_ids_.fill(-1); - } - - std::shared_ptr waitCameraReady( - const std::chrono::milliseconds timeout) - { - std::unique_lock lock(camera_mutex_); - if (!camera_cv_.wait_for(lock, timeout, [this] { return camera_ != nullptr; })) { - return nullptr; - } - return camera_; - } - -protected: - void initOnce(mjModel* model, mjData* data) override - { - setupCamera(2.5, -160.0, -25.0); - enablePiPCamera("hand_cam"); - - bool valid = true; - for (std::size_t i = 0; i < kDof; ++i) { - const std::string actuator_name = std::string(kJointNames[i]) + "_pos"; - position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str()); - const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]); - if (position_actuator_ids_[i] < 0 || joint_id < 0) { - valid = false; - continue; - } - qpos_ids_[i] = model->jnt_qposadr[joint_id]; - qvel_ids_[i] = model->jnt_dofadr[joint_id]; - position_reference_[i] = data->qpos[qpos_ids_[i]]; - } - - const int hand_cam_id = mj_name2id(model, mjOBJ_CAMERA, "hand_cam"); - if (hand_cam_id < 0) { - valid = false; - } - - auto camera = std::make_shared( - [this](std::vector& rgb, - std::vector& depth, - int& width, - int& height, - std::uint64_t& frame_id) { - return getPiPCameraRGBD(rgb, depth, width, height, frame_id); - }); - if (hand_cam_id >= 0) { - camera->setFovyDeg(model->cam_fovy[hand_cam_id]); - } - camera->setConsumeNewFrameOnly(false); - { - std::lock_guard lock(camera_mutex_); - camera_ = std::move(camera); - } - camera_cv_.notify_all(); - bridge_->markReady(valid); - } - - void controlCallback(mjModel* model, mjData* data) override - { - std::vector measured_position(kDof, 0.0); - std::vector measured_velocity(kDof, 0.0); - for (std::size_t i = 0; i < kDof; ++i) { - if (qpos_ids_[i] >= 0) { - measured_position[i] = data->qpos[qpos_ids_[i]]; - } - if (qvel_ids_[i] >= 0) { - measured_velocity[i] = data->qvel[qvel_ids_[i]]; - } - } - bridge_->publishMeasured(measured_position, measured_velocity); - - const auto commands = bridge_->commands(); - for (std::size_t i = 0; i < kDof; ++i) { - const int actuator_id = position_actuator_ids_[i]; - if (actuator_id < 0) { - continue; - } - - if (commands.mode[i] == cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { - if (last_mode_[i] != cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) { - position_reference_[i] = data->qpos[qpos_ids_[i]]; - } - position_reference_[i] += commands.velocity[i] * model->opt.timestep; - } else { - position_reference_[i] = commands.position[i]; - } - - const double lower = model->actuator_ctrlrange[2 * actuator_id]; - const double upper = model->actuator_ctrlrange[2 * actuator_id + 1]; - position_reference_[i] = std::clamp(position_reference_[i], lower, upper); - data->ctrl[actuator_id] = position_reference_[i]; - last_mode_[i] = commands.mode[i]; - } - } - -private: - std::shared_ptr bridge_; - std::array position_actuator_ids_{}; - std::array qpos_ids_{}; - std::array qvel_ids_{}; - std::array position_reference_{}; - std::array last_mode_{}; - std::mutex camera_mutex_; - std::condition_variable camera_cv_; - std::shared_ptr camera_{nullptr}; -}; - -class ZeroTouchDexHand final : public cmvr::device::AbstractDexHand { -public: - ZeroTouchDexHand() - { - id_ = "mujoco_zero_touch_dexhand"; - } - - std::string typeName() const override { return "ZeroTouchDexHand"; } - Status state() const override { return Status::STREAMING; } - std::string lastError() const override { return {}; } - - void setAngles(const std::vector& finger_joint_angles) override - { - (void)finger_joint_angles; - } - - void setTactilePollingRegions(const std::vector& regions) override - { - polling_regions_ = regions; - } - - std::vector getSensorData() override - { - std::vector data; - if (polling_regions_.empty()) { - polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP}); - } - data.reserve(polling_regions_.size()); - for (const auto& region : polling_regions_) { - data.push_back(getSensorData(region.first, region.second)); - } - return data; - } - - TactileRegionData getSensorData(const FingerType finger, const TactileRegion region) override - { - tactile_points_[0] = getResultantForce(finger, region); - TactileMatrixView view; - view.data = tactile_points_.data(); - view.rows = 1; - view.cols = 1; - return {finger, region, view, "mujoco_touch_tip"}; - } - - ResultantForce getResultantForce(const FingerType finger, const TactileRegion region) override - { - (void)finger; - (void)region; - return TactilePoint::fromFz(0); - } - -private: - std::vector polling_regions_; - std::array tactile_points_{}; -}; - struct TagCenterPixel { int tag_id{-1}; int u{0}; @@ -247,6 +78,7 @@ struct TagCenterPixel { }; bool detectTagCenterPixel(const std::shared_ptr& camera, + const int target_tag_id, const double tag_size_m, const std::chrono::milliseconds timeout, TagCenterPixel& pixel_out) @@ -276,6 +108,9 @@ bool detectTagCenterPixel(const std::shared_ptr& c TagCenterPixel best_pixel; for (const auto& tag : perception.tags()) { + if (tag.id != target_tag_id) { + continue; + } const Eigen::Vector3d p_c_tag_center = tag.T_c_t.block<3, 1>(0, 3); Eigen::Vector2d uv = Eigen::Vector2d::Zero(); if (!cmvr::ImageProcess::projectCameraPointToPixel( @@ -381,9 +216,9 @@ void run_touch_once(int u, int v) { << p_c_target.z() << "]" << ", nonzero_count=" << task->lastTouchNonzeroCount() << ", pressure_sum=" << task->lastTouchPressureSum() - << ", err_c=[" << task->lastAlignErrorCamera().x() << ", " - << task->lastAlignErrorCamera().y() << ", " - << task->lastAlignErrorCamera().z() << "]\n"; + << ", err_G=[" << task->lastAlignErrorScreenTag().x() << ", " + << task->lastAlignErrorScreenTag().y() << ", " + << task->lastAlignErrorScreenTag().z() << "]\n"; if (task->lastStatus() == cmvr::task::TouchScreenTask::Status::ALIGN_REACHED) { std::cout << "align reached, target_c=[" << p_c_target.x() << ", " @@ -437,17 +272,423 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) { const auto& config = root_config.touch_screen_task(); EXPECT_TRUE(config.has_initialization()); EXPECT_TRUE(config.has_perception()); + ASSERT_TRUE(config.perception().has_tags()); + EXPECT_EQ(config.perception().tags().screen().id(), 1); + EXPECT_EQ(config.perception().tags().hand().id(), 0); + EXPECT_DOUBLE_EQ(config.perception().tags().screen().size_m(), 0.03); + EXPECT_DOUBLE_EQ(config.perception().tags().hand().size_m(), 0.03); + EXPECT_TRUE(config.perception().has_hand_camera()); + const bool is_mujoco = std::string(file_name).find("mujoco") != std::string::npos; + EXPECT_EQ(config.devices().camera_id(), + is_mujoco ? "mujoco_hand_cam" : "right_hand_cam"); + EXPECT_EQ(config.devices().external_camera_id(), + is_mujoco ? "mujoco_external_touch_cam" : "cam5"); EXPECT_TRUE(config.has_alignment()); - EXPECT_TRUE(config.alignment().has_ibvs()); - EXPECT_TRUE(config.alignment().ibvs().has_camera_link()); + ASSERT_TRUE(config.alignment().has_calibration()); + EXPECT_TRUE(config.alignment().calibration().has_hand_tag_to_tcp()); + ASSERT_TRUE(config.alignment().target().has_hand_orientation_g()); + EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rx(), 0.0); + EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().ry(), 0.0); + EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rz(), + 3.141592653589793); + ASSERT_TRUE(config.alignment().has_pbvs()); + const auto& pbvs = config.alignment().pbvs(); + EXPECT_DOUBLE_EQ(pbvs.position_gain().x(), 2.0); + EXPECT_DOUBLE_EQ(pbvs.rotation_gain().z(), 1.5); + EXPECT_DOUBLE_EQ(pbvs.vmax6().z(), 0.05); + EXPECT_DOUBLE_EQ(pbvs.amax6().rz(), 2.0); + EXPECT_DOUBLE_EQ(pbvs.twist_filter_alpha(), 1.0); EXPECT_TRUE(config.has_touch()); EXPECT_EQ(config.touch().motion_case(), cmvr::config::TouchScreenTaskTouchConfig::kSpeedL); EXPECT_TRUE(config.has_retract()); + EXPECT_TRUE(config.retract().has_distance_m()); + EXPECT_GT(config.retract().distance_m(), 0.0); } } -TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { +TEST(TouchScreenTaskTest, TouchSpeedLJerkParsesFromTextConfig) { + cmvr::config::TouchScreenTaskRootConfig parsed; + ASSERT_TRUE(google::protobuf::TextFormat::ParseFromString( + "touch_screen_task { touch { speed_l { acceleration: 5 linear_jerk: 10 } } }", &parsed)); + ASSERT_TRUE(parsed.touch_screen_task().touch().speed_l().has_linear_jerk()); + EXPECT_DOUBLE_EQ(parsed.touch_screen_task().touch().speed_l().linear_jerk(), 10); + + const auto project_root = findProjectRoot(); + ASSERT_FALSE(project_root.empty()); + for (const auto* file_name : {"touch_screen_task.pb.txt", "touch_screen_task_mujoco.pb.txt"}) { + cmvr::config::TouchScreenTaskRootConfig root; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root / "cmvr-es/config/tasks/touch_screen_task" / file_name).string(), &root)); + cmvr::task::TouchScreenTask task(root.touch_screen_task()); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED) + << file_name; + } +} + +TEST(TouchScreenTaskTest, TouchSpeedLJerkIsOptionalAndMustBeFinitePositive) { + auto config = loadMujocoTouchConfig(findProjectRoot()); + config.mutable_touch()->mutable_speed_l()->clear_linear_jerk(); + { + cmvr::task::TouchScreenTask task(config); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED); + } + for (const double jerk : {.5, 10.0, 60.0}) { + config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk); + cmvr::task::TouchScreenTask task(config); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED); + } + for (const double jerk : {0.0, -1.0, std::numeric_limits::infinity(), + std::numeric_limits::quiet_NaN()}) { + config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk); + cmvr::task::TouchScreenTask task(config); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::INVALID_CONFIG); + } +} + +TEST(TouchScreenTaskTest, CoordinateFrameProjectionUsesBgrAxisColors) { + cv::Mat image = cv::Mat::zeros(240, 320, CV_8UC3); + cmvr::device::Rs2Intrinsics intrinsics{}; + intrinsics.fx = 200.0F; + intrinsics.fy = 200.0F; + intrinsics.cx = 160.0F; + intrinsics.cy = 120.0F; + + Eigen::Matrix4d T_C_Frame = Eigen::Matrix4d::Identity(); + T_C_Frame(2, 3) = 1.0; + cmvr::device::drawCoordinateFrame(image, + T_C_Frame, + intrinsics, + 0.2, + "G"); + + // X projects right in red and Y projects down in green for the camera + // convention used by the pinhole projection. Z is blue but projects onto + // the origin for this fronto-parallel pose. + const cv::Vec3b x_pixel = image.at(120, 180); + const cv::Vec3b y_pixel = image.at(140, 160); + EXPECT_GT(x_pixel[2], x_pixel[1]); + EXPECT_GT(x_pixel[2], x_pixel[0]); + EXPECT_GT(y_pixel[1], y_pixel[2]); + EXPECT_GT(y_pixel[1], y_pixel[0]); +} + +TEST(TouchScreenTaskTest, RunEyeToHandTouchInMujoco) { + const auto project_root = findProjectRoot(); + ASSERT_FALSE(project_root.empty()); + + cmvr::config::LoggerRootConfig logger_root; + const auto logger_config_path = project_root / "cmvr-es/config/logger/logger.pb.txt"; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + logger_config_path.string(), &logger_root)) + << logger_config_path; + ASSERT_TRUE(cmvr::logging::initLogging( + logger_root.logger(), "touch_screen_task_test", project_root)); + + cmvr::ConfigHelper::setConfigRootFromFile( + (project_root / "cmvr-es/config/cmvr_es.pb.txt").string()); + + cmvr::task::TaskManager::destroyInstance(); + cmvr::device::DeviceManager::destroyInstance(); + + // This test intentionally builds only the devices required by the + // MuJoCo Eye-to-Hand task. The task itself is still loaded from + // touch_screen_task_mujoco.pb.txt below. + cmvr::config::DeviceManagerConfig device_manager_config; + device_manager_config.set_name("touch_screen_eye_to_hand_mujoco_test"); + device_manager_config.set_version("test"); + + auto add_device = [&](const char* id, + cmvr::config::DeviceConfigEntry::DeviceType type, + const char* config_file) { + auto* entry = device_manager_config.add_devices(); + entry->set_id(id); + entry->set_type(type); + entry->set_config_file(config_file); + entry->set_enable(true); + }; + + add_device("mujoco_world", + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD, + "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"); + add_device("right_arm_mujoco_motors", + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM, + "devices/motor/mujoco_motors.pb.txt"); + add_device("mujoco_right_arm", + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM, + "devices/arm/arm_mujoco.pb.txt"); + add_device("mujoco_zero_touch_dexhand", + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND, + "devices/dexhand/dexhand.pb.txt"); + add_device("mujoco_hand_cam", + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA, + "devices/camera/camera.pb.txt"); + add_device("mujoco_external_touch_cam", + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA, + "devices/camera/camera.pb.txt"); + + auto& device_manager = + cmvr::device::DeviceManager::getInstance(device_manager_config); + auto arm = device_manager.getDevice("mujoco_right_arm"); + auto hand_camera = device_manager.getDevice( + "mujoco_hand_cam"); + auto external_camera = device_manager.getDevice( + "mujoco_external_touch_cam"); + ASSERT_NE(arm, nullptr); + ASSERT_NE(hand_camera, nullptr); + ASSERT_NE(external_camera, nullptr); + + const auto hand_mujoco_camera = + std::dynamic_pointer_cast(hand_camera); + const auto external_mujoco_camera = + std::dynamic_pointer_cast(external_camera); + ASSERT_NE(hand_mujoco_camera, nullptr); + ASSERT_NE(external_mujoco_camera, nullptr); + + device_manager.start(); + + cv::Mat hand_camera_frame; + cmvr::device::Rs2Intrinsics hand_camera_intrinsics{}; + hand_camera->getRGBImage(hand_camera_frame, hand_camera_intrinsics); + ASSERT_FALSE(hand_camera_frame.empty()) + << "mujoco_hand_cam did not produce a frame after DeviceManager::start()"; + + cv::Mat external_camera_frame; + cmvr::device::Rs2Intrinsics external_camera_intrinsics{}; + external_camera->getRGBImage(external_camera_frame, external_camera_intrinsics); + ASSERT_FALSE(external_camera_frame.empty()) + << "mujoco_external_touch_cam did not produce a frame after DeviceManager::start()"; + + auto world = cmvr::device::MotorManager::mujocoWorldFor( + "right_arm_mujoco_motors"); + ASSERT_NE(world, nullptr); + ASSERT_TRUE(world->isLoaded()); + ASSERT_TRUE(world->isRunning()); + + cmvr::MuJocoViewer viewer(world); + ASSERT_NE(viewer.model(), nullptr); + ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "hand_cam"), 0); + ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "external_touch_cam"), 0); + viewer.setupCamera(2.5, -160.0, -25.0); + + // PiP is display-only. Its visibility and layout follow camera.pb.txt; + // camera acquisition still uses each MujocoCamera's offscreen thread. + bool first_pip_camera = true; + const auto add_configured_pip = [&](const cmvr::device::MujocoCamera& camera) { + const auto& camera_config = camera.config(); + const auto& pip = camera_config.viewer_pip(); + if (!pip.enable()) { + return; + } + + const auto& render = camera_config.render(); + if (first_pip_camera) { + viewer.enablePiPCamera(camera_config.camera_name().c_str(), + pip.left(), + pip.bottom(), + pip.width(), + pip.height(), + render.width(), + render.height()); + first_pip_camera = false; + } else { + viewer.addPiPCamera(camera_config.camera_name().c_str(), + pip.left(), + pip.bottom(), + pip.width(), + pip.height(), + render.width(), + render.height()); + } + }; + add_configured_pip(*external_mujoco_camera); + add_configured_pip(*hand_mujoco_camera); + + struct Outcome { + bool init_ok{false}; + bool touch_ok{false}; + bool align_reached{false}; + bool retracting_seen{false}; + bool finished{false}; + cmvr::task::TouchScreenTask::Phase final_phase{ + cmvr::task::TouchScreenTask::Phase::IDLE}; + cmvr::task::TouchScreenTask::Status final_status{ + cmvr::task::TouchScreenTask::Status::IDLE}; + double max_pressure{0.0}; + double speedl_command_norm{0.0}; + int active_tag_id{-1}; + std::string error; + } outcome; + + // The viewer owns the lifetime of this test. Closing its window asks the + // worker to stop; a completed touch cycle must not close the viewer. + std::atomic stop_requested{false}; + std::thread scenario([&] { + try { + const auto touch_config = loadMujocoTouchConfig(project_root); + if (touch_config.devices().arm_id() != "mujoco_right_arm" || + touch_config.devices().camera_id() != "mujoco_hand_cam" || + touch_config.devices().external_camera_id() != + "mujoco_external_touch_cam") { + throw std::runtime_error( + "touch_screen_task_mujoco.pb.txt has unexpected device IDs"); + } + + cmvr::config::TaskManagerConfig task_manager_config; + auto* task_entry = task_manager_config.add_tasks(); + task_entry->set_id(touch_config.id()); + task_entry->set_type( + cmvr::config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN); + task_entry->set_run_mode( + cmvr::config::TaskConfigEntry::TASK_RUN_MODE_PERIODIC_STEP); + task_entry->set_control_period_s(0.001); + task_entry->set_config_file( + "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"); + task_entry->set_enable(true); + + auto& task_manager = + cmvr::task::TaskManager::getInstance(task_manager_config); + auto task = task_manager.getTouchScreenTask(touch_config.id()); + if (!task) { + throw std::runtime_error("TouchScreenTask not found: " + + touch_config.id()); + } + outcome.init_ok = + task->lastStatus() != + cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED; + if (!outcome.init_ok) { + throw std::runtime_error("TouchScreenTask initialization failed"); + } + + task_manager.startRunTask(); + + const auto& render = hand_mujoco_camera->config().render(); + const int target_u = render.width() > 0 ? render.width() / 2 : 640; + const int target_v = render.height() > 0 ? render.height() / 2 : 360; + std::cout << "[TouchScreenTaskMujocoTest] touch target pixel: u=" + << target_u << ", v=" << target_v << std::endl; + + const bool touch_started = task->touch(target_u, target_v); + if (!touch_started) { + throw std::runtime_error( + "task->touch failed, status=" + + std::string(cmvr::task::TouchScreenTask::statusToString( + task->lastStatus()))); + } + + outcome.touch_ok = true; + std::cout << "[TouchScreenTaskMujocoTest] touch started once" + << std::endl; + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(35); + int step_count = 0; + bool paused_at_alignment = false; + while (!stop_requested.load(std::memory_order_acquire) && + task->isBusy() && + std::chrono::steady_clock::now() < deadline) { + outcome.final_phase = task->phase(); + outcome.final_status = task->lastStatus(); + outcome.active_tag_id = task->lastActiveTagId(); + outcome.max_pressure = std::max( + outcome.max_pressure, task->lastTouchPressureSum()); + + const auto speedl = arm->getSpeedLCommandTwistBase(); + outcome.speedl_command_norm = std::max( + outcome.speedl_command_norm, + std::sqrt(speedl.vx * speedl.vx + speedl.vy * speedl.vy + + speedl.vz * speedl.vz)); + const auto phase = task->phase(); + if (task->lastStatus() == + cmvr::task::TouchScreenTask::Status::ALIGN_REACHED || + phase == cmvr::task::TouchScreenTask::Phase::TOUCHING || + phase == cmvr::task::TouchScreenTask::Phase::DWELLING || + phase == cmvr::task::TouchScreenTask::Phase::RETRACTING || + phase == cmvr::task::TouchScreenTask::Phase::DONE) { + outcome.align_reached = true; + } + if (phase == cmvr::task::TouchScreenTask::Phase::RETRACTING) { + outcome.retracting_seen = true; + } + if (phase == cmvr::task::TouchScreenTask::Phase::ALIGN_REACHED && + touch_config.alignment().pause_when_reached()) { + paused_at_alignment = true; + break; + } + if ((step_count++ % 20) == 0) { + std::cout << "[TouchScreenTaskMujocoTest] phase=" + << cmvr::task::TouchScreenTask::phaseToString( + task->phase()) + << ", status=" + << cmvr::task::TouchScreenTask::statusToString( + task->lastStatus()) + << ", active_tag=" << task->lastActiveTagId() + << ", tcp_pose=" + << cmvr::perception::TagRelativeTcpPose::statusToString( + task->tcpPoseTracker().lastStatus()) + << ", pressure=" << task->lastTouchPressureSum() + << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (stop_requested.load(std::memory_order_acquire)) { + task->stop(); + } else if (task->isBusy() && !paused_at_alignment) { + task->stop(); + throw std::runtime_error("touch scenario exceeded 35 seconds"); + } + + outcome.finished = task->isFinished(); + outcome.final_phase = task->phase(); + outcome.final_status = task->lastStatus(); + std::cout << "[TouchScreenTaskMujocoTest] touch finished once, phase=" + << cmvr::task::TouchScreenTask::phaseToString( + outcome.final_phase) + << "; viewer remains running" << std::endl; + + // Keep the viewer and its camera frames alive after the task has + // run once. The main thread sets stop_requested when the window + // is closed. + while (!stop_requested.load(std::memory_order_acquire)) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + task->stop(); + task_manager.stopRunTask(); + std::cout << "[TouchScreenTaskMujocoTest] viewer is still running; " + "close the MuJoCo window to finish gtest." << std::endl; + } catch (const std::exception& error) { + try { + cmvr::task::TaskManager::getInstance().stopRunTask(); + } catch (...) { + } + outcome.error = error.what(); + stop_requested.store(true, std::memory_order_release); + std::cerr << "[TouchScreenTaskMujocoTest] scenario error: " + << outcome.error + << "; viewer remains open, close it manually to finish gtest." + << std::endl; + } + }); + + viewer.setRunning(true); + viewer.run(); + stop_requested.store(true, std::memory_order_release); + scenario.join(); + + cmvr::task::TaskManager::destroyInstance(); + device_manager.stop(); + cmvr::device::DeviceManager::destroyInstance(); + + EXPECT_TRUE(outcome.error.empty()) << outcome.error; + EXPECT_TRUE(outcome.init_ok); + EXPECT_TRUE(outcome.touch_ok); + EXPECT_TRUE(outcome.align_reached); + EXPECT_GT(outcome.speedl_command_norm, 0.005); +} + +TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) { const auto project_root = findProjectRoot(); ASSERT_FALSE(project_root.empty()); cmvr::ConfigHelper::setConfigRootFromFile( @@ -459,36 +700,75 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { cmvr::config::DeviceManagerConfig device_manager_config; device_manager_config.set_name("touch_screen_mujoco_test"); device_manager_config.set_version("test"); + auto* world_entry = device_manager_config.add_devices(); + world_entry->set_id("mujoco_world"); + world_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD); + world_entry->set_config_file("devices/mujoco/mujoco_world.pb.txt"); + world_entry->set_enable(true); auto* motor_entry = device_manager_config.add_devices(); - motor_entry->set_id("mujoco_motors"); + motor_entry->set_id("right_arm_mujoco_motors"); motor_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); motor_entry->set_config_file("devices/motor/mujoco_motors.pb.txt"); motor_entry->set_enable(true); auto* arm_entry = device_manager_config.add_devices(); - arm_entry->set_id("right_arm_mujoco"); + arm_entry->set_id("mujoco_right_arm"); arm_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM); arm_entry->set_config_file("devices/arm/arm_mujoco.pb.txt"); arm_entry->set_enable(true); + auto* dexhand_entry = device_manager_config.add_devices(); + dexhand_entry->set_id("mujoco_zero_touch_dexhand"); + dexhand_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND); + dexhand_entry->set_config_file("devices/dexhand/dexhand.pb.txt"); + dexhand_entry->set_enable(true); auto& device_manager = cmvr::device::DeviceManager::getInstance(device_manager_config); - auto motor_system = device_manager.getDevice("mujoco_motors"); + auto motor_system = device_manager.getDevice("right_arm_mujoco_motors"); if (!motor_system) { - CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: mujoco_motors"; + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: right_arm_mujoco_motors"; return; } - auto bridge = cmvr::device::MotorManager::mujocoBridgeFor("mujoco_motors"); - if (!bridge) { - CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors"; + auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors"); + if (!world || !world->isLoaded() || !world->isRunning()) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco world is not ready: mujoco_world"; return; } - auto arm = device_manager.getDevice("right_arm_mujoco"); + auto arm = device_manager.getDevice("mujoco_right_arm"); if (!arm) { - CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: right_arm_mujoco"; + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: mujoco_right_arm"; return; } + // The task config still uses the historical ID; keep it as a test-only alias. + device_manager.registerDevice("right_arm_mujoco", arm); - TouchMujocoViewer viewer( - (project_root / "model/xiaoyan_description/dual_arm.xml").string(), bridge); + cmvr::MuJocoViewer viewer(world); + viewer.setupCamera(2.5, -160.0, -25.0); + viewer.enablePiPCamera("hand_cam"); + + auto camera = std::make_shared( + [&viewer](std::vector& rgb, + std::vector& depth, + int& width, + int& height, + std::uint64_t& frame_id) { + return viewer.getPiPCameraRGBD(rgb, depth, width, height, frame_id); + }); + camera->setConsumeNewFrameOnly(true); + { + std::lock_guard lock(world->mutex()); + const auto* model = world->model(); + const int hand_cam_id = model == nullptr + ? -1 + : mj_name2id(model, mjOBJ_CAMERA, "hand_cam"); + if (hand_cam_id < 0) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MuJoCo camera not found: hand_cam"; + return; + } + camera->setFovyDeg(model->cam_fovy[hand_cam_id]); + } + if (!camera->init()) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Failed to initialize MuJoCo camera"; + return; + } struct Outcome { bool init_ok{false}; @@ -506,17 +786,9 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { std::thread scenario([&] { try { - if (!bridge->waitUntilReady(std::chrono::seconds(10))) { - throw std::runtime_error("MuJoCo right-arm joints, actuators, or hand_cam are not ready"); - } - auto camera = viewer.waitCameraReady(std::chrono::seconds(10)); - if (!camera) { - throw std::runtime_error("MuJoCo camera is not ready"); - } - const auto touch_config = loadMujocoTouchConfig(project_root); - device_manager.registerDevice(touch_config.devices().camera_id(), camera); - device_manager.registerDevice(std::make_shared()); + device_manager.registerDevice( + touch_config.devices().camera_id(), camera); cmvr::config::TaskManagerConfig task_manager_config; auto* task_entry = task_manager_config.add_tasks(); @@ -533,11 +805,13 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { throw std::runtime_error("TouchScreenTask not found: " + touch_config.id()); } outcome.init_ok = task->lastStatus() != cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED; + task_manager.startRunTask(); TagCenterPixel target_pixel; if (!detectTagCenterPixel(camera, - touch_config.perception().apriltag().tag_size_m(), + touch_config.perception().tags().screen().id(), + touch_config.perception().tags().screen().size_m(), std::chrono::seconds(3), target_pixel)) { throw std::runtime_error("failed to detect MuJoCo AprilTag center pixel"); @@ -548,10 +822,31 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { << ", v=" << target_pixel.v << std::endl; + std::cout << "[TouchScreenTaskMujocoTest] before task->touch, phase=" + << cmvr::task::TouchScreenTask::phaseToString(task->phase()) + << ", status=" + << cmvr::task::TouchScreenTask::statusToString(task->lastStatus()) + << std::endl; + const auto touch_start = std::chrono::steady_clock::now(); outcome.touch_ok = task->touch(target_pixel.u, target_pixel.v); + std::cout << "[TouchScreenTaskMujocoTest] after task->touch, ok=" + << (outcome.touch_ok ? 1 : 0) + << ", elapsed_ms=" + << std::chrono::duration( + std::chrono::steady_clock::now() - touch_start) + .count() + << ", phase=" + << cmvr::task::TouchScreenTask::phaseToString(task->phase()) + << ", status=" + << cmvr::task::TouchScreenTask::statusToString(task->lastStatus()) + << std::endl; if (!outcome.touch_ok) { outcome.final_status = task->lastStatus(); - return; + task_manager.stopRunTask(); + viewer.requestStop(); + throw std::runtime_error( + "task->touch failed, status=" + + std::string(cmvr::task::TouchScreenTask::statusToString(outcome.final_status))); } const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(20); @@ -604,6 +899,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) { cmvr::task::TaskManager::getInstance().stopRunTask(); } catch (...) { } + viewer.requestStop(); outcome.worker_error = error.what(); std::cerr << "[TouchScreenTaskMujocoTest] scenario error: " << outcome.worker_error << "\nClose the MuJoCo viewer window to finish gtest." << std::endl; diff --git a/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat new file mode 100755 index 00000000..340807b9 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf new file mode 100644 index 00000000..21dc7c70 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf @@ -0,0 +1,3 @@ +MASTER0_DEVICE="42:e6:6d:44:c1:0f" +DEVICE_MODULES="generic" +UPDOWN_INTERFACES="eno1" diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_140412 b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_140412 new file mode 100644 index 00000000..c1741260 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_140412 @@ -0,0 +1,113 @@ +#------------------------------------------------------------------------------ +# +# EtherCAT master configuration file for use with ethercatctl. +# +# vim: spelllang=en spell tw=78 +# +#------------------------------------------------------------------------------ + +# +# Main Ethernet devices. +# +# The MASTER_DEVICE variable specifies the Ethernet device for a master +# with index 'X'. +# +# Specify the MAC address (hexadecimal with colons) of the Ethernet device to +# use. Example: "00:00:08:44:ab:66" +# +# Alternatively, a network interface name can be specified. The interface +# name will be resolved to a MAC address using the 'ip' command. +# Example: "eth0" +# +# The broadcast address "ff:ff:ff:ff:ff:ff" has a special meaning: It tells +# the master to accept the first device offered by any Ethernet driver. +# +# The MASTER_DEVICE variables also determine, how many masters will be +# created: A non-empty variable MASTER0_DEVICE will create one master, adding +# a non-empty variable MASTER1_DEVICE will create a second master, and so on. +# +# Examples: +# MASTER0_DEVICE="00:00:08:44:ab:66" +# MASTER0_DEVICE="eth0" +# +MASTER0_DEVICE="" +#MASTER1_DEVICE="" + +# +# Backup Ethernet devices +# +# The MASTER_BACKUP variables specify the devices used for redundancy. They +# behaves nearly the same as the MASTER_DEVICE variable, except that it +# does not interpret the ff:ff:ff:ff:ff:ff address. +# +#MASTER0_BACKUP="" + +# +# Ethernet driver modules to use for EtherCAT operation. +# +# Specify a non-empty list of Ethernet drivers, that shall be used for +# EtherCAT operation. +# +# Except for the generic Ethernet driver module, the init script will try to +# unload the usual Ethernet driver modules in the list and replace them with +# the EtherCAT-capable ones. If a certain (EtherCAT-capable) driver is not +# found, a warning will appear. +# +# Possible values: 8139too, e100, e1000, e1000e, r8169, generic, ccat, igb, +# igc, genet, dwmac-intel, stmmac-pci. +# Separate multiple drivers with spaces. +# A list of all matching kernel versions can be found here: +# https://docs.etherlab.org/ethercat/1.6/doxygen/devicedrivers.html +# +# Note: The e100, e1000, e1000e, r8169, ccat, igb and igc drivers are not +# built by default. Enable them with the --enable- configure switches. +# +DEVICE_MODULES="" + +# If you have any issues about network interfaces not being configured +# properly, systemd may need some additional infos about your setup. +# Have a look at the service file, you'll find some details there. +# + +# +# List of interfaces to bring up and down automatically. +# +# Specify a space-separated list of interface names (such as eth0 or +# enp0s1) that shall be brought up on `ethercatctl start` and down on +# `ethercatctl stop`. +# +# When using the generic driver, the corresponding Ethernet device has to be +# activated before the master is started, otherwise all frames will time out. +# This the perfect use-case for `UPDOWN_INTERFACES`. +# +UPDOWN_INTERFACES="" + +# +# Default SII caching method. +# +# Set the start-up caching method for all masters. The integer value +# determines, which fields are used to look up a cached SII page. It is a +# bit-field consisting of the following flags. A value of zero disables SII +# caching (default). A typical value is 7 (use vendor ID, product code and +# revision number for lookup). +# +# - 1: Vendor ID (always used) +# - 2: Product code (always used) +# - 4: Revision number +# - 8: Serial number +# - 16: Alias address +# +# Please keep in mind that in case serial number or alias address is enabled, +# only slaves with a non-zero serial number or alias benefit from caching. +# +SII_CACHING=0 + +# +# Flags for loading kernel modules. +# +# This can usually be left empty. Adjust this variable, if you have problems +# with module loading. +# +#MODPROBE_FLAGS="-b" + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_155334 b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_155334 new file mode 100644 index 00000000..9ec7a2ce --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_155334 @@ -0,0 +1,3 @@ +MASTER0_DEVICE="a0:ad:9f:c4:c2:2c" +DEVICE_MODULES="generic" +UPDOWN_INTERFACES="eno1" diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/init.d/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/etc/init.d/ethercat new file mode 100755 index 00000000..63498a9f --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/init.d/ethercat @@ -0,0 +1,123 @@ +#!/bin/sh + +#------------------------------------------------------------------------------ +# +# Init script for EtherCAT +# +# Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT Master. +# +# The IgH EtherCAT Master is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License version 2, as +# published by the Free Software Foundation. +# +# The IgH EtherCAT Master is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with the IgH EtherCAT Master; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +# +# +# vim: expandtab +# +#------------------------------------------------------------------------------ + +### BEGIN INIT INFO +# Provides: ethercat +# Required-Start: $local_fs $syslog $network +# Should-Start: $time ntp +# Required-Stop: $local_fs $syslog $network +# Should-Stop: $time ntp +# Default-Start: 3 5 +# Default-Stop: 0 1 2 6 +# Short-Description: EtherCAT master +# Description: EtherCAT master 1.7.0 +### END INIT INFO + +#------------------------------------------------------------------------------ + +ETHERCATCTL="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl -c /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat" + +#------------------------------------------------------------------------------ + +exit_success() { + if [ -r /etc/rc.status ]; then + rc_reset + rc_status -v + rc_exit + else + echo " done" + exit 0 + fi +} + +#------------------------------------------------------------------------------ + +exit_fail() { + if [ -r /etc/rc.status ]; then + rc_failed + rc_status -v + rc_exit + else + echo " failed" + exit 1 + fi +} + +#------------------------------------------------------------------------------ + +if [ -r /etc/rc.status ]; then + . /etc/rc.status + rc_reset +fi + +case "${1}" in + +start) + echo -n "Starting EtherCAT master 1.7.0 " + + if $ETHERCATCTL start; then + exit_success + else + exit_fail + fi + ;; + +stop) + echo -n "Shutting down EtherCAT master 1.7.0 " + + if $ETHERCATCTL stop; then + exit_success + else + exit_fail + fi + ;; + +restart) + $0 stop || exit 1 + sleep 1 + $0 start + ;; + +status) + $ETHERCATCTL status + exit $? + ;; + +*) + echo "USAGE: $0 {start|stop|restart|status}" + ;; + +esac + +if [ -r /etc/rc.status ]; then + rc_exit +else + exit 1 +fi + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat new file mode 100644 index 00000000..4bf8ea93 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat @@ -0,0 +1,113 @@ +#------------------------------------------------------------------------------ +# +# EtherCAT master configuration file for use with init.d. +# +# vim: spelllang=en spell tw=78 +# +#------------------------------------------------------------------------------ + +# +# Main Ethernet devices. +# +# The MASTER_DEVICE variable specifies the Ethernet device for a master +# with index 'X'. +# +# Specify the MAC address (hexadecimal with colons) of the Ethernet device to +# use. Example: "00:00:08:44:ab:66" +# +# Alternatively, a network interface name can be specified. The interface +# name will be resolved to a MAC address using the 'ip' command. +# Example: "eth0" +# +# The broadcast address "ff:ff:ff:ff:ff:ff" has a special meaning: It tells +# the master to accept the first device offered by any Ethernet driver. +# +# The MASTER_DEVICE variables also determine, how many masters will be +# created: A non-empty variable MASTER0_DEVICE will create one master, adding +# a non-empty variable MASTER1_DEVICE will create a second master, and so on. +# +# Examples: +# MASTER0_DEVICE="00:00:08:44:ab:66" +# MASTER0_DEVICE="eth0" +# +MASTER0_DEVICE="" +#MASTER1_DEVICE="" + +# +# Backup Ethernet devices +# +# The MASTER_BACKUP variables specify the devices used for redundancy. They +# behaves nearly the same as the MASTER_DEVICE variable, except that it +# does not interpret the ff:ff:ff:ff:ff:ff address. +# +#MASTER0_BACKUP="" + +# +# Ethernet driver modules to use for EtherCAT operation. +# +# Specify a non-empty list of Ethernet drivers, that shall be used for +# EtherCAT operation. +# +# Except for the generic Ethernet driver module, the init script will try to +# unload the usual Ethernet driver modules in the list and replace them with +# the EtherCAT-capable ones. If a certain (EtherCAT-capable) driver is not +# found, a warning will appear. +# +# Possible values: 8139too, e100, e1000, e1000e, r8169, generic, ccat, igb, +# igc, genet, dwmac-intel, stmmac-pci. +# Separate multiple drivers with spaces. +# A list of all matching kernel versions can be found here: +# https://docs.etherlab.org/ethercat/1.6/doxygen/devicedrivers.html +# +# Note: The e100, e1000, e1000e, r8169, ccat, igb and igc drivers are not +# built by default. Enable them with the --enable- configure switches. +# +DEVICE_MODULES="" + +# If you have any issues about network interfaces not being configured +# properly, systemd may need some additional infos about your setup. +# Have a look at the service file, you'll find some details there. +# + +# +# List of interfaces to bring up and down automatically. +# +# Specify a space-separated list of interface names (such as eth0 or +# enp0s1) that shall be brought up on `ethercatctl start` and down on +# `ethercatctl stop`. +# +# When using the generic driver, the corresponding Ethernet device has to be +# activated before the master is started, otherwise all frames will time out. +# This the perfect use-case for `UPDOWN_INTERFACES`. +# +UPDOWN_INTERFACES="" + +# +# Default SII caching method. +# +# Set the start-up caching method for all masters. The integer value +# determines, which fields are used to look up a cached SII page. It is a +# bit-field consisting of the following flags. A value of zero disables SII +# caching (default). A typical value is 7 (use vendor ID, product code and +# revision number for lookup). +# +# - 1: Vendor ID (always used) +# - 2: Product code (always used) +# - 4: Revision number +# - 8: Serial number +# - 16: Alias address +# +# Please keep in mind that in case serial number or alias address is enabled, +# only slaves with a non-zero serial number or alias benefit from caching. +# +SII_CACHING=0 + +# +# Flags for loading kernel modules. +# +# This can usually be left empty. Adjust this variable, if you have problems +# with module loading. +# +#MODPROBE_FLAGS="-b" + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/include/ecrt.h b/dependency/x86/third_party/ethercat/v1.7.0/include/ecrt.h new file mode 100644 index 00000000..64a43ec3 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/include/ecrt.h @@ -0,0 +1,3216 @@ +/***************************************************************************** + * + * Copyright (C) 2006-2026 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT master userspace library. + * + * The IgH EtherCAT master userspace library is free software; you can + * redistribute it and/or modify it under the terms of the GNU Lesser General + * Public License as published by the Free Software Foundation; version 2.1 + * of the License. + * + * The IgH EtherCAT master userspace library is distributed in the hope that + * it will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with the IgH EtherCAT master userspace library. If not, see + * . + * + ****************************************************************************/ + +/** \file + * + * EtherCAT master application interface. + * + * \defgroup ApplicationInterface EtherCAT Application Interface + * + * EtherCAT interface for realtime applications. This interface is designed + * for realtime modules that want to use EtherCAT. There are functions to + * request a master, to map process data, to communicate with slaves via CoE + * and to configure and activate the bus. + * + * Changes in version 1.7.0: + * + * - Added ecrt_master_sii_caching() to set the SII caching method and added + * the feature flag EC_HAVE_SII_CACHING and the enum type + * ec_sii_caching_fields_t. + * + * Changes in version 1.6.0: + * + * - Added the ecrt_master_scan_progress() method, the + * ec_master_scan_progress_t structure and the EC_HAVE_SCAN_PROGRESS + * definition to check for its existence. + * - Added the EoE configuration methods ecrt_slave_config_eoe_mac_address(), + * ecrt_slave_config_eoe_ip_address(), ecrt_slave_config_eoe_subnet_mask(), + * ecrt_slave_config_eoe_default_gateway(), + * ecrt_slave_config_eoe_dns_address(), + * ecrt_slave_config_eoe_hostname() and the EC_HAVE_SET_IP + * definition to check for its existence. + * - Added ecrt_slave_config_state_timeout() to set the application-layer + * state change timeout and EC_HAVE_STATE_TIMEOUT to check for its + * existence. + * + * Changes since version 1.5.2: + * + * - Added the ecrt_slave_config_flag() method and the EC_HAVE_FLAGS + * definition to check for its existence. + * - Added SoE IDN requests, including the datatype ec_soe_request_t and the + * methods ecrt_slave_config_create_soe_request(), + * ecrt_soe_request_object(), ecrt_soe_request_timeout(), + * ecrt_soe_request_data(), ecrt_soe_request_data_size(), + * ecrt_soe_request_state(), ecrt_soe_request_write() and + * ecrt_soe_request_read(). Use the EC_HAVE_SOE_REQUESTS to check, if the + * functionality is available. + * + * Changes in version 1.5.2: + * + * - Added redundancy_active flag to ec_domain_state_t. + * - Added ecrt_master_link_state() method and ec_master_link_state_t to query + * the state of a redundant link. + * - Added the EC_HAVE_REDUNDANCY define, to check, if the interface contains + * redundancy features. + * - Added ecrt_sdo_request_index() to change SDO index and subindex after + * request creation. + * - Added interface for retrieving CoE emergency messages, i. e. + * ecrt_slave_config_emerg_size(), ecrt_slave_config_emerg_pop(), + * ecrt_slave_config_emerg_clear(), ecrt_slave_config_emerg_overruns() and + * the defines EC_HAVE_EMERGENCY and EC_COE_EMERGENCY_MSG_SIZE. + * - Added interface for direct EtherCAT register access: Added data type + * ec_reg_request_t and methods ecrt_slave_config_create_reg_request(), + * ecrt_reg_request_data(), ecrt_reg_request_state(), + * ecrt_reg_request_write(), ecrt_reg_request_read() and the feature flag + * EC_HAVE_REG_ACCESS. + * - Added method to select the reference clock, + * ecrt_master_select_reference_clock() and the feature flag + * EC_HAVE_SELECT_REF_CLOCK to check, if the method is available. + * - Added method to get the reference clock time, + * ecrt_master_reference_clock_time() and the feature flag + * EC_HAVE_REF_CLOCK_TIME to have the possibility to synchronize the master + * clock to the reference clock. + * - Changed the data types of the shift times in ecrt_slave_config_dc() to + * int32_t to correctly display negative shift times. + * - Added ecrt_slave_config_reg_pdo_entry_pos() and the feature flag + * EC_HAVE_REG_BY_POS for registering PDO entries with non-unique indices + * via their positions in the mapping. + * + * Changes in version 1.5: + * + * - Added the distributed clocks feature and the respective method + * ecrt_slave_config_dc() to configure a slave for cyclic operation, and + * ecrt_master_application_time(), ecrt_master_sync_reference_clock() and + * ecrt_master_sync_slave_clocks() for offset and drift compensation. The + * EC_TIMEVAL2NANO() macro can be used for epoch time conversion, while the + * ecrt_master_sync_monitor_queue() and ecrt_master_sync_monitor_process() + * methods can be used to monitor the synchrony. + * - Improved the callback mechanism. ecrt_master_callbacks() now takes two + * callback functions for sending and receiving datagrams. + * ecrt_master_send_ext() is used to execute the sending of non-application + * datagrams. + * - Added watchdog configuration (method ecrt_slave_config_watchdog(), + * #ec_watchdog_mode_t, \a watchdog_mode parameter in ec_sync_info_t and + * ecrt_slave_config_sync_manager()). + * - Added ecrt_slave_config_complete_sdo() method to download an SDO during + * configuration via CompleteAccess. + * - Added ecrt_master_deactivate() to remove the master configuration. + * - Added ecrt_open_master() and ecrt_master_reserve() separation for + * userspace. + * - Added master information interface (methods ecrt_master(), + * ecrt_master_get_slave(), ecrt_master_get_sync_manager(), + * ecrt_master_get_pdo() and ecrt_master_get_pdo_entry()) to get information + * about the currently connected slaves and the PDO entries provided. + * - Added ecrt_master_sdo_download(), ecrt_master_sdo_download_complete() and + * ecrt_master_sdo_upload() methods to let an application transfer SDOs + * before activating the master. + * - Changed the meaning of the negative return values of + * ecrt_slave_config_reg_pdo_entry() and ecrt_slave_config_sdo*(). + * - Implemented the Vendor-specific over EtherCAT mailbox protocol. See + * ecrt_slave_config_create_voe_handler(). + * - Renamed ec_sdo_request_state_t to #ec_request_state_t, because it is also + * used by VoE handlers. + * - Removed 'const' from argument of ecrt_sdo_request_state(), because the + * userspace library has to modify object internals. + * - Added 64-bit data access macros. + * - Added ecrt_slave_config_idn() method for storing SoE IDN configurations, + * and ecrt_master_read_idn() and ecrt_master_write_idn() to read/write IDNs + * ad-hoc via the user-space library. + * - Added ecrt_master_reset() to initiate retrying to configure slaves. + * + * @{ + */ + +/****************************************************************************/ + +#ifndef __ECRT_H__ +#define __ECRT_H__ + +#ifdef __KERNEL__ +#include +#include +#include +#include // struct in_addr +#else +#include // for size_t +#include +#include // for struct timeval +#include // struct in_addr +#endif + +/***************************************************************************** + * Global definitions + ****************************************************************************/ + +/** EtherCAT realtime interface major version number. + */ +#define ECRT_VER_MAJOR 1 + +/** EtherCAT realtime interface minor version number. + */ +#define ECRT_VER_MINOR 6 + +/** EtherCAT realtime interface version word generator. + */ +#define ECRT_VERSION(a, b) (((a) << 8) + (b)) + +/** EtherCAT realtime interface version word. + */ +#define ECRT_VERSION_MAGIC ECRT_VERSION(ECRT_VER_MAJOR, ECRT_VER_MINOR) + +/***************************************************************************** + * Feature flags + ****************************************************************************/ + +/** Defined, if the redundancy features are available. + * + * I. e. if the \a redundancy_active flag in ec_domain_state_t and the + * ecrt_master_link_state() method are available. + */ +#define EC_HAVE_REDUNDANCY + +/** Defined, if the CoE emergency ring feature is available. + * + * I. e. if the ecrt_slave_config_emerg_*() methods are available. + */ +#define EC_HAVE_EMERGENCY + +/** Defined, if the register access interface is available. + * + * I. e. if the methods ecrt_slave_config_create_reg_request(), + * ecrt_reg_request_data(), ecrt_reg_request_state(), ecrt_reg_request_write() + * and ecrt_reg_request_read() are available. + */ +#define EC_HAVE_REG_ACCESS + +/** Defined if the method ecrt_master_select_reference_clock() is available. + */ +#define EC_HAVE_SELECT_REF_CLOCK + +/** Defined if the method ecrt_master_reference_clock_time() is available. + */ +#define EC_HAVE_REF_CLOCK_TIME + +/** Defined if the method ecrt_slave_config_reg_pdo_entry_pos() is available. + */ +#define EC_HAVE_REG_BY_POS + +/** Defined if the method ecrt_master_sync_reference_clock_to() is available. + */ +#define EC_HAVE_SYNC_TO + +/** Defined if the method ecrt_slave_config_flag() is available. + */ +#define EC_HAVE_FLAGS + +/** Defined if the methods ecrt_slave_config_create_soe_request(), + * ecrt_soe_request_object(), ecrt_soe_request_timeout(), + * ecrt_soe_request_data(), ecrt_soe_request_data_size(), + * ecrt_soe_request_state(), ecrt_soe_request_write() and + * ecrt_soe_request_read() and the datatype ec_soe_request_t are available. + */ +#define EC_HAVE_SOE_REQUESTS + +/** Defined, if the method ecrt_master_scan_progress() and the + * ec_master_scan_progress_t structure are available. + */ +#define EC_HAVE_SCAN_PROGRESS + +/** Defined, if the methods ecrt_slave_config_eoe_mac_address(), + * ecrt_slave_config_eoe_ip_address(), ecrt_slave_config_eoe_subnet_mask(), + * ecrt_slave_config_eoe_default_gateway(), + * ecrt_slave_config_eoe_dns_address(), ecrt_slave_config_eoe_hostname() are + * available. + */ +#define EC_HAVE_SET_IP + +/** Defined, if the method ecrt_slave_config_state_timeout() is available. + */ +#define EC_HAVE_STATE_TIMEOUT + +/** Defined, if the method ecrt_master_sii_caching() and the enum type + * ec_sii_caching_fields_t and its values are available. + */ +#define EC_HAVE_SII_CACHING + +/****************************************************************************/ + +/** Symbol visibility control macro. + */ +#ifndef EC_PUBLIC_API +# if defined(ethercat_EXPORTS) && !defined(__KERNEL__) +# define EC_PUBLIC_API __attribute__ ((visibility ("default"))) +# else +# define EC_PUBLIC_API +# endif +#endif + +/****************************************************************************/ + +/** End of list marker. + * + * This can be used with ecrt_slave_config_pdos(). + */ +#define EC_END ~0U + +/** Maximum number of sync managers per slave. + */ +#define EC_MAX_SYNC_MANAGERS 16 + +/** Maximum string length. + * + * Used in ec_slave_info_t. + */ +#define EC_MAX_STRING_LENGTH 64 + +/** Maximum number of slave ports. */ +#define EC_MAX_PORTS 4 + +/** Timeval to nanoseconds conversion. + * + * This macro converts a Unix epoch time to EtherCAT DC time. + * + * \see void ecrt_master_application_time() + * + * \param TV struct timeval containing epoch time. + */ +#define EC_TIMEVAL2NANO(TV) \ + (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_usec * 1000ULL) + +/** Size of a CoE emergency message in byte. + * + * \see ecrt_slave_config_emerg_pop(). + */ +#define EC_COE_EMERGENCY_MSG_SIZE 8 + +/***************************************************************************** + * Data types + ****************************************************************************/ + +struct ec_master; +typedef struct ec_master ec_master_t; /**< \see ec_master */ + +struct ec_slave_config; +typedef struct ec_slave_config ec_slave_config_t; /**< \see ec_slave_config */ + +struct ec_domain; +typedef struct ec_domain ec_domain_t; /**< \see ec_domain */ + +struct ec_sdo_request; +typedef struct ec_sdo_request ec_sdo_request_t; /**< \see ec_sdo_request. */ + +struct ec_soe_request; +typedef struct ec_soe_request ec_soe_request_t; /**< \see ec_soe_request. */ + +struct ec_voe_handler; +typedef struct ec_voe_handler ec_voe_handler_t; /**< \see ec_voe_handler. */ + +struct ec_reg_request; +typedef struct ec_reg_request ec_reg_request_t; /**< \see ec_reg_request. */ + +/****************************************************************************/ + +/** Master state. + * + * This is used for the output parameter of ecrt_master_state(). + * + * \see ecrt_master_state(). + */ +typedef struct { + unsigned int slaves_responding; /**< Sum of responding slaves on all + Ethernet devices. */ + unsigned int al_states : 4; /**< Application-layer states of all slaves. + The states are coded in the lower 4 bits. + If a bit is set, it means that at least one + slave in the network is in the corresponding + state: + - Bit 0: \a INIT + - Bit 1: \a PREOP + - Bit 2: \a SAFEOP + - Bit 3: \a OP */ + unsigned int link_up : 1; /**< \a true, if at least one Ethernet link is + up. */ +} ec_master_state_t; + +/****************************************************************************/ + +/** Redundant link state. + * + * This is used for the output parameter of ecrt_master_link_state(). + * + * \see ecrt_master_link_state(). + */ +typedef struct { + unsigned int slaves_responding; /**< Sum of responding slaves on the given + link. */ + unsigned int al_states : 4; /**< Application-layer states of the slaves on + the given link. The states are coded in the + lower 4 bits. If a bit is set, it means + that at least one slave in the network is in + the corresponding state: + - Bit 0: \a INIT + - Bit 1: \a PREOP + - Bit 2: \a SAFEOP + - Bit 3: \a OP */ + unsigned int link_up : 1; /**< \a true, if the given Ethernet link is up. + */ +} ec_master_link_state_t; + +/****************************************************************************/ + +/** Slave configuration state. + * + * This is used as an output parameter of ecrt_slave_config_state(). + * + * \see ecrt_slave_config_state(). + */ +typedef struct { + unsigned int online : 1; /**< The slave is online. */ + unsigned int operational : 1; /**< The slave was brought into \a OP state + using the specified configuration. */ + unsigned int al_state : 4; /**< The application-layer state of the slave. + - 1: \a INIT + - 2: \a PREOP + - 4: \a SAFEOP + - 8: \a OP + + Note that each state is coded in a different + bit! */ +} ec_slave_config_state_t; + +/****************************************************************************/ + +/** Master information. + * + * This is used as an output parameter of ecrt_master(). + * + * \see ecrt_master(). + */ +typedef struct { + unsigned int slave_count; /**< Number of slaves in the network. */ + unsigned int link_up : 1; /**< \a true, if the network link is up. */ + uint8_t scan_busy; /**< \a true, while the master is scanning the network. + */ + uint64_t app_time; /**< Application time. */ +} ec_master_info_t; + +/****************************************************************************/ + +/** Master scan progress information. + * + * This is used as an output parameter of ecrt_master_scan_progress(). + * + * \see ecrt_master_scan_progress(). + */ +typedef struct { + unsigned int slave_count; /**< Number of slaves detected. */ + unsigned int scan_index; /**< Index of the slave that is currently + scanned. If it is less than the \a + slave_count, the network scan is in progress. + */ +} ec_master_scan_progress_t; + +/****************************************************************************/ + +/** EtherCAT slave port descriptor. + */ +typedef enum { + EC_PORT_NOT_IMPLEMENTED, /**< Port is not implemented. */ + EC_PORT_NOT_CONFIGURED, /**< Port is not configured. */ + EC_PORT_EBUS, /**< Port is an E-Bus. */ + EC_PORT_MII /**< Port is a MII. */ +} ec_slave_port_desc_t; + +/****************************************************************************/ + +/** EtherCAT slave port information. + */ +typedef struct { + uint8_t link_up; /**< Link detected. */ + uint8_t loop_closed; /**< Loop closed. */ + uint8_t signal_detected; /**< Detected signal on RX port. */ +} ec_slave_port_link_t; + +/****************************************************************************/ + +/** Slave information. + * + * This is used as an output parameter of ecrt_master_get_slave(). + * + * \see ecrt_master_get_slave(). + */ +typedef struct { + uint16_t position; /**< Offset of the slave in the ring. */ + uint32_t vendor_id; /**< Vendor-ID stored on the slave. */ + uint32_t product_code; /**< Product-Code stored on the slave. */ + uint32_t revision_number; /**< Revision-Number stored on the slave. */ + uint32_t serial_number; /**< Serial-Number stored on the slave. */ + uint16_t alias; /**< The slaves alias if not equal to 0. */ + int16_t current_on_ebus; /**< Used current in mA. */ + struct { + ec_slave_port_desc_t desc; /**< Physical port type. */ + ec_slave_port_link_t link; /**< Port link state. */ + uint32_t receive_time; /**< Receive time on DC transmission delay + measurement. */ + uint16_t next_slave; /**< Ring position of next DC slave on that + port. */ + uint32_t delay_to_next_dc; /**< Delay [ns] to next DC slave. */ + } ports[EC_MAX_PORTS]; /**< Port information. */ + uint8_t al_state; /**< Current state of the slave. */ + uint8_t error_flag; /**< Error flag for that slave. */ + uint8_t sync_count; /**< Number of sync managers. */ + uint16_t sdo_count; /**< Number of SDOs. */ + char name[EC_MAX_STRING_LENGTH]; /**< Name of the slave. */ +} ec_slave_info_t; + +/****************************************************************************/ + +/** Domain working counter interpretation. + * + * This is used in ec_domain_state_t. + */ +typedef enum { + EC_WC_ZERO = 0, /**< No registered process data were exchanged. */ + EC_WC_INCOMPLETE, /**< Some of the registered process data were + exchanged. */ + EC_WC_COMPLETE /**< All registered process data were exchanged. */ +} ec_wc_state_t; + +/****************************************************************************/ + +/** Domain state. + * + * This is used for the output parameter of ecrt_domain_state(). + */ +typedef struct { + unsigned int working_counter; /**< Value of the last working counter. */ + ec_wc_state_t wc_state; /**< Working counter interpretation. */ + unsigned int redundancy_active; /**< Redundant link is in use. */ +} ec_domain_state_t; + +/****************************************************************************/ + +/** Direction type for PDO assignment functions. + */ +typedef enum { + EC_DIR_INVALID, /**< Invalid direction. Do not use this value. */ + EC_DIR_OUTPUT, /**< Values written by the master. */ + EC_DIR_INPUT, /**< Values read by the master. */ + EC_DIR_COUNT /**< Number of directions. For internal use only. */ +} ec_direction_t; + +/****************************************************************************/ + +/** Watchdog mode for sync manager configuration. + * + * Used to specify, if a sync manager's watchdog is to be enabled. + */ +typedef enum { + EC_WD_DEFAULT, /**< Use the default setting of the sync manager. */ + EC_WD_ENABLE, /**< Enable the watchdog. */ + EC_WD_DISABLE, /**< Disable the watchdog. */ +} ec_watchdog_mode_t; + +/****************************************************************************/ + +/** PDO entry configuration information. + * + * This is the data type of the \a entries field in ec_pdo_info_t. + * + * \see ecrt_slave_config_pdos(). + */ +typedef struct { + uint16_t index; /**< PDO entry index. */ + uint8_t subindex; /**< PDO entry subindex. */ + uint8_t bit_length; /**< Size of the PDO entry in bit. */ +} ec_pdo_entry_info_t; + +/****************************************************************************/ + +/** PDO configuration information. + * + * This is the data type of the \a pdos field in ec_sync_info_t. + * + * \see ecrt_slave_config_pdos(). + */ +typedef struct { + uint16_t index; /**< PDO index. */ + unsigned int n_entries; /**< Number of PDO entries in \a entries to map. + Zero means, that the default mapping shall be + used (this can only be done if the slave is + present at configuration time). */ + ec_pdo_entry_info_t const *entries; /**< Array of PDO entries to map. Can + either be \a NULL, or must contain + at least \a n_entries values. */ +} ec_pdo_info_t; + +/****************************************************************************/ + +/** Sync manager configuration information. + * + * This can be use to configure multiple sync managers including the PDO + * assignment and PDO mapping. It is used as an input parameter type in + * ecrt_slave_config_pdos(). + */ +typedef struct { + uint8_t index; /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS for a valid sync manager, + but can also be \a 0xff to mark the end of the list. */ + ec_direction_t dir; /**< Sync manager direction. */ + unsigned int n_pdos; /**< Number of PDOs in \a pdos. */ + ec_pdo_info_t const *pdos; /**< Array with PDOs to assign. This must + contain at least \a n_pdos PDOs. */ + ec_watchdog_mode_t watchdog_mode; /**< Watchdog mode. */ +} ec_sync_info_t; + +/****************************************************************************/ + +/** List record type for PDO entry mass-registration. + * + * This type is used for the array parameter of the + * ecrt_domain_reg_pdo_entry_list() + */ +typedef struct { + uint16_t alias; /**< Slave alias address. */ + uint16_t position; /**< Slave position. */ + uint32_t vendor_id; /**< Slave vendor ID. */ + uint32_t product_code; /**< Slave product code. */ + uint16_t index; /**< PDO entry index. */ + uint8_t subindex; /**< PDO entry subindex. */ + unsigned int *offset; /**< Pointer to a variable to store the PDO entry's + (byte-)offset in the process data. */ + unsigned int *bit_position; /**< Pointer to a variable to store a bit + position (0-7) within the \a offset. Can be + NULL, in which case an error is raised if + the PDO entry does not byte-align. */ +} ec_pdo_entry_reg_t; + +/****************************************************************************/ + +/** Request state. + * + * This is used as return type for ecrt_sdo_request_state() and + * ecrt_voe_handler_state(). + */ +typedef enum { + EC_REQUEST_UNUSED, /**< Not requested. */ + EC_REQUEST_BUSY, /**< Request is being processed. */ + EC_REQUEST_SUCCESS, /**< Request was processed successfully. */ + EC_REQUEST_ERROR, /**< Request processing failed. */ +} ec_request_state_t; + +/****************************************************************************/ + +/** Application-layer state. + */ +typedef enum { + EC_AL_STATE_INIT = 1, /**< Init. */ + EC_AL_STATE_PREOP = 2, /**< Pre-operational. */ + EC_AL_STATE_SAFEOP = 4, /**< Safe-operational. */ + EC_AL_STATE_OP = 8, /**< Operational. */ +} ec_al_state_t; + +/****************************************************************************/ + +/** Fields for SII caching. + * + * For use in the method ecrt_master_sii_caching(). + */ +typedef enum { + EC_SII_DISABLE_CACHING = 0, /** Disable SII caching. */ + EC_SII_VENDOR = 1, /** Use vendor ID. */ + EC_SII_PRODUCT = 2, /** Use product code. */ + EC_SII_REVISION = 4, /** Use revision number. */ + EC_SII_SERIAL = 8, /** Use serial number. */ + EC_SII_ALIAS = 16, /** Use alias address. */ +} ec_sii_caching_fields_t; + +/***************************************************************************** + * Global functions + ****************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + +/** Returns the version magic of the realtime interface. + * + * \apiusage{master_any,rt_safe} + * + * \return Value of ECRT_VERSION_MAGIC() at EtherCAT master compile time. + */ +EC_PUBLIC_API unsigned int ecrt_version_magic(void); + +/** Requests an EtherCAT master for realtime operation. + * + * Before an application can access an EtherCAT master, it has to reserve one + * for exclusive use. + * + * In userspace, this is a convenience function for ecrt_open_master() and + * ecrt_master_reserve(). + * + * This function has to be the first function an application has to call to + * use EtherCAT. The function takes the index of the master as its argument. + * The first master has index 0, the n-th master has index n - 1. The number + * of masters has to be specified when loading the master module. + * + * \apiusage{master_idle,blocking} + * + * \return Pointer to the reserved master, otherwise \a NULL. + */ +EC_PUBLIC_API ec_master_t *ecrt_request_master( + unsigned int master_index /**< Index of the master to request. */ + ); + +#ifndef __KERNEL__ + +/** Opens an EtherCAT master for userspace access. + * + * This function has to be the first function an application has to call to + * use EtherCAT. The function takes the index of the master as its argument. + * The first master has index 0, the n-th master has index n - 1. The number + * of masters has to be specified when loading the master module. + * + * For convenience, the function ecrt_request_master() can be used. + * + * \apiusage{master_idle,blocking} + * + * \return Pointer to the opened master, otherwise \a NULL. + */ +EC_PUBLIC_API ec_master_t *ecrt_open_master( + unsigned int master_index /**< Index of the master to request. */ + ); + +#endif // #ifndef __KERNEL__ + +/** Releases a requested EtherCAT master. + * + * After use, a master it has to be released to make it available for other + * applications. + * + * This method frees all created data structures. It should not be called in + * realtime context. + * + * If the master was activated, ecrt_master_deactivate() is called internally. + * + * \apiusage{master_any,blocking} + */ +EC_PUBLIC_API void ecrt_release_master( + ec_master_t *master /**< EtherCAT master */ + ); + +/***************************************************************************** + * Master methods + ****************************************************************************/ + +#ifndef __KERNEL__ + +/** Reserves an EtherCAT master for realtime operation. + * + * Before an application can use PDO/domain registration functions or SDO + * request functions on the master, it has to reserve one for exclusive use. + * + * \apiusage{master_idle,blocking} + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_reserve( + ec_master_t *master /**< EtherCAT master */ + ); + +#endif // #ifndef __KERNEL__ + +#ifdef __KERNEL__ + +/** Sets the locking callbacks. + * + * For concurrent master access, i. e. if other instances than the application + * want to send and receive datagrams on the network, the application has to + * provide a callback mechanism. This method takes two function pointers as + * its parameters. Asynchronous master access (like EoE processing) is only + * possible if the callbacks have been set. + * + * The task of the send callback (\a send_cb) is to decide, if the network + * hardware is currently accessible and whether or not to call the + * ecrt_master_send_ext() method. + * + * The task of the receive callback (\a receive_cb) is to decide, if a call to + * ecrt_master_receive() is allowed and to execute it respectively. + * + * \apiusage{master_idle,blocking} + * + * \attention This method has to be called before ecrt_master_activate(). + */ +void ecrt_master_callbacks( + ec_master_t *master, /**< EtherCAT master */ + void (*send_cb)(void *), /**< Datagram sending callback. */ + void (*receive_cb)(void *), /**< Receive callback. */ + void *cb_data /**< Arbitrary pointer passed to the callback functions. + */ + ); + +#endif /* __KERNEL__ */ + +/** Creates a new process data domain. + * + * For process data exchange, at least one process data domain is needed. + * This method creates a new process data domain and returns a pointer to the + * new domain object. This object can be used for registering PDOs and + * exchanging them in cyclic operation. + * + * This method allocates memory and should be called in non-realtime context + * before ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return Pointer to the new domain on success, else NULL. + */ +EC_PUBLIC_API ec_domain_t *ecrt_master_create_domain( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Obtains a slave configuration. + * + * Creates a slave configuration object for the given \a alias and \a position + * tuple and returns it. If a configuration with the same \a alias and \a + * position already exists, it will be re-used. In the latter case, the given + * vendor ID and product code are compared to the stored ones. On mismatch, an + * error message is raised and the function returns \a NULL. + * + * Slaves are addressed with the \a alias and \a position parameters. + * - If \a alias is zero, \a position is interpreted as the desired slave's + * ring position. + * - If \a alias is non-zero, it matches a slave with the given alias. In this + * case, \a position is interpreted as ring offset, starting from the + * aliased slave, so a position of zero means the aliased slave itself and a + * positive value matches the n-th slave behind the aliased one. + * + * If the slave with the given address is found during the configuration, + * its vendor ID and product code are matched against the given value. On + * mismatch, the slave is not configured and an error message is raised. + * + * If different slave configurations are pointing to the same slave during + * configuration, a warning is raised and only the first configuration is + * applied. + * + * This method allocates memory and should be called in non-realtime context + * before ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval >0 Pointer to the slave configuration structure. + * \retval NULL in the error case. + */ +EC_PUBLIC_API ec_slave_config_t *ecrt_master_slave_config( + ec_master_t *master, /**< EtherCAT master */ + uint16_t alias, /**< Slave alias. */ + uint16_t position, /**< Slave position. */ + uint32_t vendor_id, /**< Expected vendor ID. */ + uint32_t product_code /**< Expected product code. */ + ); + +/** Selects the reference clock for distributed clocks. + * + * If this method is not called for a certain master, or if the slave + * configuration pointer is NULL, then the first slave with DC functionality + * will provide the reference clock. + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_select_reference_clock( + ec_master_t *master, /**< EtherCAT master. */ + ec_slave_config_t *sc /**< Slave config of the slave to use as the + * reference slave (or NULL). */ + ); + +/** Obtains master information. + * + * No memory is allocated on the heap in this function. + * + * \apiusage{master_any,rt_safe} + * + * \attention The pointer to this structure must point to a valid variable. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master( + ec_master_t *master, /**< EtherCAT master */ + ec_master_info_t *master_info /**< Structure that will output the + information */ + ); + +/** Obtains network scan progress information. + * + * No memory is allocated on the heap in this function. + * + * \apiusage{master_any,rt_safe} + * + * \attention The pointer to this structure must point to a valid variable. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_scan_progress( + ec_master_t *master, /**< EtherCAT master */ + ec_master_scan_progress_t *progress /**< Structure that will output + the progress information. */ + ); + +/** Obtains slave information. + * + * Tries to find the slave with the given ring position. The obtained + * information is stored in a structure. No memory is allocated on the heap in + * this function. + * + * \apiusage{master_any,blocking} + * + * \attention The pointer to this structure must point to a valid variable. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_get_slave( + ec_master_t *master, /**< EtherCAT master */ + uint16_t slave_position, /**< Slave position. */ + ec_slave_info_t *slave_info /**< Structure that will output the + information */ + ); + +#ifndef __KERNEL__ + +/** Returns the proposed configuration of a slave's sync manager. + * + * Fills a given ec_sync_info_t structure with the attributes of a sync + * manager. The \a pdos field of the return value is left empty. Use + * ecrt_master_get_pdo() to get the PDO information. + * + * \apiusage{master_any,blocking} + * + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_master_get_sync_manager( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ec_sync_info_t *sync /**< Pointer to output structure. */ + ); + +/** Returns information about a currently assigned PDO. + * + * Fills a given ec_pdo_info_t structure with the attributes of a currently + * assigned PDO of the given sync manager. The \a entries field of the return + * value is left empty. Use ecrt_master_get_pdo_entry() to get the PDO + * entry information. + * + * \apiusage{master_any,blocking} + * + * \retval zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_master_get_pdo( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t pos, /**< Zero-based PDO position. */ + ec_pdo_info_t *pdo /**< Pointer to output structure. */ + ); + +/** Returns information about a currently mapped PDO entry. + * + * Fills a given ec_pdo_entry_info_t structure with the attributes of a + * currently mapped PDO entry of the given PDO. + * + * \apiusage{master_any,blocking} + * + * \retval zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_master_get_pdo_entry( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t pdo_pos, /**< Zero-based PDO position. */ + uint16_t entry_pos, /**< Zero-based PDO entry position. */ + ec_pdo_entry_info_t *entry /**< Pointer to output structure. */ + ); + +#endif /* #ifndef __KERNEL__ */ + +/** Executes an SDO download request to write data to a slave. + * + * This request is processed by the master state machine. This method blocks, + * until the request has been processed and may not be called in realtime + * context. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_sdo_download( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + uint8_t subindex, /**< Subindex of the SDO. */ + const uint8_t *data, /**< Data buffer to download. */ + size_t data_size, /**< Size of the data buffer. */ + uint32_t *abort_code /**< Abort code of the SDO download. */ + ); + +/** Executes an SDO download request to write data to a slave via complete + * access. + * + * This request is processed by the master state machine. This method blocks, + * until the request has been processed and may not be called in realtime + * context. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_sdo_download_complete( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + const uint8_t *data, /**< Data buffer to download. */ + size_t data_size, /**< Size of the data buffer. */ + uint32_t *abort_code /**< Abort code of the SDO download. */ + ); + +/** Executes an SDO upload request to read data from a slave. + * + * This request is processed by the master state machine. This method blocks, + * until the request has been processed and may not be called in realtime + * context. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_sdo_upload( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + uint8_t subindex, /**< Subindex of the SDO. */ + uint8_t *target, /**< Target buffer for the upload. */ + size_t target_size, /**< Size of the target buffer. */ + size_t *result_size, /**< Uploaded data size. */ + uint32_t *abort_code /**< Abort code of the SDO upload. */ + ); + +/** Executes an SoE write request. + * + * Starts writing an IDN and blocks until the request was processed, or an + * error occurred. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_write_idn( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */ + const uint8_t *data, /**< Pointer to data to write. */ + size_t data_size, /**< Size of data to write. */ + uint16_t *error_code /**< Pointer to variable, where an SoE error code + can be stored. */ + ); + +/** Executes an SoE read request. + * + * Starts reading an IDN and blocks until the request was processed, or an + * error occurred. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_read_idn( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */ + uint8_t *target, /**< Pointer to memory where the read data can be + stored. */ + size_t target_size, /**< Size of the memory \a target points to. */ + size_t *result_size, /**< Actual size of the received data. */ + uint16_t *error_code /**< Pointer to variable, where an SoE error code + can be stored. */ + ); + +/** Finishes the configuration phase and prepares for cyclic operation. + * + * This function tells the master that the configuration phase is finished and + * the realtime operation will begin. The function allocates internal memory + * for the domains and calculates the logical FMMU addresses for domain + * members. It tells the master state machine that the configuration is + * now to be applied to the network. + * + * \apiusage{master_idle,blocking} + * + * \attention After this function has been called, the realtime application is + * in charge of cyclically calling ecrt_master_send() and + * ecrt_master_receive() to ensure network communication. Before calling this + * function, the master thread is responsible for that, so these functions may + * not be called! The method itself allocates memory and should not be called + * in realtime context. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_activate( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Deactivates the master. + * + * Removes the master configuration. All objects created by + * ecrt_master_create_domain(), ecrt_master_slave_config(), ecrt_domain_data() + * ecrt_slave_config_create_sdo_request() and + * ecrt_slave_config_create_voe_handler() are freed, so pointers to them + * become invalid. + * + * \apiusage{master_op,blocking} + * + * This method should not be called in realtime context. + * \return 0 on success, otherwise negative error code. + * \retval 0 Success. + * \retval -EINVAL Master has not been activated before. + */ +EC_PUBLIC_API int ecrt_master_deactivate( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Set interval between calls to ecrt_master_send(). + * + * This information helps the master to decide, how much data can be appended + * to a frame by the master state machine. When the master is configured with + * --enable-hrtimers, this is used to calculate the scheduling of the master + * thread. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 on success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_set_send_interval( + ec_master_t *master, /**< EtherCAT master. */ + size_t send_interval /**< Send interval in us */ + ); + +/** Sends all datagrams in the queue. + * + * This method takes all datagrams, that have been queued for transmission, + * puts them into frames, and passes them to the Ethernet device for sending. + * + * Has to be called cyclically by the application after ecrt_master_activate() + * has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_send( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Fetches received frames from the hardware and processes the datagrams. + * + * Queries the network device for received frames by calling the interrupt + * service routine. Extracts received datagrams and dispatches the results to + * the datagram objects in the queue. Received datagrams, and the ones that + * timed out, will be marked, and dequeued. + * + * Has to be called cyclically by the realtime application after + * ecrt_master_activate() has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_receive( + ec_master_t *master /**< EtherCAT master. */ + ); + +#ifdef __KERNEL__ +/** Sends non-application datagrams. + * + * This method has to be called in the send callback function passed via + * ecrt_master_callbacks() to allow the sending of non-application datagrams. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + * \retval -EAGAIN Lock could not be acquired, try again later. + */ +int ecrt_master_send_ext( + ec_master_t *master /**< EtherCAT master. */ + ); +#endif + +/** Reads the current master state. + * + * Stores the master state information in the given \a state structure. + * + * This method returns a global state. For the link-specific states in a + * redundant network topology, use the ecrt_master_link_state() method. + * + * \apiusage{master_any,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_state( + const ec_master_t *master, /**< EtherCAT master. */ + ec_master_state_t *state /**< Structure to store the information. */ + ); + +/** Reads the current state of a redundant link. + * + * Stores the link state information in the given \a state structure. + * + * \apiusage{master_any,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_link_state( + const ec_master_t *master, /**< EtherCAT master. */ + unsigned int dev_idx, /**< Index of the device (0 = main device, 1 = + first backup device, ...). */ + ec_master_link_state_t *state /**< Structure to store the information. + */ + ); + +/** Sets the application time. + * + * The master has to know the application's time when operating slaves with + * distributed clocks. The time is not incremented by the master itself, so + * this method has to be called cyclically. + * + * \attention The time passed to this method is used to calculate the phase of + * the slaves' SYNC0/1 interrupts. It should be called constantly at the same + * point of the realtime cycle. So it is recommended to call it at the start + * of the calculations to avoid deviancies due to changing execution times. + * Avoid calling this method before the realtime cycle is established. + * + * The time is used when setting the slaves' System Time Offset and + * Cyclic Operation Start Time registers and when synchronizing the + * DC reference clock to the application time via + * ecrt_master_sync_reference_clock(). + * + * The time is defined as nanoseconds from 2000-01-01 00:00. Converting an + * epoch time can be done with the EC_TIMEVAL2NANO() macro, but is not + * necessary, since the absolute value is not of any interest. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_application_time( + ec_master_t *master, /**< EtherCAT master. */ + uint64_t app_time /**< Application time. */ + ); + +/** Queues the DC reference clock drift compensation datagram for sending. + * + * The reference clock will by synchronized to the application time provided + * by the last call off ecrt_master_application_time(). + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + * \retval 0 Success. + * \retval -ENXIO No reference clock found. + */ +EC_PUBLIC_API int ecrt_master_sync_reference_clock( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Queues the DC reference clock drift compensation datagram for sending. + * + * The reference clock will by synchronized to the time passed in the + * sync_time parameter. + * + * Has to be called by the application after ecrt_master_activate() + * has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + * \retval 0 Success. + * \retval -ENXIO No reference clock found. + */ +EC_PUBLIC_API int ecrt_master_sync_reference_clock_to( + ec_master_t *master, /**< EtherCAT master. */ + uint64_t sync_time /**< Sync reference clock to this time. */ + ); + +/** Queues the DC clock drift compensation datagram for sending. + * + * All slave clocks synchronized to the reference clock. + * + * Has to be called by the application after ecrt_master_activate() + * has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval 0 Success. + * \retval -ENXIO No reference clock found. + */ +EC_PUBLIC_API int ecrt_master_sync_slave_clocks( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Get the lower 32 bit of the reference clock system time. + * + * This method can be used to synchronize the master to the reference clock. + * + * The reference clock system time is queried via the + * ecrt_master_sync_slave_clocks() method, that reads the system time of the + * reference clock and writes it to the slave clocks (so be sure to call it + * cyclically to get valid data). + * + * \attention The returned time is the system time of the reference clock + * minus the transmission delay of the reference clock. + * + * Calling this method makes only sense in realtime context (after master + * activation), when the ecrt_master_sync_slave_clocks() method is called + * cyclically. + * + * \apiusage{master_op,rt_safe} + * + * \retval 0 success, system time was written into \a time. + * \retval -ENXIO No reference clock found. + * \retval -EIO Slave synchronization datagram was not received. + */ +EC_PUBLIC_API int ecrt_master_reference_clock_time( + const ec_master_t *master, /**< EtherCAT master. */ + uint32_t *time /**< Pointer to store the queried system time. */ + ); + +/** Queues the DC synchrony monitoring datagram for sending. + * + * The datagram broadcast-reads all "System time difference" registers (\a + * 0x092c) to get an upper estimation of the DC synchrony. The result can be + * checked with the ecrt_master_sync_monitor_process() method. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise a negative error code. + */ +EC_PUBLIC_API int ecrt_master_sync_monitor_queue( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Processes the DC synchrony monitoring datagram. + * + * If the sync monitoring datagram was sent before with + * ecrt_master_sync_monitor_queue(), the result can be queried with this + * method. + * + * \apiusage{master_op,rt_safe} + * + * \return Upper estimation of the maximum time difference in ns, -1 on error. + * \retval (uint32_t)-1 Error. + */ +EC_PUBLIC_API uint32_t ecrt_master_sync_monitor_process( + const ec_master_t *master /**< EtherCAT master. */ + ); + +/** Retry configuring slaves. + * + * Via this method, the application can tell the master to bring all slaves to + * OP state. In general, this is not necessary, because it is automatically + * done by the master. But with special slaves, that can be reconfigured by + * the vendor during runtime, it can be useful. + * + * Calling this method only makes sense in realtime context (after + * activation), because slaves will not be configured before. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_reset( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Set the SII caching method. + * + * Via this method, the application can tell the master to either which fields + * to use for looking up cached SII content pages or to disable SII caching at + * all. + * + * The default when starting up is defined in the master configuration file. + * The caching method stays valid as long as the master is existing, so it + * could be set by a prior application. + * + * The allowed fields are defined in ec_sii_caching_fields_t. A typical setup + * could be: + * + * \code + * if (ecrt_master_sii_caching(master, + * EC_SII_VENDOR | EC_SII_PRODUCT | EC_SII_REVISION)) { + * fprintf(stderr, "Failed to set up SII caching method.\n"); + * } + * \endcode + * + * A value of zero disables SII caching completely, thus the SII contents are + * completely loaded from every slave during scanning: + * + * \code + * if (ecrt_master_sii_caching(master, EC_SII_DISABLE_CACHING)) { + * fprintf(stderr, "Failed to disable SII caching.\n"); + * } + * \endcode + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_sii_caching( + ec_master_t *master, /**< EtherCAT master. */ + ec_sii_caching_fields_t fields /** Fields to use for cache lookup. */ + ); + +/***************************************************************************** + * Slave configuration methods + ****************************************************************************/ + +/** Configure a sync manager. + * + * Sets the direction of a sync manager. This overrides the direction bits + * from the default control register from SII. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_sync_manager( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ec_direction_t direction, /**< Input/Output. */ + ec_watchdog_mode_t watchdog_mode /** Watchdog mode. */ + ); + +/** Configure a slave's watchdog times. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_watchdog( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t watchdog_divider, /**< Number of 40 ns intervals (register + 0x0400). Used as a base unit for all + slave watchdogs^. If set to zero, the + value is not written, so the default is + used. */ + uint16_t watchdog_intervals /**< Number of base intervals for sync + manager watchdog (register 0x0420). If + set to zero, the value is not written, + so the default is used. */ + ); + +/** Add a PDO to a sync manager's PDO assignment. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_assign_add( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t index /**< Index of the PDO to assign. */ + ); + +/** Clear a sync manager's PDO assignment. + * + * This can be called before assigning PDOs via + * ecrt_slave_config_pdo_assign_add(), to clear the default assignment of a + * sync manager. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_assign_clear( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ); + +/** Add a PDO entry to the given PDO's mapping. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_mapping_add( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t pdo_index, /**< Index of the PDO. */ + uint16_t entry_index, /**< Index of the PDO entry to add to the PDO's + mapping. */ + uint8_t entry_subindex, /**< Subindex of the PDO entry to add to the + PDO's mapping. */ + uint8_t entry_bit_length /**< Size of the PDO entry in bit. */ + ); + +/** Clear the mapping of a given PDO. + * + * This can be called before mapping PDO entries via + * ecrt_slave_config_pdo_mapping_add(), to clear the default mapping. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_mapping_clear( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t pdo_index /**< Index of the PDO. */ + ); + +/** Specify a complete PDO configuration. + * + * This function is a convenience wrapper for the functions + * ecrt_slave_config_sync_manager(), ecrt_slave_config_pdo_assign_clear(), + * ecrt_slave_config_pdo_assign_add(), ecrt_slave_config_pdo_mapping_clear() + * and ecrt_slave_config_pdo_mapping_add(), that are better suitable for + * automatic code generation. + * + * The following example shows, how to specify a complete configuration, + * including the PDO mappings. With this information, the master is able to + * reserve the complete process data, even if the slave is not present at + * configuration time: + * + * \code + * ec_pdo_entry_info_t el3162_channel1[] = { + * {0x3101, 1, 8}, // status + * {0x3101, 2, 16} // value + * }; + * + * ec_pdo_entry_info_t el3162_channel2[] = { + * {0x3102, 1, 8}, // status + * {0x3102, 2, 16} // value + * }; + * + * ec_pdo_info_t el3162_pdos[] = { + * {0x1A00, 2, el3162_channel1}, + * {0x1A01, 2, el3162_channel2} + * }; + * + * ec_sync_info_t el3162_syncs[] = { + * {2, EC_DIR_OUTPUT}, + * {3, EC_DIR_INPUT, 2, el3162_pdos}, + * {0xff} + * }; + * + * if (ecrt_slave_config_pdos(sc_ana_in, EC_END, el3162_syncs)) { + * // handle error + * } + * \endcode + * + * The next example shows, how to configure the PDO assignment only. The + * entries for each assigned PDO are taken from the PDO's default mapping. + * Please note, that PDO entry registration will fail, if the PDO + * configuration is left empty and the slave is offline. + * + * \code + * ec_pdo_info_t pdos[] = { + * {0x1600}, // Channel 1 + * {0x1601} // Channel 2 + * }; + * + * ec_sync_info_t syncs[] = { + * {3, EC_DIR_INPUT, 2, pdos}, + * }; + * + * if (ecrt_slave_config_pdos(slave_config_ana_in, 1, syncs)) { + * // handle error + * } + * \endcode + * + * Processing of \a syncs will stop, if + * - the number of processed items reaches \a n_syncs, or + * - the \a index member of an ec_sync_info_t item is 0xff. In this case, + * \a n_syncs should set to a number greater than the number of list items; + * using EC_END is recommended. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_pdos( + ec_slave_config_t *sc, /**< Slave configuration. */ + unsigned int n_syncs, /**< Number of sync manager configurations in + \a syncs. */ + const ec_sync_info_t syncs[] /**< Array of sync manager + configurations. */ + ); + +/** Registers a PDO entry for process data exchange in a domain. + * + * Searches the assigned PDOs for the given PDO entry. An error is raised, if + * the given entry is not mapped. Otherwise, the corresponding sync manager + * and FMMU configurations are provided for slave configuration and the + * respective sync manager's assigned PDOs are appended to the given domain, + * if not already done. The offset of the requested PDO entry's data inside + * the domain's process data is returned. Optionally, the PDO entry bit + * position (0-7) can be retrieved via the \a bit_position output parameter. + * This pointer may be \a NULL, in this case an error is raised if the PDO + * entry does not byte-align. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval >=0 Success: Offset of the PDO entry's process data. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_reg_pdo_entry( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t entry_index, /**< Index of the PDO entry to register. */ + uint8_t entry_subindex, /**< Subindex of the PDO entry to register. */ + ec_domain_t *domain, /**< Domain. */ + unsigned int *bit_position /**< Optional address if bit addressing + is desired */ + ); + +/** Registers a PDO entry using its position. + * + * Similar to ecrt_slave_config_reg_pdo_entry(), but not using PDO indices but + * offsets in the PDO mapping, because PDO entry indices may not be unique + * inside a slave's PDO mapping. An error is raised, if + * one of the given positions is out of range. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval >=0 Success: Offset of the PDO entry's process data. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_reg_pdo_entry_pos( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. */ + unsigned int pdo_pos, /**< Position of the PDO inside the SM. */ + unsigned int entry_pos, /**< Position of the entry inside the PDO. */ + ec_domain_t *domain, /**< Domain. */ + unsigned int *bit_position /**< Optional address if bit addressing + is desired */ + ); + +/** Configure distributed clocks. + * + * Sets the AssignActivate word and the cycle and shift times for the sync + * signals. + * + * The AssignActivate word is vendor-specific and can be taken from the XML + * device description file (Device -> Dc -> AssignActivate). Set this to zero, + * if the slave shall be operated without distributed clocks (default). + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \attention The \a sync1_shift time is ignored. + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_dc( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t assign_activate, /**< AssignActivate word. */ + uint32_t sync0_cycle, /**< SYNC0 cycle time [ns]. */ + int32_t sync0_shift, /**< SYNC0 shift time [ns]. */ + uint32_t sync1_cycle, /**< SYNC1 cycle time [ns]. */ + int32_t sync1_shift /**< SYNC1 shift time [ns]. */ + ); + +/** Add an SDO configuration. + * + * An SDO configuration is stored in the slave configuration object and is + * downloaded to the slave whenever the slave is being configured by the + * master. This usually happens once on master activation, but can be repeated + * subsequently, for example after the slave's power supply failed. + * + * \attention The SDOs for PDO assignment (\p 0x1C10 - \p 0x1C2F) and PDO + * mapping (\p 0x1600 - \p 0x17FF and \p 0x1A00 - \p 0x1BFF) should not be + * configured with this function, because they are part of the slave + * configuration done by the master. Please use ecrt_slave_config_pdos() and + * friends instead. + * + * This is the generic function for adding an SDO configuration. Please note + * that the this function does not do any endianness correction. If + * datatype-specific functions are needed (that automatically correct the + * endianness), have a look at ecrt_slave_config_sdo8(), + * ecrt_slave_config_sdo16() and ecrt_slave_config_sdo32(). + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< Index of the SDO to configure. */ + uint8_t subindex, /**< Subindex of the SDO to configure. */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ + ); + +/** Add a configuration value for an 8-bit SDO. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo8( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint8_t value /**< Value to set. */ + ); + +/** Add a configuration value for a 16-bit SDO. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo16( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint16_t value /**< Value to set. */ + ); + +/** Add a configuration value for a 32-bit SDO. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo32( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint32_t value /**< Value to set. */ + ); + +/** Add configuration data for a complete SDO. + * + * The SDO data are transferred via CompleteAccess. Data for the first + * subindex (0) have to be included. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_complete_sdo( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< Index of the SDO to configure. */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ + ); + +/** Set the size of the CoE emergency ring buffer. + * + * The initial size is zero, so all messages will be dropped. This method can + * be called even after master activation, but it will clear the ring buffer! + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, or negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_size( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t elements /**< Number of records of the CoE emergency ring. */ + ); + +/** Read and remove one record from the CoE emergency ring buffer. + * + * A record consists of 8 bytes: + * + * Byte 0-1: Error code (little endian) + * Byte 2: Error register + * Byte 3-7: Data + * + * Calling this method makes only sense in realtime context (after master + * activation). + * + * \return 0 on success (record popped), or negative error code (i. e. + * -ENOENT, if ring is empty). + * + * \apiusage{master_op,any_context} + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_pop( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t *target /**< Pointer to target memory (at least + EC_COE_EMERGENCY_MSG_SIZE bytes). */ + ); + +/** Clears CoE emergency ring buffer and the overrun counter. + * + * Calling this method makes only sense in realtime context (after master + * activation). + * + * \apiusage{master_op,any_context} + * + * \return 0 on success, or negative error code. + * + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_clear( + ec_slave_config_t *sc /**< Slave configuration. */ + ); + +/** Read the number of CoE emergency overruns. + * + * The overrun counter will be incremented when a CoE emergency message could + * not be stored in the ring buffer and had to be dropped. Call + * ecrt_slave_config_emerg_clear() to reset the counter. + * + * Calling this method makes only sense in realtime context (after master + * activation). + * + * \apiusage{master_op,any_context} + * + * \return Number of overruns since last clear, or negative error code. + * + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_overruns( + const ec_slave_config_t *sc /**< Slave configuration. */ + ); + +/** Create an SDO request to exchange SDOs during realtime operation. + * + * The created SDO request object is freed automatically when the master is + * released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New SDO request, or NULL on error. + */ +EC_PUBLIC_API ec_sdo_request_t *ecrt_slave_config_create_sdo_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< SDO index. */ + uint8_t subindex, /**< SDO subindex. */ + size_t size /**< Data size to reserve. */ + ); + +/** Create an SoE request to exchange SoE IDNs during realtime operation. + * + * The created SoE request object is freed automatically when the master is + * released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New SoE request, or NULL on error. + */ +EC_PUBLIC_API ec_soe_request_t *ecrt_slave_config_create_soe_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< Sercos ID-Number. */ + size_t size /**< Data size to reserve. */ + ); + +/** Create an VoE handler to exchange vendor-specific data during realtime + * operation. + * + * The number of VoE handlers per slave configuration is not limited, but + * usually it is enough to create one for sending and one for receiving, if + * both can be done simultaneously. + * + * The created VoE handler object is freed automatically when the master is + * released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New VoE handler, or NULL on error. + */ +EC_PUBLIC_API ec_voe_handler_t *ecrt_slave_config_create_voe_handler( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t size /**< Data size to reserve. */ + ); + +/** Create a register request to exchange EtherCAT register contents during + * realtime operation. + * + * This interface should not be used to take over master functionality, + * instead it is intended for debugging and monitoring reasons. + * + * The created register request object is freed automatically when the master + * is released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New register request, or NULL on error. + */ +EC_PUBLIC_API ec_reg_request_t *ecrt_slave_config_create_reg_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t size /**< Data size to reserve. */ + ); + +/** Outputs the state of the slave configuration. + * + * Stores the state information in the given \a state structure. The state + * information is updated by the master state machine, so it may take a few + * cycles, until it changes. + * + * \attention If the state of process data exchange shall be monitored in + * realtime, ecrt_domain_state() should be used. + * + * \apiusage{master_op,rt_safe} + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_state( + const ec_slave_config_t *sc, /**< Slave configuration */ + ec_slave_config_state_t *state /**< State object to write to. */ + ); + +/** Add an SoE IDN configuration. + * + * A configuration for a Sercos-over-EtherCAT IDN is stored in the slave + * configuration object and is written to the slave whenever the slave is + * being configured by the master. This usually happens once on master + * activation, but can be repeated subsequently, for example after the slave's + * power supply failed. + * + * The \a idn parameter can be separated into several sections: + * - Bit 15: Standard data (0) or Product data (1) + * - Bit 14 - 12: Parameter set (0 - 7) + * - Bit 11 - 0: Data block number (0 - 4095) + * + * Please note that the this function does not do any endianness correction. + * Multi-byte data have to be passed in EtherCAT endianness (little-endian). + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_idn( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN. */ + ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or + SAFEOP). */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ + ); + +/** Adds a feature flag to a slave configuration. + * + * Feature flags are a generic way to configure slave-specific behavior. + * + * Multiple calls with the same slave configuration and key will overwrite the + * configuration. + * + * The following flags may be available: + * - AssignToPdi: Zero (default) keeps the slave information interface (SII) + * assigned to EtherCAT (except during transition to PREOP). Non-zero + * assigns the SII to the slave controller side before going to PREOP and + * leaves it there until a write command happens. + * - WaitBeforeSAFEOPms: Number of milliseconds to wait before commanding the + * transition from PREOP to SAFEOP. This can be used as a workaround for + * slaves that need a little time to initialize. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_flag( + ec_slave_config_t *sc, /**< Slave configuration. */ + const char *key, /**< Key as null-terminated ASCII string. */ + int32_t value /**< Value to store. */ + ); + +/** Sets the link/MAC address for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The MAC address is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_mac_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + const unsigned char *mac_address /**< MAC address. */ + ); + +/** Sets the IP address for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The IP address is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * The IP address is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit IPv4 address in network byte order (big endian). + * + * A string-represented IPv4 address can be converted to a `struct in_addr` + * for example via the POSIX function `inet_pton()` (see man 3 inet_pton): + * + * \code{.c} + * #include + * struct in_addr addr; + * if (inet_aton("192.168.0.1", &addr) == 0) { + * fprintf(stderr, "Failed to convert IP address.\n"); + * return -1; + * } + * if (ecrt_slave_config_eoe_ip_address(sc, addr)) { + * fprintf(stderr, "Failed to set IP address.\n"); + * return -1; + * } + * \endcode + * + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_ip_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr ip_address /**< IPv4 address. */ + ); + +/** Sets the subnet mask for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The subnet mask is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * The subnet mask is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit mask in network byte order (big endian). + * + * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded masks + * to `struct in_addr`. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_subnet_mask( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr subnet_mask /**< IPv4 subnet mask. */ + ); + +/** Sets the gateway address for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The gateway address is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * The address is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit IPv4 address in network byte order (big endian). + * + * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded IPv4 + * addresses to `struct in_addr`. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_default_gateway( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr gateway_address /**< Gateway's IPv4 address. */ + ); + +/** Sets the IPv4 address of the DNS server for Ethernet-over-EtherCAT (EoE) + * operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The DNS server address is stored in the slave configuration object and will + * be written to the slave during the configuration process. + * + * The address is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit IPv4 address in network byte order (big endian). + * + * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded IPv4 + * addresses to `struct in_addr`. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_dns_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr dns_address /**< IPv4 address of the DNS server. */ + ); + +/** Sets the host name for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The host name is stored in the slave configuration object and will + * be written to the slave during the configuration process. + * + * The maximum size of the host name is 32 bytes (including the zero + * terminator). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_hostname( + ec_slave_config_t *sc, /**< Slave configuration. */ + const char *name /**< Zero-terminated host name. */ + ); + +/** Sets the application-layer state transition timeout in ms. + * + * Change the maximum allowed time for a slave to make an application-layer + * state transition for the given state transition (for example from PREOP to + * SAFEOP). The default values are defined in ETG.2000. + * + * A timeout value of zero ms will restore the default value. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_state_timeout( + ec_slave_config_t *sc, /**< Slave configuration. */ + ec_al_state_t from_state, /**< Initial state. */ + ec_al_state_t to_state, /**< Target state. */ + unsigned int timeout_ms /**< Timeout in [ms]. */ + ); + +/***************************************************************************** + * Domain methods + ****************************************************************************/ + +/** Registers a bunch of PDO entries for a domain. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_reg_pdo_entry() + * + * \attention The registration array has to be terminated with an empty + * structure, or one with the \a index field set to zero! + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, else non-zero. + */ +EC_PUBLIC_API int ecrt_domain_reg_pdo_entry_list( + ec_domain_t *domain, /**< Domain. */ + const ec_pdo_entry_reg_t *pdo_entry_regs /**< Array of PDO + registrations. */ + ); + +/** Returns the current size of the domain's process data. + * + * The domain size is calculated after master activation. + * + * \apiusage{master_op,rt_safe} + * + * \return Size of the process data image, or a negative error code. + */ +EC_PUBLIC_API size_t ecrt_domain_size( + const ec_domain_t *domain /**< Domain. */ + ); + +#ifdef __KERNEL__ + +/** Provide external memory to store the domain's process data. + * + * Call this after all PDO entries have been registered and before activating + * the master. + * + * The size of the allocated memory must be at least ecrt_domain_size(), after + * all PDO entries have been registered. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + */ +void ecrt_domain_external_memory( + ec_domain_t *domain, /**< Domain. */ + uint8_t *memory /**< Address of the memory to store the process + data in. */ + ); + +#endif /* __KERNEL__ */ + +/** Returns the domain's process data. + * + * - In kernel context: If external memory was provided with + * ecrt_domain_external_memory(), the returned pointer will contain the + * address of that memory. Otherwise it will point to the internally allocated + * memory. In the latter case, this method may not be called before + * ecrt_master_activate(). + * + * - In userspace context: This method has to be called after + * ecrt_master_activate() to get the mapped domain process data memory. + * + * \apiusage{master_op,rt_safe} + * + * \return Pointer to the process data memory. + */ +EC_PUBLIC_API uint8_t *ecrt_domain_data( + const ec_domain_t *domain /**< Domain. */ + ); + +/** Determines the states of the domain's datagrams. + * + * Evaluates the working counters of the received datagrams and outputs + * statistics, if necessary. This must be called after ecrt_master_receive() + * is expected to receive the domain datagrams in order to make + * ecrt_domain_state() return the result of the last process data exchange. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_domain_process( + ec_domain_t *domain /**< Domain. */ + ); + +/** (Re-)queues all domain datagrams in the master's datagram queue. + * + * Call this function to mark the domain's datagrams for exchanging at the + * next call of ecrt_master_send(). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_domain_queue( + ec_domain_t *domain /**< Domain. */ + ); + +/** Reads the state of a domain. + * + * Stores the domain state in the given \a state structure. + * + * Using this method, the process data exchange can be monitored in realtime. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_domain_state( + const ec_domain_t *domain, /**< Domain. */ + ec_domain_state_t *state /**< Pointer to a state object to store the + information. */ + ); + +/***************************************************************************** + * SDO request methods. + ****************************************************************************/ + +/** Set the SDO index and subindex. + * + * \attention If the SDO index and/or subindex is changed while + * ecrt_sdo_request_state() returns EC_REQUEST_BUSY, this may lead to + * unexpected results. + * + * This method is meant to be called in realtime context (after master + * activation). To initialize the SDO request, the index and subindex can be + * set via ecrt_slave_config_create_sdo_request(). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_sdo_request_index( + ec_sdo_request_t *req, /**< SDO request. */ + uint16_t index, /**< SDO index. */ + uint8_t subindex /**< SDO subindex. */ + ); + +/** Set the timeout for an SDO request. + * + * If the request cannot be processed in the specified time, if will be marked + * as failed. + * + * The timeout is permanently stored in the request object and is valid until + * the next call of this method. + * + * The timeout should be defined in non-realtime context, but can also be + * changed afterwards. + * + * \apiusage{master_any,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_sdo_request_timeout( + ec_sdo_request_t *req, /**< SDO request. */ + uint32_t timeout /**< Timeout in milliseconds. Zero means no + timeout. */ + ); + +/** Access to the SDO request's data. + * + * This function returns a pointer to the request's internal SDO data memory. + * + * - After a read operation was successful, integer data can be evaluated + * using the EC_READ_*() macros as usual. Example: + * \code + * uint16_t value = EC_READ_U16(ecrt_sdo_request_data(sdo))); + * \endcode + * - If a write operation shall be triggered, the data have to be written to + * the internal memory. Use the EC_WRITE_*() macros, if you are writing + * integer data. Be sure, that the data fit into the memory. The memory size + * is a parameter of ecrt_slave_config_create_sdo_request(). + * \code + * EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF); + * \endcode + * + * \attention The return value can be invalid during a read operation, because + * the internal SDO data memory could be re-allocated if the read SDO data do + * not fit inside. + * + * This method is meant to be called in realtime context (after master + * activation), but can also be used to initialize data before. + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal SDO data memory. + * + */ +EC_PUBLIC_API uint8_t *ecrt_sdo_request_data( + const ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Returns the current SDO data size. + * + * When the SDO request is created, the data size is set to the size of the + * reserved memory. After a read operation the size is set to the size of the + * read data. The size is not modified in any other situation. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_any,rt_safe} + * + * \return SDO data size in bytes. + * + */ +EC_PUBLIC_API size_t ecrt_sdo_request_data_size( + const ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Get the current state of the SDO request. + * + * The user-space implementation fetches incoming data and stores the received + * data size in the request object, so the request is not const. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return Request state. + * + */ +EC_PUBLIC_API ec_request_state_t ecrt_sdo_request_state( +#ifdef __KERNEL__ + const +#endif + ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Schedule an SDO write operation. + * + * \attention This method may not be called while ecrt_sdo_request_state() + * returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -EINVAL Invalid input data, e.g. data size == 0. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_sdo_request() + * too small. + */ +EC_PUBLIC_API int ecrt_sdo_request_write( + ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Schedule an SDO read operation. + * + * \attention This method may not be called while ecrt_sdo_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention After calling this function, the return value of + * ecrt_sdo_request_data() must be considered as invalid while + * ecrt_sdo_request_state() returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_sdo_request_read( + ec_sdo_request_t *req /**< SDO request. */ + ); + +/***************************************************************************** + * SoE request methods. + ****************************************************************************/ + +/** Set the request's drive and Sercos ID numbers. + * + * \attention If the drive number and/or IDN is changed while + * ecrt_soe_request_state() returns EC_REQUEST_BUSY, this may lead to + * unexpected results. + * + * This method is meant to be called in realtime context (after master + * activation). To initialize the SoE request, the drive_no and IDN can be + * set via ecrt_slave_config_create_soe_request(). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_soe_request_idn( + ec_soe_request_t *req, /**< IDN request. */ + uint8_t drive_no, /**< SDO index. */ + uint16_t idn /**< SoE IDN. */ + ); + +/** Set the timeout for an SoE request. + * + * If the request cannot be processed in the specified time, if will be marked + * as failed. + * + * The timeout is permanently stored in the request object and is valid until + * the next call of this method. + * + * The timeout should be defined in non-realtime context, but can also be + * changed afterwards. + * + * \apiusage{master_any,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_soe_request_timeout( + ec_soe_request_t *req, /**< SoE request. */ + uint32_t timeout /**< Timeout in milliseconds. Zero means no + timeout. */ + ); + +/** Access to the SoE request's data. + * + * This function returns a pointer to the request's internal IDN data memory. + * + * - After a read operation was successful, integer data can be evaluated + * using the EC_READ_*() macros as usual. Example: + * \code + * uint16_t value = EC_READ_U16(ecrt_soe_request_data(idn_req))); + * \endcode + * - If a write operation shall be triggered, the data have to be written to + * the internal memory. Use the EC_WRITE_*() macros, if you are writing + * integer data. Be sure, that the data fit into the memory. The memory size + * is a parameter of ecrt_slave_config_create_soe_request(). + * \code + * EC_WRITE_U16(ecrt_soe_request_data(idn_req), 0xFFFF); + * \endcode + * + * \attention The return value can be invalidated during a read operation, + * because the internal IDN data memory could be re-allocated if the read IDN + * data do not fit inside. + * + * This method is meant to be called in realtime context (after master + * activation), but can also be used to initialize data before. + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal IDN data memory. + * + */ +EC_PUBLIC_API uint8_t *ecrt_soe_request_data( + const ec_soe_request_t *req /**< SoE request. */ + ); + +/** Returns the current IDN data size. + * + * When the SoE request is created, the data size is set to the size of the + * reserved memory. After a read operation the size is set to the size of the + * read data. The size is not modified in any other situation. + * + * \apiusage{master_any,rt_safe} + * + * \return IDN data size in bytes. + */ +EC_PUBLIC_API size_t ecrt_soe_request_data_size( + const ec_soe_request_t *req /**< SoE request. */ + ); + +/** Get the current state of the SoE request. + * + * \return Request state. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * In the user-space implementation, the method fetches the size of the + * incoming data, so the request object is not const. + * + * \apiusage{master_op,rt_safe} + */ +EC_PUBLIC_API ec_request_state_t ecrt_soe_request_state( +#ifdef __KERNEL__ + const +#endif + ec_soe_request_t *req /**< SoE request. */ + ); + +/** Schedule an SoE IDN write operation. + * + * \attention This method may not be called while ecrt_soe_request_state() + * returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -EINVAL Invalid input data, e.g. data size == 0. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_soe_request() + * too small. + */ +EC_PUBLIC_API int ecrt_soe_request_write( + ec_soe_request_t *req /**< SoE request. */ + ); + +/** Schedule an SoE IDN read operation. + * + * \attention This method may not be called while ecrt_soe_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention After calling this function, the return value of + * ecrt_soe_request_data() must be considered as invalid while + * ecrt_soe_request_state() returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_soe_request_read( + ec_soe_request_t *req /**< SoE request. */ + ); + +/***************************************************************************** + * VoE handler methods. + ****************************************************************************/ + +/** Sets the VoE header for future send operations. + * + * A VoE message shall contain a 4-byte vendor ID, followed by a 2-byte vendor + * type at as header. These numbers can be set with this function. The values + * are valid and will be used for future send operations until the next call + * of this method. + * + * This method is meant to be called in non-realtime context (before master + * activation) to initialize the header data, but it is also safe to + * change the header later on in realtime context. + * + * \apiusage{master_any,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_send_header( + ec_voe_handler_t *voe, /**< VoE handler. */ + uint32_t vendor_id, /**< Vendor ID. */ + uint16_t vendor_type /**< Vendor-specific type. */ + ); + +/** Reads the header data of a received VoE message. + * + * This method can be used to get the received VoE header information after a + * read operation has succeeded. + * + * The header information is stored at the memory given by the pointer + * parameters. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_received_header( + const ec_voe_handler_t *voe, /**< VoE handler. */ + uint32_t *vendor_id, /**< Vendor ID. */ + uint16_t *vendor_type /**< Vendor-specific type. */ + ); + +/** Access to the VoE handler's data. + * + * This function returns a pointer to the VoE handler's internal memory, that + * points to the actual VoE data right after the VoE header (see + * ecrt_voe_handler_send_header()). + * + * - After a read operation was successful, the memory contains the received + * data. The size of the received data can be determined via + * ecrt_voe_handler_data_size(). + * - Before a write operation is triggered, the data have to be written to the + * internal memory. Be sure, that the data fit into the memory. The reserved + * memory size is a parameter of ecrt_slave_config_create_voe_handler(). + * + * \attention The returned pointer is not necessarily persistent: After a read + * operation, the internal memory may have been reallocated. This can be + * avoided by reserving enough memory via the \a size parameter of + * ecrt_slave_config_create_voe_handler(). + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal memory. + */ +EC_PUBLIC_API uint8_t *ecrt_voe_handler_data( + const ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Returns the current data size. + * + * The data size is the size of the VoE data without the header (see + * ecrt_voe_handler_send_header()). + * + * When the VoE handler is created, the data size is set to the size of the + * reserved memory. At a write operation, the data size is set to the number + * of bytes to write. After a read operation the size is set to the size of + * the read data. The size is not modified in any other situation. + * + * \apiusage{master_any,rt_safe} + * + * \return Data size in bytes. + */ +EC_PUBLIC_API size_t ecrt_voe_handler_data_size( + const ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Start a VoE write operation. + * + * After this function has been called, the ecrt_voe_handler_execute() method + * must be called in every realtime cycle as long as it returns + * EC_REQUEST_BUSY. No other operation may be started while the handler is + * busy. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_voe_handler + * too small. + */ +EC_PUBLIC_API int ecrt_voe_handler_write( + ec_voe_handler_t *voe, /**< VoE handler. */ + size_t size /**< Number of bytes to write (without the VoE header). */ + ); + +/** Start a VoE read operation. + * + * After this function has been called, the ecrt_voe_handler_execute() method + * must be called in every realtime cycle as long as it returns + * EC_REQUEST_BUSY. No other operation may be started while the handler is + * busy. + * + * The state machine queries the slave's send mailbox for new data to be send + * to the master. If no data appear within the EC_VOE_RESPONSE_TIMEOUT + * (defined in master/voe_handler.c), the operation fails. + * + * On success, the size of the read data can be determined via + * ecrt_voe_handler_data_size(), while the VoE header of the received data + * can be retrieved with ecrt_voe_handler_received_header(). + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_read( + ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Start a VoE read operation without querying the sync manager status. + * + * After this function has been called, the ecrt_voe_handler_execute() method + * must be called in every realtime cycle as long as it returns + * EC_REQUEST_BUSY. No other operation may be started while the handler is + * busy. + * + * The state machine queries the slave by sending an empty mailbox. The slave + * fills its data to the master in this mailbox. If no data appear within the + * EC_VOE_RESPONSE_TIMEOUT (defined in master/voe_handler.c), the operation + * fails. + * + * On success, the size of the read data can be determined via + * ecrt_voe_handler_data_size(), while the VoE header of the received data + * can be retrieved with ecrt_voe_handler_received_header(). + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_read_nosync( + ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Execute the handler. + * + * This method executes the VoE handler. It has to be called in every realtime + * cycle as long as it returns EC_REQUEST_BUSY. + * + * \return Handler state. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + */ +EC_PUBLIC_API ec_request_state_t ecrt_voe_handler_execute( + ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/***************************************************************************** + * Register request methods. + ****************************************************************************/ + +/** Access to the register request's data. + * + * This function returns a pointer to the request's internal memory. + * + * - After a read operation was successful, integer data can be evaluated + * using the EC_READ_*() macros as usual. Example: + * \code + * uint16_t value = EC_READ_U16(ecrt_reg_request_data(reg_request))); + * \endcode + * - If a write operation shall be triggered, the data have to be written to + * the internal memory. Use the EC_WRITE_*() macros, if you are writing + * integer data. Be sure, that the data fit into the memory. The memory size + * is a parameter of ecrt_slave_config_create_reg_request(). + * \code + * EC_WRITE_U16(ecrt_reg_request_data(reg_request), 0xFFFF); + * \endcode + * + * This method is meant to be called in realtime context (after master + * activation), but can also be used to initialize data before. + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal memory. + * + */ +EC_PUBLIC_API uint8_t *ecrt_reg_request_data( + const ec_reg_request_t *req /**< Register request. */ + ); + +/** Get the current state of the register request. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return Request state. + * + */ +EC_PUBLIC_API ec_request_state_t ecrt_reg_request_state( + const ec_reg_request_t *req /**< Register request. */ + ); + +/** Schedule an register write operation. + * + * \attention This method may not be called while ecrt_reg_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention The \a size parameter is truncated to the size given at request + * creation. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_reg_request + * too small. + */ +EC_PUBLIC_API int ecrt_reg_request_write( + ec_reg_request_t *req, /**< Register request. */ + uint16_t address, /**< Register address. */ + size_t size /**< Size to write. */ + ); + +/** Schedule a register read operation. + * + * \attention This method may not be called while ecrt_reg_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention The \a size parameter is truncated to the size given at request + * creation. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_reg_request + * too small. + */ +EC_PUBLIC_API int ecrt_reg_request_read( + ec_reg_request_t *req, /**< Register request. */ + uint16_t address, /**< Register address. */ + size_t size /**< Size to write. */ + ); + +/***************************************************************************** + * Bitwise read/write macros + ****************************************************************************/ + +/** Read a certain bit of an EtherCAT data byte. + * + * \param DATA EtherCAT data pointer + * \param POS bit position + */ +#define EC_READ_BIT(DATA, POS) ((*((uint8_t *) (DATA)) >> (POS)) & 0x01) + +/** Write a certain bit of an EtherCAT data byte. + * + * \param DATA EtherCAT data pointer + * \param POS bit position + * \param VAL new bit value + */ +#define EC_WRITE_BIT(DATA, POS, VAL) \ + do { \ + if (VAL) *((uint8_t *) (DATA)) |= (1 << (POS)); \ + else *((uint8_t *) (DATA)) &= ~(1 << (POS)); \ + } while (0) + +/***************************************************************************** + * Byte-swapping functions for user space + ****************************************************************************/ + +#ifndef __KERNEL__ + +#if __BYTE_ORDER == __LITTLE_ENDIAN + +#define le16_to_cpu(x) x +#define le32_to_cpu(x) x +#define le64_to_cpu(x) x + +#define cpu_to_le16(x) x +#define cpu_to_le32(x) x +#define cpu_to_le64(x) x + +#elif __BYTE_ORDER == __BIG_ENDIAN + +#define swap16(x) \ + ((uint16_t)( \ + (((uint16_t)(x) & 0x00ffU) << 8) | \ + (((uint16_t)(x) & 0xff00U) >> 8) )) +#define swap32(x) \ + ((uint32_t)( \ + (((uint32_t)(x) & 0x000000ffUL) << 24) | \ + (((uint32_t)(x) & 0x0000ff00UL) << 8) | \ + (((uint32_t)(x) & 0x00ff0000UL) >> 8) | \ + (((uint32_t)(x) & 0xff000000UL) >> 24) )) +#define swap64(x) \ + ((uint64_t)( \ + (((uint64_t)(x) & 0x00000000000000ffULL) << 56) | \ + (((uint64_t)(x) & 0x000000000000ff00ULL) << 40) | \ + (((uint64_t)(x) & 0x0000000000ff0000ULL) << 24) | \ + (((uint64_t)(x) & 0x00000000ff000000ULL) << 8) | \ + (((uint64_t)(x) & 0x000000ff00000000ULL) >> 8) | \ + (((uint64_t)(x) & 0x0000ff0000000000ULL) >> 24) | \ + (((uint64_t)(x) & 0x00ff000000000000ULL) >> 40) | \ + (((uint64_t)(x) & 0xff00000000000000ULL) >> 56) )) + +#define le16_to_cpu(x) swap16(x) +#define le32_to_cpu(x) swap32(x) +#define le64_to_cpu(x) swap64(x) + +#define cpu_to_le16(x) swap16(x) +#define cpu_to_le32(x) swap32(x) +#define cpu_to_le64(x) swap64(x) + +#endif + +#define le16_to_cpup(x) le16_to_cpu(*((uint16_t *)(x))) +#define le32_to_cpup(x) le32_to_cpu(*((uint32_t *)(x))) +#define le64_to_cpup(x) le64_to_cpu(*((uint64_t *)(x))) + +#endif /* ifndef __KERNEL__ */ + +/***************************************************************************** + * Read macros + ****************************************************************************/ + +/** Read an 8-bit unsigned value from EtherCAT data. + * + * \return EtherCAT data value + */ +#define EC_READ_U8(DATA) \ + ((uint8_t) *((uint8_t *) (DATA))) + +/** Read an 8-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S8(DATA) \ + ((int8_t) *((uint8_t *) (DATA))) + +/** Read a 16-bit unsigned value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_U16(DATA) \ + ((uint16_t) le16_to_cpup((void *) (DATA))) + +/** Read a 16-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S16(DATA) \ + ((int16_t) le16_to_cpup((void *) (DATA))) + +/** Read a 32-bit unsigned value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_U32(DATA) \ + ((uint32_t) le32_to_cpup((void *) (DATA))) + +/** Read a 32-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S32(DATA) \ + ((int32_t) le32_to_cpup((void *) (DATA))) + +/** Read a 64-bit unsigned value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_U64(DATA) \ + ((uint64_t) le64_to_cpup((void *) (DATA))) + +/** Read a 64-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S64(DATA) \ + ((int64_t) le64_to_cpup((void *) (DATA))) + +/***************************************************************************** + * Floating-point read functions and macros (userspace only) + ****************************************************************************/ + +#ifndef __KERNEL__ + +/** Read a 32-bit floating-point value from EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \return EtherCAT data value + */ +EC_PUBLIC_API float ecrt_read_real(const void *data); + +/** Read a 32-bit floating-point value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_REAL(DATA) ecrt_read_real(DATA) + +/** Read a 64-bit floating-point value from EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \return EtherCAT data value + */ +EC_PUBLIC_API double ecrt_read_lreal(const void *data); + +/** Read a 64-bit floating-point value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_LREAL(DATA) ecrt_read_lreal(DATA) + +#endif // ifndef __KERNEL__ + +/***************************************************************************** + * Write macros + ****************************************************************************/ + +/** Write an 8-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U8(DATA, VAL) \ + do { \ + *((uint8_t *)(DATA)) = ((uint8_t) (VAL)); \ + } while (0) + +/** Write an 8-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S8(DATA, VAL) EC_WRITE_U8(DATA, VAL) + +/** Write a 16-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U16(DATA, VAL) \ + do { \ + *((uint16_t *) (DATA)) = cpu_to_le16((uint16_t) (VAL)); \ + } while (0) + +/** Write a 16-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S16(DATA, VAL) EC_WRITE_U16(DATA, VAL) + +/** Write a 32-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U32(DATA, VAL) \ + do { \ + *((uint32_t *) (DATA)) = cpu_to_le32((uint32_t) (VAL)); \ + } while (0) + +/** Write a 32-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S32(DATA, VAL) EC_WRITE_U32(DATA, VAL) + +/** Write a 64-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U64(DATA, VAL) \ + do { \ + *((uint64_t *) (DATA)) = cpu_to_le64((uint64_t) (VAL)); \ + } while (0) + +/** Write a 64-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S64(DATA, VAL) EC_WRITE_U64(DATA, VAL) + +/***************************************************************************** + * Floating-point write functions and macros (userspace only) + ****************************************************************************/ + +#ifndef __KERNEL__ + +/** Write a 32-bit floating-point value to EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \param value new value + */ +EC_PUBLIC_API void ecrt_write_real(void *data, float value); + +/** Write a 32-bit floating-point value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_REAL(DATA, VAL) ecrt_write_real(DATA, VAL) + +/** Write a 64-bit floating-point value to EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \param value new value + */ +EC_PUBLIC_API void ecrt_write_lreal(void *data, double value); + +/** Write a 64-bit floating-point value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_LREAL(DATA, VAL) ecrt_write_lreal(DATA, VAL) + +#endif // ifndef __KERNEL__ + +/****************************************************************************/ + +#ifdef __cplusplus +} +#endif + +/****************************************************************************/ + +/** @} */ + +#endif diff --git a/dependency/x86/third_party/ethercat/v1.7.0/include/ectty.h b/dependency/x86/third_party/ethercat/v1.7.0/include/ectty.h new file mode 100644 index 00000000..3178d3ce --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/include/ectty.h @@ -0,0 +1,106 @@ +/***************************************************************************** + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT master userspace library. + * + * The IgH EtherCAT master userspace library is free software; you can + * redistribute it and/or modify it under the terms of the GNU Lesser General + * Public License as published by the Free Software Foundation; version 2.1 + * of the License. + * + * The IgH EtherCAT master userspace library is distributed in the hope that + * it will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with the IgH EtherCAT master userspace library. If not, see + * . + * + ****************************************************************************/ + +/** \file + * + * EtherCAT virtual TTY interface. + * + * \defgroup TTYInterface EtherCAT Virtual TTY Interface + * + * @{ + */ + +/****************************************************************************/ + +#ifndef __ECTTY_H__ +#define __ECTTY_H__ + +#include + +/***************************************************************************** + * Data types + ****************************************************************************/ + +struct ec_tty; +typedef struct ec_tty ec_tty_t; /**< \see ec_tty */ + +/** Operations on the virtual TTY interface. + */ +typedef struct { + int (*cflag_changed)(void *, tcflag_t); /**< Called when the serial + * settings shall be changed. The + * \a cflag argument contains the + * new settings. */ +} ec_tty_operations_t; + +/***************************************************************************** + * Global functions + ****************************************************************************/ + +/** Create a virtual TTY interface. + * + * \param ops Set of callbacks. + * \param cb_data Arbitrary data, that is passed to any callback. + * + * \return Pointer to the interface object, otherwise an ERR_PTR value. + */ +ec_tty_t *ectty_create( + const ec_tty_operations_t *ops, + void *cb_data + ); + +/***************************************************************************** + * TTY interface methods + ****************************************************************************/ + +/** Releases a virtual TTY interface. + */ +void ectty_free( + ec_tty_t *tty /**< TTY interface. */ + ); + +/** Reads data to send from the TTY interface. + * + * If there are data to send, they are copied into the \a buffer. At maximum, + * \a size bytes are copied. The actual number of bytes copied is returned. + * + * \return Number of bytes copied. + */ +unsigned int ectty_tx_data( + ec_tty_t *tty, /**< TTY interface. */ + uint8_t *buffer, /**< Buffer for data to transmit. */ + size_t size /**< Available space in \a buffer. */ + ); + +/** Pushes received data to the TTY interface. + */ +void ectty_rx_data( + ec_tty_t *tty, /**< TTY interface. */ + const uint8_t *buffer, /**< Buffer with received data. */ + size_t size /**< Number of bytes in \a buffer. */ + ); + +/****************************************************************************/ + +/** @} */ + +#endif diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/cmake/ethercat/ethercat-config.cmake b/dependency/x86/third_party/ethercat/v1.7.0/lib/cmake/ethercat/ethercat-config.cmake new file mode 100644 index 00000000..49f8e81c --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/cmake/ethercat/ethercat-config.cmake @@ -0,0 +1,43 @@ +#---------------------------------------------------------------------------- +# +# Copyright (C) 2021 Bjarne von Horn, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT Master. +# +# The IgH EtherCAT Master is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License version 2, as +# published by the Free Software Foundation. +# +# The IgH EtherCAT Master is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with the IgH EtherCAT Master; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +# +# vim: tw=78 +# +#---------------------------------------------------------------------------- + + +find_library(EtherCAT_LIBRARY + NAMES ethercat + PATHS /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib +) + +find_path(EtherCAT_INCLUDE_DIR + NAMES ecrt.h + PATHS /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/include +) + +mark_as_advanced(EtherCAT_LIBRARY EtherCAT_INCLUDE_DIR) + +if(NOT TARGET EtherLab::ethercat) + add_library(EtherLab::ethercat SHARED IMPORTED) + set_target_properties(EtherLab::ethercat PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${EtherCAT_INCLUDE_DIR}" + IMPORTED_LOCATION "${EtherCAT_LIBRARY}" + ) +endif() diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a new file mode 100644 index 00000000..6ee310dc Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.la b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.la new file mode 100755 index 00000000..056b3b88 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.la @@ -0,0 +1,41 @@ +# libethercat.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-15build2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libethercat.so.1' + +# Names of this library. +library_names='libethercat.so.1.2.0 libethercat.so.1 libethercat.so' + +# The name of the static archive. +old_library='libethercat.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags='' + +# Libraries that this one depends upon. +dependency_libs='' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libethercat. +current=3 +age=2 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib' diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so new file mode 120000 index 00000000..d85df380 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so @@ -0,0 +1 @@ +libethercat.so.1.2.0 \ No newline at end of file diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1 b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1 new file mode 120000 index 00000000..d85df380 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1 @@ -0,0 +1 @@ +libethercat.so.1.2.0 \ No newline at end of file diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0 b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0 new file mode 100755 index 00000000..58bf28d2 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0 differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/devices/ec_generic.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/devices/ec_generic.ko new file mode 100644 index 00000000..8af80416 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/devices/ec_generic.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/examples/mini/ec_mini.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/examples/mini/ec_mini.ko new file mode 100644 index 00000000..0c4e304e Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/examples/mini/ec_mini.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/master/ec_master.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/master/ec_master.ko new file mode 100644 index 00000000..a5bc2c41 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/master/ec_master.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/devices/ec_generic.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/devices/ec_generic.ko new file mode 100644 index 00000000..64be6a8d Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/devices/ec_generic.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/examples/mini/ec_mini.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/examples/mini/ec_mini.ko new file mode 100644 index 00000000..1e00b08c Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/examples/mini/ec_mini.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/master/ec_master.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/master/ec_master.ko new file mode 100644 index 00000000..347595af Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-136-generic/ethercat/master/ec_master.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/pkgconfig/libethercat.pc b/dependency/x86/third_party/ethercat/v1.7.0/lib/pkgconfig/libethercat.pc new file mode 100644 index 00000000..de20cf9f --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/pkgconfig/libethercat.pc @@ -0,0 +1,34 @@ +# +# pkgconfig file for ethercat library +# +# Copyright 2021 Bjarne von Horn (vh at igh dot de) +# +# This file is part of the ethercat library. +# +# The ethercat library is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or (at your +# option) any later version. +# +# The ethercat library is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public +# License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with the ethercat library. If not, see . +# +# vim: tw=78 noexpandtab +# + +prefix=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0 +exec_prefix=${prefix} +libdir=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib +includedir=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/include + +Name: libethercat +Description: Client support library for the EtherCAT Master +URL: http://www.etherlab.org +Version: 1.7.0 +Libs: -L${libdir} -lethercat +Cflags: -I${includedir} diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/systemd/system/ethercat.service b/dependency/x86/third_party/ethercat/v1.7.0/lib/systemd/system/ethercat.service new file mode 100644 index 00000000..116b36cf --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/systemd/system/ethercat.service @@ -0,0 +1,38 @@ +# +# EtherCAT master kernel modules +# + +[Unit] +Description=EtherCAT Master Kernel Modules + +# Fine tuning of the startup dependencies below are recommended +# to provide a reliable startup routine. +# The dependencies below can be either uncommented after copying +# this file to /etc/systemd/system or by creating overrides: +# Copy the needed dependencies into +# /etc/systemd/system/ethercat.service.d/50-dependencies.conf +# in a [Unit] section. + +# +# Uncomment this, if the generic Ethernet driver is used. It assures, that the +# network interfaces are configured, before the master starts. +# +#Requires=network.target # Stop master, if network is stopped +#After=network.target # Start master, after network is ready + +# +# Uncomment this, if a native Ethernet driver is used. It assures, that the +# network interfaces are configured, after the Ethernet drivers have been +# replaced. Otherwise, the networking configuration tools could be confused. +# +#Before=network-pre.target +#Wants=network-pre.target + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl start +ExecStop=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl stop + +[Install] +WantedBy=multi-user.target diff --git a/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl b/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl new file mode 100755 index 00000000..b455d52b --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl @@ -0,0 +1,253 @@ +#!/bin/bash + +#------------------------------------------------------------------------------ +# +# Start script for EtherCAT to use with systemd +# +# Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT Master. +# +# The IgH EtherCAT Master is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License version 2, as +# published by the Free Software Foundation. +# +# The IgH EtherCAT Master is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with the IgH EtherCAT Master; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +# +# vim: expandtab sw=4 tw=78 +# +#------------------------------------------------------------------------------ + +LSMOD="/sbin/lsmod" +MODPROBE="/sbin/modprobe" +RMMOD="/sbin/rmmod" +MODINFO="/sbin/modinfo" +IP="/sbin/ip" + +ETHERCAT="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat" + +#------------------------------------------------------------------------------ + +if [ "$1" = "-c" ]; then + ETHERCAT_CONFIG="$2" + COMMAND="$3" +else + ETHERCAT_CONFIG="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf" + COMMAND="$1" +fi + +#------------------------------------------------------------------------------ + +if [ ! -r ${ETHERCAT_CONFIG} ]; then + echo ${ETHERCAT_CONFIG} not existing; + exit 6 +fi + +# shellcheck source=/etc/ethercat.conf +. ${ETHERCAT_CONFIG} + +#------------------------------------------------------------------------------ + +is_mac_address() { + local x='[0-9a-fA-F]' + echo "$1" | grep -qE "^($x$x:){5}$x$x\$" - +} + +#------------------------------------------------------------------------------ + +parse_mac_address() { + local DEVICENAMETOMAC + if [ -z "${1}" ] || is_mac_address "${1}"; then + MAC="${1}" + else + DEVICENAMETOMAC=$("${IP}" address show dev "${1}" | + awk '/link\/ether/ { print $2; }') + if is_mac_address "${DEVICENAMETOMAC}"; then + MAC="${DEVICENAMETOMAC}" + else + echo Invalid MAC address or interface name \""${1}"\" \ + in ${ETHERCAT_CONFIG} + exit 1 + fi + fi +} + +#------------------------------------------------------------------------------ + +case "$COMMAND" in + +start) + # bring up all updown interfaces before anything else + for interface in $UPDOWN_INTERFACES; do + $IP link set dev $interface up + done + + # construct DEVICES and BACKUPS from configuration variables + DEVICES="" + BACKUPS="" + MASTER_INDEX=0 + + while true; do + DEVICE=$(eval echo "\${MASTER${MASTER_INDEX}_DEVICE}") + BACKUP=$(eval echo "\${MASTER${MASTER_INDEX}_BACKUP}") + if [ -z "${DEVICE}" ]; then break; fi + + if [ ${MASTER_INDEX} -gt 0 ]; then + DEVICES=${DEVICES}, + BACKUPS=${BACKUPS}, + fi + + parse_mac_address "${DEVICE}" + DEVICES=${DEVICES}${MAC} + + parse_mac_address "${BACKUP}" + BACKUPS=${BACKUPS}${MAC} + + MASTER_INDEX=$((${MASTER_INDEX} + 1)) + done + + if [ -z "${DEVICES}" ]; then + echo "ERROR: No network cards for EtherCAT specified." + echo -n "Please edit ${ETHERCAT_CONFIG} with root permissions" + echo -n " and set MASTER0_DEVICE variable to either a " + echo "network interface name (like eth0) or to a MAC address." + exit 1 + fi + + MODULE_PARAMS=( + main_devices="${DEVICES}" + backup_devices="${BACKUPS}" + ) + + if [ -n "$SII_CACHING" ]; then + MODULE_PARAMS+=(sii_caching="$SII_CACHING") + fi + + # load master module + if ! ${MODPROBE} ${MODPROBE_FLAGS} ec_master "${MODULE_PARAMS[@]}"; then + exit 1 + fi + + LOADED_MODULES=ec_master + + # check for modules to replace + for MODULE in ${DEVICE_MODULES}; do + ECMODULE=ec_${MODULE} + if ! ${MODINFO} "${ECMODULE}" > /dev/null; then + continue # ec_* module not found + fi + + if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then + # unload standard module and check if unloading was successful + ${RMMOD} "${MODULE}" 2> /dev/null || true + if ${LSMOD} | grep "^${MODULE//-/_} " > /dev/null; then + # could not unload module + ${RMMOD} ${LOADED_MODULES} + exit 1 + fi + fi + + if ! ${MODPROBE} ${MODPROBE_FLAGS} "${ECMODULE}"; then + if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then + ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" # try to restore + fi + ${RMMOD} ${LOADED_MODULES} + exit 1 + fi + + LOADED_MODULES="${ECMODULE} ${LOADED_MODULES}" + done + + exit 0 + ;; + +#------------------------------------------------------------------------------ + +stop) + # unload EtherCAT device modules + for MODULE in ${DEVICE_MODULES} master; do + ECMODULE=ec_${MODULE} + if ! ${LSMOD} | grep -q "^${ECMODULE//-/_} "; then + continue # ec_* module not loaded + fi + if ! ${RMMOD} "${ECMODULE}"; then + exit 1 + fi; + done + + sleep 1 + + # load standard modules again + for MODULE in ${DEVICE_MODULES}; do + if [ "${MODULE}" == "generic" ] || [ "${MODULE}" == "ccat" ]; then + continue + fi + ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" + done + + # bring down all updown interfaces + for interface in $UPDOWN_INTERFACES; do + $IP link set dev $interface down + done + + exit 0 + ;; + +#------------------------------------------------------------------------------ + +restart) + $0 stop || exit 1 + sleep 1 + $0 start + ;; + +#------------------------------------------------------------------------------ + +status) + echo "Checking for EtherCAT master 1.7.0 " + + # count masters in configuration file + MASTER_COUNT=0 + while true; do + DEVICE=$(eval echo "\${MASTER${MASTER_COUNT}_DEVICE}") + if [ -z "${DEVICE}" ]; then break; fi + MASTER_COUNT=$((${MASTER_COUNT} + 1)) + done + + RESULT=0 + + for i in $(seq 0 "$((${MASTER_COUNT} - 1))"); do + echo -n "Master${i} " + + # Check if the master is in idle or operation phase + ${ETHERCAT} master --master "${i}" 2>/dev/null | \ + grep -qE 'Phase:[[:space:]]*Idle|Phase:[[:space:]]*Operation' + EXITCODE=$? + + if [ ${EXITCODE} -eq 0 ]; then + echo " running" + else + echo " dead" + RESULT=1 + fi + done + + exit ${RESULT} + ;; + +#------------------------------------------------------------------------------ + +*) + echo "USAGE: $0 [-c path/to/ethercat.conf] {start|stop|restart|status}" + exit 1 + ;; +esac + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/share/bash-completion/completions/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/share/bash-completion/completions/ethercat new file mode 100644 index 00000000..3734b55b --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/share/bash-completion/completions/ethercat @@ -0,0 +1,80 @@ +# Copyright (C) 2022 Bjarne von Horn, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT master userspace library. +# +# The IgH EtherCAT master userspace library is free software; you can +# redistribute it and/or modify it under the terms of the GNU Lesser General +# Public License as published by the Free Software Foundation; version 2.1 +# of the License. +# +# The IgH EtherCAT master userspace library is distributed in the hope that +# it will be useful, but WITHOUT ANY WARRANTY; without even the implied +# warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with the IgH EtherCAT master userspace library. If not, see +# . +# + +_ethercat_completions() +{ + local ethercat_commands="alias config crc cstruct data debug domains download eoe foe_read foe_write graph master pdos reg_read reg_write rescan sdos sii_read sii_write slaves soe_read soe_write states upload version xml" + local options="--help --force --quiet --verbose --master " + if [ "$COMP_CWORD" -eq 1 ] ; then + COMPREPLY=($(compgen -W "$ethercat_commands --help" -- "${COMP_WORDS[1]}")) + elif [[ "${COMP_WORDS[1]}" != "--help" && ! "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-a|-p|--alias|--position$ ]] ; then + case "${COMP_WORDS[1]}" in + "alias" | "config" | "cstruct" | "slaves" | "sdos" | "sii_read" | "upload" | "xml") + options+="--alias --position" + ;; + "crc") + options+="reset" + ;; + "debug") + options+="0 1 2" + ;; + "domains") + options+="--domain" + ;; + "download" | "reg_read" | "soe_read" | "soe_write") + if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-t|--type$ ]] ; then + options="bool int8 int16 int32 int64 uint8 uint16 uint32 uint64 float double string octet_string unicode_string sm8 sm16 sm32 sm64" + else + options+="--alias --position --type" + fi + ;; + "foe_read" | "foe_write") + if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-o|--output-file$ ]] ; then + COMPREPLY=($(compgen -o filenames -A file -- "${COMP_WORDS[$COMP_CWORD]}")) + else + options+="--alias --position --output-file" + COMPREPLY=($(compgen -o filenames -A file -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}")) + fi + return + ;; + "graph") + options+="DC CRC" + ;; + "pdos") + if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-s|--skin$ ]] ; then + options="default etherlab" + else + options+="--alias --position --skin" + fi + ;; + "sii_write") + options+="--alias --position" + COMPREPLY=($(compgen -o filenames -A file -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}")) + return + ;; + "states") + options+="--alias --position INIT PREOP BOOT SAFEOP OP" + ;; + + esac + COMPREPLY+=($(compgen -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}")) + fi +} + +complete -F _ethercat_completions ethercat diff --git a/docs/EYOU_ServoModule_ECAT_V145.xml b/docs/EYOU_ServoModule_ECAT_V145.xml new file mode 100644 index 00000000..92f89763 --- /dev/null +++ b/docs/EYOU_ServoModule_ECAT_V145.xml @@ -0,0 +1,5560 @@ + + + + #x1097 + Jiangsu Yiyou Robot Technology Co., Ltd. + 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 + + + + + ServoDrive + Servo Drives + 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 + + + + + EYOU_ServoModule_V145 + EYOU_ServoModule_ECAT_V145 + + + + 2000 + 9000 + 5000 + 200 + + + + + 100 + 2000 + + + + ServoDrive + + + 402 + + + + + + BIT2 + 2 + + + + BOOL + 1 + + + + DINT + 32 + + + + INT + 16 + + + + SINT + 8 + + + + UDINT + 32 + + + + UINT + 16 + + + + USINT + 8 + + + + REAL + 32 + + + + ARRAY [0..3] OF BYTE + USINT + 32 + + 0 + 4 + + + + + STRING(12) + 96 + + + + STRING(10) + 80 + + + + DT1010 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Store All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Store Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Store Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1011 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Restore All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Restore Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Restore Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1018 + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Vendor ID + UDINT + 32 + 16 + + ro + o + + + + 2 + Product code + UDINT + 32 + 48 + + ro + o + + + + 3 + Revision + UDINT + 32 + 80 + + ro + o + + + + 4 + Serial number + UDINT + 32 + 112 + + ro + o + + + + + DT1C00ARR + USINT + 32 + + 1 + 4 + + + + DT1C00 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DT1C00ARR + 32 + 16 + + ro + o + + + + + + DT10F1 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Local Error Reaction + UDINT + 32 + 16 + + rw + o + + + + 2 + Sync Error Counter Limit + UDINT + 32 + 48 + + rw + o + + + + + + DT1C32 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + + DT1C33 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + DT1600 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1601 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1602 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A00 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1A01 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A02 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1C12ARR + UINT + 32 + + 1 + 2 + + + + DT1C12 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C12ARR + 32 + 16 + + rw + o + + + + + DT1C13ARR + UINT + 32 + + 1 + 2 + + + + DT1C13 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C13ARR + 32 + 16 + + rw + o + + + + + DT2001 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Eu Node Id + USINT + 8 + 16 + + rw + o + + + + 2 + Eu Can BitRate + UINT + 16 + 32 + + rw + o + + + + + DT2010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Current Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Current Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Current Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Current Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2012 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Velocity Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Velocity Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Velocity Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Velocity Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2013 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Position Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Position Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Position Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Position Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2014 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuBrakeControl + USINT + 8 + 16 + + rw + o + + + + 2 + EuBrakeState + USINT + 8 + 32 + + ro + o + + + + 3 + EuBrakeAutoState + USINT + 8 + 48 + + rw + o + + + + + DT2016 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Servo Temperture + SINT + 8 + 16 + + ro + o + + + + 2 + EuHighTemperatureLimit + SINT + 8 + 32 + + rw + o + + + + 3 + EuHighTemperatureWindowsTime + UINT + 16 + 48 + + rw + o + + + + + DT2020 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuMotorBlockTorque + UINT + 16 + 16 + + ro + o + + + + 2 + EuMotorBlockTime + UINT + 16 + 32 + + rw + o + + + + 3 + EuMotorBlockVelocity + UDINT + 32 + 48 + + rw + o + + + + + DT2021 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVolecityFollowingErrorWindows + UDINT + 32 + 16 + + rw + o + + + + 2 + EuVolecityFollowingErrorTime + UINT + 16 + 48 + + rw + o + + + + + DT202D + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuUnderVoltageThreshold + UDINT + 32 + 16 + + rw + o + + + + 2 + EuUnderVoltageTime + UDINT + 32 + 48 + + rw + o + + + + 3 + EuOverVoltageThreshold + UDINT + 32 + 80 + + rw + o + + + + 4 + EuOverVoltageTime + UDINT + 32 + 112 + + rw + o + + + + + DT202E + 128 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVelocityFeedforwardControlSelect + INT + 16 + 16 + + rw + o + + + + 2 + EuVelocityFeedforwardFilterTimeConst + INT + 16 + 32 + + rw + o + + + + 3 + EuVelocityFeedforwardGain + INT + 16 + 48 + + rw + o + + + + 4 + EuVelocityFeedforwardOriginalValue + DINT + 32 + 64 + + ro + o + + + + 4 + EuVelocityFeedforwardFilteredValue + DINT + 32 + 96 + + ro + o + + + + + DT210F + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + FocCurBiasA + UINT + 16 + 16 + + ro + o + + + + 2 + FocCurBiasB + UINT + 16 + 32 + + ro + o + + + + 3 + FocCurBiasC + UINT + 16 + 48 + + ro + o + + + + + DT607D + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Min position limit + DINT + 32 + 16 + + rw + o + + + + 2 + Max position limit + DINT + 32 + 48 + + rw + o + + + + + DT6091 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Motor revolutions + UDINT + 32 + 16 + + ro + o + + + + 2 + Shaft revolutions + UDINT + 32 + 48 + + rw + o + + + + + DT60C1 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation data record + DINT + 32 + 16 + + rw + o + + + + + DT60C2 + 32 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation period + USINT + 8 + 16 + + rw + o + + + + 2 + Interpolation Index + SINT + 8 + 24 + + rw + o + + + + + DT60FF + 32 + + 0 + Target Velocity + UDINT + 32 + 0 + + rw + o + R + + + + + DT6502 + 32 + + 0 + Supported Drive Modes + UDINT + 32 + 0 + + ro + o + + + + + DTF000 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Module index distance + UINT + 16 + 16 + + ro + o + + + + 2 + Maximum number of modules + UINT + 16 + 32 + + ro + o + + + + + DTF010ARR + UDINT + 64 + + 1 + 2 + + + + DTF010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF010ARR + 64 + 16 + + ro + o + + + + + + DTF030ARR + UDINT + 64 + + 1 + 2 + + + + DTF030 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF030ARR + 64 + 16 + + ro + o + + + + + + DTF050ARR + UDINT + 64 + + 1 + 2 + + + + DTF050 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF050ARR + 64 + 16 + + ro + o + + + + + + + + #x1000 + Device type + UDINT + 32 + + 92010200 + + + ro + m + + + + #x1001 + Error register + USINT + 8 + + 00 + + + ro + o + + + + #x1008 + Device name + STRING(12) + 96 + + 4575504878782d787878 + + + ro + o + + + + #x1009 + Hardware version + STRING(10) + 80 + + 56332e3046343035 + + + ro + o + + + + #x100a + Software version + STRING(10) + 80 + + 56313433 + + + ro + o + + + + #x1c00 + Sync manager type + DT1C00 + 48 + + + SubIndex 000 + + 04 + + + + SubIndex 001 + + 01 + + + + SubIndex 002 + + 02 + + + + SubIndex 003 + + 03 + + + + SubIndex 004 + + 04 + + + + + ro + o + + + + #x1010 + Store Parameters + DT1010 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1011 + Restore Default Parameters + DT1011 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1018 + Identity + DT1018 + 144 + + + SubIndex 000 + + 04 + + + + Vendor ID + + 9710 + + + + Product code + + 0624 + + + + Revision + + 0002 + + + + Serial number + + 00000000 + + + + + ro + o + + + + #x10F1 + Error Settings + DT10F1 + 80 + + + SubIndex 000 + + 04 + + + + Local Error Reaction + + 01 + + + + Sync Error Counter Limit + + 04 + + + + + ro + o + + + + #x1c32 + SM output parameter + DT1C32 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 0100 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c33 + SM input parameter + DT1C33 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 2200 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c12 + RxPDO assign + DT1C12 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 0016 + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1c13 + TxPDO assign + DT1C13 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 001a + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1600 + csp/csv/cst RxPDO + DT1600 + 336 + + + SubIndex 000 + + 00 + 10 + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + 7st Output Object to be mapped + + #x00000000 + + + + 8st Output Object to be mapped + + #x00000000 + + + + 9st Output Object to be mapped + + #x00000000 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1601 + pp/pv/pt RxPDO + DT1601 + 336 + + + SubIndex 000 + + #x0A + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60830020 + + + + 6st Output Object to be mapped + + #x60840020 + + + + 7st Output Object to be mapped + + #x60810020 + + + + 8st Output Object to be mapped + + #x60870010 + + + + 9st Output Object to be mapped + + #x60600008 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1602 + pv RxPDO + DT1602 + 336 + + + SubIndex 000 + + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x60FF0020 + + + + 3st Output Object to be mapped + + #x60830020 + + + + 4st Output Object to be mapped + + #x60840020 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a00 + csp/csv/cst TxPDO + DT1A00 + 336 + + + SubIndex 000 + + 00 + 10 + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770010 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + 8st Input Object to be mapped + + #x00000000 + + + + 9st Input Object to be mapped + + #x00000000 + + + + 10st Input Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1a01 + pp/pv/pt TxPDO + DT1A01 + 336 + + + SubIndex 000 + + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770020 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a02 + pv TxPDO + DT1A02 + 336 + + + SubIndex 000 + + #x05 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x606C0020 + + + + 3st Input Object to be mapped + + #x60610008 + + + + 4st Input Object to be mapped + + #x603F0010 + + + + 5st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x2000 + Eu Motor SN + UDINT + 32 + + + Eu Motor SN + + 00 + + + + + rw + o + + + + #x2001 + Servo Config + DT2001 + 48 + + + SubIndex 000 + + 02 + + + + Eu Node Id + + 01 + + + + Eu Can BitRate + + 1000 + + + + + ro + o + + + + #x2002 + Motor Para + UDINT + 32 + + + Motor Para + + 00 + + + + + rw + o + + + + #x2003 + Soft Limit State + UDINT + 32 + + + Soft Limit State + + 00 + + + + + rw + o + + + + #x2004 + EuCommMode + USINT + 8 + + + EuCommMode + + 00 + + + + + rw + o + + + + #x2010 + Current Loop Pi + DT2010 + 80 + + + SubIndex 000 + + 04 + + + + Current Loop Kp Default + + 0040 + + + + Current Loop Ki Default + + 0001 + + + + Current Loop Kp + + 0004 + + + + Current Loop Ki + + 0001 + + + + + ro + o + + + + #x2012 + Velocity Loop Pi + DT2012 + 80 + + + SubIndex 000 + + 04 + + + + Velocity Loop Kp Default + + D007 + + + + Velocity Loop Ki Default + + 64 + + + + Velocity Loop Kp + + D007 + + + + Velocity Loop Ki + + 64 + + + + + ro + o + + + + #x2013 + Position Loop Pi + DT2013 + 80 + + + SubIndex 000 + + 04 + + + + Position Loop Kp Default + + D007 + + + + Position Loop Ki Default + + 64 + + + + Position Loop Kp + + D007 + + + + Position Loop Ki + + 64 + + + + + ro + o + + + + #x2014 + brake control + DT2014 + 64 + + + SubIndex 000 + + 03 + + + + EuBrakeControl + + 00 + + + + EuBrakeState + + 00 + + + + EuBrakeAutoState + + 01 + + + + + ro + o + + + + #x2015 + Eu Pwm Inv + USINT + 8 + + + Eu Pwm Inv + + 00 + + + + + ro + o + + + + #x2016 + Servo Temperture + DT2016 + 64 + + + SubIndex 000 + + 03 + + + + EuTemperature + + 19 + + + + EuHighTemperatureLimit + + 55 + + + + EuHighTemperatureWindowsTime + + B80B + + + + + ro + o + + + + #x2017 + EuVelocityIntLimit + UINT + 16 + + + EuVelocityIntLimit + + D007 + + + + + rw + o + + + + #x2020 + Motor Block + DT2020 + 80 + + + SubIndex 000 + + 03 + + + + EuMotorBlockTorque + + E803 + + + + EuMotorBlockTime + + B80B + + + + EuMotorBlockVelocity + + 1027 + + + + + ro + o + + + + #x2021 + velocity floowing error + DT2021 + 64 + + + SubIndex 000 + + 02 + + + + EuVolecityFollowingErrorWindows + + A08601 + + + + EuVolecityFollowingErrorTime + + 0BB8 + + + + + ro + o + + + + #x2022 + torque window + UINT + 16 + + + torque window + + 0A + + + + + rw + o + + + + #x2023 + Torque window time + UINT + 16 + + + Torque window time + + 01 + + + + + rw + o + + + + #x2024 + EuOverSpeedThreshold + UDINT + 32 + + + EuOverSpeedThreshold + + AAAA42 + + + + + rw + o + + + + #x2025 + EuOverSpeedTime + UINT + 16 + + + EuOverSpeedTime + + 64 + + + + + rw + o + + + + #x2026 + EuBrakeDelayTime + USINT + 8 + + + EuBrakeDelayTime + + 64 + + + + + rw + o + + + + #x2027 + EuAutoMagnetAngleAlignmentFlag + UDINT + 32 + + + EuAutoMagnetAngleAlignmentFlag + + 00 + + + + + rw + o + + + + #x2028 + EuI2tLimit + UINT + 16 + + + EuI2tLimit + + 64 + + + + + rw + o + + + + #x2029 + EuI2tValue + UINT + 16 + + + EuI2tValue + + 00 + + + + + ro + o + + + + #x202A + EuFirstEncoderValue + DINT + 32 + + + EuFirstEncoderValue + + 00 + + + + + ro + o + + + + #x202B + EuSecondEncoderValue + DINT + 32 + + + EuSecondEncoderValue + + 00 + + + + + ro + o + + + + #x202C + EuThetaBias + DINT + 32 + + + EuThetaBias + + 00 + + + + + ro + o + + + + #x202D + Eu Voltage Threshold + DT202D + 144 + + + SubIndex 000 + + 04 + + + + EuUnderVoltageThreshold + + 283707 + + + + EuUnderVoltageTime + + 05 + + + + EuOverVoltageThreshold + + A00901 + + + + EuOverVoltageTime + + 05 + + + + + ro + o + + + + #x202E + Eu Velocity Feedforward + DT202E + 128 + + + SubIndex 000 + + 05 + + + + EuVelocityFeedforwardControlSelect + + 01 + + + + EuVelocityFeedforwardFilterTimeConst + + 00 + + + + EuVelocityFeedforwardGain + + 00 + + + + EuVelocityFeedforwardOriginalValue + + 00 + + + + EuVelocityFeedforwardFilteredValue + + 00 + + + + + ro + o + + + + #x2100 + EuDisableFpgaMu150Spi + DINT + 32 + + + EuDisableFpgaMu150Spi + + 00 + + + + + rw + o + + + + #x2104 + ServoMagnetAngleAlignment + UDINT + 32 + + + ServoMagnetAngleAlignment + + 00 + + + + + rw + o + + + + #x2109 + EuGoToBoot + DINT + 32 + + + EuGoToBoot + + 00 + + + + + rw + o + + + + #x210A + EuCompStart + DINT + 32 + + + EuCompStart + + 00 + + + + + rw + o + + + + #x210B + EuCompEn + DINT + 32 + + + EuCompEn + + 00 + + + + + rw + o + + + + #x210C + EuCompBias + DINT + 32 + + + EuCompBias + + 00 + + + + + rw + o + + + + #x210D + EuCompState + DINT + 32 + + + EuCompState + + 00 + + + + + rw + o + + + + #x210E + EuFocCurBiasSet + USINT + 8 + + + EuFocCurBiasSet + + 00 + + + + + rw + o + + + + #x210F + Foc Cur Bias + DT210F + 144 + + + SubIndex 000 + + 03 + + + + FocCurBiasA + + 00 + + + + FocCurBiasB + + 00 + + + + FocCurBiasC + + 00 + + + + + ro + o + + + + #x2110 + EuTorqueFactor + UINT + 16 + + + EuTorqueFactor + + E803 + + + + + rw + o + + + + #x2201 + EuSysOutPulseStep + UDINT + 32 + + + EuSysOutPulseStep + + 00 + + + + + rw + o + + + + #x2301 + Position Velocity Mix Position Kp + UINT + 16 + + + Position Velocity Mix Position Kp + + 00 + + + + + rw + o + + + + #x2302 + Position Velocity Mix Velocity Kd + UINT + 16 + + + Position Velocity Mix Velocity Kd + + 00 + + + + + rw + o + + + + #x603F + Error Code + UINT + 16 + + + Error Code + + 00 + + + + + ro + o + T + + + + #x6040 + Control Word + UINT + 16 + + + Control Word + + 00 + + + + + rw + o + R + + + + #x6041 + Status Word + UINT + 16 + + + Status Word + + 00 + + + + + ro + o + T + + + + #x605A + Quickstop Option Code + INT + 16 + + + Quickstop Option Code + + 02 + + + + + rw + o + + + + #x605B + Shutdown Option Code + INT + 16 + + + Shutdown Option Code + + 00 + + + + + rw + o + + + + #x605C + Disable Operation Option Code + INT + 16 + + + Disable Operation Option Code + + 01 + + + + + rw + o + + + + #x605D + Halt option code + INT + 16 + + + Halt option code + + 01 + + + + + rw + o + + + + #x605E + Fault Reaction Code + INT + 16 + + + Fault Reaction Code + + 02 + + + + + rw + o + + + + #x6060 + Modes of Operation + SINT + 8 + + + Modes of Operation + + 00 + + + + + rw + o + R + + + + #x6061 + Modes of Operation Display + SINT + 8 + + + Modes of Operation Display + + 00 + + + + + ro + o + T + + + + #x6062 + Position demannd value + DINT + 32 + + + Position demannd value + + 00 + + + + + ro + o + T + + + + #x6064 + Position Actual Value + DINT + 32 + + + Position Actual Value + + 00 + + + + + ro + o + T + + + + #x6065 + Maximal following error + UDINT + 32 + + + Maximal following error + + E09304 + + + + + rw + o + + + + #x6067 + Position window + UDINT + 32 + + + Position window + + 64 + + + + + rw + o + + + + #x6068 + Position window time + UINT + 16 + + + Position window time + + 01 + + + + + rw + o + + + + #x606B + Velocity demand value + DINT + 32 + + + Velocity demand value + + 00 + + + + + ro + o + T + + + + #x606C + Velocity Actual Value + DINT + 32 + + + Velocity Actual Value + + 00 + + + + + ro + o + T + + + + #x606D + Velocity window + UINT + 16 + + + Velocity window + + 1027 + + + + + ro + o + + + + #x606E + Velocity window time + UINT + 16 + + + Velocity window time + + 01 + + + + + ro + o + + + + #x606F + Velocity threshold + UINT + 16 + + + Velocity threshold + + E803 + + + + + ro + o + + + + #x6070 + Velocity threshold time + UINT + 16 + + + Velocity threshold time + + 01 + + + + + ro + o + + + + #x6071 + Target Torque + INT + 16 + + + Target Torque + + 00 + + + + + rw + o + + + + #x6072 + Max Torque + UINT + 16 + + + Max Torque + + D007 + + + + + rw + o + + + + #x6074 + Torque demand value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6076 + Motor rated torque + UDINT + 32 + + + Motor rated torque + + 00 + + + + + rw + o + + + + #x6077 + Torque Actual Value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6078 + Current actual value + INT + 16 + + + Current actual value + + 00 + + + + + ro + o + T + + + + #x6079 + DC link circuit voltage + UDINT + 32 + + + DC link circuit voltage + + 00 + + + + + rw + o + + + + #x607A + Target Position + DINT + 32 + + + Target Position + + 00 + + + + + rw + o + R + + + + #x607C + Home Offset + DINT + 32 + + + Home Offset + + 00 + + + + + rw + o + + + + #x607D + Software Position Limit + DT607D + 80 + + + SubIndex 000 + + 02 + + + + Min position limit + + 006CCA88 + + + + Max position limit + + 00943577 + + + + + ro + o + + + + #x607F + Max Profile Velocity + UDINT + 32 + + + Max Profile Velocity + + 00 + + + + + rw + o + + + + #x6081 + Profile Velocity + UDINT + 32 + + + Profile Velocity + + 00 + + + + + rw + o + R + + + + #x6083 + Profile Acceleration + UDINT + 32 + + + Profile Acceleration + + 00 + + + + + rw + o + R + + + + #x6084 + Profile Deceleration + UDINT + 32 + + + Profile Deceleration + + 00 + + + + + rw + o + R + + + + #x6085 + Quickstop Declaration + DINT + 32 + + + Quickstop Declaration + + 00 + + + + + rw + o + + + + #x6087 + Torque slope + UDINT + 32 + + + Torque slope + + E803 + + + + + rw + o + + + + #x6091 + Gear ratio + DT6091 + 80 + + + SubIndex 000 + + 02 + + + + Motor revolutions + + 000051 + + + + Shaft revolutions + + 000051 + + + + + ro + o + + + + #x60B1 + Velocity Offset + DINT + 32 + + + Velocity Offset + + 00 + + + + + rw + o + + + + #x60B2 + Torque Offset + INT + 16 + + + Torque Offset + + 00 + + + + + rw + o + + + + #x60C1 + Interpolation Data Record + DT60C1 + 48 + + + SubIndex 000 + + 01 + + + + Interpolation data record + + 00 + + + + + ro + o + + + + #x60C2 + Interpolation Time Period + DT60C2 + 32 + + + SubIndex 000 + + 02 + + + + Interpolation period + + 01 + + + + Interpolation Index + + -3 + + + + + ro + o + + + + #x60F4 + Following error actual value + DINT + 32 + + + Following error actual value + + 00 + + + + + ro + o + T + + + + #x60FF + Target Velocity + DINT + 32 + + + Target Velocity + + 00 + + + + + rw + o + R + + + + #x6502 + Supported Drive Modes + UDINT + 32 + + + Supported Drive Modes + + 0001 + + + + + ro + o + + + + #xf000 + Modular device profile + DTF000 + 48 + + + SubIndex 000 + + 02 + + + + Module index distance + + 2003 + + + + Maximum number of modules + + 02 + + + + + ro + o + + + + #xf010 + Module profile list + DTF010 + 80 + + + SubIndex 000 + + 02 + + + + SubIndex 001 + + 92010200 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf030 + Configured module Ident list + DTF030 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf050 + Module detected list + DTF050 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + + + Outputs + Inputs + MBoxState + MBoxOut + MBoxIn + Outputs + Inputs + + + + + + Synchron + SM-Synchron + #x0 + 0 + 0 + 0 + + + DC + DC-Synchron + #x300 + 0 + 0 + 0 + + + + + Axis 0 + #x119800 + #x219800 + #x319800 + + + + 2048 + 800E00CC8813f000000000800000 + + 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 + + + + + Axis (csv,csp,cst) + dynamic switchbewteen csp/csv + + #x1600 + #x1601 + #x1602 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6060 + 0 + 8 + Mode Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1a00 + #x1a01 + #x1a02 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + 402 + + + + Axis (pp,pv,pt) + Axis only supports pp + + #x1601 + #x1600 + #x1602 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6081 + 0 + 32 + Profile Velocity + object 0x6081:0 + UDINT + + + #x6087 + 0 + 32 + Torque Slope + object 0x6087:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1a01 + #x1a00 + #x1a02 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + + + SO + #x6060 + 0 + 08 + Modes of operation + + + + + 402 + + + + pv - axis + Axis only supports pv + + #x1602 + #x1600 + #x1601 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1a02 + #x1a00 + #x1a01 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + + + SO + #x6060 + 0 + 08 + Modes of operation + + + + + 402 + + + + + diff --git a/docs/EYOU_ServoModule_ECAT_V145_no_slot.xml b/docs/EYOU_ServoModule_ECAT_V145_no_slot.xml new file mode 100644 index 00000000..d8204e8f --- /dev/null +++ b/docs/EYOU_ServoModule_ECAT_V145_no_slot.xml @@ -0,0 +1,5495 @@ + + + + #x1097 + Jiangsu Yiyou Robot Technology Co., Ltd. + 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 + + + + + ServoDrive + Servo Drives + 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 + + + + + EYOU_ServoModule_V145 + EYOU_ServoModule_ECAT_V145 + + + + 2000 + 9000 + 5000 + 200 + + + + + 100 + 2000 + + + + ServoDrive + + + 402 + + + + + + BIT2 + 2 + + + + BOOL + 1 + + + + DINT + 32 + + + + INT + 16 + + + + SINT + 8 + + + + UDINT + 32 + + + + UINT + 16 + + + + USINT + 8 + + + + REAL + 32 + + + + ARRAY [0..3] OF BYTE + USINT + 32 + + 0 + 4 + + + + + STRING(12) + 96 + + + + STRING(10) + 80 + + + + DT1010 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Store All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Store Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Store Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1011 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Restore All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Restore Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Restore Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1018 + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Vendor ID + UDINT + 32 + 16 + + ro + o + + + + 2 + Product code + UDINT + 32 + 48 + + ro + o + + + + 3 + Revision + UDINT + 32 + 80 + + ro + o + + + + 4 + Serial number + UDINT + 32 + 112 + + ro + o + + + + + DT1C00ARR + USINT + 32 + + 1 + 4 + + + + DT1C00 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DT1C00ARR + 32 + 16 + + ro + o + + + + + + DT10F1 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Local Error Reaction + UDINT + 32 + 16 + + rw + o + + + + 2 + Sync Error Counter Limit + UDINT + 32 + 48 + + rw + o + + + + + + DT1C32 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + + DT1C33 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + DT1600 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1601 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1602 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A00 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1A01 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A02 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1C12ARR + UINT + 32 + + 1 + 2 + + + + DT1C12 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C12ARR + 32 + 16 + + rw + o + + + + + DT1C13ARR + UINT + 32 + + 1 + 2 + + + + DT1C13 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C13ARR + 32 + 16 + + rw + o + + + + + DT2001 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Eu Node Id + USINT + 8 + 16 + + rw + o + + + + 2 + Eu Can BitRate + UINT + 16 + 32 + + rw + o + + + + + DT2010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Current Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Current Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Current Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Current Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2012 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Velocity Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Velocity Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Velocity Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Velocity Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2013 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Position Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Position Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Position Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Position Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2014 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuBrakeControl + USINT + 8 + 16 + + rw + o + + + + 2 + EuBrakeState + USINT + 8 + 32 + + ro + o + + + + 3 + EuBrakeAutoState + USINT + 8 + 48 + + rw + o + + + + + DT2016 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Servo Temperture + SINT + 8 + 16 + + ro + o + + + + 2 + EuHighTemperatureLimit + SINT + 8 + 32 + + rw + o + + + + 3 + EuHighTemperatureWindowsTime + UINT + 16 + 48 + + rw + o + + + + + DT2020 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuMotorBlockTorque + UINT + 16 + 16 + + ro + o + + + + 2 + EuMotorBlockTime + UINT + 16 + 32 + + rw + o + + + + 3 + EuMotorBlockVelocity + UDINT + 32 + 48 + + rw + o + + + + + DT2021 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVolecityFollowingErrorWindows + UDINT + 32 + 16 + + rw + o + + + + 2 + EuVolecityFollowingErrorTime + UINT + 16 + 48 + + rw + o + + + + + DT202D + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuUnderVoltageThreshold + UDINT + 32 + 16 + + rw + o + + + + 2 + EuUnderVoltageTime + UDINT + 32 + 48 + + rw + o + + + + 3 + EuOverVoltageThreshold + UDINT + 32 + 80 + + rw + o + + + + 4 + EuOverVoltageTime + UDINT + 32 + 112 + + rw + o + + + + + DT202E + 128 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVelocityFeedforwardControlSelect + INT + 16 + 16 + + rw + o + + + + 2 + EuVelocityFeedforwardFilterTimeConst + INT + 16 + 32 + + rw + o + + + + 3 + EuVelocityFeedforwardGain + INT + 16 + 48 + + rw + o + + + + 4 + EuVelocityFeedforwardOriginalValue + DINT + 32 + 64 + + ro + o + + + + 4 + EuVelocityFeedforwardFilteredValue + DINT + 32 + 96 + + ro + o + + + + + DT210F + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + FocCurBiasA + UINT + 16 + 16 + + ro + o + + + + 2 + FocCurBiasB + UINT + 16 + 32 + + ro + o + + + + 3 + FocCurBiasC + UINT + 16 + 48 + + ro + o + + + + + DT607D + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Min position limit + DINT + 32 + 16 + + rw + o + + + + 2 + Max position limit + DINT + 32 + 48 + + rw + o + + + + + DT6091 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Motor revolutions + UDINT + 32 + 16 + + ro + o + + + + 2 + Shaft revolutions + UDINT + 32 + 48 + + rw + o + + + + + DT60C1 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation data record + DINT + 32 + 16 + + rw + o + + + + + DT60C2 + 32 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation period + USINT + 8 + 16 + + rw + o + + + + 2 + Interpolation Index + SINT + 8 + 24 + + rw + o + + + + + DT60FF + 32 + + 0 + Target Velocity + UDINT + 32 + 0 + + rw + o + R + + + + + DT6502 + 32 + + 0 + Supported Drive Modes + UDINT + 32 + 0 + + ro + o + + + + + DTF000 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Module index distance + UINT + 16 + 16 + + ro + o + + + + 2 + Maximum number of modules + UINT + 16 + 32 + + ro + o + + + + + DTF010ARR + UDINT + 64 + + 1 + 2 + + + + DTF010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF010ARR + 64 + 16 + + ro + o + + + + + + DTF030ARR + UDINT + 64 + + 1 + 2 + + + + DTF030 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF030ARR + 64 + 16 + + ro + o + + + + + + DTF050ARR + UDINT + 64 + + 1 + 2 + + + + DTF050 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF050ARR + 64 + 16 + + ro + o + + + + + + + + #x1000 + Device type + UDINT + 32 + + 92010200 + + + ro + m + + + + #x1001 + Error register + USINT + 8 + + 00 + + + ro + o + + + + #x1008 + Device name + STRING(12) + 96 + + 4575504878782d787878 + + + ro + o + + + + #x1009 + Hardware version + STRING(10) + 80 + + 56332e3046343035 + + + ro + o + + + + #x100a + Software version + STRING(10) + 80 + + 56313433 + + + ro + o + + + + #x1c00 + Sync manager type + DT1C00 + 48 + + + SubIndex 000 + + 04 + + + + SubIndex 001 + + 01 + + + + SubIndex 002 + + 02 + + + + SubIndex 003 + + 03 + + + + SubIndex 004 + + 04 + + + + + ro + o + + + + #x1010 + Store Parameters + DT1010 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1011 + Restore Default Parameters + DT1011 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1018 + Identity + DT1018 + 144 + + + SubIndex 000 + + 04 + + + + Vendor ID + + 9710 + + + + Product code + + 0624 + + + + Revision + + 0002 + + + + Serial number + + 00000000 + + + + + ro + o + + + + #x10F1 + Error Settings + DT10F1 + 80 + + + SubIndex 000 + + 04 + + + + Local Error Reaction + + 01 + + + + Sync Error Counter Limit + + 04 + + + + + ro + o + + + + #x1c32 + SM output parameter + DT1C32 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 0100 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c33 + SM input parameter + DT1C33 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 2200 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c12 + RxPDO assign + DT1C12 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 0016 + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1c13 + TxPDO assign + DT1C13 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 001a + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1600 + csp/csv/cst RxPDO + DT1600 + 336 + + + SubIndex 000 + + 00 + 10 + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + 7st Output Object to be mapped + + #x00000000 + + + + 8st Output Object to be mapped + + #x00000000 + + + + 9st Output Object to be mapped + + #x00000000 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1601 + pp/pv/pt RxPDO + DT1601 + 336 + + + SubIndex 000 + + #x0A + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60830020 + + + + 6st Output Object to be mapped + + #x60840020 + + + + 7st Output Object to be mapped + + #x60810020 + + + + 8st Output Object to be mapped + + #x60870010 + + + + 9st Output Object to be mapped + + #x60600008 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1602 + pv RxPDO + DT1602 + 336 + + + SubIndex 000 + + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x60FF0020 + + + + 3st Output Object to be mapped + + #x60830020 + + + + 4st Output Object to be mapped + + #x60840020 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a00 + csp/csv/cst TxPDO + DT1A00 + 336 + + + SubIndex 000 + + 00 + 10 + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770010 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + 8st Input Object to be mapped + + #x00000000 + + + + 9st Input Object to be mapped + + #x00000000 + + + + 10st Input Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1a01 + pp/pv/pt TxPDO + DT1A01 + 336 + + + SubIndex 000 + + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770020 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a02 + pv TxPDO + DT1A02 + 336 + + + SubIndex 000 + + #x05 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x606C0020 + + + + 3st Input Object to be mapped + + #x60610008 + + + + 4st Input Object to be mapped + + #x603F0010 + + + + 5st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x2000 + Eu Motor SN + UDINT + 32 + + + Eu Motor SN + + 00 + + + + + rw + o + + + + #x2001 + Servo Config + DT2001 + 48 + + + SubIndex 000 + + 02 + + + + Eu Node Id + + 01 + + + + Eu Can BitRate + + 1000 + + + + + ro + o + + + + #x2002 + Motor Para + UDINT + 32 + + + Motor Para + + 00 + + + + + rw + o + + + + #x2003 + Soft Limit State + UDINT + 32 + + + Soft Limit State + + 00 + + + + + rw + o + + + + #x2004 + EuCommMode + USINT + 8 + + + EuCommMode + + 00 + + + + + rw + o + + + + #x2010 + Current Loop Pi + DT2010 + 80 + + + SubIndex 000 + + 04 + + + + Current Loop Kp Default + + 0040 + + + + Current Loop Ki Default + + 0001 + + + + Current Loop Kp + + 0004 + + + + Current Loop Ki + + 0001 + + + + + ro + o + + + + #x2012 + Velocity Loop Pi + DT2012 + 80 + + + SubIndex 000 + + 04 + + + + Velocity Loop Kp Default + + D007 + + + + Velocity Loop Ki Default + + 64 + + + + Velocity Loop Kp + + D007 + + + + Velocity Loop Ki + + 64 + + + + + ro + o + + + + #x2013 + Position Loop Pi + DT2013 + 80 + + + SubIndex 000 + + 04 + + + + Position Loop Kp Default + + D007 + + + + Position Loop Ki Default + + 64 + + + + Position Loop Kp + + D007 + + + + Position Loop Ki + + 64 + + + + + ro + o + + + + #x2014 + brake control + DT2014 + 64 + + + SubIndex 000 + + 03 + + + + EuBrakeControl + + 00 + + + + EuBrakeState + + 00 + + + + EuBrakeAutoState + + 01 + + + + + ro + o + + + + #x2015 + Eu Pwm Inv + USINT + 8 + + + Eu Pwm Inv + + 00 + + + + + ro + o + + + + #x2016 + Servo Temperture + DT2016 + 64 + + + SubIndex 000 + + 03 + + + + EuTemperature + + 19 + + + + EuHighTemperatureLimit + + 55 + + + + EuHighTemperatureWindowsTime + + B80B + + + + + ro + o + + + + #x2017 + EuVelocityIntLimit + UINT + 16 + + + EuVelocityIntLimit + + D007 + + + + + rw + o + + + + #x2020 + Motor Block + DT2020 + 80 + + + SubIndex 000 + + 03 + + + + EuMotorBlockTorque + + E803 + + + + EuMotorBlockTime + + B80B + + + + EuMotorBlockVelocity + + 1027 + + + + + ro + o + + + + #x2021 + velocity floowing error + DT2021 + 64 + + + SubIndex 000 + + 02 + + + + EuVolecityFollowingErrorWindows + + A08601 + + + + EuVolecityFollowingErrorTime + + 0BB8 + + + + + ro + o + + + + #x2022 + torque window + UINT + 16 + + + torque window + + 0A + + + + + rw + o + + + + #x2023 + Torque window time + UINT + 16 + + + Torque window time + + 01 + + + + + rw + o + + + + #x2024 + EuOverSpeedThreshold + UDINT + 32 + + + EuOverSpeedThreshold + + AAAA42 + + + + + rw + o + + + + #x2025 + EuOverSpeedTime + UINT + 16 + + + EuOverSpeedTime + + 64 + + + + + rw + o + + + + #x2026 + EuBrakeDelayTime + USINT + 8 + + + EuBrakeDelayTime + + 64 + + + + + rw + o + + + + #x2027 + EuAutoMagnetAngleAlignmentFlag + UDINT + 32 + + + EuAutoMagnetAngleAlignmentFlag + + 00 + + + + + rw + o + + + + #x2028 + EuI2tLimit + UINT + 16 + + + EuI2tLimit + + 64 + + + + + rw + o + + + + #x2029 + EuI2tValue + UINT + 16 + + + EuI2tValue + + 00 + + + + + ro + o + + + + #x202A + EuFirstEncoderValue + DINT + 32 + + + EuFirstEncoderValue + + 00 + + + + + ro + o + + + + #x202B + EuSecondEncoderValue + DINT + 32 + + + EuSecondEncoderValue + + 00 + + + + + ro + o + + + + #x202C + EuThetaBias + DINT + 32 + + + EuThetaBias + + 00 + + + + + ro + o + + + + #x202D + Eu Voltage Threshold + DT202D + 144 + + + SubIndex 000 + + 04 + + + + EuUnderVoltageThreshold + + 283707 + + + + EuUnderVoltageTime + + 05 + + + + EuOverVoltageThreshold + + A00901 + + + + EuOverVoltageTime + + 05 + + + + + ro + o + + + + #x202E + Eu Velocity Feedforward + DT202E + 128 + + + SubIndex 000 + + 05 + + + + EuVelocityFeedforwardControlSelect + + 01 + + + + EuVelocityFeedforwardFilterTimeConst + + 00 + + + + EuVelocityFeedforwardGain + + 00 + + + + EuVelocityFeedforwardOriginalValue + + 00 + + + + EuVelocityFeedforwardFilteredValue + + 00 + + + + + ro + o + + + + #x2100 + EuDisableFpgaMu150Spi + DINT + 32 + + + EuDisableFpgaMu150Spi + + 00 + + + + + rw + o + + + + #x2104 + ServoMagnetAngleAlignment + UDINT + 32 + + + ServoMagnetAngleAlignment + + 00 + + + + + rw + o + + + + #x2109 + EuGoToBoot + DINT + 32 + + + EuGoToBoot + + 00 + + + + + rw + o + + + + #x210A + EuCompStart + DINT + 32 + + + EuCompStart + + 00 + + + + + rw + o + + + + #x210B + EuCompEn + DINT + 32 + + + EuCompEn + + 00 + + + + + rw + o + + + + #x210C + EuCompBias + DINT + 32 + + + EuCompBias + + 00 + + + + + rw + o + + + + #x210D + EuCompState + DINT + 32 + + + EuCompState + + 00 + + + + + rw + o + + + + #x210E + EuFocCurBiasSet + USINT + 8 + + + EuFocCurBiasSet + + 00 + + + + + rw + o + + + + #x210F + Foc Cur Bias + DT210F + 144 + + + SubIndex 000 + + 03 + + + + FocCurBiasA + + 00 + + + + FocCurBiasB + + 00 + + + + FocCurBiasC + + 00 + + + + + ro + o + + + + #x2110 + EuTorqueFactor + UINT + 16 + + + EuTorqueFactor + + E803 + + + + + rw + o + + + + #x2201 + EuSysOutPulseStep + UDINT + 32 + + + EuSysOutPulseStep + + 00 + + + + + rw + o + + + + #x2301 + Position Velocity Mix Position Kp + UINT + 16 + + + Position Velocity Mix Position Kp + + 00 + + + + + rw + o + + + + #x2302 + Position Velocity Mix Velocity Kd + UINT + 16 + + + Position Velocity Mix Velocity Kd + + 00 + + + + + rw + o + + + + #x603F + Error Code + UINT + 16 + + + Error Code + + 00 + + + + + ro + o + T + + + + #x6040 + Control Word + UINT + 16 + + + Control Word + + 00 + + + + + rw + o + R + + + + #x6041 + Status Word + UINT + 16 + + + Status Word + + 00 + + + + + ro + o + T + + + + #x605A + Quickstop Option Code + INT + 16 + + + Quickstop Option Code + + 02 + + + + + rw + o + + + + #x605B + Shutdown Option Code + INT + 16 + + + Shutdown Option Code + + 00 + + + + + rw + o + + + + #x605C + Disable Operation Option Code + INT + 16 + + + Disable Operation Option Code + + 01 + + + + + rw + o + + + + #x605D + Halt option code + INT + 16 + + + Halt option code + + 01 + + + + + rw + o + + + + #x605E + Fault Reaction Code + INT + 16 + + + Fault Reaction Code + + 02 + + + + + rw + o + + + + #x6060 + Modes of Operation + SINT + 8 + + + Modes of Operation + + 00 + + + + + rw + o + R + + + + #x6061 + Modes of Operation Display + SINT + 8 + + + Modes of Operation Display + + 00 + + + + + ro + o + T + + + + #x6062 + Position demannd value + DINT + 32 + + + Position demannd value + + 00 + + + + + ro + o + T + + + + #x6064 + Position Actual Value + DINT + 32 + + + Position Actual Value + + 00 + + + + + ro + o + T + + + + #x6065 + Maximal following error + UDINT + 32 + + + Maximal following error + + E09304 + + + + + rw + o + + + + #x6067 + Position window + UDINT + 32 + + + Position window + + 64 + + + + + rw + o + + + + #x6068 + Position window time + UINT + 16 + + + Position window time + + 01 + + + + + rw + o + + + + #x606B + Velocity demand value + DINT + 32 + + + Velocity demand value + + 00 + + + + + ro + o + T + + + + #x606C + Velocity Actual Value + DINT + 32 + + + Velocity Actual Value + + 00 + + + + + ro + o + T + + + + #x606D + Velocity window + UINT + 16 + + + Velocity window + + 1027 + + + + + ro + o + + + + #x606E + Velocity window time + UINT + 16 + + + Velocity window time + + 01 + + + + + ro + o + + + + #x606F + Velocity threshold + UINT + 16 + + + Velocity threshold + + E803 + + + + + ro + o + + + + #x6070 + Velocity threshold time + UINT + 16 + + + Velocity threshold time + + 01 + + + + + ro + o + + + + #x6071 + Target Torque + INT + 16 + + + Target Torque + + 00 + + + + + rw + o + + + + #x6072 + Max Torque + UINT + 16 + + + Max Torque + + D007 + + + + + rw + o + + + + #x6074 + Torque demand value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6076 + Motor rated torque + UDINT + 32 + + + Motor rated torque + + 00 + + + + + rw + o + + + + #x6077 + Torque Actual Value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6078 + Current actual value + INT + 16 + + + Current actual value + + 00 + + + + + ro + o + T + + + + #x6079 + DC link circuit voltage + UDINT + 32 + + + DC link circuit voltage + + 00 + + + + + rw + o + + + + #x607A + Target Position + DINT + 32 + + + Target Position + + 00 + + + + + rw + o + R + + + + #x607C + Home Offset + DINT + 32 + + + Home Offset + + 00 + + + + + rw + o + + + + #x607D + Software Position Limit + DT607D + 80 + + + SubIndex 000 + + 02 + + + + Min position limit + + 006CCA88 + + + + Max position limit + + 00943577 + + + + + ro + o + + + + #x607F + Max Profile Velocity + UDINT + 32 + + + Max Profile Velocity + + 00 + + + + + rw + o + + + + #x6081 + Profile Velocity + UDINT + 32 + + + Profile Velocity + + 00 + + + + + rw + o + R + + + + #x6083 + Profile Acceleration + UDINT + 32 + + + Profile Acceleration + + 00 + + + + + rw + o + R + + + + #x6084 + Profile Deceleration + UDINT + 32 + + + Profile Deceleration + + 00 + + + + + rw + o + R + + + + #x6085 + Quickstop Declaration + DINT + 32 + + + Quickstop Declaration + + 00 + + + + + rw + o + + + + #x6087 + Torque slope + UDINT + 32 + + + Torque slope + + E803 + + + + + rw + o + + + + #x6091 + Gear ratio + DT6091 + 80 + + + SubIndex 000 + + 02 + + + + Motor revolutions + + 000051 + + + + Shaft revolutions + + 000051 + + + + + ro + o + + + + #x60B1 + Velocity Offset + DINT + 32 + + + Velocity Offset + + 00 + + + + + rw + o + + + + #x60B2 + Torque Offset + INT + 16 + + + Torque Offset + + 00 + + + + + rw + o + + + + #x60C1 + Interpolation Data Record + DT60C1 + 48 + + + SubIndex 000 + + 01 + + + + Interpolation data record + + 00 + + + + + ro + o + + + + #x60C2 + Interpolation Time Period + DT60C2 + 32 + + + SubIndex 000 + + 02 + + + + Interpolation period + + 01 + + + + Interpolation Index + + -3 + + + + + ro + o + + + + #x60F4 + Following error actual value + DINT + 32 + + + Following error actual value + + 00 + + + + + ro + o + T + + + + #x60FF + Target Velocity + DINT + 32 + + + Target Velocity + + 00 + + + + + rw + o + R + + + + #x6502 + Supported Drive Modes + UDINT + 32 + + + Supported Drive Modes + + 0001 + + + + + ro + o + + + + #xf000 + Modular device profile + DTF000 + 48 + + + SubIndex 000 + + 02 + + + + Module index distance + + 2003 + + + + Maximum number of modules + + 02 + + + + + ro + o + + + + #xf010 + Module profile list + DTF010 + 80 + + + SubIndex 000 + + 02 + + + + SubIndex 001 + + 92010200 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf030 + Configured module Ident list + DTF030 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf050 + Module detected list + DTF050 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + + + Outputs + Inputs + MBoxState + MBoxOut + MBoxIn + Outputs + Inputs + + #x1600 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6060 + 0 + 8 + Mode Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1601 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6081 + 0 + 32 + Profile Velocity + object 0x6081:0 + UDINT + + + #x6087 + 0 + 32 + Torque Slope + object 0x6087:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1602 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1A00 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + #x1A01 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + #x1A02 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + + + + + Synchron + SM-Synchron + #x0 + 0 + 0 + 0 + + + DC + DC-Synchron + #x300 + 0 + 0 + 0 + + + + 2048 + 800E00CC8813f000000000800000 + + 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 + + + + diff --git a/docs/ethercat_motor_tutorial.md b/docs/ethercat_motor_tutorial.md index 5720c0bc..e1565180 100644 --- a/docs/ethercat_motor_tutorial.md +++ b/docs/ethercat_motor_tutorial.md @@ -1,25 +1,51 @@ # EtherCAT 电机接入教程 -这份文档只说明新增一种 EtherCAT 电机需要改哪里、怎么写。 +当前已接入意优 `EYOU_ServoModule_ECAT_V145`，协议为 EtherCAT CoE + CiA402，PDO 使用 `docs/EYOU_ServoModule_ECAT_V145_no_slot.xml` 中的 `0x1600/0x1A00` 映射。 -## 1. 增加 vendor +ESI XML 作为厂商通信说明和对照资料保存，运行时不直接解析 XML。实际 PDO 映射写在： -修改 `protos/cmvr/config/motor_config/motor_config.proto`： - -```proto -enum MotorVendor { - MOTOR_VENDOR_UNKNOWN = 0; - MOTOR_VENDOR_TI5 = 1; - MOTOR_VENDOR_MUJOCO = 2; - MOTOR_VENDOR_XXX = 3; -} +```text +cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h ``` -`MOTOR_VENDOR_XXX` 改成真实厂商名，例如 `MOTOR_VENDOR_FOO`。不要复用 `TI5`。 +## 1. 系统依赖 -## 2. 写电机配置 +IgH EtherCAT userspace 已安装在： -新增配置文件： +```text +dependency/x86/third_party/ethercat/v1.7.0 +``` + +真机运行前，系统里还需要安装/加载 IgH master 内核模块。使用仓库脚本启动 EtherCAT master： + +```bash +sudo script/ethercat/start_ethercat.sh eno1 +script/ethercat/status_ethercat.sh +``` + +其中 `eno1` 是连接 EtherCAT 从站的网卡。脚本会读取该网卡 MAC，写入： + +```text +dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf +``` + +并通过 bundled IgH 的 `ethercatctl -c` 启动 master。能看到 EYOU slave 后再启动程序。 + +停止 EtherCAT： + +```bash +sudo script/ethercat/stop_ethercat.sh eno1 +``` + +如果这张网卡要恢复给普通网络使用： + +```bash +sudo script/ethercat/stop_ethercat.sh eno1 --restore-network +``` + +## 2. 电机配置 + +新增或修改： ```text cmvr-es/config/devices/motor/ethercat_motors.pb.txt @@ -34,20 +60,36 @@ motor { motor_groups { id: "right_arm_ethercat" bus_type: MOTOR_BUS_ETHERCAT - vendor: MOTOR_VENDOR_XXX + vendor: MOTOR_VENDOR_EYOU protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 - tool_frame: "R_FINGER_TIP" ethercat { - master_id: "eth0" + master_index: 0 cycle_us: 1000 - slaves { motor_id: 1 slave_index: 0 vendor_id: 0x00000000 product_code: 0x00000000 } - slaves { motor_id: 2 slave_index: 1 vendor_id: 0x00000000 product_code: 0x00000000 } - slaves { motor_id: 3 slave_index: 2 vendor_id: 0x00000000 product_code: 0x00000000 } - slaves { motor_id: 4 slave_index: 3 vendor_id: 0x00000000 product_code: 0x00000000 } - slaves { motor_id: 5 slave_index: 4 vendor_id: 0x00000000 product_code: 0x00000000 } - slaves { motor_id: 6 slave_index: 5 vendor_id: 0x00000000 product_code: 0x00000000 } - slaves { motor_id: 7 slave_index: 6 vendor_id: 0x00000000 product_code: 0x00000000 } + + cia402 { + profile_position_trigger_delay_ms: 2 + 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 + } + + 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 { @@ -63,21 +105,26 @@ motor { } motors { - motors { id: 1 joint_name: "R_SHOULDER_P" } - motors { id: 2 joint_name: "R_SHOULDER_R" } - motors { id: 3 joint_name: "R_SHOULDER_Y" } - motors { id: 4 joint_name: "R_ELBOW_R" } - motors { id: 5 joint_name: "R_WRIST_P" } - motors { id: 6 joint_name: "R_WRIST_Y" } - motors { id: 7 joint_name: "R_WRIST_R" } + 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 } } } } ``` -`motors.motors.id` 是系统内的电机逻辑 id。`ethercat.slaves.motor_id` 必须和它对应。 +字段说明： -`joint_limits` 按 `joint_name` 读取，和 CAN、MuJoCo 电机保持同一个风格。 +- `master_index`：IgH master 编号，第一张 EtherCAT master 是 `0`。 +- `alias`：EtherCAT alias，一般为 `0`。 +- `position`：slave 在 EtherCAT 链路上的顺序，从 `0` 开始。 +- `motor_id`：系统内电机 id，必须和 `motors.motors.id` 对应。 +- `encoder_counts_per_rev`：电机编码器每转 count 数，用于 `rad <-> count` 换算。 +- `gear_ratio`：电机轴到关节输出轴的减速比，用于接口层 `rad/rad/s` 和驱动器 raw count/counts/s 换算。 ## 3. 注册设备 @@ -98,15 +145,9 @@ devices { } ``` -## 4. 机械臂使用 EtherCAT group +## 4. 机械臂使用 EtherCAT 电机 -修改机械臂配置，例如： - -```text -cmvr-es/config/devices/arm/arm.pb.txt -``` - -把 motor backend 改成： +修改机械臂配置中的 motor backend： ```proto motor { @@ -122,116 +163,66 @@ motor { } ``` -## 5. 实现 bus runtime +## 5. 代码结构 -EtherCAT 总线资源放在： +EtherCAT 总线运行时： ```text cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp ``` -`EthercatMotorBusRuntime` 负责： +它负责： + +- 打开 IgH master +- 创建 domain +- 使用 `vendor + protocol` 选择 PDO mapping +- 按 mapping 配置 `0x1600/0x1A00` PDO +- 注册每个非 padding PDO entry 的 offset +- 启动 cyclic loop +- 按 `motor_id + index + subindex` 提供通用 PDO 读写接口 + +CiA402 EtherCAT 电机驱动： ```text -读取 ethercat 配置 -初始化 EtherCAT master -扫描/校验 slave_index、vendor_id、product_code -启动 cyclic loop -保存 command/feedback buffer -停止 cyclic loop +cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h +cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h +cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h +cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp ``` -bus runtime 不创建具体电机，也不关心厂商；它只保存总线连接、线程和数据缓存。 - -## 6. 增加具体电机 driver - -新增目录： +EYOU 私有适配： ```text -cmvr-es/devices/motor/drivers/xxx_ethercat/ - CMakeLists.txt - include/xxx_ethercat_motor.h - include/xxx_ethercat_motor_protocol.h - src/xxx_ethercat_motor.cpp - src/xxx_ethercat_motor_protocol.cpp +cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h +cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h +cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h +cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h +cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp +cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp ``` -`XxxEthercatMotor` 继承 `AbstractMotor`。 +它负责： -`XxxEthercatMotorProtocol` 继承 `MotorProtocolInterface`，把 `setTarget`、`setQd`、`getQ` 等接口转换成 EtherCAT command/feedback。 +- CiA402 `6040/6041` 状态机 +- 设置 `6060` 运行模式 +- 写 `607A/60FF/6071` +- 读 `6064/606C/6077/603F` +- `rad` 和 encoder count 的换算 -## 7. 在 MotorManager 里创建 EtherCAT 电机 - -修改： - -```text -cmvr-es/devices/motor/manager/src/motor_manager.cpp -``` - -在 `MotorManager::createEthercatMotors_()` 里按 `vendor + protocol` 创建具体电机： - -```cpp -auto ethercat_bus_runtime = - std::dynamic_pointer_cast(bus_runtime); - -if (group_cfg.vendor() == config::MOTOR_VENDOR_XXX && - group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { - auto protocol = std::make_shared(ethercat_bus_runtime); - - std::vector> motors; - motors.reserve(motor_cfgs.size()); - for (const auto& cfg : motor_cfgs) { - auto motor = std::make_shared(cfg); - motor->setProtocol(protocol); - if (!motor->init()) { - return {}; - } - motors.push_back(std::move(motor)); - } - return motors; -} -``` - -## 8. 加入 CMake - -修改： - -```text -cmvr-es/devices/motor/manager/CMakeLists.txt -``` - -给 `motor_manager` 链接新增的具体 EtherCAT 电机 target。 - -修改： - -```text -cmvr-es/devices/motor/CMakeLists.txt -``` - -加入： - -```cmake -add_subdirectory(drivers/xxx_ethercat) -``` - -## 9. 验证配置 - -先验证 proto： +## 6. 编译验证 ```bash -./output/bin/protoc \ - --encode=cmvr.config.MotorRootConfig \ - -I protos \ - protos/cmvr/config/motor_config/motor_config.proto \ - < cmvr-es/config/devices/motor/ethercat_motors.pb.txt \ - > /tmp/ethercat_motors.pb.bin +cmake -S . -B cmake-build-debug +cmake --build cmake-build-debug --target motor_manager ``` -再编译： +真机验证顺序： ```bash -cmake --build cmake-build-debug --target mujoco_manual_ui_test +sudo script/ethercat/start_ethercat.sh eno1 +script/ethercat/status_ethercat.sh +cmake --build cmake-build-debug --target cmvr_es ``` -真机联调时先只验证初始化日志：master 打开、slave 数量、vendor/product 校验、cyclic loop 启动、每个 motor 注册成功。然后再下发运动命令。 +第一次联调先不要大幅度运动。先看 master 是否打开、slave 是否进入 OP、状态字是否更新，再给单个电机小角度目标。 diff --git a/docs/意优CANopen&EtherCAT应用手册V2.2.pdf b/docs/意优CANopen&EtherCAT应用手册V2.2.pdf new file mode 100644 index 00000000..e1c3df2a Binary files /dev/null and b/docs/意优CANopen&EtherCAT应用手册V2.2.pdf differ diff --git a/model/april_tag/tag36_11_00001.png b/model/april_tag/tag36_11_00001.png new file mode 100644 index 00000000..3d73cfb0 Binary files /dev/null and b/model/april_tag/tag36_11_00001.png differ diff --git a/model/april_tag/tag36_11_00002_1000.png b/model/april_tag/tag36_11_00002_1000.png new file mode 100644 index 00000000..6f98d5c1 Binary files /dev/null and b/model/april_tag/tag36_11_00002_1000.png differ diff --git a/model/april_tag/tag36_11_00003_1000.png b/model/april_tag/tag36_11_00003_1000.png new file mode 100644 index 00000000..cd8482ae Binary files /dev/null and b/model/april_tag/tag36_11_00003_1000.png differ diff --git a/model/april_tag/tag36_11_00004_1000.png b/model/april_tag/tag36_11_00004_1000.png new file mode 100644 index 00000000..19d95b12 Binary files /dev/null and b/model/april_tag/tag36_11_00004_1000.png 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/geometries.usd b/model/xiaoyan_description/dual_arm/payloads/geometries.usd new file mode 100644 index 00000000..8aeaa987 Binary files /dev/null and b/model/xiaoyan_description/dual_arm/payloads/geometries.usd differ 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/geometries.usd b/model/xiaoyan_description/dual_arm_1/payloads/geometries.usd new file mode 100644 index 00000000..399b49af Binary files /dev/null and b/model/xiaoyan_description/dual_arm_1/payloads/geometries.usd differ 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/model/xiaoyan_description/right_arm_eye_to_hand.xml b/model/xiaoyan_description/right_arm_eye_to_hand.xml new file mode 100644 index 00000000..a843a93a --- /dev/null +++ b/model/xiaoyan_description/right_arm_eye_to_hand.xml @@ -0,0 +1,809 @@ + + + + + + + + diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index 71535314..a7ee04f0 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -49,6 +49,11 @@ message VendorRobotArmBackendConfig { } message SpeedLPlannerConfig { + // Default true: preserve acceleration through same-axis velocity reversal. + optional bool continuous_linear_reversal = 21; + // Arm-level speedL limits. Command requests may lower, but not raise them. + // Linear units: m/s, m/s^2, m/s^3. Angular units: rad/s, rad/s^2, rad/s^3. + // Unset/nonpositive values use the planner defaults. double linear_velocity_max = 1; double linear_acceleration_max = 2; double linear_jerk_max = 3; @@ -71,6 +76,8 @@ message CartesianVelocityControllerConfig { double stop_command_velocity_norm = 3; double stop_measured_velocity_norm = 4; double stop_acceleration = 5; + // 等待 Cartesian 速度运动停止的最长时间，单位为秒。 + optional double stop_timeout_s = 6; } message ToppraJointMotionPlannerConfig { @@ -84,6 +91,15 @@ message MoveJConfig { oneof algorithm { ToppraJointMotionPlannerConfig toppra_joint_motion_planner = 1; } + + // MoveJ 轨迹发送完成后，等待关节实际状态稳定的最长时间，单位为秒。 + optional double settle_timeout_s = 2; + // MoveJ 完成时允许的最大关节位置误差，单位为弧度。 + optional double settle_position_tolerance_rad = 3; + // MoveJ 完成时允许的最大关节速度，单位为弧度/秒。 + optional double settle_velocity_tolerance_rad_s = 4; + // 位置和速度连续满足条件的采样次数。 + optional int32 settle_stable_sample_count = 5; } message MoveLPlannerConfig { diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto index c238d2bf..751e6f9a 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -23,6 +23,7 @@ message DeviceConfigEntry { DEVICE_TYPE_MUJOCO_VIEWER = 19; } + // For DEVICE_TYPE_MOTOR_SYSTEM, this is the motor_group id selected from config_file. string id = 1; DeviceType type = 2; string config_file = 3; @@ -30,6 +31,8 @@ message DeviceConfigEntry { } message DeviceManagerConfig { + reserved 20; + string name = 1; string version = 2; string description = 3; diff --git a/protos/cmvr/config/dexhand_config/dexhand_config.proto b/protos/cmvr/config/dexhand_config/dexhand_config.proto index b822f3f5..1568aa3e 100644 --- a/protos/cmvr/config/dexhand_config/dexhand_config.proto +++ b/protos/cmvr/config/dexhand_config/dexhand_config.proto @@ -36,6 +36,13 @@ message PX6AXGen3{ string tactile_region = 16; string sensor_name = 17; PX6AXGen3PollingReadMode polling_read_mode = 18; + // Maximum age since the sample's request was sent. 0 uses 50 ms. + // Independent of response_timeout_ms: stale cached data must fail promptly. + int32 max_sample_age_ms = 19; +} + +message ZeroSimTouchDexHand { + string id = 1; } @@ -47,6 +54,7 @@ message DexHandDeviceConfig { oneof backend { RH56DFTPDexHandConfig rh56dftp = 10; PX6AXGen3 px_6ax_gen3 = 11; + ZeroSimTouchDexHand zero_sim_touch = 12; } } diff --git a/protos/cmvr/config/joint_limits_config.proto b/protos/cmvr/config/joint_limits_config.proto index c3da751b..268c05af 100644 --- a/protos/cmvr/config/joint_limits_config.proto +++ b/protos/cmvr/config/joint_limits_config.proto @@ -33,7 +33,6 @@ message JointLimitAvoidanceConfig { double gain = 2; double margin_ratio = 3; double max_push = 4; - double weight = 5; } message JointLimitPolicyConfig { 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/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..28d148c1 100644 --- a/protos/cmvr/config/task_manager_config/task_manager_config.proto +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -6,6 +6,7 @@ message TaskConfigEntry { TASK_TYPE_UNKNOWN = 0; TASK_TYPE_TOUCH_SCREEN = 1; TASK_TYPE_GRPC_SERVER = 3; + TASK_TYPE_SELF_COLLISION = 4; reserved 2; reserved "TASK_TYPE_ARM_CONTROL"; } diff --git a/protos/cmvr/config/touch_screen_algorithm_config.proto b/protos/cmvr/config/touch_screen_algorithm_config.proto index e7e577fe..c7c72bfc 100644 --- a/protos/cmvr/config/touch_screen_algorithm_config.proto +++ b/protos/cmvr/config/touch_screen_algorithm_config.proto @@ -11,17 +11,10 @@ message TouchScreenApriltagConfig { optional TouchScreenTargetPointMethod target_point_method = 3; } -message TouchScreenIbvsConfig { - optional string camera_link = 1; - optional double lambda = 2; - optional double mu = 3; - optional double qdot_max = 4; - .cmvr.common.Vec6 vmax6 = 5; - .cmvr.common.Vec6 amax6 = 6; - optional double twist_filter_alpha = 7; - - .cmvr.common.Mat3 r_camera_to_visp = 12; - .cmvr.common.Mat3 r_camera_to_urdf = 13; - - repeated string control_joint_names = 14; +message TouchScreenTaskPbvsConfig { + .cmvr.common.Vec3 position_gain = 1; + .cmvr.common.Vec3 rotation_gain = 2; + .cmvr.common.Vec6 vmax6 = 3; + .cmvr.common.Vec6 amax6 = 4; + optional double twist_filter_alpha = 5; } diff --git a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto index c48681a7..08446991 100644 --- a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto +++ b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto @@ -15,9 +15,10 @@ message TouchScreenTaskDevicesConfig { reserved 1; reserved "robot_id"; - optional string arm_id = 4; - optional string dexhand_id = 2; optional string camera_id = 3; + optional string arm_id = 4; + optional string external_camera_id = 5; + optional string dexhand_id = 2; } message TouchScreenTaskInitializationConfig { @@ -26,33 +27,79 @@ message TouchScreenTaskInitializationConfig { repeated TouchScreenInitJointPoint joint_positions = 3; optional double velocity = 4; optional double acceleration = 5; + // 当前关节位置误差小于该值时可跳过初始化 MoveJ，单位为弧度。 + optional double skip_position_tolerance_rad = 6; + // 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。 + optional double skip_velocity_tolerance_rad_s = 7; +} + +message TouchScreenTaskTagConfig { + optional int32 id = 1; + optional double size_m = 2; +} + +message TouchScreenTaskTagsConfig { + TouchScreenTaskTagConfig screen = 1; + TouchScreenTaskTagConfig hand = 2; +} + +message TouchScreenTaskHandCameraConfig { + reserved 1; + reserved "camera_id"; + optional TouchScreenDepthPolicy depth_policy = 2; + optional TouchScreenTargetPointMethod target_point_method = 3; } message TouchScreenTaskPerceptionConfig { - TouchScreenApriltagConfig apriltag = 1; + reserved 1, 2, 6; + reserved "apriltag", "external_apriltag", "external_camera"; + + TouchScreenTaskTagsConfig tags = 4; + TouchScreenTaskHandCameraConfig hand_camera = 5; } message TouchScreenAlignmentTargetConfig { - .cmvr.common.Vec3 position_in_camera = 1; - .cmvr.common.Vec3 rotation_vector = 2; + reserved 2; + reserved "rotation_vector"; + + // Target orientation of Hand Tag H relative to Screen Tag G. + // rx, ry and rz are fixed-axis (extrinsic) rotations about G.X, G.Y and G.Z, + // applied in that order, in radians. + .cmvr.common.Euler hand_orientation_G = 6; optional TouchScreenAlignMode mode = 3; + .cmvr.common.Vec3 position_offset_G = 4; + .cmvr.common.Vec3 rotation_offset_G = 5; +} + +message TouchScreenTaskAlignmentCalibrationConfig { + .cmvr.common.Mat4 hand_tag_to_tcp = 1; + reserved 2; + reserved "external_camera_to_base"; } message TouchScreenTaskAlignmentConfig { reserved 1; reserved "kinematics"; - TouchScreenIbvsConfig ibvs = 2; TouchScreenAlignmentTargetConfig target = 3; .cmvr.common.Vec6 error_threshold = 4; optional int32 stable_frames = 5; + // Separate time limit for alignment and, when enabled, the pause after reaching. + // The pause timer starts on entering ALIGN_REACHED. Either timeout fails the + // task and attempts to return to initialization when after_finish is enabled. optional double timeout_s = 6; + // Pause after alignment instead of starting touch, until timeout_s expires. optional bool pause_when_reached = 7; + TouchScreenTaskPbvsConfig pbvs = 8; + TouchScreenTaskAlignmentCalibrationConfig calibration = 9; } message TouchScreenTouchSpeedLConfig { .cmvr.common.Vec6 twist_tool = 1; optional double acceleration = 2; optional double max_distance_m = 3; + // Requested linear jerk during approach, m/s^3, capped by the arm's + // linear_jerk_max. Unset uses that arm limit. + optional double linear_jerk = 4; } message TouchScreenTouchMoveLConfig { @@ -68,6 +115,8 @@ message TouchScreenTactileTriggerConfig { optional TouchScreenFingerType finger = 1; optional TouchScreenTactileRegion region = 2; optional TouchScreenTactileCriterion criterion = 3; + // Threshold in newtons (N), applied to the selected force criterion summed + // over the requested tactile regions. Equality also triggers contact. optional double force_threshold = 4; } @@ -83,8 +132,15 @@ message TouchScreenTaskTouchConfig { message TouchScreenTaskRetractConfig { .cmvr.common.Vec6 twist_tool = 1; + // Requested acceleration, capped by the arm's speedL acceleration limits. optional double acceleration = 2; - optional double duration_s = 3; + reserved 3; + reserved "duration_s"; + // Signed displacement along the retract direction before stopping, in meters. + optional double distance_m = 4; + // Requested linear jerk for braking and reversal, m/s^3, capped by the arm's + // linear_jerk_max. Unset uses that arm limit. + optional double linear_jerk = 5; } message TouchScreenTaskConfig { @@ -95,6 +151,10 @@ message TouchScreenTaskConfig { TouchScreenTaskTouchConfig touch = 5; TouchScreenTaskRetractConfig retract = 6; optional string id = 7; + // 是否在外部相机编码后的 gRPC 视频流中绘制坐标系，仅影响显示帧；未配置时默认开启。 + optional bool debug_draw_coordinate_frames = 8; + // G/H 坐标轴长度，单位为米。 + optional double debug_coordinate_axis_length_m = 9; } message TouchScreenTaskRootConfig { 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/request.txt b/request.txt index 8642cc9a..ae926009 100644 --- a/request.txt +++ b/request.txt @@ -33,6 +33,8 @@ third_party/modbus/3.1.11 third_party/visp/3.7.0 third_party/mainif/0.0.5 third_party/matplotplusplus/1.2.0 +third_party/ethercat/v1.7.0 third_party/huayan_robot/v1.0 +third_party/aubo_sdk/v0.27.1 diff --git a/script/ethercat/start_ethercat.sh b/script/ethercat/start_ethercat.sh new file mode 100755 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 100755 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 100755 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 diff --git a/script/test_px_6ax_gen3.sh b/script/test_px_6ax_gen3.sh new file mode 100755 index 00000000..a3e35af7 --- /dev/null +++ b/script/test_px_6ax_gen3.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Real USB sensor test. The executable only sends force-read requests. +# Example: ./script/test_px_6ax_gen3.sh --port /dev/ttyACM0 --duration-s 20 --mode sync +repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)" +build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}" +cmake --build "$build_dir" --target px_6ax_gen3_real_test -j 4 +sensor_build_dir="$build_dir/cmvr-es/devices/dexhand/px_6ax_gen3" +# Put build libraries ahead of installed ones so this tests the current driver/proto. +exec env LD_LIBRARY_PATH="$build_dir:$sensor_build_dir:$build_dir/cmvr-es/hardware:$repo_root/output/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" \ + "$sensor_build_dir/px_6ax_gen3_real_test" "$@" diff --git a/script/test_speedl_reversal_mujoco.sh b/script/test_speedl_reversal_mujoco.sh new file mode 100755 index 00000000..e4b87d89 --- /dev/null +++ b/script/test_speedl_reversal_mujoco.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Headless MuJoCo only. Never initializes the physical robot. +repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)" +build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}" +cmake --build "$build_dir" --target speedl_reversal_mujoco_test -j 4 +cd "$repo_root" +python3 - "$build_dir" "$@" <<'PY' +from pathlib import Path +import os +import sys + +build = Path(sys.argv[1]).resolve() +# Prefer every freshly built project library over output/lib's installed copy. +paths = sorted({str(path.parent) for path in build.rglob('*.so')}) +paths.append(str(Path('output/lib').resolve())) +env = os.environ.copy() +env['LD_LIBRARY_PATH'] = ':'.join(paths + [env.get('LD_LIBRARY_PATH', '')]) +binary = build / 'cmvr-es/devices/arm/motor_robot_arm/speedl_reversal_mujoco_test' +os.execve(str(binary), [str(binary), *sys.argv[2:]], env) +PY diff --git a/scripts/isaac_sim/README.md b/scripts/isaac_sim/README.md new file mode 100644 index 00000000..04e81ab5 --- /dev/null +++ b/scripts/isaac_sim/README.md @@ -0,0 +1,145 @@ +# 简化碰撞体生成器 + +统一入口 `generate_collision_primitives.py` 根据输入和输出扩展名自动处理 URDF 或 +USD。它读取机器人各个 link 的可视网格，并拟合为 box、sphere 或 cylinder；自动 +模式会选择包围体积最小的几何体。 + +URDF 输入会生成一份包含 `` 的新 URDF；USD 输入会生成引用原 USD 的 +overlay。两种流程都不会修改源文件。USD 生成器还会把长度单位、质量单位和 up axis +复制到 overlay 根层，避免使用默认的厘米制和 Y-up 坐标系。当前双臂模型使用米制、 +Z-up 坐标系。 + +## 预览拟合结果 + +使用 `--dry-run` 只计算和打印拟合结果，不生成输出文件： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --dry-run +``` + +调试配置时，可使用 `--only R_ELBOW_R_S` 只拟合一个 link。 + +## 生成并验证 overlay USD + +先生成到 `/tmp` 进行测试： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \ + --output /tmp/dual_arm_collision_test.usda \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --replace \ + --validate +``` + +在 Isaac Sim 中打开 `/tmp/dual_arm_collision_test.usda`，并在 Viewport 中启用 +Guide Geometry，即可查看碰撞体。默认情况下，overlay 会停用输入 USD 中已有的 +网格碰撞实例，避免旧碰撞体和新碰撞体同时生效。 + +确认结果后，可以生成到项目目录： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \ + --output model/xiaoyan_description/dual_arm_collision.usda \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --replace \ + --validate +``` + +## 配置单个 link + +使用 link 名称编写单独配置： + +```toml +[links.R_ELBOW_R_S] +primitive = "cylinder" +axis = "y" +padding = 0.002 +scale = 1.0 +``` + +`primitive` 支持以下取值： + +- `auto`：自动选择包围体积最小的几何体 +- `box`：盒体 +- `sphere`：球体 +- `cylinder`：圆柱体 + +盒体设置 `alignment = "link"` 后，会使用与 link 局部 XYZ 轴平行的 AABB， +不会产生自由旋转的斜包围盒。圆柱体设置 `axis = "x"`、`"y"` 或 `"z"` 后， +圆柱轴会固定到对应的 link 局部轴。 + +其他常用参数： + +- `padding`：在碰撞体外侧增加的绝对尺寸，单位为米 +- `scale`：以碰撞体中心为基准进行整体缩放 +- `enabled = false`：跳过该 link + +## USD 中缺少可视网格时回退到 STL + +如果导入后的 USD 中缺少某个 link 的可视网格，可以使用 `mesh_file` 指向 URDF +使用的原始 STL： + +```toml +[links.L_WRIST_R_S] +mesh_file = "../../model/xiaoyan_description/meshes/L_WRIST_R_S.STL" +primitive = "box" +alignment = "link" +``` + +相对路径以 TOML 配置文件所在目录为基准。STL 顶点必须使用该 link 的局部坐标系。 + +## 直接生成带简化碰撞体的 URDF + +对于 URDF，统一入口会读取每个 link 的 visual mesh，应用 `` 和 +`` 后，把拟合结果写成新的 ``。原始 URDF 不会被修改。 + +先只预览拟合结果： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm.urdf \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --dry-run +``` + +生成并验证新的 URDF： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm.urdf \ + --output model/xiaoyan_description/dual_arm_collision.urdf \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --replace \ + --validate +``` + +默认行为是：只对实际生成碰撞体的 link 删除旧 ``，然后写入一个名为 +`AUTO_COLLISION_BOX`、`AUTO_COLLISION_SPHERE` 或 `AUTO_COLLISION_CYLINDER` 的新碰撞体。 +配置为 `enabled = false` 的 link 完全不改动，所以当前配置会保留 `base_link` 的底座 +圆柱，以及 `R_FINGER_TIP`、`R_CAM` 的原有球体。 + +调试时可用 `--only L_WRIST_P_S` 只处理一个 link。需要保留某个已存在碰撞体并在其后 +追加自动碰撞体时，使用 `--keep-existing`。 + +## 使用 MeshCat 查看 URDF 碰撞体 + +生成后可直接启动 MeshCat 查看器。STL 按 URDF 原始材质显示，亮绿色线框是 URDF 的 +``： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf +``` + +完整选项和关节位置设置方法见 `scripts/meshcat/README.md`。 diff --git a/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-312.pyc b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-312.pyc new file mode 100644 index 00000000..55da4544 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-312.pyc differ diff --git a/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-313.pyc b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-313.pyc new file mode 100644 index 00000000..d44724f3 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-313.pyc differ diff --git a/scripts/isaac_sim/__pycache__/generate_urdf_collision_primitives.cpython-312.pyc b/scripts/isaac_sim/__pycache__/generate_urdf_collision_primitives.cpython-312.pyc new file mode 100644 index 00000000..d2ca3891 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/generate_urdf_collision_primitives.cpython-312.pyc differ diff --git a/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-312.pyc b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-312.pyc new file mode 100644 index 00000000..59bbcfd2 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-312.pyc differ diff --git a/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-313.pyc b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-313.pyc new file mode 100644 index 00000000..117859e4 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-313.pyc differ diff --git a/scripts/isaac_sim/dual_arm_collision.toml b/scripts/isaac_sim/dual_arm_collision.toml new file mode 100644 index 00000000..12f73d69 --- /dev/null +++ b/scripts/isaac_sim/dual_arm_collision.toml @@ -0,0 +1,84 @@ +# 默认配置会应用到所有未单独覆盖的 link。 +[defaults] +# auto 会分别拟合 box、sphere、cylinder，并选择包围体积最小的一种。 +primitive = "auto" +# auto 模式允许参与比较的碰撞体类型。 +allowed_primitives = ["box", "sphere", "cylinder"] +# 碰撞体向外扩张的绝对尺寸，单位为米。0 表示不扩张。 +padding = 0.0 +# 以拟合中心为基准缩放碰撞体。1.0 表示保持原始拟合尺寸。 +scale = 1.0 +# trimesh 搜索最小包围圆柱时的方向采样密度；越大越精确，但计算越慢。 +cylinder_sample_count = 6 +# 圆柱方向优化的角度容差。 +cylinder_angle_tol = 0.001 +# 仅用于 USD：如果输入中仍有旧的 STL 网格碰撞体，在 overlay 中将其停用。 +# URDF 默认替换生成 link 上的旧碰撞体；传入 --keep-existing 可改为追加。 +disable_existing_mesh_collisions = true + +# base_link 已在 dual_arm.urdf 中明确配置了固定圆柱碰撞体 base_column， +# 不需要根据可视模型重复生成。 +[links.base_link] +enabled = false + +# 指尖和相机在 dual_arm.urdf 中已经有明确的 sphere 碰撞体，跳过自动拟合。 +[links.R_FINGER_TIP] +enabled = false + +[links.R_CAM] +enabled = false + +# 躯干使用与 PELVIS_S 局部 XYZ 轴平行的长方体。 +# alignment="link" 表示使用 link-local AABB，不允许盒体自由倾斜。 +[links.PELVIS_S] +primitive = "box" +alignment = "link" + +# 左右长连杆在各自 link 局部坐标中主要沿 Y 轴延伸。 +# 这些 link 使用轴对齐长方体，避免最小体积 OBB 出现肉眼可见的倾斜。 +[links.L_SHOULDER_R_S] +primitive = "box" +alignment = "link" + +[links.R_SHOULDER_R_S] +primitive = "box" +alignment = "link" + +[links.L_SHOULDER_Y_S] +primitive = "box" +alignment = "link" + +[links.R_SHOULDER_Y_S] +primitive = "box" +alignment = "link" + +[links.L_ELBOW_R_S] +primitive = "box" +alignment = "link" + +[links.R_ELBOW_R_S] +primitive = "box" +alignment = "link" + +# 左右前臂使用圆柱体，圆柱轴固定为 link 局部 Y 轴。 +# 如果更重视减少自碰撞误报，也可以改为 primitive="box"、alignment="link"。 +[links.L_WRIST_P_S] +primitive = "cylinder" +axis = "y" + +[links.R_WRIST_P_S] +primitive = "cylinder" +axis = "y" + +# 最新导入的 USD 没有提供左右末端腕部的可视网格点， +# 因此回退到 URDF 使用的原始 STL。mesh_file 相对本 TOML 文件解析。 +# 末端腕部外形不规则且截面不圆，使用 link 轴对齐长方体。 +[links.L_WRIST_R_S] +mesh_file = "../../model/xiaoyan_description/meshes/L_WRIST_R_S.STL" +primitive = "box" +alignment = "link" + +[links.R_WRIST_R_S] +mesh_file = "../../model/xiaoyan_description/meshes/R_WRIST_R_S.STL" +primitive = "box" +alignment = "link" diff --git a/scripts/isaac_sim/generate_collision_primitives.py b/scripts/isaac_sim/generate_collision_primitives.py new file mode 100644 index 00000000..1174ed71 --- /dev/null +++ b/scripts/isaac_sim/generate_collision_primitives.py @@ -0,0 +1,900 @@ +#!/usr/bin/env python3 +"""Fit primitive colliders to a robot's visual meshes and write URDF or USD. + +The input and output formats are selected from their file extensions. URDF +output is a new complete document; USD output is an overlay of the input USD. +The source asset is never modified. +""" + +from __future__ import annotations + +import argparse +import os +from dataclasses import dataclass +from pathlib import Path +import sys +import tomllib +import traceback +import xml.etree.ElementTree as ET +from typing import Any + +import numpy as np +import trimesh + + +GENERATOR_TAG = "trimesh-primitives-v1" +GENERATED_PREFIX = "AUTO_COLLISION_" +PRIMITIVE_TYPES = ("box", "sphere", "cylinder") + + +@dataclass(frozen=True) +class PrimitiveFit: + kind: str + transform: np.ndarray + dimensions: tuple[float, ...] + volume: float + + +def _points_array(points: np.ndarray) -> np.ndarray: + result = np.asarray(points, dtype=np.float64) + if result.ndim != 2 or result.shape[1] != 3 or len(result) < 4: + raise ValueError("at least four 3D points are required") + if not np.isfinite(result).all(): + raise ValueError("points contain NaN or infinity") + return result + + +def fit_box(points: np.ndarray, padding: float = 0.0, scale: float = 1.0) -> PrimitiveFit: + points = _points_array(points) + to_box, extents = trimesh.bounds.oriented_bounds(points) + extents = np.asarray(extents, dtype=np.float64) * scale + 2.0 * padding + transform = np.linalg.inv(np.asarray(to_box, dtype=np.float64)) + return PrimitiveFit("box", transform, tuple(extents), float(np.prod(extents))) + + +def fit_link_aligned_box( + points: np.ndarray, padding: float = 0.0, scale: float = 1.0 +) -> PrimitiveFit: + points = _points_array(points) + lower = points.min(axis=0) + upper = points.max(axis=0) + extents = (upper - lower) * scale + 2.0 * padding + transform = np.eye(4) + transform[:3, 3] = (lower + upper) / 2.0 + return PrimitiveFit("box", transform, tuple(extents), float(np.prod(extents))) + + +def fit_sphere(points: np.ndarray, padding: float = 0.0, scale: float = 1.0) -> PrimitiveFit: + points = _points_array(points) + center, radius = trimesh.nsphere.minimum_nsphere(points) + radius = float(radius) * scale + padding + transform = np.eye(4) + transform[:3, 3] = center + return PrimitiveFit("sphere", transform, (radius,), float(4.0 * np.pi * radius**3 / 3.0)) + + +def fit_cylinder( + points: np.ndarray, + padding: float = 0.0, + scale: float = 1.0, + sample_count: int = 6, + angle_tol: float = 0.001, +) -> PrimitiveFit: + points = _points_array(points) + result = trimesh.bounds.minimum_cylinder( + points, sample_count=sample_count, angle_tol=angle_tol + ) + radius = float(result["radius"]) * scale + padding + height = float(result["height"]) * scale + 2.0 * padding + transform = np.asarray(result["transform"], dtype=np.float64) + volume = float(np.pi * radius**2 * height) + return PrimitiveFit("cylinder", transform, (radius, height), volume) + + +def fit_axis_aligned_cylinder( + points: np.ndarray, + axis: str, + padding: float = 0.0, + scale: float = 1.0, +) -> PrimitiveFit: + points = _points_array(points) + axis = axis.lower() + if axis not in "xyz": + raise ValueError(f"cylinder axis must be x, y, or z: {axis}") + axis_index = "xyz".index(axis) + radial_indices = [index for index in range(3) if index != axis_index] + radial_center, radius = trimesh.nsphere.minimum_nsphere( + points[:, radial_indices] + ) + axial_min = float(points[:, axis_index].min()) + axial_max = float(points[:, axis_index].max()) + + center = np.zeros(3) + center[axis_index] = (axial_min + axial_max) / 2.0 + center[radial_indices] = radial_center + radius = float(radius) * scale + padding + height = (axial_max - axial_min) * scale + 2.0 * padding + + transform = np.eye(4) + if axis == "x": + transform[:3, :3] = np.array( + [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]] + ) + elif axis == "y": + transform[:3, :3] = np.array( + [[1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, -1.0, 0.0]] + ) + transform[:3, 3] = center + volume = float(np.pi * radius**2 * height) + return PrimitiveFit("cylinder", transform, (radius, height), volume) + + +def fit_primitive( + points: np.ndarray, + kind: str, + *, + allowed: list[str], + padding: float, + scale: float, + cylinder_sample_count: int, + cylinder_angle_tol: float, + alignment: str = "oriented", + axis: str | None = None, +) -> PrimitiveFit: + def fit(candidate: str) -> PrimitiveFit: + if candidate == "box": + if alignment == "link": + return fit_link_aligned_box(points, padding, scale) + return fit_box(points, padding, scale) + if candidate == "sphere": + return fit_sphere(points, padding, scale) + if candidate == "cylinder": + if axis: + return fit_axis_aligned_cylinder(points, axis, padding, scale) + return fit_cylinder( + points, + padding, + scale, + cylinder_sample_count, + cylinder_angle_tol, + ) + raise ValueError(f"unsupported primitive type: {candidate}") + + if kind != "auto": + return fit(kind) + + candidates: list[PrimitiveFit] = [] + failures: list[str] = [] + for candidate in allowed: + try: + candidates.append(fit(candidate)) + except Exception as exc: # A degenerate mesh may fail one fitter only. + failures.append(f"{candidate}: {exc}") + if not candidates: + raise RuntimeError("all primitive fits failed: " + "; ".join(failures)) + return min(candidates, key=lambda candidate: candidate.volume) + + +def load_config(path: Path | None) -> dict[str, Any]: + if path is None: + return {} + with path.open("rb") as stream: + config = tomllib.load(stream) + config["_config_dir"] = str(path.parent.resolve()) + return config + + +def link_settings(config: dict[str, Any], link_name: str) -> dict[str, Any]: + settings = dict(config.get("defaults", {})) + settings.update(config.get("links", {}).get(link_name, {})) + return settings + + +def _find_robot_root(stage: Any, requested_path: str | None) -> Any: + if requested_path: + prim = stage.GetPrimAtPath(requested_path) + if not prim: + raise ValueError(f"robot root does not exist: {requested_path}") + return prim + + default_prim = stage.GetDefaultPrim() + if default_prim and default_prim.GetRelationship("isaac:physics:robotLinks").IsValid(): + return default_prim + + for prim in stage.Traverse(): + if prim.GetRelationship("isaac:physics:robotLinks").IsValid(): + return prim + raise RuntimeError("could not find an Isaac robotLinks relationship; pass --robot-root") + + +def find_robot_links(stage: Any, robot_root_path: str | None) -> list[Any]: + root = _find_robot_root(stage, robot_root_path) + targets = root.GetRelationship("isaac:physics:robotLinks").GetTargets() + links = [stage.GetPrimAtPath(path) for path in targets] + links = [prim for prim in links if prim] + if not links: + raise RuntimeError(f"robot has no resolved links: {root.GetPath()}") + return links + + +def _computed_purpose(prim: Any, UsdGeom: Any) -> str: + imageable = UsdGeom.Imageable(prim) + if not imageable: + return "" + return str(imageable.ComputePurpose()) + + +def collect_visual_points(link: Any, link_paths: set[str], Usd: Any, UsdGeom: Any, UsdPhysics: Any) -> tuple[np.ndarray, set[str]]: + """Return visual vertices in link coordinates and direct mesh-collider roots.""" + cache = UsdGeom.XformCache() + link_to_world = cache.GetLocalToWorldTransform(link) + world_to_link = np.asarray(link_to_world.GetInverse(), dtype=np.float64) + point_sets: list[np.ndarray] = [] + mesh_collision_roots: set[str] = set() + + for child in link.GetChildren(): + if str(child.GetPath()) in link_paths or child.GetName().startswith(GENERATED_PREFIX): + continue + + child_has_mesh_collision = False + for prim in Usd.PrimRange(child, Usd.TraverseInstanceProxies()): + if not prim.IsA(UsdGeom.Mesh): + continue + purpose = _computed_purpose(prim, UsdGeom) + is_collision = prim.HasAPI(UsdPhysics.CollisionAPI) or purpose == str(UsdGeom.Tokens.guide) + if is_collision: + child_has_mesh_collision = True + continue + if purpose not in ("", str(UsdGeom.Tokens.default_), str(UsdGeom.Tokens.render)): + continue + + points = np.asarray(UsdGeom.Mesh(prim).GetPointsAttr().Get(), dtype=np.float64) + if not len(points): + continue + mesh_to_world = np.asarray(cache.GetLocalToWorldTransform(prim), dtype=np.float64) + mesh_to_link = mesh_to_world @ world_to_link + local_points = points @ mesh_to_link[:3, :3] + mesh_to_link[3, :3] + point_sets.append(local_points) + + if child_has_mesh_collision: + mesh_collision_roots.add(str(child.GetPath())) + + if not point_sets: + raise RuntimeError(f"no visual mesh vertices found below {link.GetPath()}") + return np.concatenate(point_sets), mesh_collision_roots + + +def collect_mesh_file_points(path: Path) -> np.ndarray: + """Load a URDF visual mesh whose vertices are already link-local.""" + if not path.is_file(): + raise FileNotFoundError(path) + loaded = trimesh.load(path, force="mesh") + if isinstance(loaded, trimesh.Scene): + meshes = tuple(loaded.geometry.values()) + if not meshes: + raise RuntimeError(f"mesh file has no geometry: {path}") + loaded = trimesh.util.concatenate(meshes) + return _points_array(np.asarray(loaded.vertices, dtype=np.float64)) + + +def _set_transform(prim: Any, transform: np.ndarray, scale: tuple[float, float, float] | None, Gf: Any, UsdGeom: Any) -> None: + xformable = UsdGeom.Xformable(prim) + translation = transform[:3, 3] + quaternion = trimesh.transformations.quaternion_from_matrix(transform) + xformable.AddTranslateOp().Set(Gf.Vec3d(*translation.tolist())) + xformable.AddOrientOp(UsdGeom.XformOp.PrecisionDouble).Set( + Gf.Quatd(float(quaternion[0]), Gf.Vec3d(*quaternion[1:4].tolist())) + ) + if scale is not None: + xformable.AddScaleOp().Set(Gf.Vec3d(*scale)) + + +def prepare_authoring_links(stage: Any, link_paths: list[str]) -> None: + """De-instance only branches that contain a link requiring a new child.""" + instance_roots: set[str] = set() + for link_path in link_paths: + link = stage.GetPrimAtPath(link_path) + if not link: + continue + if link.IsInstance(): + instance_roots.add(link_path) + continue + if not link.IsInstanceProxy(): + continue + instance_root = link + while instance_root.IsInstanceProxy(): + instance_root = instance_root.GetParent() + if not instance_root or not instance_root.IsInstance(): + raise RuntimeError(f"could not find an instance root for collision link: {link_path}") + instance_roots.add(str(instance_root.GetPath())) + + if not instance_roots: + return + for instance_root in instance_roots: + stage.OverridePrim(instance_root).SetInstanceable(False) + stage.GetRootLayer().Save() + stage.Reload() + + still_proxies = [ + link_path + for link_path in link_paths + if stage.GetPrimAtPath(link_path).IsInstanceProxy() + ] + if still_proxies: + raise RuntimeError(f"links remained instance proxies: {still_proxies}") + + +def author_primitive(stage: Any, link_path: str, fit: PrimitiveFit, Gf: Any, Sdf: Any, UsdGeom: Any, UsdPhysics: Any) -> str: + prim_path = f"{link_path}/{GENERATED_PREFIX}{fit.kind.upper()}" + if fit.kind == "box": + shape = UsdGeom.Cube.Define(stage, prim_path) + shape.CreateSizeAttr(1.0) + scale = tuple(float(value) for value in fit.dimensions) + elif fit.kind == "sphere": + shape = UsdGeom.Sphere.Define(stage, prim_path) + shape.CreateRadiusAttr(float(fit.dimensions[0])) + scale = None + elif fit.kind == "cylinder": + shape = UsdGeom.Cylinder.Define(stage, prim_path) + shape.CreateAxisAttr(UsdGeom.Tokens.z) + shape.CreateRadiusAttr(float(fit.dimensions[0])) + shape.CreateHeightAttr(float(fit.dimensions[1])) + scale = None + else: + raise AssertionError(fit.kind) + + prim = shape.GetPrim() + _set_transform(prim, fit.transform, scale, Gf, UsdGeom) + UsdPhysics.CollisionAPI.Apply(prim).CreateCollisionEnabledAttr(True) + UsdGeom.Imageable(prim).CreatePurposeAttr(UsdGeom.Tokens.guide) + prim.SetCustomDataByKey("collisionGenerator", GENERATOR_TAG) + prim.CreateAttribute("collision:primitiveType", Sdf.ValueTypeNames.Token, custom=True).Set(fit.kind) + return prim_path + + +def create_overlay_stage( + input_path: Path, + temporary_output: Path, + default_prim_path: str, + source_stage: Any, + Usd: Any, +) -> Any: + stage = Usd.Stage.CreateNew(str(temporary_output)) + relative_input = os.path.relpath(input_path, temporary_output.parent) + stage.GetRootLayer().subLayerPaths = [relative_input] + for metadata_key in ("upAxis", "metersPerUnit", "kilogramsPerUnit"): + metadata_value = source_stage.GetMetadata(metadata_key) + if metadata_value is not None: + stage.SetMetadata(metadata_key, metadata_value) + source_default = stage.GetPrimAtPath(default_prim_path) + if not source_default: + raise RuntimeError(f"default prim did not compose into overlay: {default_prim_path}") + stage.SetDefaultPrim(source_default) + return stage + + +def validate_usd_output(path: Path, expected_count: int, Usd: Any, UsdGeom: Any, UsdPhysics: Any) -> None: + stage = Usd.Stage.Open(str(path)) + if not stage.GetDefaultPrim(): + raise RuntimeError("output USD has no default prim") + generated = [ + prim + for prim in stage.Traverse() + if prim.GetCustomDataByKey("collisionGenerator") == GENERATOR_TAG + ] + if len(generated) != expected_count: + raise RuntimeError(f"expected {expected_count} generated colliders, found {len(generated)}") + for prim in generated: + if prim.GetTypeName() not in ("Cube", "Sphere", "Cylinder"): + raise RuntimeError(f"generated collider is not a primitive: {prim.GetPath()}") + if not prim.HasAPI(UsdPhysics.CollisionAPI): + raise RuntimeError(f"CollisionAPI missing: {prim.GetPath()}") + if _computed_purpose(prim, UsdGeom) != str(UsdGeom.Tokens.guide): + raise RuntimeError(f"guide purpose missing: {prim.GetPath()}") + + +def run_usd(args: argparse.Namespace, Usd: Any, UsdGeom: Any, UsdPhysics: Any, Gf: Any, Sdf: Any) -> int: + input_path = args.input.resolve() + if not input_path.is_file(): + raise FileNotFoundError(input_path) + if not args.dry_run and args.output is None: + raise ValueError("--output is required unless --dry-run is used") + + config = load_config(args.config.resolve() if args.config else None) + defaults = config.get("defaults", {}) + allowed = list(defaults.get("allowed_primitives", PRIMITIVE_TYPES)) + invalid = set(allowed) - set(PRIMITIVE_TYPES) + if invalid: + raise ValueError(f"invalid allowed_primitives: {sorted(invalid)}") + + source_stage = Usd.Stage.Open(str(input_path)) + if not source_stage: + raise RuntimeError(f"could not open USD: {input_path}") + links = find_robot_links(source_stage, args.robot_root) + source_default = source_stage.GetDefaultPrim() + if not source_default: + raise RuntimeError("input USD has no default prim") + link_paths = {str(link.GetPath()) for link in links} + selected = set(args.only) + results: list[tuple[str, PrimitiveFit, set[str]]] = [] + + for link in links: + name = link.GetName() + if selected and name not in selected: + continue + settings = link_settings(config, name) + if not settings.get("enabled", True): + print(f"SKIP {name}: disabled by configuration") + continue + try: + points, collision_roots = collect_visual_points( + link, link_paths, Usd, UsdGeom, UsdPhysics + ) + except RuntimeError as error: + mesh_file = settings.get("mesh_file") + if not mesh_file: + raise RuntimeError( + f"{error}; no mesh_file configured for link name {name!r}" + ) from error + mesh_path = Path(mesh_file) + if not mesh_path.is_absolute(): + mesh_path = Path(config["_config_dir"]) / mesh_path + points = collect_mesh_file_points(mesh_path.resolve()) + collision_roots = set() + print(f"FALLBACK {name}: loaded {mesh_path}") + kind = str(settings.get("primitive", "auto")) + if kind not in (*PRIMITIVE_TYPES, "auto"): + raise ValueError(f"invalid primitive for {name}: {kind}") + fit = fit_primitive( + points, + kind, + allowed=list(settings.get("allowed_primitives", allowed)), + padding=float(settings.get("padding", 0.0)), + scale=float(settings.get("scale", 1.0)), + cylinder_sample_count=int(settings.get("cylinder_sample_count", defaults.get("cylinder_sample_count", 6))), + cylinder_angle_tol=float(settings.get("cylinder_angle_tol", defaults.get("cylinder_angle_tol", 0.001))), + alignment=str(settings.get("alignment", "oriented")), + axis=str(settings["axis"]) if "axis" in settings else None, + ) + results.append((str(link.GetPath()), fit, collision_roots)) + dimensions = ", ".join(f"{value:.6f}" for value in fit.dimensions) + print(f"FIT {name}: {fit.kind} ({dimensions}), vertices={len(points)}, volume={fit.volume:.8f}") + + if selected: + found = {Path(path).name for path, _, _ in results} + missing = selected - found + if missing: + raise ValueError(f"selected links were not generated: {sorted(missing)}") + if args.dry_run: + print(f"Dry run complete: {len(results)} collider(s) fitted") + return 0 + + output_path = args.output.resolve() + if output_path == input_path: + raise ValueError("input and output must be different files") + if output_path.exists() and not args.replace: + raise FileExistsError(f"output exists; pass --replace: {output_path}") + output_path.parent.mkdir(parents=True, exist_ok=True) + temporary = output_path.with_name(f".{output_path.stem}.tmp{output_path.suffix}") + if temporary.exists(): + temporary.unlink() + + print(f"CREATE overlay: {temporary}", flush=True) + output_stage = create_overlay_stage( + input_path, + temporary, + str(source_default.GetPath()), + source_stage, + Usd, + ) + print("CREATE overlay: composed", flush=True) + prepare_authoring_links(output_stage, [link_path for link_path, _, _ in results]) + disable_meshes = bool(defaults.get("disable_existing_mesh_collisions", True)) + for link_path, fit, collision_roots in results: + if disable_meshes: + for collision_root in collision_roots: + output_stage.OverridePrim(collision_root).SetActive(False) + authored = author_primitive( + output_stage, link_path, fit, Gf, Sdf, UsdGeom, UsdPhysics + ) + print(f"WRITE {authored}") + output_stage.GetRootLayer().Save() + del output_stage + os.replace(temporary, output_path) + + if args.validate: + validate_usd_output(output_path, len(results), Usd, UsdGeom, UsdPhysics) + print(f"Validated {len(results)} generated collider(s)") + print(f"Output: {output_path}") + return 0 + + +def _parse_vector( + value: str | None, size: int, default: tuple[float, ...] +) -> np.ndarray: + if value is None: + return np.asarray(default, dtype=np.float64) + result = np.fromstring(value, sep=" ", dtype=np.float64) + if len(result) != size or not np.isfinite(result).all(): + raise ValueError(f"expected {size} finite values, got {value!r}") + return result + + +def _urdf_origin_transform(origin: ET.Element | None) -> np.ndarray: + if origin is None: + return np.eye(4) + xyz = _parse_vector(origin.get("xyz"), 3, (0.0, 0.0, 0.0)) + rpy = _parse_vector(origin.get("rpy"), 3, (0.0, 0.0, 0.0)) + transform = trimesh.transformations.euler_matrix(*rpy, axes="sxyz") + transform[:3, 3] = xyz + return transform + + +def _resolve_urdf_mesh_path(filename: str, urdf_path: Path) -> Path: + if filename.startswith("file://"): + path = Path(filename.removeprefix("file://")) + elif filename.startswith("package://"): + package_path = Path(filename.removeprefix("package://")) + if len(package_path.parts) < 2: + raise ValueError(f"invalid package URI: {filename}") + package_name, relative_parts = package_path.parts[0], package_path.parts[1:] + candidates = [ + parent / package_name / Path(*relative_parts) + for parent in (urdf_path.parent, *urdf_path.parents) + ] + candidates.extend( + parent / Path(*relative_parts) + for parent in urdf_path.parents + if parent.name == package_name + ) + for candidate in candidates: + if candidate.is_file(): + return candidate.resolve() + raise FileNotFoundError( + f"could not resolve {filename!r} relative to {urdf_path}" + ) + else: + path = Path(filename) + if not path.is_absolute(): + path = urdf_path.parent / path + path = path.resolve() + if not path.is_file(): + raise FileNotFoundError(path) + return path + + +def collect_urdf_visual_points(link: ET.Element, urdf_path: Path) -> np.ndarray: + """Collect all visual mesh vertices in the link-local coordinate frame.""" + point_sets: list[np.ndarray] = [] + for visual in link.findall("visual"): + visual_transform = _urdf_origin_transform(visual.find("origin")) + geometry = visual.find("geometry") + mesh = geometry.find("mesh") if geometry is not None else None + if mesh is None: + continue + filename = mesh.get("filename") + if not filename: + raise ValueError( + f"visual mesh has no filename in link {link.get('name')!r}" + ) + vertices = collect_mesh_file_points( + _resolve_urdf_mesh_path(filename, urdf_path) + ) + mesh_scale = _parse_vector(mesh.get("scale"), 3, (1.0, 1.0, 1.0)) + vertices = trimesh.transform_points(vertices * mesh_scale, visual_transform) + point_sets.append(vertices) + if not point_sets: + raise RuntimeError(f"no visual mesh found in link {link.get('name')!r}") + return np.concatenate(point_sets) + + +def _format_number(value: float) -> str: + if abs(value) < 5e-13: + value = 0.0 + return f"{value:.12g}" + + +def _format_vector(values: np.ndarray | tuple[float, ...]) -> str: + return " ".join(_format_number(float(value)) for value in values) + + +def create_urdf_collision(fit: PrimitiveFit) -> ET.Element: + collision = ET.Element( + "collision", {"name": f"{GENERATED_PREFIX}{fit.kind.upper()}"} + ) + translation = fit.transform[:3, 3] + rpy = trimesh.transformations.euler_from_matrix(fit.transform, axes="sxyz") + ET.SubElement( + collision, + "origin", + {"xyz": _format_vector(translation), "rpy": _format_vector(rpy)}, + ) + geometry = ET.SubElement(collision, "geometry") + if fit.kind == "box": + ET.SubElement(geometry, "box", {"size": _format_vector(fit.dimensions)}) + elif fit.kind == "sphere": + ET.SubElement( + geometry, "sphere", {"radius": _format_number(fit.dimensions[0])} + ) + elif fit.kind == "cylinder": + ET.SubElement( + geometry, + "cylinder", + { + "radius": _format_number(fit.dimensions[0]), + "length": _format_number(fit.dimensions[1]), + }, + ) + else: + raise AssertionError(fit.kind) + return collision + + +def _fit_urdf_link( + link: ET.Element, + urdf_path: Path, + config: dict[str, Any], + defaults: dict[str, Any], + allowed: list[str], +) -> tuple[PrimitiveFit, int]: + name = link.get("name", "") + settings = link_settings(config, name) + try: + points = collect_urdf_visual_points(link, urdf_path) + except RuntimeError as error: + mesh_file = settings.get("mesh_file") + if not mesh_file: + raise RuntimeError(str(error)) from error + mesh_path = Path(mesh_file) + if not mesh_path.is_absolute(): + mesh_path = Path(config["_config_dir"]) / mesh_path + points = collect_mesh_file_points(mesh_path.resolve()) + print(f"FALLBACK {name}: loaded {mesh_path}") + + kind = str(settings.get("primitive", "auto")) + if kind not in (*PRIMITIVE_TYPES, "auto"): + raise ValueError(f"invalid primitive for {name}: {kind}") + link_allowed = list(settings.get("allowed_primitives", allowed)) + invalid = set(link_allowed) - set(PRIMITIVE_TYPES) + if invalid: + raise ValueError(f"invalid allowed_primitives for {name}: {sorted(invalid)}") + fit = fit_primitive( + points, + kind, + allowed=link_allowed, + padding=float(settings.get("padding", 0.0)), + scale=float(settings.get("scale", 1.0)), + cylinder_sample_count=int( + settings.get( + "cylinder_sample_count", defaults.get("cylinder_sample_count", 6) + ) + ), + cylinder_angle_tol=float( + settings.get( + "cylinder_angle_tol", defaults.get("cylinder_angle_tol", 0.001) + ) + ), + alignment=str(settings.get("alignment", "oriented")), + axis=str(settings["axis"]) if "axis" in settings else None, + ) + return fit, len(points) + + +def _parse_urdf(path: Path) -> ET.ElementTree: + parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True)) + tree = ET.parse(path, parser=parser) + root = tree.getroot() + if root.tag != "robot": + raise ValueError(f"URDF root must be , found <{root.tag}>") + return tree + + +def validate_urdf_output(path: Path, expected: dict[str, PrimitiveFit]) -> None: + root = _parse_urdf(path).getroot() + links = {link.get("name", ""): link for link in root.findall("link")} + missing = set(expected) - set(links) + if missing: + raise RuntimeError(f"output URDF is missing links: {sorted(missing)}") + for name, fit in expected.items(): + generated = [ + collision + for collision in links[name].findall("collision") + if collision.get("name", "").startswith(GENERATED_PREFIX) + ] + if len(generated) != 1: + raise RuntimeError( + f"expected one generated collider on {name}, found {len(generated)}" + ) + geometry = generated[0].find("geometry") + primitive_count = ( + sum(geometry.find(kind) is not None for kind in PRIMITIVE_TYPES) + if geometry is not None + else 0 + ) + if primitive_count != 1: + raise RuntimeError(f"invalid generated collision geometry on {name}") + written_transform = _urdf_origin_transform(generated[0].find("origin")) + if not np.allclose(written_transform, fit.transform, atol=1e-9, rtol=1e-9): + error = float(np.max(np.abs(written_transform - fit.transform))) + raise RuntimeError( + f"generated collision transform changed on {name}: max error {error}" + ) + primitive = geometry.find(fit.kind) + if primitive is None: + raise RuntimeError(f"expected {fit.kind} collision geometry on {name}") + if fit.kind == "box": + dimensions = _parse_vector(primitive.get("size"), 3, ()) + elif fit.kind == "sphere": + dimensions = np.asarray([float(primitive.get("radius", "nan"))]) + else: + dimensions = np.asarray( + [ + float(primitive.get("radius", "nan")), + float(primitive.get("length", "nan")), + ] + ) + expected_dimensions = np.asarray(fit.dimensions) + if ( + not np.isfinite(dimensions).all() + or (dimensions <= 0.0).any() + or not np.allclose( + dimensions, expected_dimensions, atol=1e-9, rtol=1e-9 + ) + ): + raise RuntimeError( + f"generated collision dimensions changed on {name}: " + f"expected {expected_dimensions}, found {dimensions}" + ) + + +def run_urdf(args: argparse.Namespace) -> int: + input_path = args.input.resolve() + if not input_path.is_file(): + raise FileNotFoundError(input_path) + if not args.dry_run and args.output is None: + raise ValueError("--output is required unless --dry-run is used") + + config = load_config(args.config.resolve() if args.config else None) + defaults = config.get("defaults", {}) + allowed = list(defaults.get("allowed_primitives", PRIMITIVE_TYPES)) + invalid = set(allowed) - set(PRIMITIVE_TYPES) + if invalid: + raise ValueError(f"invalid allowed_primitives: {sorted(invalid)}") + + tree = _parse_urdf(input_path) + root = tree.getroot() + selected = set(args.only) + known_names = {link.get("name", "") for link in root.findall("link")} + unknown = selected - known_names + if unknown: + raise ValueError(f"selected links do not exist: {sorted(unknown)}") + + results: list[tuple[ET.Element, PrimitiveFit]] = [] + for link in root.findall("link"): + name = link.get("name", "") + if selected and name not in selected: + continue + settings = link_settings(config, name) + if not settings.get("enabled", True): + print(f"SKIP {name}: disabled by configuration") + continue + fit, vertex_count = _fit_urdf_link( + link, input_path, config, defaults, allowed + ) + results.append((link, fit)) + dimensions = ", ".join(f"{value:.6f}" for value in fit.dimensions) + print( + f"FIT {name}: {fit.kind} ({dimensions}), " + f"vertices={vertex_count}, volume={fit.volume:.8f}" + ) + + if args.dry_run: + print(f"Dry run complete: {len(results)} collider(s) fitted") + return 0 + + output_path = args.output.resolve() + if output_path == input_path: + raise ValueError("input and output must be different files") + if output_path.exists() and not args.replace: + raise FileExistsError(f"output exists; pass --replace: {output_path}") + + expected: dict[str, PrimitiveFit] = {} + for link, fit in results: + if not args.keep_existing: + for collision in list(link.findall("collision")): + link.remove(collision) + link.append(create_urdf_collision(fit)) + expected[link.get("name", "")] = fit + + output_path.parent.mkdir(parents=True, exist_ok=True) + temporary = output_path.with_name(f".{output_path.stem}.tmp{output_path.suffix}") + if temporary.exists(): + temporary.unlink() + ET.indent(tree, space=" ") + tree.write(temporary, encoding="utf-8", xml_declaration=True) + os.replace(temporary, output_path) + + if args.validate: + validate_urdf_output(output_path, expected) + print(f"Validated {len(expected)} generated collider(s)") + print(f"Output: {output_path}") + return 0 + + +def parse_args(argv: list[str]) -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--input", required=True, type=Path, help="source URDF or USD") + parser.add_argument("--output", type=Path, help="generated URDF or overlay USD") + parser.add_argument("--config", type=Path, help="TOML fitting configuration") + parser.add_argument( + "--robot-root", help="USD robot root prim path, if auto-detection fails" + ) + parser.add_argument( + "--only", + action="append", + default=[], + metavar="LINK", + help="generate only selected link (repeatable)", + ) + parser.add_argument( + "--keep-existing", + action="store_true", + help="URDF only: append instead of replacing collisions on generated links", + ) + parser.add_argument( + "--dry-run", action="store_true", help="fit and print without writing output" + ) + parser.add_argument( + "--replace", action="store_true", help="atomically replace an existing output" + ) + parser.add_argument( + "--validate", action="store_true", help="reopen and validate generated output" + ) + return parser.parse_args(argv) + + +def _asset_format(path: Path) -> str: + suffix = path.suffix.lower() + if suffix == ".urdf": + return "urdf" + if suffix in (".usd", ".usda", ".usdc"): + return "usd" + raise ValueError( + f"unsupported file extension {path.suffix!r}; expected .urdf, .usd, .usda, or .usdc" + ) + + +def _validate_format_options(args: argparse.Namespace, asset_format: str) -> None: + if args.output is not None and _asset_format(args.output) != asset_format: + raise ValueError("input and output formats must match") + if asset_format == "urdf" and args.robot_root: + raise ValueError("--robot-root is only valid for USD input") + if asset_format == "usd" and args.keep_existing: + raise ValueError("--keep-existing is only valid for URDF input") + + +def main(argv: list[str] | None = None) -> int: + try: + args = parse_args(sys.argv[1:] if argv is None else argv) + asset_format = _asset_format(args.input) + _validate_format_options(args, asset_format) + if asset_format == "urdf": + return run_urdf(args) + + from isaacsim import SimulationApp + + simulation_app = SimulationApp({"headless": True}) + try: + from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics + + return run_usd(args, Usd, UsdGeom, UsdPhysics, Gf, Sdf) + finally: + simulation_app.close() + except Exception: + traceback.print_exc() + sys.stderr.flush() + return 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/meshcat/README.md b/scripts/meshcat/README.md new file mode 100644 index 00000000..09a52583 --- /dev/null +++ b/scripts/meshcat/README.md @@ -0,0 +1,80 @@ +# URDF 碰撞体查看器 + +`view_urdf_collisions.py` 使用 MeshCat 显示 URDF 中的碰撞体。默认按 Isaac Sim 风格 +原样显示视觉 STL 和 URDF 材质，并用亮绿色线框显示 ``。它直接解析 URDF +的 link、joint 和 origin，不依赖 Pinocchio。 + +## 安装 + +当前 `cmvr-es` Conda 环境已经安装 MeshCat。其他环境可执行： + +```bash +python -m pip install -r scripts/meshcat/requirements.txt +``` + +## 运行 + +推荐先激活 `cmvr-es` 环境，再进入项目根目录： + +```bash +conda activate cmvr-es +cd /home/lgv/cmvr/0-workspace/cmvr-es + +python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf +``` + +不能在 `/home/lgv/Desktop` 等其他目录直接使用上述相对路径，否则 Python 会在当前 +目录中查找 `scripts/` 和 `model/`。另外，MeshCat 安装在 `cmvr-es` 环境中，当前提示符 +如果是 `(base)`，需要先执行 `conda activate cmvr-es`。 + +如果需要从任意目录启动，使用完整绝对路径和 `conda run`： + +```bash +conda run -n cmvr-es python \ + /home/lgv/cmvr/0-workspace/cmvr-es/scripts/meshcat/view_urdf_collisions.py \ + --input /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_collision.urdf +``` + +脚本会打开浏览器，并持续运行到按下 `Ctrl+C`。如果不希望自动打开浏览器，可增加 +`--no-browser`，然后手动打开终端输出的 `MeshCat URL`。 + +需要关闭终端后继续查看，或者希望页面刷新后仍然保留场景时，导出独立 HTML： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf \ + --export-html /tmp/dual_arm_collision_meshcat.html +``` + +独立 HTML 会嵌入视觉 STL 和碰撞体，因此文件较大，但不再依赖后台 Python 进程或 +WebSocket 连接。 + +只显示碰撞体： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf \ + --collision-only +``` + +设置关节位置，旋转关节的单位为弧度： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf \ + --joint L_SHOULDER_P=0.5 \ + --joint R_SHOULDER_P=-0.5 +``` + +其他显示选项： + +- `--solid-collisions`：把碰撞体切换为半透明实体 +- `--visual-opacity 0.15`：需要透视内部时降低视觉模型透明度 +- `--collision-opacity 0.8`：调整碰撞体线框或实体透明度 +- `--collision-color 00ff66`：使用十六进制颜色覆盖默认绿色 +- `--collision-cylinder-lines 12`：默认 12 条轴向母线，每 30° 一条 diff --git a/scripts/meshcat/__pycache__/view_urdf_collisions.cpython-313.pyc b/scripts/meshcat/__pycache__/view_urdf_collisions.cpython-313.pyc new file mode 100644 index 00000000..c64f7e7a Binary files /dev/null and b/scripts/meshcat/__pycache__/view_urdf_collisions.cpython-313.pyc differ diff --git a/scripts/meshcat/requirements.txt b/scripts/meshcat/requirements.txt new file mode 100644 index 00000000..a8ef6068 --- /dev/null +++ b/scripts/meshcat/requirements.txt @@ -0,0 +1,2 @@ +meshcat==0.3.2 +numpy>=1.24 diff --git a/scripts/meshcat/view_urdf_collisions.py b/scripts/meshcat/view_urdf_collisions.py new file mode 100644 index 00000000..c8f81964 --- /dev/null +++ b/scripts/meshcat/view_urdf_collisions.py @@ -0,0 +1,721 @@ +#!/usr/bin/env python3 +"""Visualize URDF collision geometry and optional visual meshes in MeshCat.""" + +from __future__ import annotations + +import argparse +from dataclasses import dataclass +import math +import os +from pathlib import Path +import subprocess +import sys +import time +import traceback +import webbrowser +import xml.etree.ElementTree as ET + +import meshcat +import meshcat.geometry as geometry +import numpy as np + + +@dataclass(frozen=True) +class Joint: + name: str + kind: str + parent: str + child: str + origin: np.ndarray + axis: np.ndarray + mimic: tuple[str, float, float] | None + + +def parse_vector( + value: str | None, size: int, default: tuple[float, ...] +) -> np.ndarray: + if value is None: + return np.asarray(default, dtype=np.float64) + parts = value.split() + if len(parts) != size: + raise ValueError(f"expected {size} values, got {value!r}") + result = np.asarray([float(part) for part in parts], dtype=np.float64) + if not np.isfinite(result).all(): + raise ValueError(f"values contain NaN or infinity: {value!r}") + return result + + +def rpy_rotation(rpy: np.ndarray) -> np.ndarray: + roll, pitch, yaw = rpy + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + return np.array( + [ + [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], + [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], + [-sp, cp * sr, cp * cr], + ], + dtype=np.float64, + ) + + +def origin_transform(origin: ET.Element | None) -> np.ndarray: + transform = np.eye(4) + if origin is None: + return transform + transform[:3, :3] = rpy_rotation( + parse_vector(origin.get("rpy"), 3, (0.0, 0.0, 0.0)) + ) + transform[:3, 3] = parse_vector( + origin.get("xyz"), 3, (0.0, 0.0, 0.0) + ) + return transform + + +def axis_angle_transform(axis: np.ndarray, angle: float) -> np.ndarray: + norm = float(np.linalg.norm(axis)) + if norm < 1e-12: + raise ValueError("joint axis must not be zero") + x, y, z = axis / norm + c, s = math.cos(angle), math.sin(angle) + one_minus_c = 1.0 - c + transform = np.eye(4) + transform[:3, :3] = np.array( + [ + [c + x * x * one_minus_c, x * y * one_minus_c - z * s, x * z * one_minus_c + y * s], + [y * x * one_minus_c + z * s, c + y * y * one_minus_c, y * z * one_minus_c - x * s], + [z * x * one_minus_c - y * s, z * y * one_minus_c + x * s, c + z * z * one_minus_c], + ], + dtype=np.float64, + ) + return transform + + +def translation_transform(offset: np.ndarray) -> np.ndarray: + transform = np.eye(4) + transform[:3, 3] = offset + return transform + + +def parse_joint_values(values: list[str]) -> dict[str, float]: + result: dict[str, float] = {} + for assignment in values: + name, separator, raw_value = assignment.partition("=") + if not separator or not name or not raw_value: + raise ValueError( + f"invalid --joint value {assignment!r}; expected NAME=VALUE" + ) + if name in result: + raise ValueError(f"joint value specified more than once: {name}") + value = float(raw_value) + if not math.isfinite(value): + raise ValueError(f"joint value must be finite: {assignment!r}") + result[name] = value + return result + + +def parse_joints(root: ET.Element) -> dict[str, Joint]: + joints: dict[str, Joint] = {} + children: set[str] = set() + for element in root.findall("joint"): + name = element.get("name") + kind = element.get("type") + parent_element = element.find("parent") + child_element = element.find("child") + if not name or not kind or parent_element is None or child_element is None: + raise ValueError("every joint needs name, type, parent, and child") + parent = parent_element.get("link") + child = child_element.get("link") + if not parent or not child: + raise ValueError(f"joint {name!r} has an empty parent or child") + if name in joints: + raise ValueError(f"duplicate joint name: {name}") + if child in children: + raise ValueError(f"link {child!r} has more than one parent joint") + axis_element = element.find("axis") + axis = parse_vector( + axis_element.get("xyz") if axis_element is not None else None, + 3, + (1.0, 0.0, 0.0), + ) + mimic_element = element.find("mimic") + mimic = None + if mimic_element is not None: + source = mimic_element.get("joint") + if not source: + raise ValueError(f"mimic joint {name!r} has no source joint") + mimic = ( + source, + float(mimic_element.get("multiplier", "1")), + float(mimic_element.get("offset", "0")), + ) + joints[name] = Joint( + name=name, + kind=kind, + parent=parent, + child=child, + origin=origin_transform(element.find("origin")), + axis=axis, + mimic=mimic, + ) + children.add(child) + return joints + + +def resolve_joint_values( + joints: dict[str, Joint], requested: dict[str, float] +) -> dict[str, float]: + unknown = set(requested) - set(joints) + if unknown: + raise ValueError(f"unknown joints: {sorted(unknown)}") + fixed = [name for name in requested if joints[name].kind == "fixed"] + if fixed: + raise ValueError(f"fixed joints cannot be assigned: {sorted(fixed)}") + + resolved: dict[str, float] = {} + + def resolve(name: str, stack: set[str]) -> float: + if name in resolved: + return resolved[name] + if name in stack: + raise ValueError(f"mimic joint cycle contains {name!r}") + joint = joints[name] + if name in requested: + value = requested[name] + elif joint.mimic is not None: + source, multiplier, offset = joint.mimic + if source not in joints: + raise ValueError( + f"mimic joint {name!r} references unknown joint {source!r}" + ) + value = multiplier * resolve(source, stack | {name}) + offset + else: + value = 0.0 + resolved[name] = value + return value + + for joint_name in joints: + resolve(joint_name, set()) + return resolved + + +def joint_motion(joint: Joint, value: float) -> np.ndarray: + if joint.kind == "fixed": + return np.eye(4) + if joint.kind in ("revolute", "continuous"): + return axis_angle_transform(joint.axis, value) + if joint.kind == "prismatic": + norm = float(np.linalg.norm(joint.axis)) + if norm < 1e-12: + raise ValueError(f"joint {joint.name!r} axis must not be zero") + axis = joint.axis / norm + return translation_transform(axis * value) + raise ValueError( + f"joint {joint.name!r} uses unsupported type {joint.kind!r}; " + "supported types are fixed, revolute, continuous, and prismatic" + ) + + +def compute_link_transforms( + root: ET.Element, joints: dict[str, Joint], values: dict[str, float] +) -> dict[str, np.ndarray]: + links = {element.get("name") for element in root.findall("link")} + if None in links: + raise ValueError("every link needs a name") + children = {joint.child for joint in joints.values()} + roots = links - children + if not roots: + raise ValueError("URDF has no root link") + + transforms = {name: np.eye(4) for name in roots} + pending = list(joints.values()) + while pending: + unresolved: list[Joint] = [] + for joint in pending: + if joint.parent not in links or joint.child not in links: + raise ValueError( + f"joint {joint.name!r} references a missing parent or child link" + ) + if joint.parent not in transforms: + unresolved.append(joint) + continue + transforms[joint.child] = ( + transforms[joint.parent] + @ joint.origin + @ joint_motion(joint, values[joint.name]) + ) + if len(unresolved) == len(pending): + names = [joint.name for joint in unresolved] + raise ValueError(f"joint graph is cyclic or disconnected: {names}") + pending = unresolved + return transforms + + +def resolve_mesh_path(filename: str, urdf_path: Path) -> Path: + if filename.startswith("file://"): + path = Path(filename.removeprefix("file://")) + elif filename.startswith("package://"): + package_path = Path(filename.removeprefix("package://")) + if len(package_path.parts) < 2: + raise ValueError(f"invalid package URI: {filename}") + package_name, relative_parts = package_path.parts[0], package_path.parts[1:] + candidates = [ + parent / package_name / Path(*relative_parts) + for parent in (urdf_path.parent, *urdf_path.parents) + ] + candidates.extend( + parent / Path(*relative_parts) + for parent in urdf_path.parents + if parent.name == package_name + ) + for candidate in candidates: + if candidate.is_file(): + return candidate.resolve() + raise FileNotFoundError( + f"could not resolve {filename!r} relative to {urdf_path}" + ) + else: + path = Path(filename) + if not path.is_absolute(): + path = urdf_path.parent / path + path = path.resolve() + if not path.is_file(): + raise FileNotFoundError(path) + return path + + +def load_mesh(path: Path) -> geometry.Geometry: + suffix = path.suffix.lower() + if suffix == ".stl": + return geometry.StlMeshGeometry.from_file(str(path)) + if suffix == ".obj": + return geometry.ObjMeshGeometry.from_file(str(path)) + if suffix == ".dae": + return geometry.DaeMeshGeometry.from_file(str(path)) + raise ValueError( + f"unsupported mesh format {path.suffix!r}: {path}; " + "MeshCat viewer supports STL, OBJ, and DAE" + ) + + +def cylinder_dimensions(cylinder: ET.Element) -> tuple[float, float]: + radius = float(cylinder.get("radius", "nan")) + length = float(cylinder.get("length", "nan")) + if ( + not math.isfinite(radius) + or not math.isfinite(length) + or radius <= 0.0 + or length <= 0.0 + ): + raise ValueError( + f"cylinder radius and length must be positive: {radius}, {length}" + ) + return radius, length + + +def cylinder_correction() -> np.ndarray: + correction = np.eye(4) + # Three.js cylinders use local Y; URDF cylinders use local Z. + correction[:3, :3] = rpy_rotation(np.array([math.pi / 2.0, 0.0, 0.0])) + return correction + + +def cylinder_wireframe( + radius: float, + length: float, + generator_count: int, + color: int, + opacity_value: float, + ring_segments: int = 64, +) -> geometry.LineSegments: + vertices: list[tuple[float, float, float]] = [] + half_length = length / 2.0 + + # Smooth top and bottom rings, without cap triangulation spokes. + for y in (-half_length, half_length): + for index in range(ring_segments): + first = 2.0 * math.pi * index / ring_segments + second = 2.0 * math.pi * (index + 1) / ring_segments + vertices.extend( + [ + (radius * math.cos(first), y, radius * math.sin(first)), + (radius * math.cos(second), y, radius * math.sin(second)), + ] + ) + + # Sparse axial generator lines; six means one line every 60 degrees. + for index in range(generator_count): + angle = 2.0 * math.pi * index / generator_count + x = radius * math.cos(angle) + z = radius * math.sin(angle) + vertices.extend([(x, -half_length, z), (x, half_length, z)]) + + points = np.asarray(vertices, dtype=np.float32).T + material = geometry.LineBasicMaterial( + color=color, + transparent=opacity_value < 1.0, + opacity=opacity_value, + ) + return geometry.LineSegments(geometry.PointsGeometry(points), material) + + +def geometry_object( + geometry_element: ET.Element, urdf_path: Path +) -> tuple[geometry.Geometry, np.ndarray]: + box = geometry_element.find("box") + sphere = geometry_element.find("sphere") + cylinder = geometry_element.find("cylinder") + mesh = geometry_element.find("mesh") + correction = np.eye(4) + + if box is not None: + size = parse_vector(box.get("size"), 3, ()) + if (size <= 0.0).any(): + raise ValueError(f"box size must be positive: {size}") + return geometry.Box(size), correction + if sphere is not None: + radius = float(sphere.get("radius", "nan")) + if not math.isfinite(radius) or radius <= 0.0: + raise ValueError(f"sphere radius must be positive: {radius}") + return geometry.Sphere(radius), correction + if cylinder is not None: + radius, length = cylinder_dimensions(cylinder) + return geometry.Cylinder(length, radius), cylinder_correction() + if mesh is not None: + filename = mesh.get("filename") + if not filename: + raise ValueError("mesh geometry has no filename") + scale = parse_vector(mesh.get("scale"), 3, (1.0, 1.0, 1.0)) + correction[:3, :3] = np.diag(scale) + return load_mesh(resolve_mesh_path(filename, urdf_path)), correction + raise ValueError("geometry must contain box, sphere, cylinder, or mesh") + + +def parse_rgba(value: str) -> tuple[float, float, float, float]: + rgba = parse_vector(value, 4, ()) + if ((rgba < 0.0) | (rgba > 1.0)).any(): + raise ValueError(f"RGBA values must be between 0 and 1: {value!r}") + return tuple(float(component) for component in rgba) + + +def rgb_integer(rgb: tuple[float, float, float]) -> int: + red, green, blue = (round(component * 255.0) for component in rgb) + return (red << 16) | (green << 8) | blue + + +def visual_rgba( + visual: ET.Element, + named_materials: dict[str, tuple[float, float, float, float]], +) -> tuple[float, float, float, float]: + material = visual.find("material") + if material is None: + return (0.65, 0.68, 0.72, 1.0) + color = material.find("color") + if color is not None and color.get("rgba"): + return parse_rgba(color.get("rgba", "")) + name = material.get("name") + if name and name in named_materials: + return named_materials[name] + return (0.65, 0.68, 0.72, 1.0) + + +def named_materials( + root: ET.Element, +) -> dict[str, tuple[float, float, float, float]]: + result: dict[str, tuple[float, float, float, float]] = {} + for material in root.findall("material"): + name = material.get("name") + color = material.find("color") + if name and color is not None and color.get("rgba"): + result[name] = parse_rgba(color.get("rgba", "")) + return result + + +def parse_color(value: str) -> int: + normalized = value.removeprefix("#").removeprefix("0x") + if len(normalized) != 6: + raise argparse.ArgumentTypeError("color must use RRGGBB format") + try: + result = int(normalized, 16) + except ValueError as error: + raise argparse.ArgumentTypeError("color must use RRGGBB format") from error + return result + + +def opacity(value: str) -> float: + result = float(value) + if not 0.0 <= result <= 1.0: + raise argparse.ArgumentTypeError("opacity must be between 0 and 1") + return result + + +def cylinder_lines(value: str) -> int: + result = int(value) + if result < 3: + raise argparse.ArgumentTypeError("cylinder line count must be at least 3") + return result + + +def safe_name(value: str) -> str: + return value.replace("/", "_") + + +def render_urdf( + viewer: meshcat.Visualizer, + root: ET.Element, + urdf_path: Path, + link_transforms: dict[str, np.ndarray], + *, + collision_only: bool, + visual_opacity: float, + collision_opacity: float, + collision_color: int, + wireframe: bool, + collision_cylinder_lines: int, +) -> tuple[int, int]: + viewer.delete() + materials = named_materials(root) + visual_count = 0 + collision_count = 0 + collision_material = geometry.MeshPhongMaterial( + color=collision_color, + transparent=collision_opacity < 1.0, + opacity=collision_opacity, + wireframe=wireframe, + ) + + for link in root.findall("link"): + link_name = link.get("name", "") + link_transform = link_transforms[link_name] + if not collision_only: + for index, visual in enumerate(link.findall("visual")): + geometry_element = visual.find("geometry") + if geometry_element is None: + raise ValueError(f"visual geometry missing on link {link_name!r}") + shape, correction = geometry_object(geometry_element, urdf_path) + rgba = visual_rgba(visual, materials) + alpha = visual_opacity * rgba[3] + material = geometry.MeshPhongMaterial( + color=rgb_integer(rgba[:3]), + transparent=alpha < 1.0, + opacity=alpha, + ) + node = viewer[ + f"robot/visual/{safe_name(link_name)}/visual_{index}" + ] + node.set_object(shape, material) + node.set_transform( + link_transform + @ origin_transform(visual.find("origin")) + @ correction + ) + visual_count += 1 + + for index, collision in enumerate(link.findall("collision")): + geometry_element = collision.find("geometry") + if geometry_element is None: + raise ValueError(f"collision geometry missing on link {link_name!r}") + cylinder = geometry_element.find("cylinder") + if wireframe and cylinder is not None: + radius, length = cylinder_dimensions(cylinder) + shape = cylinder_wireframe( + radius, + length, + collision_cylinder_lines, + collision_color, + collision_opacity, + ) + correction = cylinder_correction() + custom_line_object = True + else: + shape, correction = geometry_object(geometry_element, urdf_path) + custom_line_object = False + node = viewer[ + f"robot/collision/{safe_name(link_name)}/collision_{index}" + ] + if custom_line_object: + node.set_object(shape) + else: + node.set_object(shape, collision_material) + node.set_transform( + link_transform + @ origin_transform(collision.find("origin")) + @ correction + ) + collision_count += 1 + return visual_count, collision_count + + +def close_viewer(viewer: meshcat.Visualizer) -> None: + """Close MeshCat 0.3.x without relying on its broken Visualizer.close().""" + window = viewer.window + window.zmq_socket.close(linger=0) + server_process = window.server_proc + if server_process is None or server_process.poll() is not None: + return + server_process.terminate() + try: + server_process.wait(timeout=3.0) + except subprocess.TimeoutExpired: + server_process.kill() + server_process.wait(timeout=3.0) + + +def export_static_html(viewer: meshcat.Visualizer, output: Path) -> None: + output = output.resolve() + if output.suffix.lower() != ".html": + raise ValueError(f"MeshCat snapshot must use an .html extension: {output}") + output.parent.mkdir(parents=True, exist_ok=True) + temporary = output.with_name(f".{output.stem}.tmp{output.suffix}") + temporary.write_text(viewer.static_html(), encoding="utf-8") + os.replace(temporary, output) + + +def parse_args(argv: list[str]) -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--input", required=True, type=Path, help="URDF to display") + parser.add_argument( + "--joint", + action="append", + default=[], + metavar="NAME=VALUE", + help="joint position in radians, or meters for prismatic joints", + ) + parser.add_argument( + "--collision-only", + action="store_true", + help="hide visual geometry and display only collisions", + ) + parser.add_argument( + "--visual-opacity", + type=opacity, + default=1.0, + help="visual geometry opacity (default: 1.0)", + ) + parser.add_argument( + "--collision-opacity", + type=opacity, + default=1.0, + help="collision geometry opacity (default: 1.0)", + ) + parser.add_argument( + "--collision-color", + type=parse_color, + default=parse_color("00ff00"), + metavar="RRGGBB", + help="collision color in hexadecimal (default: 00ff00)", + ) + parser.add_argument( + "--collision-cylinder-lines", + "--collision-cylinder-segments", + dest="collision_cylinder_lines", + type=cylinder_lines, + default=12, + metavar="COUNT", + help="axial lines on collision cylinders (default: 12, every 30 degrees)", + ) + collision_style = parser.add_mutually_exclusive_group() + collision_style.add_argument( + "--wireframe", + dest="wireframe", + action="store_true", + default=True, + help="draw collision geometry as wireframe (default)", + ) + collision_style.add_argument( + "--solid-collisions", + dest="wireframe", + action="store_false", + help="draw collision geometry as translucent solids", + ) + parser.add_argument( + "--no-browser", action="store_true", help="do not automatically open a browser" + ) + parser.add_argument( + "--export-html", + type=Path, + help="write a standalone MeshCat HTML snapshot and exit", + ) + parser.add_argument( + "--exit-after-load", + action="store_true", + help=argparse.SUPPRESS, + ) + parser.add_argument( + "--zmq-url", help="connect to an existing MeshCat ZMQ server" + ) + return parser.parse_args(argv) + + +def run(args: argparse.Namespace) -> int: + urdf_path = args.input.resolve() + if not urdf_path.is_file(): + raise FileNotFoundError(urdf_path) + if urdf_path.suffix.lower() != ".urdf": + raise ValueError(f"input must be a .urdf file: {urdf_path}") + root = ET.parse(urdf_path).getroot() + if root.tag != "robot": + raise ValueError(f"URDF root must be , found <{root.tag}>") + + joints = parse_joints(root) + requested = parse_joint_values(args.joint) + values = resolve_joint_values(joints, requested) + link_transforms = compute_link_transforms(root, joints, values) + + viewer = meshcat.Visualizer(zmq_url=args.zmq_url) + visual_count, collision_count = render_urdf( + viewer, + root, + urdf_path, + link_transforms, + collision_only=args.collision_only, + visual_opacity=args.visual_opacity, + collision_opacity=args.collision_opacity, + collision_color=args.collision_color, + wireframe=args.wireframe, + collision_cylinder_lines=args.collision_cylinder_lines, + ) + if collision_count == 0: + raise RuntimeError(f"URDF contains no elements: {urdf_path}") + + url = viewer.url() + print(f"Loaded: {urdf_path}") + print(f"Visual geometry: {visual_count}") + print(f"Collision geometry: {collision_count}") + print(f"MeshCat URL: {url}", flush=True) + if args.export_html is not None: + export_static_html(viewer, args.export_html) + snapshot_uri = args.export_html.resolve().as_uri() + print(f"Standalone snapshot: {snapshot_uri}", flush=True) + if not args.no_browser: + webbrowser.open(snapshot_uri, new=2) + close_viewer(viewer) + return 0 + if not args.no_browser: + viewer.open() + if args.exit_after_load: + close_viewer(viewer) + return 0 + + print("Press Ctrl+C to stop the viewer.", flush=True) + try: + while True: + time.sleep(1.0) + except KeyboardInterrupt: + print("Stopping MeshCat viewer.") + finally: + close_viewer(viewer) + return 0 + + +def main(argv: list[str] | None = None) -> int: + try: + return run(parse_args(sys.argv[1:] if argv is None else argv)) + except Exception: + traceback.print_exc() + return 1 + + +if __name__ == "__main__": + raise SystemExit(main())