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

@@ -2,3 +2,4 @@ add_subdirectory(motion_planner)
 add_subdirectory(kinematics/ik_solver)
 add_subdirectory(perception)
 add_subdirectory(controllers)
+add_subdirectory(collision_detection)

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)

cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp

@@ -0,0 +1,53 @@
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <string>
+#include <vector>
+
+#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<std::string> 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<double> samples_us;
+    samples_us.reserve(kIterations);
+    std::vector<double> q(joint_names.size(), 0.0);
+    for (std::size_t iteration = 0; iteration < kIterations; ++iteration) {
+        q[0] = 0.2 * static_cast<double>(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<double, std::micro>(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<std::size_t>(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;
+}

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 <chrono>
+#include <string>
+
+#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

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 <cstddef>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <Eigen/Geometry>
+
+namespace cmvr {
+
+struct CollisionPair {
+    std::string first;
+    std::string second;
+};
+
+struct SelfCollisionOptions {
+    std::vector<CollisionPair> 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<CollisionObjectPose, Eigen::aligned_allocator<CollisionObjectPose>> 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<std::string>& active_joint_names,
+              const SelfCollisionOptions& options,
+              std::string* error = nullptr);
+
+    bool makeSnapshot(const std::vector<double>& joint_positions,
+                      CollisionGeometrySnapshot* snapshot,
+                      std::string* error = nullptr);
+
+    SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot);
+    SelfCollisionResult check(const std::vector<double>& joint_positions);
+
+    bool initialized() const;
+    std::size_t dof() const;
+    std::size_t activePairCount() const;
+    const std::vector<std::string>& jointNames() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> impl_;
+};
+
+} // namespace cmvr
+
+#endif // CMVR_ES_SELF_COLLISION_CHECKER_H

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 <algorithm>
+#include <cmath>
+#include <limits>
+
+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<double>(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<double>::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<double>::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

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 <algorithm>
+#include <cmath>
+#include <filesystem>
+#include <limits>
+#include <set>
+#include <sstream>
+#include <unordered_set>
+#include <utility>
+
+#include <pinocchio/algorithm/geometry.hpp>
+#include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/collision/distance.hpp>
+#include <pinocchio/multibody/data.hpp>
+#include <pinocchio/multibody/geometry.hpp>
+#include <pinocchio/multibody/model.hpp>
+#include <pinocchio/parsers/urdf.hpp>
+
+namespace cmvr {
+namespace {
+
+using LinkPairKey = std::pair<std::string, std::string>;
+
+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<std::string>& 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<pinocchio::JointIndex> active_joint_ids;
+        std::unordered_set<std::string> 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<std::string> 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<LinkPairKey> 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<pinocchio::Data>(model_);
+        geometry_data_ = std::make_unique<pinocchio::GeometryData>(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<double>& 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<double>::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<double>& 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<std::string> joint_names_;
+    std::vector<int> joint_q_indices_;
+    std::vector<pinocchio::GeomIndex> selected_geometry_indices_;
+    std::vector<std::string> geometry_link_names_;
+    pinocchio::Model model_;
+    pinocchio::GeometryModel geometry_model_;
+    std::unique_ptr<pinocchio::Data> data_;
+    std::unique_ptr<pinocchio::GeometryData> geometry_data_;
+    Eigen::VectorXd neutral_q_;
+};
+
+SelfCollisionChecker::SelfCollisionChecker()
+    : impl_(std::make_unique<Impl>())
+{
+}
+
+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<std::string>& 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<double>& 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<double>& 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<std::string>& SelfCollisionChecker::jointNames() const
+{
+    return impl_->joint_names_;
+}
+
+} // namespace cmvr

cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp

@@ -0,0 +1,110 @@
+#include <chrono>
+#include <string>
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#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<std::string> 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<double>(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<double>(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

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
+  }
 }

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
+  }
+}

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";

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
 )

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 <memory>
+#include <mutex>
+#include <string>
+
+#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<device::RobotArm> 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

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 <cmath>
+#include <iomanip>
+#include <sstream>
+#include <utility>
+
+#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<device::RobotArm>(
+        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<device::RobotArm> 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

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<Task> createTouchScreenTask(const config::TaskConfigEntry
     return std::make_shared<TouchScreenTask>(cfg);
 }
 
+inline std::shared_ptr<Task> 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<SelfCollisionTask>(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;
         }

model/xiaoyan_description/dual_arm_collision.urdf

@@ -0,0 +1,517 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot name="dual_arm">
+  <!--   <mujoco>-->
+  <!--    <compiler-->
+  <!--    meshdir="meshes"-->
+  <!--    balanceinertia="true"-->
+  <!--    discardvisual="false" />-->
+  <!--   </mujoco>-->
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0.6" rpy="0 0 0" />
+      <geometry>
+        <cylinder radius="0.05" length="1.2" />
+      </geometry>
+      <material name="gray">
+        <color rgba="0.5 0.5 0.5 1.0" />
+      </material>
+    </visual>
+    <collision name="base_column">
+      <origin xyz="0 0 0.6" rpy="0 0 0" />
+      <geometry>
+        <cylinder radius="0.05" length="1.2" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="base_fixed" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 1.2" />
+    <parent link="base_link" />
+    <child link="PELVIS_S" />
+  </joint>
+  <link name="PELVIS_S">
+    <inertial>
+      <origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
+      <mass value="2.10624590271277" />
+      <inertia ixx="0.00165706865324979" ixy="3.13663044821662E-09" ixz="6.84209533216046E-07" iyy="0.00136736758630241" iyz="-1.01014607878816E-10" izz="0.00168295663089359" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="0 0 0.0402499998957" rpy="0 0 0" />
+      <geometry>
+        <box size="0.0769999921322 0.216000005603 0.0804999986589" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="L_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282134 0.0704593083751867 1.15261874861217E-06" rpy="0 0 0" />
+      <mass value="0.880737519698403" />
+      <inertia ixx="0.000583190308378148" ixy="-1.53153074560473E-05" ixz="6.58466471754072E-09" iyy="0.000445532440067177" iyz="6.37001404038516E-09" izz="0.000465648071069952" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.00434836359419 0.0607499987818 3.99221537266e-08" rpy="1.57079632679 0.000113532653235 3.14159265359" />
+      <geometry>
+        <box size="0.0717031309621 0.0729998270933 0.104499998502" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_SHOULDER_P" type="revolute">
+    <origin xyz="0 0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="L_SHOULDER_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+  <link name="L_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975857 0.091739327988169 -1.67085072999562E-08" rpy="0 0 0" />
+      <mass value="0.594788424442483" />
+      <inertia ixx="0.000380970396712656" ixy="4.80751844573946E-05" ixz="-1.34913746586865E-11" iyy="0.000320962178597474" iyz="-1.00039763442409E-09" izz="0.000414771047901155" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.0392500016196 0.0562499994412 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.0785000020061 0.172499997541 0.0649999976158" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 0.0765 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_P_S" />
+    <child link="L_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+  <link name="L_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014801 0.0863620459174068 9.50748668682166E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia ixx="0.000327003834287552" ixy="-1.8057438966771E-05" ixz="7.28778136267901E-10" iyy="0.000213830709361531" iyz="1.49273668517817E-10" izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.000899999402463 0.062000001315 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.0609999988228 0.144000004046 0.0629920661449" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 0.1475 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_R_S" />
+    <child link="L_SHOULDER_Y_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.18" upper="0" effort="80" velocity="3.8758" />
+  </joint>
+  <link name="L_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303443 0.0603199964106564 2.99655911639718E-07" rpy="0 0 0" />
+      <mass value="0.393571904406493" />
+      <inertia ixx="0.00017277119386253" ixy="2.34549801867355E-05" ixz="-1.90560556659271E-09" iyy="0.000155340245267897" iyz="8.82493073600007E-09" izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.0345000004275 0.03925000038 1.86264514923e-09" rpy="0 0 0" />
+      <geometry>
+        <box size="0.068999995688 0.128500001505 0.0579999983311" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_ELBOW_R" type="revolute">
+    <origin xyz="0.034 0.1025 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_Y_S" />
+    <child link="L_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2.05" upper="0" effort="50" velocity="4.71" />
+  </joint>
+  <link name="L_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39659173115092E-10 0.0675972604744393 0.019200551565574" rpy="0 0 0" />
+      <mass value="0.442332465815496" />
+      <inertia ixx="0.000476754055455895" ixy="-4.61505824713347E-15" ixz="6.03808112272229E-18" iyy="0.000103466585850499" iyz="-4.88426705501198E-05" izz="0.000482956345754213" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_CYLINDER">
+      <origin xyz="0 0.0887468701694 0.0122111458903" rpy="-1.57079632679 0 0" />
+      <geometry>
+        <cylinder radius="0.0376536743138" length="0.167493741494" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_WRIST_P" type="revolute">
+    <origin xyz="-0.034 0.0965 0" rpy="0 0 0" />
+    <parent link="L_ELBOW_R_S" />
+    <child link="L_WRIST_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="0" upper="3.14" effort="50" velocity="4.71" />
+  </joint>
+  <link name="L_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887331864 -5.06426789392833E-10 -0.0341253783666609" rpy="0 0 0" />
+      <mass value="0.23573847772002" />
+      <inertia ixx="5.10686940455785E-05" ixy="7.45706654358429E-16" ixz="6.2619568923368E-06" iyy="6.00636019624519E-05" iyz="1.27989654155383E-16" izz="5.32182903609189E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.00375000014901 0 -0.0327469492331" rpy="-1.57079632679 0 1.57079632679" />
+      <geometry>
+        <box size="0.0409999005497 0.0534939002246 0.0595000013709" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_WRIST_Y" type="revolute">
+    <origin xyz="0 0.1525 0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_P_S" />
+    <child link="L_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+  <link name="L_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0161477357620243 0.0955046558857351 -0.00499392489444117" rpy="0 0 0" />
+      <mass value="0.504894112043562" />
+      <inertia ixx="0.000186833711781125" ixy="-3.99488705622731E-06" ixz="-1.51920720398336E-06" iyy="0.000179960980467837" iyz="3.6992669535716E-06" izz="0.000280756101568308" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.00501008890569 0.110783384182 -0.0104531301185" rpy="0 0 0" />
+      <geometry>
+        <box size="0.119021866471 0.253566769883 0.0697322357446" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="L_WRIST_R" type="revolute">
+    <origin xyz="0.0258 0 -0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_Y_S" />
+    <child link="L_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-1.57" upper="0.26" effort="50" velocity="4.71" />
+  </joint>
+  <link name="R_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
+      <mass value="0.880737519698404" />
+      <inertia ixx="0.000583190308378149" ixy="1.53153074560471E-05" ixz="-6.58466471736542E-09" iyy="0.000445532440067178" iyz="6.37001403998779E-09" izz="0.000465648071069953" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.00434836359419 -0.0607499992475 3.99221537266e-08" rpy="1.57079632679 -0.000113532653235 0" />
+      <geometry>
+        <box size="0.0717031309621 0.0729998270933 0.104499997571" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_SHOULDER_P" type="revolute">
+    <origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="R_SHOULDER_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="3.14" effort="120" velocity="3.351" />
+  </joint>
+  <link name="R_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
+      <mass value="0.59478842444248" />
+      <inertia ixx="0.000380970396712653" ixy="-4.80751844573946E-05" ixz="1.34913746041655E-11" iyy="0.000320962178597472" iyz="-1.00039763417375E-09" izz="0.000414771047901152" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.0392500016195 -0.0562499994412 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.0785000020061 0.172499997541 0.0649999976158" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 -0.0765 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_P_S" />
+    <child link="R_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.78" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+  <link name="R_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia ixx="0.000327003834287551" ixy="1.80574389667711E-05" ixz="-7.28778136077729E-10" iyy="0.000213830709361532" iyz="1.49273668428957E-10" izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.000899999402463 -0.0620000017807 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.0609999988228 0.144000003114 0.0629920661449" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 -0.1475 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_R_S" />
+    <child link="R_SHOULDER_Y_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="3.14" effort="80" velocity="3.8758" />
+  </joint>
+  <link name="R_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
+      <mass value="0.393571904406492" />
+      <inertia ixx="0.000172771193862529" ixy="-2.34549801867353E-05" ixz="1.90560556668771E-09" iyy="0.000155340245267897" iyz="8.82493073602971E-09" izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.0345000004275 -0.03925000038 1.86264514923e-09" rpy="0 0 0" />
+      <geometry>
+        <box size="0.068999995688 0.128500001505 0.0579999983311" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_ELBOW_R" type="revolute">
+    <origin xyz="0.034 -0.1025 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_Y_S" />
+    <child link="R_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="2.05" effort="80" velocity="3.8758" />
+  </joint>
+  <link name="R_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
+      <mass value="0.442332465815497" />
+      <inertia ixx="0.000476754055455896" ixy="-4.61462558956188E-15" ixz="-5.91762926004267E-18" iyy="0.0001034665858505" iyz="4.88426705501212E-05" izz="0.000482956345754214" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_CYLINDER">
+      <origin xyz="0 -0.0887468706351 0.0122110777877" rpy="-1.57079632679 0 0" />
+      <geometry>
+        <cylinder radius="0.0376537318089" length="0.167493740562" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_WRIST_P" type="revolute">
+    <origin xyz="-0.034 -0.0965 0" rpy="0 0 0" />
+    <parent link="R_ELBOW_R_S" />
+    <child link="R_WRIST_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="3.14" effort="50" velocity="4.71" />
+  </joint>
+  <link name="R_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
+      <mass value="0.235738477720019" />
+      <inertia ixx="5.1068694045578E-05" ixy="7.45771487459288E-16" ixz="6.26195689233664E-06" iyy="6.00636019624515E-05" iyz="1.27828046342987E-16" izz="5.32182903609188E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.00375000014901 0 -0.0327469492331" rpy="-1.57079632679 0 1.57079632679" />
+      <geometry>
+        <box size="0.0409999005497 0.0534939002246 0.0595000013709" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_WRIST_Y" type="revolute">
+    <origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_P_S" />
+    <child link="R_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+  <link name="R_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
+      <mass value="0.504366058218534" />
+      <inertia ixx="0.000185559065855467" ixy="5.75889766751509E-06" ixz="2.40683898454438E-06" iyy="0.000131246007872084" iyz="1.60533277040148E-06" izz="0.000271800951396149" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision name="AUTO_COLLISION_BOX">
+      <origin xyz="-0.0136338975281 -0.109391543083 -0.014153839089" rpy="0 0 0" />
+      <geometry>
+        <box size="0.111523482949 0.251791322604 0.0779597964138" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_WRIST_R" type="revolute">
+    <origin xyz="0.03 0 -0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_Y_S" />
+    <child link="R_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.57" upper="1.57" effort="50" velocity="4.71" />
+  </joint>
+  <link name="R_FINGER_TIP">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.004" />
+      </geometry>
+      <material name="">
+        <color rgba="0 1 1 1" />
+        <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.005" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_FINGER_TIP_FIXED" type="fixed">
+    <origin xyz="0.00684256  -0.284077 0.00801525" rpy="0 0 0" />
+    <parent link="R_WRIST_R_S" />
+    <child link="R_FINGER_TIP" />
+    <axis xyz="0 0 1" />
+  </joint>
+  <link name="R_CAM">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0" />
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.004" />
+      </geometry>
+      <material name="">
+        <color rgba="0 1 0 1" />
+        <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <sphere radius="0.005" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="R_CAM_FIXED" type="fixed">
+    <origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267 0 3.1415926535" />
+    <parent link="R_WRIST_R_S" />
+    <child link="R_CAM" />
+    <axis xyz="0 0 1" />
+  </joint>
+</robot>

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;
+}

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";
   }