From 0257ac85cae224551a8e4702d71aa46107035bdc Mon Sep 17 00:00:00 2001 From: lgv Date: Mon, 27 Jul 2026 15:37:39 +0800 Subject: [PATCH] feat(collision): add self-collision monitoring task Add Pinocchio and Coal based self-collision checking with collision-pair filtering and displacement-based sampling. Integrate a periodic safety task with warning and stop thresholds, plus the simplified collision URDF and runtime configuration. --- cmvr-es/algorithms/CMakeLists.txt | 1 + .../collision_detection/CMakeLists.txt | 48 ++ .../benchmark/self_collision_benchmark.cpp | 53 ++ .../include/distance_sampling_policy.h | 46 ++ .../include/self_collision_checker.h | 82 +++ .../src/distance_sampling_policy.cpp | 100 ++++ .../src/self_collision_checker.cpp | 403 ++++++++++++++ .../test/self_collision_checker_test.cpp | 110 ++++ cmvr-es/config/manager/task_manager.pb.txt | 8 + .../self_collision_task.pb.txt | 24 + .../manager/task_manager/src/task_manager.cpp | 2 + cmvr-es/task/CMakeLists.txt | 2 + .../include/self_collision_task.h | 69 +++ .../src/self_collision_task.cpp | 304 ++++++++++ cmvr-es/task/task_factory.h | 26 + .../dual_arm_collision.urdf | 517 ++++++++++++++++++ .../self_collision_task_config.proto | 35 ++ .../task_manager_config.proto | 1 + 18 files changed, 1831 insertions(+) create mode 100644 cmvr-es/algorithms/collision_detection/CMakeLists.txt create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp create mode 100644 cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp create mode 100644 cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt create mode 100644 cmvr-es/task/self_collision_task/include/self_collision_task.h create mode 100644 cmvr-es/task/self_collision_task/src/self_collision_task.cpp create mode 100644 model/xiaoyan_description/dual_arm_collision.urdf create mode 100644 protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto 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..cfe18169 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp @@ -0,0 +1,110 @@ +#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", +}; + +std::string collisionUrdfPath() +{ + return std::string(CMVR_ES_SOURCE_DIR) + + "/model/xiaoyan_description/dual_arm_collision.urdf"; +} + +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(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/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index db13dfa9..64db2085 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -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: true + } } diff --git a/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt b/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt new file mode 100644 index 00000000..d6c5623f --- /dev/null +++ b/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt @@ -0,0 +1,24 @@ +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 + } +} 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/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index c8a2960f..e16bbb5f 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/cmvr-es/task/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(task touch_screen_task/src/touch_screen_task.cpp + self_collision_task/src/self_collision_task.cpp ) target_include_directories(task PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -10,6 +11,7 @@ target_link_libraries(task cmvr_es::common cmvr_es::ik_solver cmvr_es::base_motion + cmvr_es::self_collision_checker PRIVATE cmvr_es::device_manager ) diff --git a/cmvr-es/task/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..ae45d795 --- /dev/null +++ b/cmvr-es/task/self_collision_task/include/self_collision_task.h @@ -0,0 +1,69 @@ +#ifndef CMVR_ES_SELF_COLLISION_TASK_H +#define CMVR_ES_SELF_COLLISION_TASK_H + +#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, +}; + +struct SelfCollisionTaskStatus { + CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN}; + SelfCollisionResult result; + bool stop_latched{false}; +}; + +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; + +private: + static bool validateConfig(const config::SelfCollisionTaskConfig& config, + std::string* error); + static const char* safetyLevelToString(CollisionSafetyLevel level); + + config::SelfCollisionTaskConfig config_; + std::string id_; + std::shared_ptr arm_; + SelfCollisionChecker checker_; + DistanceSamplingPolicy sampling_; + + mutable std::mutex mutex_; + TaskState state_{TaskState::UNINITIALIZED}; + SelfCollisionTaskStatus latest_status_{}; + 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..e3a94ba0 --- /dev/null +++ b/cmvr-es/task/self_collision_task/src/self_collision_task.cpp @@ -0,0 +1,304 @@ +#include "task/self_collision_task/include/self_collision_task.h" + +#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"); + } + 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_ = {}; + 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_ = {}; + 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 joint_state = arm->getJointState(); + CollisionGeometrySnapshot snapshot; + std::string error; + if (!checker_.makeSnapshot(joint_state.position, &snapshot, &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + const auto now = DistanceSamplingPolicy::Clock::now(); + if (!sampling_.shouldCheck(snapshot, now)) { + return true; + } + + SelfCollisionResult result = checker_.check(snapshot); + if (!result.valid) { + std::lock_guard lock(mutex_); + last_error_ = result.error.empty() ? "Self-collision distance check failed" : result.error; + state_ = TaskState::FAILED; + return false; + } + 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; + 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 (level == CollisionSafetyLevel::STOP && !latest_status_.stop_latched) { + latest_status_.stop_latched = true; + trigger_stop = true; + } + } + + 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 (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::stop() +{ + std::lock_guard lock(mutex_); + if (state_ != TaskState::FAILED) { + state_ = TaskState::STOPPED; + } +} + +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); + 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; + } + 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"; +} + +} // 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/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/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..ded6d8cd --- /dev/null +++ b/protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto @@ -0,0 +1,35 @@ +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 SelfCollisionTaskConfig { + string id = 1; + string arm_id = 2; + SelfCollisionCheckerConfig checker = 10; + DistanceSamplingConfig sampling = 11; + CollisionSafetyConfig safety = 12; +} + +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"; }