Merge branch 'lgv_dev_collision' into dev

MUJOCO_LOG.TXT

@@ -0,0 +1,6 @@
+Fri Jul 24 15:39:05 2026
+ERROR: could not create window
+
+Fri Jul 24 15:40:37 2026
+ERROR: could not create window
+

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.urdf

@@ -8,44 +8,30 @@
 <!--   </mujoco>-->
 
 
-<!--    <link name="base_link">-->
+  <link name="base_link">
-<!--      <visual>-->
+    <visual>
-<!--        <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
+      <origin xyz="0 0 0.6" rpy="0 0 0"/>
-<!--        <geometry>-->
+      <geometry>
-<!--          <cylinder radius="0.05" length="1.2"/>-->
+        <cylinder radius="0.05" length="1.2"/>
-<!--        </geometry>-->
+      </geometry>
-<!--        <material name="gray">-->
+      <material name="gray">
-<!--          <color rgba="0.5 0.5 0.5 1.0"/>-->
+        <color rgba="0.5 0.5 0.5 1.0"/>
-<!--        </material>-->
+      </material>
-<!--      </visual>-->
+    </visual>
 
-<!--      <collision>-->
+    <collision name="base_column">
-<!--        <origin xyz="0 0 0.6" rpy="0 0 0"/>-->
+      <origin xyz="0 0 0.6" rpy="0 0 0"/>
-<!--        <geometry>-->
+      <geometry>
-<!--          <cylinder radius="0.05" length="1.2"/>-->
+        <cylinder radius="0.05" length="1.2"/>
-<!--        </geometry>-->
+      </geometry>
-<!--      </collision>-->
+    </collision>
+  </link>
 
-<!--        <inertial>-->
+  <joint name="base_fixed" type="fixed">
-
+    <origin rpy="0 0 0" xyz="0 0 1.2"/>
-<!--          <origin xyz="0 0 0" rpy="0 0 0"/>-->
+    <parent link="base_link"/>
-<!--          <mass value="25.4469"/>-->
+    <child link="PELVIS_S"/>
-
+  </joint>
-<!--          <inertia-->
-<!--            ixx="3.06953"-->
-<!--            ixy="0.0"-->
-<!--            ixz="0.0"-->
-<!--            iyy="3.06953"-->
-<!--            iyz="0.0"-->
-<!--            izz="0.03181"/>-->
-<!--        </inertial>-->
-<!--    </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>
@@ -68,12 +54,6 @@
         <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/PELVIS_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <link name="L_SHOULDER_P_S">
@@ -97,12 +77,6 @@
         <color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="L_SHOULDER_P" type="revolute">
@@ -134,12 +108,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="L_SHOULDER_R" type="revolute">
@@ -172,12 +140,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="L_SHOULDER_Y" type="revolute">
@@ -209,12 +171,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/L_ELBOW_R_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="L_ELBOW_R" type="revolute">
@@ -246,12 +202,6 @@
         <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/L_WRIST_P_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="L_WRIST_P" type="revolute">
@@ -283,12 +233,6 @@
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/L_WRIST_Y_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="L_WRIST_Y" type="revolute">
@@ -320,12 +264,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/L_WRIST_R_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="L_WRIST_R" type="revolute">
@@ -357,12 +295,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="R_SHOULDER_P" type="revolute">
@@ -394,12 +326,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="R_SHOULDER_R" type="revolute">
@@ -432,12 +358,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="R_SHOULDER_Y" type="revolute">
@@ -469,12 +389,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/R_ELBOW_R_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="R_ELBOW_R" type="revolute">
@@ -506,12 +420,6 @@
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/R_WRIST_P_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="R_WRIST_P" type="revolute">
@@ -543,12 +451,6 @@
         <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/R_WRIST_Y_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="R_WRIST_Y" type="revolute">
@@ -581,12 +483,6 @@
         <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
       </material>
     </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="meshes/R_WRIST_R_S.STL" />
-      </geometry>
-    </collision>
   </link>
 
   <joint name="R_WRIST_R" type="revolute">

model/xiaoyan_description/dual_arm/dual_arm.usda

@@ -0,0 +1,52 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Xform "dual_arm" (
+    prepend references = @./payloads/base.usda@
+    variants = {
+        string Physics = "physx"
+    }
+    append variantSets = "Physics"
+)
+{
+    variantSet "Physics" = {
+        "mujoco" (
+            prepend payload = @./payloads/Physics/mujoco.usda@
+        ) {
+
+        }
+        "none" {
+
+        }
+        "physics" (
+            prepend payload = @./payloads/Physics/physics.usda@
+        ) {
+
+        }
+        "physx" (
+            prepend payload = @./payloads/Physics/physx.usda@
+        ) {
+
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm/payloads/Physics/mujoco.usda

@@ -0,0 +1,444 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./physics.usda@
+    ]
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Physics"
+    {
+        def MjcActuator "L_SHOULDER_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_P>
+        }
+
+        def MjcActuator "L_SHOULDER_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_R>
+        }
+
+        def MjcActuator "L_SHOULDER_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_Y>
+        }
+
+        def MjcActuator "L_ELBOW_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_ELBOW_R>
+        }
+
+        def MjcActuator "L_WRIST_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_P>
+        }
+
+        def MjcActuator "L_WRIST_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_Y>
+        }
+
+        def MjcActuator "L_WRIST_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_R>
+        }
+
+        def MjcActuator "R_SHOULDER_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_P>
+        }
+
+        def MjcActuator "R_SHOULDER_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_R>
+        }
+
+        def MjcActuator "R_SHOULDER_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_Y>
+        }
+
+        def MjcActuator "R_ELBOW_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_ELBOW_R>
+        }
+
+        def MjcActuator "R_WRIST_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_P>
+        }
+
+        def MjcActuator "R_WRIST_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_Y>
+        }
+
+        def MjcActuator "R_WRIST_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_R>
+        }
+
+        over "root_joint"
+        {
+        }
+
+        over "L_SHOULDER_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_ELBOW_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_ELBOW_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_FINGER_TIP_FIXED"
+        {
+        }
+
+        over "R_CAM_FIXED"
+        {
+        }
+    }
+
+    over "Geometry"
+    {
+        over "PELVIS_S"
+        {
+            over "L_SHOULDER_P_S"
+            {
+                over "L_SHOULDER_R_S"
+                {
+                    over "L_SHOULDER_Y_S"
+                    {
+                        over "L_ELBOW_R_S"
+                        {
+                            over "L_WRIST_P_S"
+                            {
+                                over "L_WRIST_Y_S"
+                                {
+                                    over "L_WRIST_R_S"
+                                    {
+                                        over "L_WRIST_R_S"
+                                        {
+                                        }
+
+                                        over "L_WRIST_R_S_1"
+                                        {
+                                        }
+                                    }
+
+                                    over "L_WRIST_Y_S"
+                                    {
+                                    }
+
+                                    over "L_WRIST_Y_S_1"
+                                    {
+                                    }
+                                }
+
+                                over "L_WRIST_P_S"
+                                {
+                                }
+
+                                over "L_WRIST_P_S_1"
+                                {
+                                }
+                            }
+
+                            over "L_ELBOW_R_S"
+                            {
+                            }
+
+                            over "L_ELBOW_R_S_1"
+                            {
+                            }
+                        }
+
+                        over "L_SHOULDER_Y_S"
+                        {
+                        }
+
+                        over "L_SHOULDER_Y_S_1"
+                        {
+                        }
+                    }
+
+                    over "L_SHOULDER_R_S"
+                    {
+                    }
+
+                    over "L_SHOULDER_R_S_1"
+                    {
+                    }
+                }
+
+                over "L_SHOULDER_P_S"
+                {
+                }
+
+                over "L_SHOULDER_P_S_1"
+                {
+                }
+            }
+
+            over "R_SHOULDER_P_S"
+            {
+                over "R_SHOULDER_R_S"
+                {
+                    over "R_SHOULDER_Y_S"
+                    {
+                        over "R_ELBOW_R_S"
+                        {
+                            over "R_WRIST_P_S"
+                            {
+                                over "R_WRIST_Y_S"
+                                {
+                                    over "R_WRIST_R_S"
+                                    {
+                                        over "R_FINGER_TIP"
+                                        {
+                                            over "sphere"
+                                            {
+                                            }
+
+                                            over "sphere_1"
+                                            {
+                                            }
+                                        }
+
+                                        over "R_CAM"
+                                        {
+                                            over "sphere"
+                                            {
+                                            }
+
+                                            over "sphere_1"
+                                            {
+                                            }
+                                        }
+
+                                        over "R_WRIST_R_S"
+                                        {
+                                        }
+
+                                        over "R_WRIST_R_S_1"
+                                        {
+                                        }
+                                    }
+
+                                    over "R_WRIST_Y_S"
+                                    {
+                                    }
+
+                                    over "R_WRIST_Y_S_1"
+                                    {
+                                    }
+                                }
+
+                                over "R_WRIST_P_S"
+                                {
+                                }
+
+                                over "R_WRIST_P_S_1"
+                                {
+                                }
+                            }
+
+                            over "R_ELBOW_R_S"
+                            {
+                            }
+
+                            over "R_ELBOW_R_S_1"
+                            {
+                            }
+                        }
+
+                        over "R_SHOULDER_Y_S"
+                        {
+                        }
+
+                        over "R_SHOULDER_Y_S_1"
+                        {
+                        }
+                    }
+
+                    over "R_SHOULDER_R_S"
+                    {
+                    }
+
+                    over "R_SHOULDER_R_S_1"
+                    {
+                    }
+                }
+
+                over "R_SHOULDER_P_S"
+                {
+                }
+
+                over "R_SHOULDER_P_S_1"
+                {
+                }
+            }
+
+            over "PELVIS_S"
+            {
+            }
+
+            over "PELVIS_S_1"
+            {
+            }
+        }
+    }
+
+    over "Materials"
+    {
+        over "material_16"
+        {
+        }
+
+        over "material_17"
+        {
+        }
+    }
+
+    over "VisualMaterials"
+    {
+    }
+}
+

model/xiaoyan_description/dual_arm/payloads/Physics/physics.usda

@@ -0,0 +1,530 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Geometry"
+    {
+        over "PELVIS_S" (
+            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI", "PhysicsMassAPI"]
+        )
+        {
+            bool newton:selfCollisionEnabled = 0
+            point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627)
+            float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747)
+            float physics:mass = 2.106246
+            quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252)
+
+            over "L_SHOULDER_P_S" (
+                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+            )
+            {
+                point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188)
+                float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366)
+                float physics:mass = 0.88073754
+                quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823)
+
+                over "L_SHOULDER_R_S" (
+                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                )
+                {
+                    point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8)
+                    float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106)
+                    float physics:mass = 0.59478843
+                    quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278)
+
+                    over "L_SHOULDER_Y_S" (
+                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                    )
+                    {
+                        point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9)
+                        float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517)
+                        float physics:mass = 0.56340605
+                        quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022)
+
+                        over "L_ELBOW_R_S" (
+                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                        )
+                        {
+                            point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7)
+                            float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762)
+                            float physics:mass = 0.3935719
+                            quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766)
+
+                            over "L_WRIST_P_S" (
+                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                            )
+                            {
+                                point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552)
+                                float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185)
+                                float physics:mass = 0.44233248
+                                quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155)
+
+                                over "L_WRIST_Y_S" (
+                                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                )
+                                {
+                                    point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538)
+                                    float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636)
+                                    float physics:mass = 0.23573847
+                                    quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041)
+
+                                    over "L_WRIST_R_S" (
+                                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                    )
+                                    {
+                                        point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925)
+                                        float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117)
+                                        float physics:mass = 0.50489414
+                                        quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542)
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            over "R_SHOULDER_P_S" (
+                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+            )
+            {
+                point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992)
+                float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366)
+                float physics:mass = 0.88073754
+                quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165)
+
+                over "R_SHOULDER_R_S" (
+                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                )
+                {
+                    point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8)
+                    float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106)
+                    float physics:mass = 0.59478843
+                    quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957)
+
+                    over "R_SHOULDER_Y_S" (
+                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                    )
+                    {
+                        point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9)
+                        float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517)
+                        float physics:mass = 0.56340605
+                        quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771)
+
+                        over "R_ELBOW_R_S" (
+                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                        )
+                        {
+                            point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7)
+                            float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762)
+                            float physics:mass = 0.3935719
+                            quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363)
+
+                            over "R_WRIST_P_S" (
+                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                            )
+                            {
+                                point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552)
+                                float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185)
+                                float physics:mass = 0.44233248
+                                quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155)
+
+                                over "R_WRIST_Y_S" (
+                                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                )
+                                {
+                                    point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538)
+                                    float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636)
+                                    float physics:mass = 0.23573847
+                                    quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041)
+
+                                    over "R_WRIST_R_S" (
+                                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                    )
+                                    {
+                                        point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534)
+                                        float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028)
+                                        float physics:mass = 0.50436604
+                                        quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874)
+
+                                        over "R_FINGER_TIP" (
+                                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                        )
+                                        {
+                                            point3f physics:centerOfMass = (0, 0, 0)
+                                            float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001)
+                                            float physics:mass = 0
+                                            quatf physics:principalAxes = (1, 0, 0, 0)
+
+                                            over "sphere_1" (
+                                                prepend apiSchemas = ["NewtonCollisionAPI"]
+                                            )
+                                            {
+                                            }
+                                        }
+
+                                        over "R_CAM" (
+                                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                        )
+                                        {
+                                            point3f physics:centerOfMass = (0, 0, 0)
+                                            float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001)
+                                            float physics:mass = 0
+                                            quatf physics:principalAxes = (1, 0, 0, 0)
+
+                                            over "sphere_1" (
+                                                prepend apiSchemas = ["NewtonCollisionAPI"]
+                                            )
+                                            {
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    over "Physics"
+    {
+        def PhysicsRevoluteJoint "L_SHOULDER_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S>
+            point3f physics:localPos0 = (0, 0.0945, 0.042)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -89.95438
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "L_SHOULDER_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>
+            point3f physics:localPos0 = (0.035, 0.0765, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -114.59156
+            float physics:upperLimit = 114.59156
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "L_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>
+            point3f physics:localPos0 = (-0.035, 0.1475, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -124.9048
+            float physics:upperLimit = 0
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "L_ELBOW_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>
+            point3f physics:localPos0 = (0.034, 0.1025, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -117.456345
+            float physics:upperLimit = 0
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>
+            point3f physics:localPos0 = (-0.034, 0.0965, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Z"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>
+            point3f physics:localPos0 = (0, 0.1525, 0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 44.69071
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 0.79
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S/L_WRIST_R_S>
+            point3f physics:localPos0 = (0.0258, 0, -0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -89.95438
+            float physics:upperLimit = 14.896903
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S>
+            point3f physics:localPos0 = (0, -0.0945, 0.042)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>
+            point3f physics:localPos0 = (0.035, -0.0765, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>
+            point3f physics:localPos0 = (-0.035, -0.1475, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "R_ELBOW_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>
+            point3f physics:localPos0 = (0.034, -0.1025, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 117.456345
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>
+            point3f physics:localPos0 = (-0.034, -0.0965, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Z"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>
+            point3f physics:localPos0 = (0, -0.1525, 0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 44.69071
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 0.79
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            point3f physics:localPos0 = (0.03, 0, -0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -32.658596
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsFixedJoint "root_joint"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S>
+            point3f physics:localPos0 = (0, 0, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "R_FINGER_TIP_FIXED"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_FINGER_TIP>
+            point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "R_CAM_FIXED"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_CAM>
+            point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm/payloads/Physics/physx.usda

@@ -0,0 +1,129 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./physics.usda@
+    ]
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Physics"
+    {
+        over "L_SHOULDER_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "L_SHOULDER_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "L_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "L_ELBOW_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "L_WRIST_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "L_WRIST_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 45.263668
+        }
+
+        over "L_WRIST_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "R_SHOULDER_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "R_SHOULDER_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "R_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "R_ELBOW_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "R_WRIST_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "R_WRIST_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 45.263668
+        }
+
+        over "R_WRIST_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm/payloads/base.usda

@@ -0,0 +1,594 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./robot.usda@
+    ]
+    upAxis = "Z"
+)
+
+def Xform "dual_arm" (
+    prepend apiSchemas = ["GeomModelAPI"]
+    assetInfo = {
+        string name = "dual_arm"
+    }
+    kind = "component"
+)
+{
+    float3[] extentsHint = [(-0.043500002, -0.958077, -0.011133737), (0.080300845, 0.9075668, 0.12106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.043500002, -0.959077, -0.011133737), (0.080300845, 0.9075668, 0.12206)]
+
+    def Scope "Materials"
+    {
+        def Material "material_16" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/material_16>
+        )
+        {
+        }
+
+        def Material "material_17" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/material_17>
+        )
+        {
+        }
+    }
+
+    def Scope "Geometry"
+    {
+        def Xform "PELVIS_S"
+        {
+            def Xform "PELVIS_S" (
+                instanceable = true
+                prepend references = @./instances.usda@</Instances/PELVIS_S>
+            )
+            {
+                quatd xformOp:orient = (1, 0, 0, 0)
+                double3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 0)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+            }
+
+            def Xform "L_SHOULDER_P_S"
+            {
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0.0945, 0.042)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                def Xform "L_SHOULDER_P_S" (
+                    instanceable = true
+                    prepend references = @./instances.usda@</Instances/L_SHOULDER_P_S>
+                )
+                {
+                    quatd xformOp:orient = (1, 0, 0, 0)
+                    double3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                }
+
+                def Xform "L_SHOULDER_R_S"
+                {
+                    quatf xformOp:orient = (1, 0, 0, 0)
+                    float3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0.035, 0.0765, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                    def Xform "L_SHOULDER_R_S" (
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/L_SHOULDER_R_S>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+
+                    def Xform "L_SHOULDER_Y_S"
+                    {
+                        quatf xformOp:orient = (1, 0, 0, 0)
+                        float3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (-0.035, 0.1475, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                        def Xform "L_SHOULDER_Y_S" (
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/L_SHOULDER_Y_S>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+
+                        def Xform "L_ELBOW_R_S"
+                        {
+                            quatf xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0.034, 0.1025, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                            def Xform "L_ELBOW_R_S" (
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/L_ELBOW_R_S>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+
+                            def Xform "L_WRIST_P_S"
+                            {
+                                quatf xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (-0.034, 0.0965, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                def Xform "L_WRIST_P_S" (
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/L_WRIST_P_S>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+
+                                def Xform "L_WRIST_Y_S"
+                                {
+                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0.1525, 0.039)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                    def Xform "L_WRIST_Y_S" (
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/L_WRIST_Y_S>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+
+                                    def Xform "L_WRIST_R_S"
+                                    {
+                                        quatf xformOp:orient = (1, 0, 0, 0)
+                                        float3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0.0258, 0, -0.039)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                        def Xform "L_WRIST_R_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/L_WRIST_R_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+
+                                        def Xform "L_WRIST_R_S_1" (
+                                            displayName = "L_WRIST_R_S"
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/L_WRIST_R_S_1>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+                                    }
+
+                                    def Xform "L_WRIST_Y_S_1" (
+                                        displayName = "L_WRIST_Y_S"
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/L_WRIST_Y_S_1>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+                                }
+
+                                def Xform "L_WRIST_P_S_1" (
+                                    displayName = "L_WRIST_P_S"
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/L_WRIST_P_S_1>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+                            }
+
+                            def Xform "L_ELBOW_R_S_1" (
+                                displayName = "L_ELBOW_R_S"
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/L_ELBOW_R_S_1>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+                        }
+
+                        def Xform "L_SHOULDER_Y_S_1" (
+                            displayName = "L_SHOULDER_Y_S"
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/L_SHOULDER_Y_S_1>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+                    }
+
+                    def Xform "L_SHOULDER_R_S_1" (
+                        displayName = "L_SHOULDER_R_S"
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/L_SHOULDER_R_S_1>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+                }
+
+                def Xform "L_SHOULDER_P_S_1" (
+                    displayName = "L_SHOULDER_P_S"
+                    instanceable = true
+                    prepend references = @./instances.usda@</Instances/L_SHOULDER_P_S_1>
+                )
+                {
+                    quatd xformOp:orient = (1, 0, 0, 0)
+                    double3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                }
+            }
+
+            def Xform "R_SHOULDER_P_S"
+            {
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, -0.0945, 0.042)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                def Xform "R_SHOULDER_P_S" (
+                    instanceable = true
+                    prepend references = @./instances.usda@</Instances/R_SHOULDER_P_S>
+                )
+                {
+                    quatd xformOp:orient = (1, 0, 0, 0)
+                    double3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                }
+
+                def Xform "R_SHOULDER_R_S"
+                {
+                    quatf xformOp:orient = (1, 0, 0, 0)
+                    float3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0.035, -0.0765, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                    def Xform "R_SHOULDER_R_S" (
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/R_SHOULDER_R_S>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+
+                    def Xform "R_SHOULDER_Y_S"
+                    {
+                        quatf xformOp:orient = (1, 0, 0, 0)
+                        float3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (-0.035, -0.1475, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                        def Xform "R_SHOULDER_Y_S" (
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/R_SHOULDER_Y_S>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+
+                        def Xform "R_ELBOW_R_S"
+                        {
+                            quatf xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0.034, -0.1025, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                            def Xform "R_ELBOW_R_S" (
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/R_ELBOW_R_S>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+
+                            def Xform "R_WRIST_P_S"
+                            {
+                                quatf xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (-0.034, -0.0965, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                def Xform "R_WRIST_P_S" (
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/R_WRIST_P_S>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+
+                                def Xform "R_WRIST_Y_S"
+                                {
+                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, -0.1525, 0.039)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                    def Xform "R_WRIST_Y_S" (
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/R_WRIST_Y_S>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+
+                                    def Xform "R_WRIST_R_S"
+                                    {
+                                        quatf xformOp:orient = (1, 0, 0, 0)
+                                        float3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0.03, 0, -0.039)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                        def Xform "R_WRIST_R_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/R_WRIST_R_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+
+                                        def Xform "R_FINGER_TIP"
+                                        {
+                                            quatf xformOp:orient = (1, 0, 0, 0)
+                                            float3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                            def Sphere "sphere" (
+                                                prepend apiSchemas = ["MaterialBindingAPI"]
+                                            )
+                                            {
+                                                float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)]
+                                                rel material:binding = </dual_arm/Materials/material_16>
+                                                double radius = 0.004
+                                                quatf xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0, 0, 0)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+
+                                            def Sphere "sphere_1" (
+                                                prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                displayName = "sphere"
+                                            )
+                                            {
+                                                float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)]
+                                                uniform token purpose = "guide"
+                                                double radius = 0.005
+                                                quatf xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0, 0, 0)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+                                        }
+
+                                        def Xform "R_CAM"
+                                        {
+                                            quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677)
+                                            float3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (-0.01212, -0.17655, 0.07506)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                            def Sphere "sphere" (
+                                                prepend apiSchemas = ["MaterialBindingAPI"]
+                                            )
+                                            {
+                                                float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)]
+                                                rel material:binding = </dual_arm/Materials/material_17>
+                                                double radius = 0.004
+                                                quatf xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0, 0, 0)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+
+                                            def Sphere "sphere_1" (
+                                                prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                displayName = "sphere"
+                                            )
+                                            {
+                                                float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)]
+                                                uniform token purpose = "guide"
+                                                double radius = 0.005
+                                                quatf xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0, 0, 0)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+                                        }
+
+                                        def Xform "R_WRIST_R_S_1" (
+                                            displayName = "R_WRIST_R_S"
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/R_WRIST_R_S_1>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+                                    }
+
+                                    def Xform "R_WRIST_Y_S_1" (
+                                        displayName = "R_WRIST_Y_S"
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/R_WRIST_Y_S_1>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+                                }
+
+                                def Xform "R_WRIST_P_S_1" (
+                                    displayName = "R_WRIST_P_S"
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/R_WRIST_P_S_1>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+                            }
+
+                            def Xform "R_ELBOW_R_S_1" (
+                                displayName = "R_ELBOW_R_S"
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/R_ELBOW_R_S_1>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+                        }
+
+                        def Xform "R_SHOULDER_Y_S_1" (
+                            displayName = "R_SHOULDER_Y_S"
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/R_SHOULDER_Y_S_1>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+                    }
+
+                    def Xform "R_SHOULDER_R_S_1" (
+                        displayName = "R_SHOULDER_R_S"
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/R_SHOULDER_R_S_1>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+                }
+
+                def Xform "R_SHOULDER_P_S_1" (
+                    displayName = "R_SHOULDER_P_S"
+                    instanceable = true
+                    prepend references = @./instances.usda@</Instances/R_SHOULDER_P_S_1>
+                )
+                {
+                    quatd xformOp:orient = (1, 0, 0, 0)
+                    double3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                }
+            }
+
+            def Xform "PELVIS_S_1" (
+                displayName = "PELVIS_S"
+                instanceable = true
+                prepend references = @./instances.usda@</Instances/PELVIS_S_1>
+            )
+            {
+                quatd xformOp:orient = (1, 0, 0, 0)
+                double3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 0)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+            }
+        }
+    }
+
+    def Scope "Physics"
+    {
+    }
+
+    def Scope "VisualMaterials"
+    {
+    }
+}
+

model/xiaoyan_description/dual_arm/payloads/instances.usda

@@ -0,0 +1,564 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Scope "Instances"
+{
+    def Xform "PELVIS_S" (
+        prepend references = @./geometries.usd@</Geometries/PELVIS_S>
+    )
+    {
+        over "PELVIS_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/PELVIS_S/VisualMaterials/material_1>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_1" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_1>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "PELVIS_S_1" (
+        prepend references = @./geometries.usd@</Geometries/PELVIS_S>
+    )
+    {
+        over "PELVIS_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "L_SHOULDER_P_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_P_S>
+    )
+    {
+        over "L_SHOULDER_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_P_S/VisualMaterials/material_2>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_2" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_2>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_P_S_1" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_P_S>
+    )
+    {
+        over "L_SHOULDER_P_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "L_SHOULDER_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_R_S>
+    )
+    {
+        over "L_SHOULDER_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_R_S/VisualMaterials/material_3>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_3" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_R_S_1" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_R_S>
+    )
+    {
+        over "L_SHOULDER_R_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "L_SHOULDER_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_Y_S>
+    )
+    {
+        over "L_SHOULDER_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_Y_S/VisualMaterials/material_4>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_4" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_Y_S_1" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_Y_S>
+    )
+    {
+        over "L_SHOULDER_Y_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "L_ELBOW_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_ELBOW_R_S>
+    )
+    {
+        over "L_ELBOW_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_ELBOW_R_S/VisualMaterials/material_5>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_5" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_ELBOW_R_S_1" (
+        prepend references = @./geometries.usd@</Geometries/L_ELBOW_R_S>
+    )
+    {
+        over "L_ELBOW_R_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "L_WRIST_P_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_P_S>
+    )
+    {
+        over "L_WRIST_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_P_S/VisualMaterials/material_6>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_6" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_1>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_P_S_1" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_P_S>
+    )
+    {
+        over "L_WRIST_P_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "L_WRIST_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_Y_S>
+    )
+    {
+        over "L_WRIST_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_Y_S/VisualMaterials/material_7>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_7" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_Y_S_1" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_Y_S>
+    )
+    {
+        over "L_WRIST_Y_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "L_WRIST_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_R_S>
+    )
+    {
+        over "L_WRIST_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_R_S/VisualMaterials/material_8>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_8" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_R_S_1" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_R_S>
+    )
+    {
+        over "L_WRIST_R_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "R_SHOULDER_P_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_P_S>
+    )
+    {
+        over "R_SHOULDER_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_P_S/VisualMaterials/material_9>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_9" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_P_S_1" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_P_S>
+    )
+    {
+        over "R_SHOULDER_P_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "R_SHOULDER_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_R_S>
+    )
+    {
+        over "R_SHOULDER_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_R_S/VisualMaterials/material_10>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_10" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_R_S_1" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_R_S>
+    )
+    {
+        over "R_SHOULDER_R_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "R_SHOULDER_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_Y_S>
+    )
+    {
+        over "R_SHOULDER_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_Y_S/VisualMaterials/material_11>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_11" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_Y_S_1" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_Y_S>
+    )
+    {
+        over "R_SHOULDER_Y_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "R_ELBOW_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_ELBOW_R_S>
+    )
+    {
+        over "R_ELBOW_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_ELBOW_R_S/VisualMaterials/material_12>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_12" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_ELBOW_R_S_1" (
+        prepend references = @./geometries.usd@</Geometries/R_ELBOW_R_S>
+    )
+    {
+        over "R_ELBOW_R_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "R_WRIST_P_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_P_S>
+    )
+    {
+        over "R_WRIST_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_P_S/VisualMaterials/material_13>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_13" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_P_S_1" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_P_S>
+    )
+    {
+        over "R_WRIST_P_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "R_WRIST_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_Y_S>
+    )
+    {
+        over "R_WRIST_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_Y_S/VisualMaterials/material_14>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_14" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_Y_S_1" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_Y_S>
+    )
+    {
+        over "R_WRIST_Y_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+
+    def Xform "R_WRIST_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_R_S>
+    )
+    {
+        over "R_WRIST_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_R_S/VisualMaterials/material_15>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_15" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_R_S_1" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_R_S>
+    )
+    {
+        over "R_WRIST_R_S" (
+            apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"]
+        )
+        {
+            token physics:approximation = "convexHull"
+            token purpose = "guide"
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm/payloads/materials.usda

@@ -0,0 +1,222 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Scope "Materials"
+{
+    def Material "material_16"
+    {
+        color3f inputs:diffuseColor = (0, 1, 1)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_16/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_16/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_16.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_16.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_16.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_16.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_17"
+    {
+        color3f inputs:diffuseColor = (0, 1, 0)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_17/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_17/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_17.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_17.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_17.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_17.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_1"
+    {
+        color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_1/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_1/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_1.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_1.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_1.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_1.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_2"
+    {
+        color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_2/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_2/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_2.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_2.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_2.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_2.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_3"
+    {
+        color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_3/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_3/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_3.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_3.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_3.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_3.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_7"
+    {
+        color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_7/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_7/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_7.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_7.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_7.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_7.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm/payloads/robot.usda

@@ -0,0 +1,260 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+over "dual_arm" (
+    prepend apiSchemas = ["IsaacRobotAPI"]
+)
+{
+    prepend rel isaac:physics:robotJoints = [
+        </dual_arm/Physics/root_joint>,
+        </dual_arm/Physics/L_SHOULDER_P>,
+        </dual_arm/Physics/R_SHOULDER_P>,
+        </dual_arm/Physics/L_SHOULDER_R>,
+        </dual_arm/Physics/L_SHOULDER_Y>,
+        </dual_arm/Physics/L_ELBOW_R>,
+        </dual_arm/Physics/L_WRIST_P>,
+        </dual_arm/Physics/L_WRIST_Y>,
+        </dual_arm/Physics/L_WRIST_R>,
+        </dual_arm/Physics/R_SHOULDER_R>,
+        </dual_arm/Physics/R_SHOULDER_Y>,
+        </dual_arm/Physics/R_ELBOW_R>,
+        </dual_arm/Physics/R_WRIST_P>,
+        </dual_arm/Physics/R_WRIST_Y>,
+        </dual_arm/Physics/R_WRIST_R>,
+        </dual_arm/Physics/R_FINGER_TIP_FIXED>,
+        </dual_arm/Physics/R_CAM_FIXED>,
+    ]
+    prepend rel isaac:physics:robotLinks = [
+        </dual_arm/Geometry/PELVIS_S>,
+        </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S>,
+        </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>,
+        </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>,
+        </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>,
+        </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>,
+        </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>,
+        </dual_arm/Geometry/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S/L_WRIST_R_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_FINGER_TIP>,
+        </dual_arm/Geometry/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_CAM>,
+    ]
+    token isaac:robotType = "Default"
+
+    over "Geometry"
+    {
+        over "PELVIS_S" (
+            prepend apiSchemas = ["IsaacLinkAPI"]
+        )
+        {
+            over "L_SHOULDER_P_S" (
+                prepend apiSchemas = ["IsaacLinkAPI"]
+            )
+            {
+                over "L_SHOULDER_R_S" (
+                    prepend apiSchemas = ["IsaacLinkAPI"]
+                )
+                {
+                    over "L_SHOULDER_Y_S" (
+                        prepend apiSchemas = ["IsaacLinkAPI"]
+                    )
+                    {
+                        over "L_ELBOW_R_S" (
+                            prepend apiSchemas = ["IsaacLinkAPI"]
+                        )
+                        {
+                            over "L_WRIST_P_S" (
+                                prepend apiSchemas = ["IsaacLinkAPI"]
+                            )
+                            {
+                                over "L_WRIST_Y_S" (
+                                    prepend apiSchemas = ["IsaacLinkAPI"]
+                                )
+                                {
+                                    over "L_WRIST_R_S" (
+                                        prepend apiSchemas = ["IsaacLinkAPI"]
+                                    )
+                                    {
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            over "R_SHOULDER_P_S" (
+                prepend apiSchemas = ["IsaacLinkAPI"]
+            )
+            {
+                over "R_SHOULDER_R_S" (
+                    prepend apiSchemas = ["IsaacLinkAPI"]
+                )
+                {
+                    over "R_SHOULDER_Y_S" (
+                        prepend apiSchemas = ["IsaacLinkAPI"]
+                    )
+                    {
+                        over "R_ELBOW_R_S" (
+                            prepend apiSchemas = ["IsaacLinkAPI"]
+                        )
+                        {
+                            over "R_WRIST_P_S" (
+                                prepend apiSchemas = ["IsaacLinkAPI"]
+                            )
+                            {
+                                over "R_WRIST_Y_S" (
+                                    prepend apiSchemas = ["IsaacLinkAPI"]
+                                )
+                                {
+                                    over "R_WRIST_R_S" (
+                                        prepend apiSchemas = ["IsaacLinkAPI"]
+                                    )
+                                    {
+                                        over "R_FINGER_TIP" (
+                                            prepend apiSchemas = ["IsaacLinkAPI"]
+                                        )
+                                        {
+                                        }
+
+                                        over "R_CAM" (
+                                            prepend apiSchemas = ["IsaacLinkAPI"]
+                                        )
+                                        {
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    over "Physics"
+    {
+        over "root_joint" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_ELBOW_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_ELBOW_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_FINGER_TIP_FIXED" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_CAM_FIXED" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_1/dual_arm.usda

@@ -0,0 +1,52 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Xform "dual_arm" (
+    prepend references = @./payloads/base.usda@
+    variants = {
+        string Physics = "physx"
+    }
+    append variantSets = "Physics"
+)
+{
+    variantSet "Physics" = {
+        "mujoco" (
+            prepend payload = @./payloads/Physics/mujoco.usda@
+        ) {
+
+        }
+        "none" {
+
+        }
+        "physics" (
+            prepend payload = @./payloads/Physics/physics.usda@
+        ) {
+
+        }
+        "physx" (
+            prepend payload = @./payloads/Physics/physx.usda@
+        ) {
+
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_1/payloads/Physics/mujoco.usda

@@ -0,0 +1,400 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./physics.usda@
+    ]
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Physics"
+    {
+        def MjcActuator "L_SHOULDER_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_P>
+        }
+
+        def MjcActuator "L_SHOULDER_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_R>
+        }
+
+        def MjcActuator "L_SHOULDER_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_Y>
+        }
+
+        def MjcActuator "L_ELBOW_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_ELBOW_R>
+        }
+
+        def MjcActuator "L_WRIST_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_P>
+        }
+
+        def MjcActuator "L_WRIST_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_Y>
+        }
+
+        def MjcActuator "L_WRIST_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_R>
+        }
+
+        def MjcActuator "R_SHOULDER_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_P>
+        }
+
+        def MjcActuator "R_SHOULDER_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_R>
+        }
+
+        def MjcActuator "R_SHOULDER_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_Y>
+        }
+
+        def MjcActuator "R_ELBOW_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_ELBOW_R>
+        }
+
+        def MjcActuator "R_WRIST_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_P>
+        }
+
+        def MjcActuator "R_WRIST_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_Y>
+        }
+
+        def MjcActuator "R_WRIST_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_R>
+        }
+
+        over "root_joint"
+        {
+        }
+
+        over "base_fixed"
+        {
+        }
+
+        over "L_SHOULDER_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_ELBOW_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_ELBOW_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_FINGER_TIP_FIXED"
+        {
+        }
+
+        over "R_CAM_FIXED"
+        {
+        }
+    }
+
+    over "Geometry"
+    {
+        over "base_link"
+        {
+            over "PELVIS_S"
+            {
+                over "L_SHOULDER_P_S"
+                {
+                    over "L_SHOULDER_R_S"
+                    {
+                        over "L_SHOULDER_Y_S"
+                        {
+                            over "L_ELBOW_R_S"
+                            {
+                                over "L_WRIST_P_S"
+                                {
+                                    over "L_WRIST_Y_S"
+                                    {
+                                        over "L_WRIST_R_S"
+                                        {
+                                        }
+
+                                        over "L_WRIST_Y_S"
+                                        {
+                                        }
+                                    }
+
+                                    over "L_WRIST_P_S"
+                                    {
+                                    }
+                                }
+
+                                over "L_ELBOW_R_S"
+                                {
+                                }
+                            }
+
+                            over "L_SHOULDER_Y_S"
+                            {
+                            }
+                        }
+
+                        over "L_SHOULDER_R_S"
+                        {
+                        }
+                    }
+
+                    over "L_SHOULDER_P_S"
+                    {
+                    }
+                }
+
+                over "R_SHOULDER_P_S"
+                {
+                    over "R_SHOULDER_R_S"
+                    {
+                        over "R_SHOULDER_Y_S"
+                        {
+                            over "R_ELBOW_R_S"
+                            {
+                                over "R_WRIST_P_S"
+                                {
+                                    over "R_WRIST_Y_S"
+                                    {
+                                        over "R_WRIST_R_S"
+                                        {
+                                            over "R_FINGER_TIP"
+                                            {
+                                                over "sphere"
+                                                {
+                                                }
+
+                                                over "sphere_1"
+                                                {
+                                                }
+                                            }
+
+                                            over "R_CAM"
+                                            {
+                                                over "sphere"
+                                                {
+                                                }
+
+                                                over "sphere_1"
+                                                {
+                                                }
+                                            }
+
+                                            over "R_WRIST_R_S"
+                                            {
+                                            }
+                                        }
+
+                                        over "R_WRIST_Y_S"
+                                        {
+                                        }
+                                    }
+
+                                    over "R_WRIST_P_S"
+                                    {
+                                    }
+                                }
+
+                                over "R_ELBOW_R_S"
+                                {
+                                }
+                            }
+
+                            over "R_SHOULDER_Y_S"
+                            {
+                            }
+                        }
+
+                        over "R_SHOULDER_R_S"
+                        {
+                        }
+                    }
+
+                    over "R_SHOULDER_P_S"
+                    {
+                    }
+                }
+
+                over "PELVIS_S"
+                {
+                }
+            }
+
+            over "cylinder"
+            {
+            }
+
+            over "base_column"
+            {
+            }
+        }
+    }
+
+    over "Materials"
+    {
+        over "gray"
+        {
+        }
+
+        over "material_16"
+        {
+        }
+
+        over "material_17"
+        {
+        }
+    }
+
+    over "VisualMaterials"
+    {
+    }
+}
+

model/xiaoyan_description/dual_arm_1/payloads/Physics/physics.usda

@@ -0,0 +1,554 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Geometry"
+    {
+        over "base_link" (
+            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI"]
+        )
+        {
+            bool newton:selfCollisionEnabled = 0
+
+            over "PELVIS_S" (
+                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+            )
+            {
+                point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627)
+                float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747)
+                float physics:mass = 2.106246
+                quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252)
+
+                over "L_SHOULDER_P_S" (
+                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                )
+                {
+                    point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188)
+                    float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366)
+                    float physics:mass = 0.88073754
+                    quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823)
+
+                    over "L_SHOULDER_R_S" (
+                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                    )
+                    {
+                        point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8)
+                        float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106)
+                        float physics:mass = 0.59478843
+                        quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278)
+
+                        over "L_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                        )
+                        {
+                            point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9)
+                            float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517)
+                            float physics:mass = 0.56340605
+                            quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022)
+
+                            over "L_ELBOW_R_S" (
+                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                            )
+                            {
+                                point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7)
+                                float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762)
+                                float physics:mass = 0.3935719
+                                quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766)
+
+                                over "L_WRIST_P_S" (
+                                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                )
+                                {
+                                    point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552)
+                                    float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185)
+                                    float physics:mass = 0.44233248
+                                    quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155)
+
+                                    over "L_WRIST_Y_S" (
+                                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                    )
+                                    {
+                                        point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538)
+                                        float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636)
+                                        float physics:mass = 0.23573847
+                                        quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041)
+
+                                        over "L_WRIST_R_S" (
+                                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                        )
+                                        {
+                                            point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925)
+                                            float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117)
+                                            float physics:mass = 0.50489414
+                                            quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542)
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+
+                over "R_SHOULDER_P_S" (
+                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                )
+                {
+                    point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992)
+                    float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366)
+                    float physics:mass = 0.88073754
+                    quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165)
+
+                    over "R_SHOULDER_R_S" (
+                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                    )
+                    {
+                        point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8)
+                        float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106)
+                        float physics:mass = 0.59478843
+                        quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957)
+
+                        over "R_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                        )
+                        {
+                            point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9)
+                            float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517)
+                            float physics:mass = 0.56340605
+                            quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771)
+
+                            over "R_ELBOW_R_S" (
+                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                            )
+                            {
+                                point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7)
+                                float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762)
+                                float physics:mass = 0.3935719
+                                quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363)
+
+                                over "R_WRIST_P_S" (
+                                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                )
+                                {
+                                    point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552)
+                                    float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185)
+                                    float physics:mass = 0.44233248
+                                    quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155)
+
+                                    over "R_WRIST_Y_S" (
+                                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                    )
+                                    {
+                                        point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538)
+                                        float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636)
+                                        float physics:mass = 0.23573847
+                                        quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041)
+
+                                        over "R_WRIST_R_S" (
+                                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                        )
+                                        {
+                                            point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534)
+                                            float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028)
+                                            float physics:mass = 0.50436604
+                                            quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874)
+
+                                            over "R_FINGER_TIP" (
+                                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                            )
+                                            {
+                                                point3f physics:centerOfMass = (0, 0, 0)
+                                                float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001)
+                                                float physics:mass = 0
+                                                quatf physics:principalAxes = (1, 0, 0, 0)
+
+                                                over "sphere_1" (
+                                                    prepend apiSchemas = ["NewtonCollisionAPI"]
+                                                )
+                                                {
+                                                }
+                                            }
+
+                                            over "R_CAM" (
+                                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                            )
+                                            {
+                                                point3f physics:centerOfMass = (0, 0, 0)
+                                                float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001)
+                                                float physics:mass = 0
+                                                quatf physics:principalAxes = (1, 0, 0, 0)
+
+                                                over "sphere_1" (
+                                                    prepend apiSchemas = ["NewtonCollisionAPI"]
+                                                )
+                                                {
+                                                }
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            over "base_column" (
+                prepend apiSchemas = ["NewtonCollisionAPI"]
+            )
+            {
+            }
+        }
+    }
+
+    over "Physics"
+    {
+        def PhysicsRevoluteJoint "L_SHOULDER_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S>
+            point3f physics:localPos0 = (0, 0.0945, 0.042)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -89.95438
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "L_SHOULDER_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>
+            point3f physics:localPos0 = (0.035, 0.0765, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -114.59156
+            float physics:upperLimit = 114.59156
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "L_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>
+            point3f physics:localPos0 = (-0.035, 0.1475, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -124.9048
+            float physics:upperLimit = 0
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "L_ELBOW_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>
+            point3f physics:localPos0 = (0.034, 0.1025, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -117.456345
+            float physics:upperLimit = 0
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>
+            point3f physics:localPos0 = (-0.034, 0.0965, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Z"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>
+            point3f physics:localPos0 = (0, 0.1525, 0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 44.69071
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 0.79
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S/L_WRIST_R_S>
+            point3f physics:localPos0 = (0.0258, 0, -0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -89.95438
+            float physics:upperLimit = 14.896903
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S>
+            point3f physics:localPos0 = (0, -0.0945, 0.042)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>
+            point3f physics:localPos0 = (0.035, -0.0765, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>
+            point3f physics:localPos0 = (-0.035, -0.1475, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "R_ELBOW_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>
+            point3f physics:localPos0 = (0.034, -0.1025, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 117.456345
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>
+            point3f physics:localPos0 = (-0.034, -0.0965, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Z"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>
+            point3f physics:localPos0 = (0, -0.1525, 0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 44.69071
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 0.79
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            point3f physics:localPos0 = (0.03, 0, -0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -32.658596
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsFixedJoint "root_joint"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link>
+            point3f physics:localPos0 = (0, 0, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "base_fixed"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S>
+            point3f physics:localPos0 = (0, 0, 1.2)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "R_FINGER_TIP_FIXED"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_FINGER_TIP>
+            point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "R_CAM_FIXED"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_CAM>
+            point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_1/payloads/Physics/physx.usda

@@ -0,0 +1,129 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./physics.usda@
+    ]
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Physics"
+    {
+        over "L_SHOULDER_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "L_SHOULDER_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "L_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "L_ELBOW_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "L_WRIST_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "L_WRIST_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 45.263668
+        }
+
+        over "L_WRIST_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "R_SHOULDER_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "R_SHOULDER_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "R_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "R_ELBOW_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "R_WRIST_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "R_WRIST_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 45.263668
+        }
+
+        over "R_WRIST_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_1/payloads/base.usda

@@ -0,0 +1,451 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./robot.usda@
+    ]
+    upAxis = "Z"
+)
+
+def Xform "dual_arm" (
+    prepend apiSchemas = ["GeomModelAPI"]
+    assetInfo = {
+        string name = "dual_arm"
+    }
+    kind = "component"
+)
+{
+    float3[] extentsHint = [(-0.05, -0.958077, -2.3841858e-8), (0.080300845, 0.9075668, 1.32106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.05, -0.959077, -2.3841858e-8), (0.05, 0.05, 1.32206)]
+
+    def Scope "Materials"
+    {
+        def Material "gray" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/gray>
+        )
+        {
+        }
+
+        def Material "material_16" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/material_16>
+        )
+        {
+        }
+
+        def Material "material_17" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/material_17>
+        )
+        {
+        }
+    }
+
+    def Scope "Geometry"
+    {
+        def Xform "base_link"
+        {
+            def Cylinder "cylinder" (
+                prepend apiSchemas = ["MaterialBindingAPI"]
+            )
+            {
+                uniform token axis = "Z"
+                float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)]
+                double height = 1.2
+                rel material:binding = </dual_arm/Materials/gray>
+                double radius = 0.05
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 0.6)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+            }
+
+            def Xform "PELVIS_S"
+            {
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 1.2)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                def Xform "PELVIS_S" (
+                    instanceable = true
+                    prepend references = @./instances.usda@</Instances/PELVIS_S>
+                )
+                {
+                    quatd xformOp:orient = (1, 0, 0, 0)
+                    double3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                }
+
+                def Xform "L_SHOULDER_P_S"
+                {
+                    quatf xformOp:orient = (1, 0, 0, 0)
+                    float3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0.0945, 0.042)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                    def Xform "L_SHOULDER_P_S" (
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/L_SHOULDER_P_S>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+
+                    def Xform "L_SHOULDER_R_S"
+                    {
+                        quatf xformOp:orient = (1, 0, 0, 0)
+                        float3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0.035, 0.0765, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                        def Xform "L_SHOULDER_R_S" (
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/L_SHOULDER_R_S>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+
+                        def Xform "L_SHOULDER_Y_S"
+                        {
+                            quatf xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (-0.035, 0.1475, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                            def Xform "L_SHOULDER_Y_S" (
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/L_SHOULDER_Y_S>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+
+                            def Xform "L_ELBOW_R_S"
+                            {
+                                quatf xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0.034, 0.1025, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                def Xform "L_ELBOW_R_S" (
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/L_ELBOW_R_S>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+
+                                def Xform "L_WRIST_P_S"
+                                {
+                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (-0.034, 0.0965, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                    def Xform "L_WRIST_P_S" (
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/L_WRIST_P_S>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+
+                                    def Xform "L_WRIST_Y_S"
+                                    {
+                                        quatf xformOp:orient = (1, 0, 0, 0)
+                                        float3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0.1525, 0.039)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                        def Xform "L_WRIST_Y_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/L_WRIST_Y_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+
+                                        def Xform "L_WRIST_R_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/L_WRIST_R_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0.0258, 0, -0.039)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+
+                def Xform "R_SHOULDER_P_S"
+                {
+                    quatf xformOp:orient = (1, 0, 0, 0)
+                    float3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, -0.0945, 0.042)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                    def Xform "R_SHOULDER_P_S" (
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/R_SHOULDER_P_S>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+
+                    def Xform "R_SHOULDER_R_S"
+                    {
+                        quatf xformOp:orient = (1, 0, 0, 0)
+                        float3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0.035, -0.0765, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                        def Xform "R_SHOULDER_R_S" (
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/R_SHOULDER_R_S>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+
+                        def Xform "R_SHOULDER_Y_S"
+                        {
+                            quatf xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (-0.035, -0.1475, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                            def Xform "R_SHOULDER_Y_S" (
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/R_SHOULDER_Y_S>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+
+                            def Xform "R_ELBOW_R_S"
+                            {
+                                quatf xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0.034, -0.1025, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                def Xform "R_ELBOW_R_S" (
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/R_ELBOW_R_S>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+
+                                def Xform "R_WRIST_P_S"
+                                {
+                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (-0.034, -0.0965, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                    def Xform "R_WRIST_P_S" (
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/R_WRIST_P_S>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+
+                                    def Xform "R_WRIST_Y_S"
+                                    {
+                                        quatf xformOp:orient = (1, 0, 0, 0)
+                                        float3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, -0.1525, 0.039)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                        def Xform "R_WRIST_Y_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/R_WRIST_Y_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+
+                                        def Xform "R_WRIST_R_S"
+                                        {
+                                            quatf xformOp:orient = (1, 0, 0, 0)
+                                            float3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0.03, 0, -0.039)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                            def Xform "R_WRIST_R_S" (
+                                                instanceable = true
+                                                prepend references = @./instances.usda@</Instances/R_WRIST_R_S>
+                                            )
+                                            {
+                                                quatd xformOp:orient = (1, 0, 0, 0)
+                                                double3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0, 0, 0)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+
+                                            def Xform "R_FINGER_TIP"
+                                            {
+                                                quatf xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                                def Sphere "sphere" (
+                                                    prepend apiSchemas = ["MaterialBindingAPI"]
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)]
+                                                    rel material:binding = </dual_arm/Materials/material_16>
+                                                    double radius = 0.004
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+
+                                                def Sphere "sphere_1" (
+                                                    prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                    displayName = "sphere"
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)]
+                                                    uniform token purpose = "guide"
+                                                    double radius = 0.005
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+                                            }
+
+                                            def Xform "R_CAM"
+                                            {
+                                                quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (-0.01212, -0.17655, 0.07506)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                                def Sphere "sphere" (
+                                                    prepend apiSchemas = ["MaterialBindingAPI"]
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)]
+                                                    rel material:binding = </dual_arm/Materials/material_17>
+                                                    double radius = 0.004
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+
+                                                def Sphere "sphere_1" (
+                                                    prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                    displayName = "sphere"
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)]
+                                                    uniform token purpose = "guide"
+                                                    double radius = 0.005
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            def Cylinder "base_column" (
+                prepend apiSchemas = ["PhysicsCollisionAPI"]
+            )
+            {
+                uniform token axis = "Z"
+                float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)]
+                double height = 1.2
+                uniform token purpose = "guide"
+                double radius = 0.05
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 0.6)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+            }
+        }
+    }
+
+    def Scope "Physics"
+    {
+    }
+
+    def Scope "VisualMaterials"
+    {
+    }
+}
+

model/xiaoyan_description/dual_arm_1/payloads/instances.usda

@@ -0,0 +1,369 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Scope "Instances"
+{
+    def Xform "PELVIS_S" (
+        prepend references = @./geometries.usd@</Geometries/PELVIS_S>
+    )
+    {
+        over "PELVIS_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/PELVIS_S/VisualMaterials/material_1>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_1" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_1>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_P_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_P_S>
+    )
+    {
+        over "L_SHOULDER_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_P_S/VisualMaterials/material_2>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_2" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_2>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_R_S>
+    )
+    {
+        over "L_SHOULDER_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_R_S/VisualMaterials/material_3>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_3" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_Y_S>
+    )
+    {
+        over "L_SHOULDER_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_Y_S/VisualMaterials/material_4>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_4" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_ELBOW_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_ELBOW_R_S>
+    )
+    {
+        over "L_ELBOW_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_ELBOW_R_S/VisualMaterials/material_5>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_5" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_P_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_P_S>
+    )
+    {
+        over "L_WRIST_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_P_S/VisualMaterials/material_6>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_6" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_1>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_Y_S>
+    )
+    {
+        over "L_WRIST_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_Y_S/VisualMaterials/material_7>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_7" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_R_S>
+    )
+    {
+        over "L_WRIST_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_R_S/VisualMaterials/material_8>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_8" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_P_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_P_S>
+    )
+    {
+        over "R_SHOULDER_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_P_S/VisualMaterials/material_9>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_9" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_R_S>
+    )
+    {
+        over "R_SHOULDER_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_R_S/VisualMaterials/material_10>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_10" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_Y_S>
+    )
+    {
+        over "R_SHOULDER_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_Y_S/VisualMaterials/material_11>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_11" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_ELBOW_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_ELBOW_R_S>
+    )
+    {
+        over "R_ELBOW_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_ELBOW_R_S/VisualMaterials/material_12>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_12" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_P_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_P_S>
+    )
+    {
+        over "R_WRIST_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_P_S/VisualMaterials/material_13>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_13" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_Y_S>
+    )
+    {
+        over "R_WRIST_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_Y_S/VisualMaterials/material_14>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_14" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_R_S>
+    )
+    {
+        over "R_WRIST_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_R_S/VisualMaterials/material_15>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_15" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_1/payloads/materials.usda

@@ -0,0 +1,255 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Scope "Materials"
+{
+    def Material "gray"
+    {
+        color3f inputs:diffuseColor = (0.21404114, 0.21404114, 0.21404114)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/gray/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/gray/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/gray.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/gray.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/gray.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/gray.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_16"
+    {
+        color3f inputs:diffuseColor = (0, 1, 1)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_16/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_16/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_16.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_16.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_16.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_16.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_17"
+    {
+        color3f inputs:diffuseColor = (0, 1, 0)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_17/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_17/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_17.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_17.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_17.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_17.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_1"
+    {
+        color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_1/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_1/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_1.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_1.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_1.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_1.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_2"
+    {
+        color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_2/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_2/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_2.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_2.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_2.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_2.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_3"
+    {
+        color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_3/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_3/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_3.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_3.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_3.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_3.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_7"
+    {
+        color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_7/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_7/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_7.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_7.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_7.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_7.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_1/payloads/robot.usda

@@ -0,0 +1,273 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+over "dual_arm" (
+    prepend apiSchemas = ["IsaacRobotAPI"]
+)
+{
+    prepend rel isaac:physics:robotJoints = [
+        </dual_arm/Physics/root_joint>,
+        </dual_arm/Physics/base_fixed>,
+        </dual_arm/Physics/L_SHOULDER_P>,
+        </dual_arm/Physics/R_SHOULDER_P>,
+        </dual_arm/Physics/L_SHOULDER_R>,
+        </dual_arm/Physics/L_SHOULDER_Y>,
+        </dual_arm/Physics/L_ELBOW_R>,
+        </dual_arm/Physics/L_WRIST_P>,
+        </dual_arm/Physics/L_WRIST_Y>,
+        </dual_arm/Physics/L_WRIST_R>,
+        </dual_arm/Physics/R_SHOULDER_R>,
+        </dual_arm/Physics/R_SHOULDER_Y>,
+        </dual_arm/Physics/R_ELBOW_R>,
+        </dual_arm/Physics/R_WRIST_P>,
+        </dual_arm/Physics/R_WRIST_Y>,
+        </dual_arm/Physics/R_WRIST_R>,
+        </dual_arm/Physics/R_FINGER_TIP_FIXED>,
+        </dual_arm/Physics/R_CAM_FIXED>,
+    ]
+    prepend rel isaac:physics:robotLinks = [
+        </dual_arm/Geometry/base_link>,
+        </dual_arm/Geometry/base_link/PELVIS_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S/L_WRIST_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_FINGER_TIP>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_CAM>,
+    ]
+    token isaac:robotType = "Manipulator"
+
+    over "Geometry"
+    {
+        over "base_link" (
+            prepend apiSchemas = ["IsaacLinkAPI"]
+        )
+        {
+            over "PELVIS_S" (
+                prepend apiSchemas = ["IsaacLinkAPI"]
+            )
+            {
+                over "L_SHOULDER_P_S" (
+                    prepend apiSchemas = ["IsaacLinkAPI"]
+                )
+                {
+                    over "L_SHOULDER_R_S" (
+                        prepend apiSchemas = ["IsaacLinkAPI"]
+                    )
+                    {
+                        over "L_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["IsaacLinkAPI"]
+                        )
+                        {
+                            over "L_ELBOW_R_S" (
+                                prepend apiSchemas = ["IsaacLinkAPI"]
+                            )
+                            {
+                                over "L_WRIST_P_S" (
+                                    prepend apiSchemas = ["IsaacLinkAPI"]
+                                )
+                                {
+                                    over "L_WRIST_Y_S" (
+                                        prepend apiSchemas = ["IsaacLinkAPI"]
+                                    )
+                                    {
+                                        over "L_WRIST_R_S" (
+                                            prepend apiSchemas = ["IsaacLinkAPI"]
+                                        )
+                                        {
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+
+                over "R_SHOULDER_P_S" (
+                    prepend apiSchemas = ["IsaacLinkAPI"]
+                )
+                {
+                    over "R_SHOULDER_R_S" (
+                        prepend apiSchemas = ["IsaacLinkAPI"]
+                    )
+                    {
+                        over "R_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["IsaacLinkAPI"]
+                        )
+                        {
+                            over "R_ELBOW_R_S" (
+                                prepend apiSchemas = ["IsaacLinkAPI"]
+                            )
+                            {
+                                over "R_WRIST_P_S" (
+                                    prepend apiSchemas = ["IsaacLinkAPI"]
+                                )
+                                {
+                                    over "R_WRIST_Y_S" (
+                                        prepend apiSchemas = ["IsaacLinkAPI"]
+                                    )
+                                    {
+                                        over "R_WRIST_R_S" (
+                                            prepend apiSchemas = ["IsaacLinkAPI"]
+                                        )
+                                        {
+                                            over "R_FINGER_TIP" (
+                                                prepend apiSchemas = ["IsaacLinkAPI"]
+                                            )
+                                            {
+                                            }
+
+                                            over "R_CAM" (
+                                                prepend apiSchemas = ["IsaacLinkAPI"]
+                                            )
+                                            {
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    over "Physics"
+    {
+        over "root_joint" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "base_fixed" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_ELBOW_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_ELBOW_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_FINGER_TIP_FIXED" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_CAM_FIXED" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_2/dual_arm.usda

@@ -0,0 +1,52 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Xform "dual_arm" (
+    prepend references = @./payloads/base.usda@
+    variants = {
+        string Physics = "physx"
+    }
+    append variantSets = "Physics"
+)
+{
+    variantSet "Physics" = {
+        "mujoco" (
+            prepend payload = @./payloads/Physics/mujoco.usda@
+        ) {
+
+        }
+        "none" {
+
+        }
+        "physics" (
+            prepend payload = @./payloads/Physics/physics.usda@
+        ) {
+
+        }
+        "physx" (
+            prepend payload = @./payloads/Physics/physx.usda@
+        ) {
+
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_2/payloads/Physics/mujoco.usda

@@ -0,0 +1,400 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./physics.usda@
+    ]
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Physics"
+    {
+        def MjcActuator "L_SHOULDER_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_P>
+        }
+
+        def MjcActuator "L_SHOULDER_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_R>
+        }
+
+        def MjcActuator "L_SHOULDER_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_SHOULDER_Y>
+        }
+
+        def MjcActuator "L_ELBOW_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_ELBOW_R>
+        }
+
+        def MjcActuator "L_WRIST_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_P>
+        }
+
+        def MjcActuator "L_WRIST_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_Y>
+        }
+
+        def MjcActuator "L_WRIST_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/L_WRIST_R>
+        }
+
+        def MjcActuator "R_SHOULDER_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_P>
+        }
+
+        def MjcActuator "R_SHOULDER_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 120
+            uniform double mjc:forceRange:min = -120
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_R>
+        }
+
+        def MjcActuator "R_SHOULDER_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_SHOULDER_Y>
+        }
+
+        def MjcActuator "R_ELBOW_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 80
+            uniform double mjc:forceRange:min = -80
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_ELBOW_R>
+        }
+
+        def MjcActuator "R_WRIST_P_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_P>
+        }
+
+        def MjcActuator "R_WRIST_Y_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_Y>
+        }
+
+        def MjcActuator "R_WRIST_R_actuator"
+        {
+            uniform double mjc:forceRange:max = 50
+            uniform double mjc:forceRange:min = -50
+            custom rel mjc:target
+            prepend rel mjc:target = </dual_arm/Physics/R_WRIST_R>
+        }
+
+        over "root_joint"
+        {
+        }
+
+        over "base_fixed"
+        {
+        }
+
+        over "L_SHOULDER_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_ELBOW_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "L_WRIST_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_ELBOW_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_P" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_Y" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_WRIST_R" (
+            delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+        }
+
+        over "R_FINGER_TIP_FIXED"
+        {
+        }
+
+        over "R_CAM_FIXED"
+        {
+        }
+    }
+
+    over "Geometry"
+    {
+        over "base_link"
+        {
+            over "PELVIS_S"
+            {
+                over "L_SHOULDER_P_S"
+                {
+                    over "L_SHOULDER_R_S"
+                    {
+                        over "L_SHOULDER_Y_S"
+                        {
+                            over "L_ELBOW_R_S"
+                            {
+                                over "L_WRIST_P_S"
+                                {
+                                    over "L_WRIST_Y_S"
+                                    {
+                                        over "L_WRIST_R_S"
+                                        {
+                                        }
+
+                                        over "L_WRIST_Y_S"
+                                        {
+                                        }
+                                    }
+
+                                    over "L_WRIST_P_S"
+                                    {
+                                    }
+                                }
+
+                                over "L_ELBOW_R_S"
+                                {
+                                }
+                            }
+
+                            over "L_SHOULDER_Y_S"
+                            {
+                            }
+                        }
+
+                        over "L_SHOULDER_R_S"
+                        {
+                        }
+                    }
+
+                    over "L_SHOULDER_P_S"
+                    {
+                    }
+                }
+
+                over "R_SHOULDER_P_S"
+                {
+                    over "R_SHOULDER_R_S"
+                    {
+                        over "R_SHOULDER_Y_S"
+                        {
+                            over "R_ELBOW_R_S"
+                            {
+                                over "R_WRIST_P_S"
+                                {
+                                    over "R_WRIST_Y_S"
+                                    {
+                                        over "R_WRIST_R_S"
+                                        {
+                                            over "R_FINGER_TIP"
+                                            {
+                                                over "sphere"
+                                                {
+                                                }
+
+                                                over "sphere_1"
+                                                {
+                                                }
+                                            }
+
+                                            over "R_CAM"
+                                            {
+                                                over "sphere"
+                                                {
+                                                }
+
+                                                over "sphere_1"
+                                                {
+                                                }
+                                            }
+
+                                            over "R_WRIST_R_S"
+                                            {
+                                            }
+                                        }
+
+                                        over "R_WRIST_Y_S"
+                                        {
+                                        }
+                                    }
+
+                                    over "R_WRIST_P_S"
+                                    {
+                                    }
+                                }
+
+                                over "R_ELBOW_R_S"
+                                {
+                                }
+                            }
+
+                            over "R_SHOULDER_Y_S"
+                            {
+                            }
+                        }
+
+                        over "R_SHOULDER_R_S"
+                        {
+                        }
+                    }
+
+                    over "R_SHOULDER_P_S"
+                    {
+                    }
+                }
+
+                over "PELVIS_S"
+                {
+                }
+            }
+
+            over "cylinder"
+            {
+            }
+
+            over "base_column"
+            {
+            }
+        }
+    }
+
+    over "Materials"
+    {
+        over "gray"
+        {
+        }
+
+        over "material_16"
+        {
+        }
+
+        over "material_17"
+        {
+        }
+    }
+
+    over "VisualMaterials"
+    {
+    }
+}
+

model/xiaoyan_description/dual_arm_2/payloads/Physics/physics.usda

@@ -0,0 +1,554 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Geometry"
+    {
+        over "base_link" (
+            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI"]
+        )
+        {
+            bool newton:selfCollisionEnabled = 0
+
+            over "PELVIS_S" (
+                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+            )
+            {
+                point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627)
+                float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747)
+                float physics:mass = 2.106246
+                quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252)
+
+                over "L_SHOULDER_P_S" (
+                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                )
+                {
+                    point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188)
+                    float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366)
+                    float physics:mass = 0.88073754
+                    quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823)
+
+                    over "L_SHOULDER_R_S" (
+                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                    )
+                    {
+                        point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8)
+                        float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106)
+                        float physics:mass = 0.59478843
+                        quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278)
+
+                        over "L_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                        )
+                        {
+                            point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9)
+                            float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517)
+                            float physics:mass = 0.56340605
+                            quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022)
+
+                            over "L_ELBOW_R_S" (
+                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                            )
+                            {
+                                point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7)
+                                float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762)
+                                float physics:mass = 0.3935719
+                                quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766)
+
+                                over "L_WRIST_P_S" (
+                                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                )
+                                {
+                                    point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552)
+                                    float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185)
+                                    float physics:mass = 0.44233248
+                                    quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155)
+
+                                    over "L_WRIST_Y_S" (
+                                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                    )
+                                    {
+                                        point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538)
+                                        float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636)
+                                        float physics:mass = 0.23573847
+                                        quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041)
+
+                                        over "L_WRIST_R_S" (
+                                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                        )
+                                        {
+                                            point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925)
+                                            float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117)
+                                            float physics:mass = 0.50489414
+                                            quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542)
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+
+                over "R_SHOULDER_P_S" (
+                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                )
+                {
+                    point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992)
+                    float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366)
+                    float physics:mass = 0.88073754
+                    quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165)
+
+                    over "R_SHOULDER_R_S" (
+                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                    )
+                    {
+                        point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8)
+                        float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106)
+                        float physics:mass = 0.59478843
+                        quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957)
+
+                        over "R_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                        )
+                        {
+                            point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9)
+                            float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517)
+                            float physics:mass = 0.56340605
+                            quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771)
+
+                            over "R_ELBOW_R_S" (
+                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                            )
+                            {
+                                point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7)
+                                float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762)
+                                float physics:mass = 0.3935719
+                                quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363)
+
+                                over "R_WRIST_P_S" (
+                                    prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                )
+                                {
+                                    point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552)
+                                    float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185)
+                                    float physics:mass = 0.44233248
+                                    quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155)
+
+                                    over "R_WRIST_Y_S" (
+                                        prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                    )
+                                    {
+                                        point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538)
+                                        float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636)
+                                        float physics:mass = 0.23573847
+                                        quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041)
+
+                                        over "R_WRIST_R_S" (
+                                            prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                        )
+                                        {
+                                            point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534)
+                                            float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028)
+                                            float physics:mass = 0.50436604
+                                            quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874)
+
+                                            over "R_FINGER_TIP" (
+                                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                            )
+                                            {
+                                                point3f physics:centerOfMass = (0, 0, 0)
+                                                float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001)
+                                                float physics:mass = 0
+                                                quatf physics:principalAxes = (1, 0, 0, 0)
+
+                                                over "sphere_1" (
+                                                    prepend apiSchemas = ["NewtonCollisionAPI"]
+                                                )
+                                                {
+                                                }
+                                            }
+
+                                            over "R_CAM" (
+                                                prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"]
+                                            )
+                                            {
+                                                point3f physics:centerOfMass = (0, 0, 0)
+                                                float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001)
+                                                float physics:mass = 0
+                                                quatf physics:principalAxes = (1, 0, 0, 0)
+
+                                                over "sphere_1" (
+                                                    prepend apiSchemas = ["NewtonCollisionAPI"]
+                                                )
+                                                {
+                                                }
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            over "base_column" (
+                prepend apiSchemas = ["NewtonCollisionAPI"]
+            )
+            {
+            }
+        }
+    }
+
+    over "Physics"
+    {
+        def PhysicsRevoluteJoint "L_SHOULDER_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S>
+            point3f physics:localPos0 = (0, 0.0945, 0.042)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -89.95438
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "L_SHOULDER_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>
+            point3f physics:localPos0 = (0.035, 0.0765, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -114.59156
+            float physics:upperLimit = 114.59156
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "L_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>
+            point3f physics:localPos0 = (-0.035, 0.1475, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -124.9048
+            float physics:upperLimit = 0
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "L_ELBOW_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>
+            point3f physics:localPos0 = (0.034, 0.1025, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -117.456345
+            float physics:upperLimit = 0
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>
+            point3f physics:localPos0 = (-0.034, 0.0965, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Z"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>
+            point3f physics:localPos0 = (0, 0.1525, 0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 44.69071
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 0.79
+        }
+
+        def PhysicsRevoluteJoint "L_WRIST_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S/L_WRIST_R_S>
+            point3f physics:localPos0 = (0.0258, 0, -0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -89.95438
+            float physics:upperLimit = 14.896903
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S>
+            point3f physics:localPos0 = (0, -0.0945, 0.042)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 120
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>
+            point3f physics:localPos0 = (0.035, -0.0765, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 120
+            custom float urdf:limit:velocity = 3.351
+        }
+
+        def PhysicsRevoluteJoint "R_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>
+            point3f physics:localPos0 = (-0.035, -0.1475, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "R_ELBOW_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 80
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>
+            point3f physics:localPos0 = (0.034, -0.1025, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 117.456345
+            custom float urdf:limit:effort = 80
+            custom float urdf:limit:velocity = 3.8758
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_P" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Y"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>
+            point3f physics:localPos0 = (-0.034, -0.0965, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (0, -1, 0, 0)
+            quatf physics:localRot1 = (0, -1, 0, 0)
+            float physics:lowerLimit = -179.90875
+            float physics:upperLimit = 179.90875
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_Y" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "Z"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>
+            point3f physics:localPos0 = (0, -0.1525, 0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -44.69071
+            float physics:upperLimit = 44.69071
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 0.79
+        }
+
+        def PhysicsRevoluteJoint "R_WRIST_R" (
+            prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"]
+        )
+        {
+            float drive:angular:physics:maxForce = 50
+            uniform token physics:axis = "X"
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            point3f physics:localPos0 = (0.03, 0, -0.039)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+            float physics:lowerLimit = -32.658596
+            float physics:upperLimit = 89.95438
+            custom float urdf:limit:effort = 50
+            custom float urdf:limit:velocity = 4.71
+        }
+
+        def PhysicsFixedJoint "root_joint"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link>
+            point3f physics:localPos0 = (0, 0, 0)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "base_fixed"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S>
+            point3f physics:localPos0 = (0, 0, 1.2)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "R_FINGER_TIP_FIXED"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_FINGER_TIP>
+            point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (1, 0, 0, 0)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+
+        def PhysicsFixedJoint "R_CAM_FIXED"
+        {
+            custom rel physics:body0
+            prepend rel physics:body0 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>
+            custom rel physics:body1
+            prepend rel physics:body1 = </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_CAM>
+            point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506)
+            point3f physics:localPos1 = (0, 0, 0)
+            quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677)
+            quatf physics:localRot1 = (1, 0, 0, 0)
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_2/payloads/Physics/physx.usda

@@ -0,0 +1,129 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./physics.usda@
+    ]
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Physics"
+    {
+        over "L_SHOULDER_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "L_SHOULDER_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "L_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "L_ELBOW_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "L_WRIST_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "L_WRIST_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 45.263668
+        }
+
+        over "L_WRIST_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "R_SHOULDER_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "R_SHOULDER_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 191.99817
+        }
+
+        over "R_SHOULDER_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "R_ELBOW_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 222.06699
+        }
+
+        over "R_WRIST_P" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+
+        over "R_WRIST_Y" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 45.263668
+        }
+
+        over "R_WRIST_R" (
+            prepend apiSchemas = ["PhysxJointAPI"]
+        )
+        {
+            float physxJoint:maxJointVelocity = 269.86313
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_2/payloads/base.usda

@@ -0,0 +1,451 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @./robot.usda@
+    ]
+    upAxis = "Z"
+)
+
+def Xform "dual_arm" (
+    prepend apiSchemas = ["GeomModelAPI"]
+    assetInfo = {
+        string name = "dual_arm"
+    }
+    kind = "component"
+)
+{
+    float3[] extentsHint = [(-0.05, -0.958077, -2.3841858e-8), (0.080300845, 0.9075668, 1.32106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.05, -0.959077, -2.3841858e-8), (0.05, 0.05, 1.32206)]
+
+    def Scope "Materials"
+    {
+        def Material "gray" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/gray>
+        )
+        {
+        }
+
+        def Material "material_16" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/material_16>
+        )
+        {
+        }
+
+        def Material "material_17" (
+            instanceable = true
+            prepend references = @./materials.usda@</Materials/material_17>
+        )
+        {
+        }
+    }
+
+    def Scope "Geometry"
+    {
+        def Xform "base_link"
+        {
+            def Cylinder "cylinder" (
+                prepend apiSchemas = ["MaterialBindingAPI"]
+            )
+            {
+                uniform token axis = "Z"
+                float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)]
+                double height = 1.2
+                rel material:binding = </dual_arm/Materials/gray>
+                double radius = 0.05
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 0.6)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+            }
+
+            def Xform "PELVIS_S"
+            {
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 1.2)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                def Xform "PELVIS_S" (
+                    instanceable = true
+                    prepend references = @./instances.usda@</Instances/PELVIS_S>
+                )
+                {
+                    quatd xformOp:orient = (1, 0, 0, 0)
+                    double3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0, 0)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                }
+
+                def Xform "L_SHOULDER_P_S"
+                {
+                    quatf xformOp:orient = (1, 0, 0, 0)
+                    float3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, 0.0945, 0.042)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                    def Xform "L_SHOULDER_P_S" (
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/L_SHOULDER_P_S>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+
+                    def Xform "L_SHOULDER_R_S"
+                    {
+                        quatf xformOp:orient = (1, 0, 0, 0)
+                        float3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0.035, 0.0765, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                        def Xform "L_SHOULDER_R_S" (
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/L_SHOULDER_R_S>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+
+                        def Xform "L_SHOULDER_Y_S"
+                        {
+                            quatf xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (-0.035, 0.1475, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                            def Xform "L_SHOULDER_Y_S" (
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/L_SHOULDER_Y_S>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+
+                            def Xform "L_ELBOW_R_S"
+                            {
+                                quatf xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0.034, 0.1025, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                def Xform "L_ELBOW_R_S" (
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/L_ELBOW_R_S>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+
+                                def Xform "L_WRIST_P_S"
+                                {
+                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (-0.034, 0.0965, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                    def Xform "L_WRIST_P_S" (
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/L_WRIST_P_S>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+
+                                    def Xform "L_WRIST_Y_S"
+                                    {
+                                        quatf xformOp:orient = (1, 0, 0, 0)
+                                        float3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0.1525, 0.039)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                        def Xform "L_WRIST_Y_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/L_WRIST_Y_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+
+                                        def Xform "L_WRIST_R_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/L_WRIST_R_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0.0258, 0, -0.039)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+
+                def Xform "R_SHOULDER_P_S"
+                {
+                    quatf xformOp:orient = (1, 0, 0, 0)
+                    float3 xformOp:scale = (1, 1, 1)
+                    double3 xformOp:translate = (0, -0.0945, 0.042)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                    def Xform "R_SHOULDER_P_S" (
+                        instanceable = true
+                        prepend references = @./instances.usda@</Instances/R_SHOULDER_P_S>
+                    )
+                    {
+                        quatd xformOp:orient = (1, 0, 0, 0)
+                        double3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0, 0, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+
+                    def Xform "R_SHOULDER_R_S"
+                    {
+                        quatf xformOp:orient = (1, 0, 0, 0)
+                        float3 xformOp:scale = (1, 1, 1)
+                        double3 xformOp:translate = (0.035, -0.0765, 0)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                        def Xform "R_SHOULDER_R_S" (
+                            instanceable = true
+                            prepend references = @./instances.usda@</Instances/R_SHOULDER_R_S>
+                        )
+                        {
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            double3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (0, 0, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+
+                        def Xform "R_SHOULDER_Y_S"
+                        {
+                            quatf xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (1, 1, 1)
+                            double3 xformOp:translate = (-0.035, -0.1475, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                            def Xform "R_SHOULDER_Y_S" (
+                                instanceable = true
+                                prepend references = @./instances.usda@</Instances/R_SHOULDER_Y_S>
+                            )
+                            {
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                double3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0, 0, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+
+                            def Xform "R_ELBOW_R_S"
+                            {
+                                quatf xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (1, 1, 1)
+                                double3 xformOp:translate = (0.034, -0.1025, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                def Xform "R_ELBOW_R_S" (
+                                    instanceable = true
+                                    prepend references = @./instances.usda@</Instances/R_ELBOW_R_S>
+                                )
+                                {
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    double3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (0, 0, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+
+                                def Xform "R_WRIST_P_S"
+                                {
+                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (1, 1, 1)
+                                    double3 xformOp:translate = (-0.034, -0.0965, 0)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                    def Xform "R_WRIST_P_S" (
+                                        instanceable = true
+                                        prepend references = @./instances.usda@</Instances/R_WRIST_P_S>
+                                    )
+                                    {
+                                        quatd xformOp:orient = (1, 0, 0, 0)
+                                        double3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, 0, 0)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                    }
+
+                                    def Xform "R_WRIST_Y_S"
+                                    {
+                                        quatf xformOp:orient = (1, 0, 0, 0)
+                                        float3 xformOp:scale = (1, 1, 1)
+                                        double3 xformOp:translate = (0, -0.1525, 0.039)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                        def Xform "R_WRIST_Y_S" (
+                                            instanceable = true
+                                            prepend references = @./instances.usda@</Instances/R_WRIST_Y_S>
+                                        )
+                                        {
+                                            quatd xformOp:orient = (1, 0, 0, 0)
+                                            double3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0, 0, 0)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+
+                                        def Xform "R_WRIST_R_S"
+                                        {
+                                            quatf xformOp:orient = (1, 0, 0, 0)
+                                            float3 xformOp:scale = (1, 1, 1)
+                                            double3 xformOp:translate = (0.03, 0, -0.039)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                            def Xform "R_WRIST_R_S" (
+                                                instanceable = true
+                                                prepend references = @./instances.usda@</Instances/R_WRIST_R_S>
+                                            )
+                                            {
+                                                quatd xformOp:orient = (1, 0, 0, 0)
+                                                double3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0, 0, 0)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+
+                                            def Xform "R_FINGER_TIP"
+                                            {
+                                                quatf xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                                def Sphere "sphere" (
+                                                    prepend apiSchemas = ["MaterialBindingAPI"]
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)]
+                                                    rel material:binding = </dual_arm/Materials/material_16>
+                                                    double radius = 0.004
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+
+                                                def Sphere "sphere_1" (
+                                                    prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                    displayName = "sphere"
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)]
+                                                    uniform token purpose = "guide"
+                                                    double radius = 0.005
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+                                            }
+
+                                            def Xform "R_CAM"
+                                            {
+                                                quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677)
+                                                float3 xformOp:scale = (1, 1, 1)
+                                                double3 xformOp:translate = (-0.01212, -0.17655, 0.07506)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+
+                                                def Sphere "sphere" (
+                                                    prepend apiSchemas = ["MaterialBindingAPI"]
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)]
+                                                    rel material:binding = </dual_arm/Materials/material_17>
+                                                    double radius = 0.004
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+
+                                                def Sphere "sphere_1" (
+                                                    prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                    displayName = "sphere"
+                                                )
+                                                {
+                                                    float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)]
+                                                    uniform token purpose = "guide"
+                                                    double radius = 0.005
+                                                    quatf xformOp:orient = (1, 0, 0, 0)
+                                                    float3 xformOp:scale = (1, 1, 1)
+                                                    double3 xformOp:translate = (0, 0, 0)
+                                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                                }
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+
+            def Cylinder "base_column" (
+                prepend apiSchemas = ["PhysicsCollisionAPI"]
+            )
+            {
+                uniform token axis = "Z"
+                float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)]
+                double height = 1.2
+                uniform token purpose = "guide"
+                double radius = 0.05
+                quatf xformOp:orient = (1, 0, 0, 0)
+                float3 xformOp:scale = (1, 1, 1)
+                double3 xformOp:translate = (0, 0, 0.6)
+                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+            }
+        }
+    }
+
+    def Scope "Physics"
+    {
+    }
+
+    def Scope "VisualMaterials"
+    {
+    }
+}
+

model/xiaoyan_description/dual_arm_2/payloads/instances.usda

@@ -0,0 +1,369 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Scope "Instances"
+{
+    def Xform "PELVIS_S" (
+        prepend references = @./geometries.usd@</Geometries/PELVIS_S>
+    )
+    {
+        over "PELVIS_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/PELVIS_S/VisualMaterials/material_1>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_1" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_1>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_P_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_P_S>
+    )
+    {
+        over "L_SHOULDER_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_P_S/VisualMaterials/material_2>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_2" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_2>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_R_S>
+    )
+    {
+        over "L_SHOULDER_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_R_S/VisualMaterials/material_3>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_3" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_SHOULDER_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/L_SHOULDER_Y_S>
+    )
+    {
+        over "L_SHOULDER_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_SHOULDER_Y_S/VisualMaterials/material_4>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_4" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_ELBOW_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_ELBOW_R_S>
+    )
+    {
+        over "L_ELBOW_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_ELBOW_R_S/VisualMaterials/material_5>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_5" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_P_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_P_S>
+    )
+    {
+        over "L_WRIST_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_P_S/VisualMaterials/material_6>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_6" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_1>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_Y_S>
+    )
+    {
+        over "L_WRIST_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_Y_S/VisualMaterials/material_7>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_7" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "L_WRIST_R_S" (
+        prepend references = @./geometries.usd@</Geometries/L_WRIST_R_S>
+    )
+    {
+        over "L_WRIST_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/L_WRIST_R_S/VisualMaterials/material_8>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_8" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_P_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_P_S>
+    )
+    {
+        over "R_SHOULDER_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_P_S/VisualMaterials/material_9>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_9" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_R_S>
+    )
+    {
+        over "R_SHOULDER_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_R_S/VisualMaterials/material_10>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_10" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_SHOULDER_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/R_SHOULDER_Y_S>
+    )
+    {
+        over "R_SHOULDER_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_SHOULDER_Y_S/VisualMaterials/material_11>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_11" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_ELBOW_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_ELBOW_R_S>
+    )
+    {
+        over "R_ELBOW_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_ELBOW_R_S/VisualMaterials/material_12>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_12" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_P_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_P_S>
+    )
+    {
+        over "R_WRIST_P_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_P_S/VisualMaterials/material_13>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_13" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_Y_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_Y_S>
+    )
+    {
+        over "R_WRIST_Y_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_Y_S/VisualMaterials/material_14>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_14" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_7>
+            )
+            {
+            }
+        }
+    }
+
+    def Xform "R_WRIST_R_S" (
+        prepend references = @./geometries.usd@</Geometries/R_WRIST_R_S>
+    )
+    {
+        over "R_WRIST_R_S" (
+            apiSchemas = ["MaterialBindingAPI"]
+        )
+        {
+            custom rel material:binding
+            prepend rel material:binding = </Instances/R_WRIST_R_S/VisualMaterials/material_15>
+        }
+
+        def Scope "VisualMaterials"
+        {
+            def Material "material_15" (
+                instanceable = true
+                prepend references = @./materials.usda@</Materials/material_3>
+            )
+            {
+            }
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_2/payloads/materials.usda

@@ -0,0 +1,255 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+
+
+Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+def Scope "Materials"
+{
+    def Material "gray"
+    {
+        color3f inputs:diffuseColor = (0.21404114, 0.21404114, 0.21404114)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/gray/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/gray/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/gray.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/gray.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/gray.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/gray.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_16"
+    {
+        color3f inputs:diffuseColor = (0, 1, 1)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_16/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_16/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_16.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_16.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_16.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_16.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_17"
+    {
+        color3f inputs:diffuseColor = (0, 1, 0)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_17/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_17/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_17.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_17.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_17.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_17.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_1"
+    {
+        color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_1/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_1/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_1.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_1.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_1.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_1.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_2"
+    {
+        color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_2/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_2/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_2.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_2.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_2.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_2.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_3"
+    {
+        color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_3/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_3/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_3.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_3.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_3.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_3.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+
+    def Material "material_7"
+    {
+        color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737)
+        float inputs:metallic = 0
+        float inputs:opacity = 1
+        float inputs:roughness = 0.5
+        token inputs:wrapMode = "repeat"
+        token outputs:displacement (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:displacement.connect = </Materials/material_7/PreviewSurface.outputs:displacement>
+        token outputs:surface (
+            displayGroup = "Outputs"
+        )
+        prepend token outputs:surface.connect = </Materials/material_7/PreviewSurface.outputs:surface>
+        token outputs:volume (
+            displayGroup = "Outputs"
+        )
+
+        def Shader "PreviewSurface" (
+            apiSchemas = ["NodeDefAPI"]
+        )
+        {
+            token info:id = "UsdPreviewSurface"
+            prepend color3f inputs:diffuseColor.connect = </Materials/material_7.inputs:diffuseColor>
+            prepend float inputs:metallic.connect = </Materials/material_7.inputs:metallic>
+            prepend float inputs:opacity.connect = </Materials/material_7.inputs:opacity>
+            prepend float inputs:roughness.connect = </Materials/material_7.inputs:roughness>
+            token outputs:displacement
+            token outputs:surface
+        }
+    }
+}
+

model/xiaoyan_description/dual_arm_2/payloads/robot.usda

@@ -0,0 +1,273 @@
+#usda 1.0
+(
+    customLayerData = {
+        string creator = "URDF USD Converter v0.1.3"
+    }
+    defaultPrim = "dual_arm"
+    doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc
+
+
+Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd
+"""
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    upAxis = "Z"
+)
+
+over "dual_arm" (
+    prepend apiSchemas = ["IsaacRobotAPI"]
+)
+{
+    prepend rel isaac:physics:robotJoints = [
+        </dual_arm/Physics/root_joint>,
+        </dual_arm/Physics/base_fixed>,
+        </dual_arm/Physics/L_SHOULDER_P>,
+        </dual_arm/Physics/R_SHOULDER_P>,
+        </dual_arm/Physics/L_SHOULDER_R>,
+        </dual_arm/Physics/L_SHOULDER_Y>,
+        </dual_arm/Physics/L_ELBOW_R>,
+        </dual_arm/Physics/L_WRIST_P>,
+        </dual_arm/Physics/L_WRIST_Y>,
+        </dual_arm/Physics/L_WRIST_R>,
+        </dual_arm/Physics/R_SHOULDER_R>,
+        </dual_arm/Physics/R_SHOULDER_Y>,
+        </dual_arm/Physics/R_ELBOW_R>,
+        </dual_arm/Physics/R_WRIST_P>,
+        </dual_arm/Physics/R_WRIST_Y>,
+        </dual_arm/Physics/R_WRIST_R>,
+        </dual_arm/Physics/R_FINGER_TIP_FIXED>,
+        </dual_arm/Physics/R_CAM_FIXED>,
+    ]
+    prepend rel isaac:physics:robotLinks = [
+        </dual_arm/Geometry/base_link>,
+        </dual_arm/Geometry/base_link/PELVIS_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/L_SHOULDER_P_S/L_SHOULDER_R_S/L_SHOULDER_Y_S/L_ELBOW_R_S/L_WRIST_P_S/L_WRIST_Y_S/L_WRIST_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_FINGER_TIP>,
+        </dual_arm/Geometry/base_link/PELVIS_S/R_SHOULDER_P_S/R_SHOULDER_R_S/R_SHOULDER_Y_S/R_ELBOW_R_S/R_WRIST_P_S/R_WRIST_Y_S/R_WRIST_R_S/R_CAM>,
+    ]
+    token isaac:robotType = "Manipulator"
+
+    over "Geometry"
+    {
+        over "base_link" (
+            prepend apiSchemas = ["IsaacLinkAPI"]
+        )
+        {
+            over "PELVIS_S" (
+                prepend apiSchemas = ["IsaacLinkAPI"]
+            )
+            {
+                over "L_SHOULDER_P_S" (
+                    prepend apiSchemas = ["IsaacLinkAPI"]
+                )
+                {
+                    over "L_SHOULDER_R_S" (
+                        prepend apiSchemas = ["IsaacLinkAPI"]
+                    )
+                    {
+                        over "L_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["IsaacLinkAPI"]
+                        )
+                        {
+                            over "L_ELBOW_R_S" (
+                                prepend apiSchemas = ["IsaacLinkAPI"]
+                            )
+                            {
+                                over "L_WRIST_P_S" (
+                                    prepend apiSchemas = ["IsaacLinkAPI"]
+                                )
+                                {
+                                    over "L_WRIST_Y_S" (
+                                        prepend apiSchemas = ["IsaacLinkAPI"]
+                                    )
+                                    {
+                                        over "L_WRIST_R_S" (
+                                            prepend apiSchemas = ["IsaacLinkAPI"]
+                                        )
+                                        {
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+
+                over "R_SHOULDER_P_S" (
+                    prepend apiSchemas = ["IsaacLinkAPI"]
+                )
+                {
+                    over "R_SHOULDER_R_S" (
+                        prepend apiSchemas = ["IsaacLinkAPI"]
+                    )
+                    {
+                        over "R_SHOULDER_Y_S" (
+                            prepend apiSchemas = ["IsaacLinkAPI"]
+                        )
+                        {
+                            over "R_ELBOW_R_S" (
+                                prepend apiSchemas = ["IsaacLinkAPI"]
+                            )
+                            {
+                                over "R_WRIST_P_S" (
+                                    prepend apiSchemas = ["IsaacLinkAPI"]
+                                )
+                                {
+                                    over "R_WRIST_Y_S" (
+                                        prepend apiSchemas = ["IsaacLinkAPI"]
+                                    )
+                                    {
+                                        over "R_WRIST_R_S" (
+                                            prepend apiSchemas = ["IsaacLinkAPI"]
+                                        )
+                                        {
+                                            over "R_FINGER_TIP" (
+                                                prepend apiSchemas = ["IsaacLinkAPI"]
+                                            )
+                                            {
+                                            }
+
+                                            over "R_CAM" (
+                                                prepend apiSchemas = ["IsaacLinkAPI"]
+                                            )
+                                            {
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    over "Physics"
+    {
+        over "root_joint" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "base_fixed" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_SHOULDER_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_ELBOW_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "L_WRIST_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_SHOULDER_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_ELBOW_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_P" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_Y" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_WRIST_R" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_FINGER_TIP_FIXED" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+
+        over "R_CAM_FIXED" (
+            prepend apiSchemas = ["IsaacJointAPI"]
+        )
+        {
+        }
+    }
+}
+

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>

model/xiaoyan_description/dual_arm_collision.usda

@@ -0,0 +1,324 @@
+#usda 1.0
+(
+    defaultPrim = "dual_arm"
+    kilogramsPerUnit = 1
+    metersPerUnit = 1
+    subLayers = [
+        @dual_arm_2/dual_arm.usda@
+    ]
+    upAxis = "Z"
+)
+
+over "dual_arm"
+{
+    over "Geometry"
+    {
+        over "base_link"
+        {
+            over "PELVIS_S"
+            {
+                over "L_SHOULDER_P_S"
+                {
+                    over "L_SHOULDER_R_S"
+                    {
+                        over "L_SHOULDER_Y_S"
+                        {
+                            over "L_ELBOW_R_S"
+                            {
+                                over "L_WRIST_P_S"
+                                {
+                                    over "L_WRIST_Y_S"
+                                    {
+                                        over "L_WRIST_R_S" (
+                                            instanceable = false
+                                        )
+                                        {
+                                            def Cube "AUTO_COLLISION_BOX" (
+                                                prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                customData = {
+                                                    string collisionGenerator = "trimesh-primitives-v1"
+                                                }
+                                            )
+                                            {
+                                                custom token collision:primitiveType = "box"
+                                                bool physics:collisionEnabled = 1
+                                                uniform token purpose = "guide"
+                                                double size = 1
+                                                quatd xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (0.11902186, 0.25356677, 0.069732234)
+                                                double3 xformOp:translate = (-0.0050100889056921005, 0.11078338418155909, -0.01045313011854887)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+                                        }
+
+                                        def Cube "AUTO_COLLISION_BOX" (
+                                            prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                            customData = {
+                                                string collisionGenerator = "trimesh-primitives-v1"
+                                            }
+                                        )
+                                        {
+                                            custom token collision:primitiveType = "box"
+                                            bool physics:collisionEnabled = 1
+                                            uniform token purpose = "guide"
+                                            double size = 1
+                                            quatd xformOp:orient = (0.5000000000000001, -0.5000000000000001, -0.5000000000000001, 0.5000000000000001)
+                                            float3 xformOp:scale = (0.0409999, 0.053493902, 0.0595)
+                                            double3 xformOp:translate = (-0.0037500001490116098, -5.408977040638672e-19, -0.03274694923311472)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+                                    }
+
+                                    def Cylinder "AUTO_COLLISION_CYLINDER" (
+                                        prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                        customData = {
+                                            string collisionGenerator = "trimesh-primitives-v1"
+                                        }
+                                    )
+                                    {
+                                        uniform token axis = "Z"
+                                        custom token collision:primitiveType = "cylinder"
+                                        double height = 0.16749374149367213
+                                        bool physics:collisionEnabled = 1
+                                        uniform token purpose = "guide"
+                                        double radius = 0.03765367431379833
+                                        quatd xformOp:orient = (0.7071067811865475, -0.7071067811865475, 0, 0)
+                                        double3 xformOp:translate = (3.469446951953614e-18, 0.08874687016941607, 0.012211145890315668)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
+                                    }
+                                }
+
+                                def Cube "AUTO_COLLISION_BOX" (
+                                    prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                    customData = {
+                                        string collisionGenerator = "trimesh-primitives-v1"
+                                    }
+                                )
+                                {
+                                    custom token collision:primitiveType = "box"
+                                    bool physics:collisionEnabled = 1
+                                    uniform token purpose = "guide"
+                                    double size = 1
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (0.069, 0.1285, 0.058)
+                                    double3 xformOp:translate = (-0.034500000427457156, 0.03925000037997961, 1.862645149230957e-9)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+                            }
+
+                            def Cube "AUTO_COLLISION_BOX" (
+                                prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                customData = {
+                                    string collisionGenerator = "trimesh-primitives-v1"
+                                }
+                            )
+                            {
+                                custom token collision:primitiveType = "box"
+                                bool physics:collisionEnabled = 1
+                                uniform token purpose = "guide"
+                                double size = 1
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (0.060999997, 0.14400001, 0.062992066)
+                                double3 xformOp:translate = (-0.0008999994024634361, 0.062000001315027475, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+                        }
+
+                        def Cube "AUTO_COLLISION_BOX" (
+                            prepend apiSchemas = ["PhysicsCollisionAPI"]
+                            customData = {
+                                string collisionGenerator = "trimesh-primitives-v1"
+                            }
+                        )
+                        {
+                            custom token collision:primitiveType = "box"
+                            bool physics:collisionEnabled = 1
+                            uniform token purpose = "guide"
+                            double size = 1
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (0.0785, 0.1725, 0.065)
+                            double3 xformOp:translate = (-0.03925000161955211, 0.056249999441206455, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+                    }
+
+                    def Cube "AUTO_COLLISION_BOX" (
+                        prepend apiSchemas = ["PhysicsCollisionAPI"]
+                        customData = {
+                            string collisionGenerator = "trimesh-primitives-v1"
+                        }
+                    )
+                    {
+                        custom token collision:primitiveType = "box"
+                        bool physics:collisionEnabled = 1
+                        uniform token purpose = "guide"
+                        double size = 1
+                        quatd xformOp:orient = (0.000040139854472664035, -0.00004013985447262077, 0.7071067800472512, 0.7071067800472514)
+                        float3 xformOp:scale = (0.07170313, 0.07299983, 0.104499996)
+                        double3 xformOp:translate = (-0.004348363594192078, 0.06074999878183007, 3.992215372663401e-8)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+                }
+
+                def Cube "AUTO_COLLISION_BOX" (
+                    prepend apiSchemas = ["PhysicsCollisionAPI"]
+                    customData = {
+                        string collisionGenerator = "trimesh-primitives-v1"
+                    }
+                )
+                {
+                    custom token collision:primitiveType = "box"
+                    bool physics:collisionEnabled = 1
+                    uniform token purpose = "guide"
+                    double size = 1
+                    quatd xformOp:orient = (1, 0, 0, 0)
+                    float3 xformOp:scale = (0.07699999, 0.216, 0.0805)
+                    double3 xformOp:translate = (0, 0, 0.04024999989568795)
+                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                }
+
+                over "R_SHOULDER_P_S"
+                {
+                    def Cube "AUTO_COLLISION_BOX" (
+                        prepend apiSchemas = ["PhysicsCollisionAPI"]
+                        customData = {
+                            string collisionGenerator = "trimesh-primitives-v1"
+                        }
+                    )
+                    {
+                        custom token collision:primitiveType = "box"
+                        bool physics:collisionEnabled = 1
+                        uniform token purpose = "guide"
+                        double size = 1
+                        quatd xformOp:orient = (0.7071067800472514, 0.7071067800472515, -0.0000401398544727094, 0.00004013985447257145)
+                        float3 xformOp:scale = (0.07170313, 0.07299983, 0.104499996)
+                        double3 xformOp:translate = (-0.004348363594192072, -0.06074999924749136, 3.992215372663644e-8)
+                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                    }
+
+                    over "R_SHOULDER_R_S"
+                    {
+                        def Cube "AUTO_COLLISION_BOX" (
+                            prepend apiSchemas = ["PhysicsCollisionAPI"]
+                            customData = {
+                                string collisionGenerator = "trimesh-primitives-v1"
+                            }
+                        )
+                        {
+                            custom token collision:primitiveType = "box"
+                            bool physics:collisionEnabled = 1
+                            uniform token purpose = "guide"
+                            double size = 1
+                            quatd xformOp:orient = (1, 0, 0, 0)
+                            float3 xformOp:scale = (0.0785, 0.1725, 0.065)
+                            double3 xformOp:translate = (-0.03925000161954845, -0.056249999441206455, 0)
+                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                        }
+
+                        over "R_SHOULDER_Y_S"
+                        {
+                            def Cube "AUTO_COLLISION_BOX" (
+                                prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                customData = {
+                                    string collisionGenerator = "trimesh-primitives-v1"
+                                }
+                            )
+                            {
+                                custom token collision:primitiveType = "box"
+                                bool physics:collisionEnabled = 1
+                                uniform token purpose = "guide"
+                                double size = 1
+                                quatd xformOp:orient = (1, 0, 0, 0)
+                                float3 xformOp:scale = (0.060999997, 0.14400001, 0.062992066)
+                                double3 xformOp:translate = (-0.0008999994024634361, -0.06200000178068876, 0)
+                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                            }
+
+                            over "R_ELBOW_R_S"
+                            {
+                                def Cube "AUTO_COLLISION_BOX" (
+                                    prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                    customData = {
+                                        string collisionGenerator = "trimesh-primitives-v1"
+                                    }
+                                )
+                                {
+                                    custom token collision:primitiveType = "box"
+                                    bool physics:collisionEnabled = 1
+                                    uniform token purpose = "guide"
+                                    double size = 1
+                                    quatd xformOp:orient = (1, 0, 0, 0)
+                                    float3 xformOp:scale = (0.069, 0.1285, 0.058)
+                                    double3 xformOp:translate = (-0.03450000042745616, -0.03925000037997961, 1.862645149230957e-9)
+                                    uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                }
+
+                                over "R_WRIST_P_S"
+                                {
+                                    def Cylinder "AUTO_COLLISION_CYLINDER" (
+                                        prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                        customData = {
+                                            string collisionGenerator = "trimesh-primitives-v1"
+                                        }
+                                    )
+                                    {
+                                        uniform token axis = "Z"
+                                        custom token collision:primitiveType = "cylinder"
+                                        double height = 0.16749374056234956
+                                        bool physics:collisionEnabled = 1
+                                        uniform token purpose = "guide"
+                                        double radius = 0.03765373180894358
+                                        quatd xformOp:orient = (0.7071067811865475, -0.7071067811865475, 0, 0)
+                                        double3 xformOp:translate = (3.469446951953614e-18, -0.08874687063507736, 0.012211077787740575)
+                                        uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"]
+                                    }
+
+                                    over "R_WRIST_Y_S"
+                                    {
+                                        def Cube "AUTO_COLLISION_BOX" (
+                                            prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                            customData = {
+                                                string collisionGenerator = "trimesh-primitives-v1"
+                                            }
+                                        )
+                                        {
+                                            custom token collision:primitiveType = "box"
+                                            bool physics:collisionEnabled = 1
+                                            uniform token purpose = "guide"
+                                            double size = 1
+                                            quatd xformOp:orient = (0.5000000000000001, -0.5000000000000001, -0.5000000000000001, 0.5000000000000001)
+                                            float3 xformOp:scale = (0.0409999, 0.053493902, 0.0595)
+                                            double3 xformOp:translate = (-0.0037500001490116098, -5.408977040638672e-19, -0.03274694923311472)
+                                            uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                        }
+
+                                        over "R_WRIST_R_S"
+                                        {
+                                            def Cube "AUTO_COLLISION_BOX" (
+                                                prepend apiSchemas = ["PhysicsCollisionAPI"]
+                                                customData = {
+                                                    string collisionGenerator = "trimesh-primitives-v1"
+                                                }
+                                            )
+                                            {
+                                                custom token collision:primitiveType = "box"
+                                                bool physics:collisionEnabled = 1
+                                                uniform token purpose = "guide"
+                                                double size = 1
+                                                quatd xformOp:orient = (1, 0, 0, 0)
+                                                float3 xformOp:scale = (0.11152348, 0.25179133, 0.0779598)
+                                                double3 xformOp:translate = (-0.01363389752805233, -0.1093915430828929, -0.014153839088976383)
+                                                uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"]
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+

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

scripts/isaac_sim/README.md

@@ -0,0 +1,145 @@
+# 简化碰撞体生成器
+
+统一入口 `generate_collision_primitives.py` 根据输入和输出扩展名自动处理 URDF 或
+USD。它读取机器人各个 link 的可视网格，并拟合为 box、sphere 或 cylinder；自动
+模式会选择包围体积最小的几何体。
+
+URDF 输入会生成一份包含 `<collision>` 的新 URDF；USD 输入会生成引用原 USD 的
+overlay。两种流程都不会修改源文件。USD 生成器还会把长度单位、质量单位和 up axis
+复制到 overlay 根层，避免使用默认的厘米制和 Y-up 坐标系。当前双臂模型使用米制、
+Z-up 坐标系。
+
+## 预览拟合结果
+
+使用 `--dry-run` 只计算和打印拟合结果，不生成输出文件：
+
+```bash
+/home/lgv/app/isaacsim/python.sh \
+  scripts/isaac_sim/generate_collision_primitives.py \
+  --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \
+  --config scripts/isaac_sim/dual_arm_collision.toml \
+  --dry-run
+```
+
+调试配置时，可使用 `--only R_ELBOW_R_S` 只拟合一个 link。
+
+## 生成并验证 overlay USD
+
+先生成到 `/tmp` 进行测试：
+
+```bash
+/home/lgv/app/isaacsim/python.sh \
+  scripts/isaac_sim/generate_collision_primitives.py \
+  --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \
+  --output /tmp/dual_arm_collision_test.usda \
+  --config scripts/isaac_sim/dual_arm_collision.toml \
+  --replace \
+  --validate
+```
+
+在 Isaac Sim 中打开 `/tmp/dual_arm_collision_test.usda`，并在 Viewport 中启用
+Guide Geometry，即可查看碰撞体。默认情况下，overlay 会停用输入 USD 中已有的
+网格碰撞实例，避免旧碰撞体和新碰撞体同时生效。
+
+确认结果后，可以生成到项目目录：
+
+```bash
+/home/lgv/app/isaacsim/python.sh \
+  scripts/isaac_sim/generate_collision_primitives.py \
+  --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \
+  --output model/xiaoyan_description/dual_arm_collision.usda \
+  --config scripts/isaac_sim/dual_arm_collision.toml \
+  --replace \
+  --validate
+```
+
+## 配置单个 link
+
+使用 link 名称编写单独配置：
+
+```toml
+[links.R_ELBOW_R_S]
+primitive = "cylinder"
+axis = "y"
+padding = 0.002
+scale = 1.0
+```
+
+`primitive` 支持以下取值：
+
+- `auto`：自动选择包围体积最小的几何体
+- `box`：盒体
+- `sphere`：球体
+- `cylinder`：圆柱体
+
+盒体设置 `alignment = "link"` 后，会使用与 link 局部 XYZ 轴平行的 AABB，
+不会产生自由旋转的斜包围盒。圆柱体设置 `axis = "x"`、`"y"` 或 `"z"` 后，
+圆柱轴会固定到对应的 link 局部轴。
+
+其他常用参数：
+
+- `padding`：在碰撞体外侧增加的绝对尺寸，单位为米
+- `scale`：以碰撞体中心为基准进行整体缩放
+- `enabled = false`：跳过该 link
+
+## USD 中缺少可视网格时回退到 STL
+
+如果导入后的 USD 中缺少某个 link 的可视网格，可以使用 `mesh_file` 指向 URDF
+使用的原始 STL：
+
+```toml
+[links.L_WRIST_R_S]
+mesh_file = "../../model/xiaoyan_description/meshes/L_WRIST_R_S.STL"
+primitive = "box"
+alignment = "link"
+```
+
+相对路径以 TOML 配置文件所在目录为基准。STL 顶点必须使用该 link 的局部坐标系。
+
+## 直接生成带简化碰撞体的 URDF
+
+对于 URDF，统一入口会读取每个 link 的 visual mesh，应用 `<visual><origin>` 和
+`<mesh scale>` 后，把拟合结果写成新的 `<collision>`。原始 URDF 不会被修改。
+
+先只预览拟合结果：
+
+```bash
+/home/lgv/app/isaacsim/python.sh \
+  scripts/isaac_sim/generate_collision_primitives.py \
+  --input model/xiaoyan_description/dual_arm.urdf \
+  --config scripts/isaac_sim/dual_arm_collision.toml \
+  --dry-run
+```
+
+生成并验证新的 URDF：
+
+```bash
+/home/lgv/app/isaacsim/python.sh \
+  scripts/isaac_sim/generate_collision_primitives.py \
+  --input model/xiaoyan_description/dual_arm.urdf \
+  --output model/xiaoyan_description/dual_arm_collision.urdf \
+  --config scripts/isaac_sim/dual_arm_collision.toml \
+  --replace \
+  --validate
+```
+
+默认行为是：只对实际生成碰撞体的 link 删除旧 `<collision>`，然后写入一个名为
+`AUTO_COLLISION_BOX`、`AUTO_COLLISION_SPHERE` 或 `AUTO_COLLISION_CYLINDER` 的新碰撞体。
+配置为 `enabled = false` 的 link 完全不改动，所以当前配置会保留 `base_link` 的底座
+圆柱，以及 `R_FINGER_TIP`、`R_CAM` 的原有球体。
+
+调试时可用 `--only L_WRIST_P_S` 只处理一个 link。需要保留某个已存在碰撞体并在其后
+追加自动碰撞体时，使用 `--keep-existing`。
+
+## 使用 MeshCat 查看 URDF 碰撞体
+
+生成后可直接启动 MeshCat 查看器。STL 按 URDF 原始材质显示，亮绿色线框是 URDF 的
+`<collision>`：
+
+```bash
+conda run -n cmvr-es python \
+  scripts/meshcat/view_urdf_collisions.py \
+  --input model/xiaoyan_description/dual_arm_collision.urdf
+```
+
+完整选项和关节位置设置方法见 `scripts/meshcat/README.md`。

scripts/isaac_sim/dual_arm_collision.toml

@@ -0,0 +1,84 @@
+# 默认配置会应用到所有未单独覆盖的 link。
+[defaults]
+# auto 会分别拟合 box、sphere、cylinder，并选择包围体积最小的一种。
+primitive = "auto"
+# auto 模式允许参与比较的碰撞体类型。
+allowed_primitives = ["box", "sphere", "cylinder"]
+# 碰撞体向外扩张的绝对尺寸，单位为米。0 表示不扩张。
+padding = 0.0
+# 以拟合中心为基准缩放碰撞体。1.0 表示保持原始拟合尺寸。
+scale = 1.0
+# trimesh 搜索最小包围圆柱时的方向采样密度；越大越精确，但计算越慢。
+cylinder_sample_count = 6
+# 圆柱方向优化的角度容差。
+cylinder_angle_tol = 0.001
+# 仅用于 USD：如果输入中仍有旧的 STL 网格碰撞体，在 overlay 中将其停用。
+# URDF 默认替换生成 link 上的旧碰撞体；传入 --keep-existing 可改为追加。
+disable_existing_mesh_collisions = true
+
+# base_link 已在 dual_arm.urdf 中明确配置了固定圆柱碰撞体 base_column，
+# 不需要根据可视模型重复生成。
+[links.base_link]
+enabled = false
+
+# 指尖和相机在 dual_arm.urdf 中已经有明确的 sphere 碰撞体，跳过自动拟合。
+[links.R_FINGER_TIP]
+enabled = false
+
+[links.R_CAM]
+enabled = false
+
+# 躯干使用与 PELVIS_S 局部 XYZ 轴平行的长方体。
+# alignment="link" 表示使用 link-local AABB，不允许盒体自由倾斜。
+[links.PELVIS_S]
+primitive = "box"
+alignment = "link"
+
+# 左右长连杆在各自 link 局部坐标中主要沿 Y 轴延伸。
+# 这些 link 使用轴对齐长方体，避免最小体积 OBB 出现肉眼可见的倾斜。
+[links.L_SHOULDER_R_S]
+primitive = "box"
+alignment = "link"
+
+[links.R_SHOULDER_R_S]
+primitive = "box"
+alignment = "link"
+
+[links.L_SHOULDER_Y_S]
+primitive = "box"
+alignment = "link"
+
+[links.R_SHOULDER_Y_S]
+primitive = "box"
+alignment = "link"
+
+[links.L_ELBOW_R_S]
+primitive = "box"
+alignment = "link"
+
+[links.R_ELBOW_R_S]
+primitive = "box"
+alignment = "link"
+
+# 左右前臂使用圆柱体，圆柱轴固定为 link 局部 Y 轴。
+# 如果更重视减少自碰撞误报，也可以改为 primitive="box"、alignment="link"。
+[links.L_WRIST_P_S]
+primitive = "cylinder"
+axis = "y"
+
+[links.R_WRIST_P_S]
+primitive = "cylinder"
+axis = "y"
+
+# 最新导入的 USD 没有提供左右末端腕部的可视网格点，
+# 因此回退到 URDF 使用的原始 STL。mesh_file 相对本 TOML 文件解析。
+# 末端腕部外形不规则且截面不圆，使用 link 轴对齐长方体。
+[links.L_WRIST_R_S]
+mesh_file = "../../model/xiaoyan_description/meshes/L_WRIST_R_S.STL"
+primitive = "box"
+alignment = "link"
+
+[links.R_WRIST_R_S]
+mesh_file = "../../model/xiaoyan_description/meshes/R_WRIST_R_S.STL"
+primitive = "box"
+alignment = "link"

scripts/isaac_sim/generate_collision_primitives.py

@@ -0,0 +1,900 @@
+#!/usr/bin/env python3
+"""Fit primitive colliders to a robot's visual meshes and write URDF or USD.
+
+The input and output formats are selected from their file extensions. URDF
+output is a new complete document; USD output is an overlay of the input USD.
+The source asset is never modified.
+"""
+
+from __future__ import annotations
+
+import argparse
+import os
+from dataclasses import dataclass
+from pathlib import Path
+import sys
+import tomllib
+import traceback
+import xml.etree.ElementTree as ET
+from typing import Any
+
+import numpy as np
+import trimesh
+
+
+GENERATOR_TAG = "trimesh-primitives-v1"
+GENERATED_PREFIX = "AUTO_COLLISION_"
+PRIMITIVE_TYPES = ("box", "sphere", "cylinder")
+
+
+@dataclass(frozen=True)
+class PrimitiveFit:
+    kind: str
+    transform: np.ndarray
+    dimensions: tuple[float, ...]
+    volume: float
+
+
+def _points_array(points: np.ndarray) -> np.ndarray:
+    result = np.asarray(points, dtype=np.float64)
+    if result.ndim != 2 or result.shape[1] != 3 or len(result) < 4:
+        raise ValueError("at least four 3D points are required")
+    if not np.isfinite(result).all():
+        raise ValueError("points contain NaN or infinity")
+    return result
+
+
+def fit_box(points: np.ndarray, padding: float = 0.0, scale: float = 1.0) -> PrimitiveFit:
+    points = _points_array(points)
+    to_box, extents = trimesh.bounds.oriented_bounds(points)
+    extents = np.asarray(extents, dtype=np.float64) * scale + 2.0 * padding
+    transform = np.linalg.inv(np.asarray(to_box, dtype=np.float64))
+    return PrimitiveFit("box", transform, tuple(extents), float(np.prod(extents)))
+
+
+def fit_link_aligned_box(
+    points: np.ndarray, padding: float = 0.0, scale: float = 1.0
+) -> PrimitiveFit:
+    points = _points_array(points)
+    lower = points.min(axis=0)
+    upper = points.max(axis=0)
+    extents = (upper - lower) * scale + 2.0 * padding
+    transform = np.eye(4)
+    transform[:3, 3] = (lower + upper) / 2.0
+    return PrimitiveFit("box", transform, tuple(extents), float(np.prod(extents)))
+
+
+def fit_sphere(points: np.ndarray, padding: float = 0.0, scale: float = 1.0) -> PrimitiveFit:
+    points = _points_array(points)
+    center, radius = trimesh.nsphere.minimum_nsphere(points)
+    radius = float(radius) * scale + padding
+    transform = np.eye(4)
+    transform[:3, 3] = center
+    return PrimitiveFit("sphere", transform, (radius,), float(4.0 * np.pi * radius**3 / 3.0))
+
+
+def fit_cylinder(
+    points: np.ndarray,
+    padding: float = 0.0,
+    scale: float = 1.0,
+    sample_count: int = 6,
+    angle_tol: float = 0.001,
+) -> PrimitiveFit:
+    points = _points_array(points)
+    result = trimesh.bounds.minimum_cylinder(
+        points, sample_count=sample_count, angle_tol=angle_tol
+    )
+    radius = float(result["radius"]) * scale + padding
+    height = float(result["height"]) * scale + 2.0 * padding
+    transform = np.asarray(result["transform"], dtype=np.float64)
+    volume = float(np.pi * radius**2 * height)
+    return PrimitiveFit("cylinder", transform, (radius, height), volume)
+
+
+def fit_axis_aligned_cylinder(
+    points: np.ndarray,
+    axis: str,
+    padding: float = 0.0,
+    scale: float = 1.0,
+) -> PrimitiveFit:
+    points = _points_array(points)
+    axis = axis.lower()
+    if axis not in "xyz":
+        raise ValueError(f"cylinder axis must be x, y, or z: {axis}")
+    axis_index = "xyz".index(axis)
+    radial_indices = [index for index in range(3) if index != axis_index]
+    radial_center, radius = trimesh.nsphere.minimum_nsphere(
+        points[:, radial_indices]
+    )
+    axial_min = float(points[:, axis_index].min())
+    axial_max = float(points[:, axis_index].max())
+
+    center = np.zeros(3)
+    center[axis_index] = (axial_min + axial_max) / 2.0
+    center[radial_indices] = radial_center
+    radius = float(radius) * scale + padding
+    height = (axial_max - axial_min) * scale + 2.0 * padding
+
+    transform = np.eye(4)
+    if axis == "x":
+        transform[:3, :3] = np.array(
+            [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]]
+        )
+    elif axis == "y":
+        transform[:3, :3] = np.array(
+            [[1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, -1.0, 0.0]]
+        )
+    transform[:3, 3] = center
+    volume = float(np.pi * radius**2 * height)
+    return PrimitiveFit("cylinder", transform, (radius, height), volume)
+
+
+def fit_primitive(
+    points: np.ndarray,
+    kind: str,
+    *,
+    allowed: list[str],
+    padding: float,
+    scale: float,
+    cylinder_sample_count: int,
+    cylinder_angle_tol: float,
+    alignment: str = "oriented",
+    axis: str | None = None,
+) -> PrimitiveFit:
+    def fit(candidate: str) -> PrimitiveFit:
+        if candidate == "box":
+            if alignment == "link":
+                return fit_link_aligned_box(points, padding, scale)
+            return fit_box(points, padding, scale)
+        if candidate == "sphere":
+            return fit_sphere(points, padding, scale)
+        if candidate == "cylinder":
+            if axis:
+                return fit_axis_aligned_cylinder(points, axis, padding, scale)
+            return fit_cylinder(
+                points,
+                padding,
+                scale,
+                cylinder_sample_count,
+                cylinder_angle_tol,
+            )
+        raise ValueError(f"unsupported primitive type: {candidate}")
+
+    if kind != "auto":
+        return fit(kind)
+
+    candidates: list[PrimitiveFit] = []
+    failures: list[str] = []
+    for candidate in allowed:
+        try:
+            candidates.append(fit(candidate))
+        except Exception as exc:  # A degenerate mesh may fail one fitter only.
+            failures.append(f"{candidate}: {exc}")
+    if not candidates:
+        raise RuntimeError("all primitive fits failed: " + "; ".join(failures))
+    return min(candidates, key=lambda candidate: candidate.volume)
+
+
+def load_config(path: Path | None) -> dict[str, Any]:
+    if path is None:
+        return {}
+    with path.open("rb") as stream:
+        config = tomllib.load(stream)
+    config["_config_dir"] = str(path.parent.resolve())
+    return config
+
+
+def link_settings(config: dict[str, Any], link_name: str) -> dict[str, Any]:
+    settings = dict(config.get("defaults", {}))
+    settings.update(config.get("links", {}).get(link_name, {}))
+    return settings
+
+
+def _find_robot_root(stage: Any, requested_path: str | None) -> Any:
+    if requested_path:
+        prim = stage.GetPrimAtPath(requested_path)
+        if not prim:
+            raise ValueError(f"robot root does not exist: {requested_path}")
+        return prim
+
+    default_prim = stage.GetDefaultPrim()
+    if default_prim and default_prim.GetRelationship("isaac:physics:robotLinks").IsValid():
+        return default_prim
+
+    for prim in stage.Traverse():
+        if prim.GetRelationship("isaac:physics:robotLinks").IsValid():
+            return prim
+    raise RuntimeError("could not find an Isaac robotLinks relationship; pass --robot-root")
+
+
+def find_robot_links(stage: Any, robot_root_path: str | None) -> list[Any]:
+    root = _find_robot_root(stage, robot_root_path)
+    targets = root.GetRelationship("isaac:physics:robotLinks").GetTargets()
+    links = [stage.GetPrimAtPath(path) for path in targets]
+    links = [prim for prim in links if prim]
+    if not links:
+        raise RuntimeError(f"robot has no resolved links: {root.GetPath()}")
+    return links
+
+
+def _computed_purpose(prim: Any, UsdGeom: Any) -> str:
+    imageable = UsdGeom.Imageable(prim)
+    if not imageable:
+        return ""
+    return str(imageable.ComputePurpose())
+
+
+def collect_visual_points(link: Any, link_paths: set[str], Usd: Any, UsdGeom: Any, UsdPhysics: Any) -> tuple[np.ndarray, set[str]]:
+    """Return visual vertices in link coordinates and direct mesh-collider roots."""
+    cache = UsdGeom.XformCache()
+    link_to_world = cache.GetLocalToWorldTransform(link)
+    world_to_link = np.asarray(link_to_world.GetInverse(), dtype=np.float64)
+    point_sets: list[np.ndarray] = []
+    mesh_collision_roots: set[str] = set()
+
+    for child in link.GetChildren():
+        if str(child.GetPath()) in link_paths or child.GetName().startswith(GENERATED_PREFIX):
+            continue
+
+        child_has_mesh_collision = False
+        for prim in Usd.PrimRange(child, Usd.TraverseInstanceProxies()):
+            if not prim.IsA(UsdGeom.Mesh):
+                continue
+            purpose = _computed_purpose(prim, UsdGeom)
+            is_collision = prim.HasAPI(UsdPhysics.CollisionAPI) or purpose == str(UsdGeom.Tokens.guide)
+            if is_collision:
+                child_has_mesh_collision = True
+                continue
+            if purpose not in ("", str(UsdGeom.Tokens.default_), str(UsdGeom.Tokens.render)):
+                continue
+
+            points = np.asarray(UsdGeom.Mesh(prim).GetPointsAttr().Get(), dtype=np.float64)
+            if not len(points):
+                continue
+            mesh_to_world = np.asarray(cache.GetLocalToWorldTransform(prim), dtype=np.float64)
+            mesh_to_link = mesh_to_world @ world_to_link
+            local_points = points @ mesh_to_link[:3, :3] + mesh_to_link[3, :3]
+            point_sets.append(local_points)
+
+        if child_has_mesh_collision:
+            mesh_collision_roots.add(str(child.GetPath()))
+
+    if not point_sets:
+        raise RuntimeError(f"no visual mesh vertices found below {link.GetPath()}")
+    return np.concatenate(point_sets), mesh_collision_roots
+
+
+def collect_mesh_file_points(path: Path) -> np.ndarray:
+    """Load a URDF visual mesh whose vertices are already link-local."""
+    if not path.is_file():
+        raise FileNotFoundError(path)
+    loaded = trimesh.load(path, force="mesh")
+    if isinstance(loaded, trimesh.Scene):
+        meshes = tuple(loaded.geometry.values())
+        if not meshes:
+            raise RuntimeError(f"mesh file has no geometry: {path}")
+        loaded = trimesh.util.concatenate(meshes)
+    return _points_array(np.asarray(loaded.vertices, dtype=np.float64))
+
+
+def _set_transform(prim: Any, transform: np.ndarray, scale: tuple[float, float, float] | None, Gf: Any, UsdGeom: Any) -> None:
+    xformable = UsdGeom.Xformable(prim)
+    translation = transform[:3, 3]
+    quaternion = trimesh.transformations.quaternion_from_matrix(transform)
+    xformable.AddTranslateOp().Set(Gf.Vec3d(*translation.tolist()))
+    xformable.AddOrientOp(UsdGeom.XformOp.PrecisionDouble).Set(
+        Gf.Quatd(float(quaternion[0]), Gf.Vec3d(*quaternion[1:4].tolist()))
+    )
+    if scale is not None:
+        xformable.AddScaleOp().Set(Gf.Vec3d(*scale))
+
+
+def prepare_authoring_links(stage: Any, link_paths: list[str]) -> None:
+    """De-instance only branches that contain a link requiring a new child."""
+    instance_roots: set[str] = set()
+    for link_path in link_paths:
+        link = stage.GetPrimAtPath(link_path)
+        if not link:
+            continue
+        if link.IsInstance():
+            instance_roots.add(link_path)
+            continue
+        if not link.IsInstanceProxy():
+            continue
+        instance_root = link
+        while instance_root.IsInstanceProxy():
+            instance_root = instance_root.GetParent()
+        if not instance_root or not instance_root.IsInstance():
+            raise RuntimeError(f"could not find an instance root for collision link: {link_path}")
+        instance_roots.add(str(instance_root.GetPath()))
+
+    if not instance_roots:
+        return
+    for instance_root in instance_roots:
+        stage.OverridePrim(instance_root).SetInstanceable(False)
+    stage.GetRootLayer().Save()
+    stage.Reload()
+
+    still_proxies = [
+        link_path
+        for link_path in link_paths
+        if stage.GetPrimAtPath(link_path).IsInstanceProxy()
+    ]
+    if still_proxies:
+        raise RuntimeError(f"links remained instance proxies: {still_proxies}")
+
+
+def author_primitive(stage: Any, link_path: str, fit: PrimitiveFit, Gf: Any, Sdf: Any, UsdGeom: Any, UsdPhysics: Any) -> str:
+    prim_path = f"{link_path}/{GENERATED_PREFIX}{fit.kind.upper()}"
+    if fit.kind == "box":
+        shape = UsdGeom.Cube.Define(stage, prim_path)
+        shape.CreateSizeAttr(1.0)
+        scale = tuple(float(value) for value in fit.dimensions)
+    elif fit.kind == "sphere":
+        shape = UsdGeom.Sphere.Define(stage, prim_path)
+        shape.CreateRadiusAttr(float(fit.dimensions[0]))
+        scale = None
+    elif fit.kind == "cylinder":
+        shape = UsdGeom.Cylinder.Define(stage, prim_path)
+        shape.CreateAxisAttr(UsdGeom.Tokens.z)
+        shape.CreateRadiusAttr(float(fit.dimensions[0]))
+        shape.CreateHeightAttr(float(fit.dimensions[1]))
+        scale = None
+    else:
+        raise AssertionError(fit.kind)
+
+    prim = shape.GetPrim()
+    _set_transform(prim, fit.transform, scale, Gf, UsdGeom)
+    UsdPhysics.CollisionAPI.Apply(prim).CreateCollisionEnabledAttr(True)
+    UsdGeom.Imageable(prim).CreatePurposeAttr(UsdGeom.Tokens.guide)
+    prim.SetCustomDataByKey("collisionGenerator", GENERATOR_TAG)
+    prim.CreateAttribute("collision:primitiveType", Sdf.ValueTypeNames.Token, custom=True).Set(fit.kind)
+    return prim_path
+
+
+def create_overlay_stage(
+    input_path: Path,
+    temporary_output: Path,
+    default_prim_path: str,
+    source_stage: Any,
+    Usd: Any,
+) -> Any:
+    stage = Usd.Stage.CreateNew(str(temporary_output))
+    relative_input = os.path.relpath(input_path, temporary_output.parent)
+    stage.GetRootLayer().subLayerPaths = [relative_input]
+    for metadata_key in ("upAxis", "metersPerUnit", "kilogramsPerUnit"):
+        metadata_value = source_stage.GetMetadata(metadata_key)
+        if metadata_value is not None:
+            stage.SetMetadata(metadata_key, metadata_value)
+    source_default = stage.GetPrimAtPath(default_prim_path)
+    if not source_default:
+        raise RuntimeError(f"default prim did not compose into overlay: {default_prim_path}")
+    stage.SetDefaultPrim(source_default)
+    return stage
+
+
+def validate_usd_output(path: Path, expected_count: int, Usd: Any, UsdGeom: Any, UsdPhysics: Any) -> None:
+    stage = Usd.Stage.Open(str(path))
+    if not stage.GetDefaultPrim():
+        raise RuntimeError("output USD has no default prim")
+    generated = [
+        prim
+        for prim in stage.Traverse()
+        if prim.GetCustomDataByKey("collisionGenerator") == GENERATOR_TAG
+    ]
+    if len(generated) != expected_count:
+        raise RuntimeError(f"expected {expected_count} generated colliders, found {len(generated)}")
+    for prim in generated:
+        if prim.GetTypeName() not in ("Cube", "Sphere", "Cylinder"):
+            raise RuntimeError(f"generated collider is not a primitive: {prim.GetPath()}")
+        if not prim.HasAPI(UsdPhysics.CollisionAPI):
+            raise RuntimeError(f"CollisionAPI missing: {prim.GetPath()}")
+        if _computed_purpose(prim, UsdGeom) != str(UsdGeom.Tokens.guide):
+            raise RuntimeError(f"guide purpose missing: {prim.GetPath()}")
+
+
+def run_usd(args: argparse.Namespace, Usd: Any, UsdGeom: Any, UsdPhysics: Any, Gf: Any, Sdf: Any) -> int:
+    input_path = args.input.resolve()
+    if not input_path.is_file():
+        raise FileNotFoundError(input_path)
+    if not args.dry_run and args.output is None:
+        raise ValueError("--output is required unless --dry-run is used")
+
+    config = load_config(args.config.resolve() if args.config else None)
+    defaults = config.get("defaults", {})
+    allowed = list(defaults.get("allowed_primitives", PRIMITIVE_TYPES))
+    invalid = set(allowed) - set(PRIMITIVE_TYPES)
+    if invalid:
+        raise ValueError(f"invalid allowed_primitives: {sorted(invalid)}")
+
+    source_stage = Usd.Stage.Open(str(input_path))
+    if not source_stage:
+        raise RuntimeError(f"could not open USD: {input_path}")
+    links = find_robot_links(source_stage, args.robot_root)
+    source_default = source_stage.GetDefaultPrim()
+    if not source_default:
+        raise RuntimeError("input USD has no default prim")
+    link_paths = {str(link.GetPath()) for link in links}
+    selected = set(args.only)
+    results: list[tuple[str, PrimitiveFit, set[str]]] = []
+
+    for link in links:
+        name = link.GetName()
+        if selected and name not in selected:
+            continue
+        settings = link_settings(config, name)
+        if not settings.get("enabled", True):
+            print(f"SKIP {name}: disabled by configuration")
+            continue
+        try:
+            points, collision_roots = collect_visual_points(
+                link, link_paths, Usd, UsdGeom, UsdPhysics
+            )
+        except RuntimeError as error:
+            mesh_file = settings.get("mesh_file")
+            if not mesh_file:
+                raise RuntimeError(
+                    f"{error}; no mesh_file configured for link name {name!r}"
+                ) from error
+            mesh_path = Path(mesh_file)
+            if not mesh_path.is_absolute():
+                mesh_path = Path(config["_config_dir"]) / mesh_path
+            points = collect_mesh_file_points(mesh_path.resolve())
+            collision_roots = set()
+            print(f"FALLBACK {name}: loaded {mesh_path}")
+        kind = str(settings.get("primitive", "auto"))
+        if kind not in (*PRIMITIVE_TYPES, "auto"):
+            raise ValueError(f"invalid primitive for {name}: {kind}")
+        fit = fit_primitive(
+            points,
+            kind,
+            allowed=list(settings.get("allowed_primitives", allowed)),
+            padding=float(settings.get("padding", 0.0)),
+            scale=float(settings.get("scale", 1.0)),
+            cylinder_sample_count=int(settings.get("cylinder_sample_count", defaults.get("cylinder_sample_count", 6))),
+            cylinder_angle_tol=float(settings.get("cylinder_angle_tol", defaults.get("cylinder_angle_tol", 0.001))),
+            alignment=str(settings.get("alignment", "oriented")),
+            axis=str(settings["axis"]) if "axis" in settings else None,
+        )
+        results.append((str(link.GetPath()), fit, collision_roots))
+        dimensions = ", ".join(f"{value:.6f}" for value in fit.dimensions)
+        print(f"FIT  {name}: {fit.kind} ({dimensions}), vertices={len(points)}, volume={fit.volume:.8f}")
+
+    if selected:
+        found = {Path(path).name for path, _, _ in results}
+        missing = selected - found
+        if missing:
+            raise ValueError(f"selected links were not generated: {sorted(missing)}")
+    if args.dry_run:
+        print(f"Dry run complete: {len(results)} collider(s) fitted")
+        return 0
+
+    output_path = args.output.resolve()
+    if output_path == input_path:
+        raise ValueError("input and output must be different files")
+    if output_path.exists() and not args.replace:
+        raise FileExistsError(f"output exists; pass --replace: {output_path}")
+    output_path.parent.mkdir(parents=True, exist_ok=True)
+    temporary = output_path.with_name(f".{output_path.stem}.tmp{output_path.suffix}")
+    if temporary.exists():
+        temporary.unlink()
+
+    print(f"CREATE overlay: {temporary}", flush=True)
+    output_stage = create_overlay_stage(
+        input_path,
+        temporary,
+        str(source_default.GetPath()),
+        source_stage,
+        Usd,
+    )
+    print("CREATE overlay: composed", flush=True)
+    prepare_authoring_links(output_stage, [link_path for link_path, _, _ in results])
+    disable_meshes = bool(defaults.get("disable_existing_mesh_collisions", True))
+    for link_path, fit, collision_roots in results:
+        if disable_meshes:
+            for collision_root in collision_roots:
+                output_stage.OverridePrim(collision_root).SetActive(False)
+        authored = author_primitive(
+            output_stage, link_path, fit, Gf, Sdf, UsdGeom, UsdPhysics
+        )
+        print(f"WRITE {authored}")
+    output_stage.GetRootLayer().Save()
+    del output_stage
+    os.replace(temporary, output_path)
+
+    if args.validate:
+        validate_usd_output(output_path, len(results), Usd, UsdGeom, UsdPhysics)
+        print(f"Validated {len(results)} generated collider(s)")
+    print(f"Output: {output_path}")
+    return 0
+
+
+def _parse_vector(
+    value: str | None, size: int, default: tuple[float, ...]
+) -> np.ndarray:
+    if value is None:
+        return np.asarray(default, dtype=np.float64)
+    result = np.fromstring(value, sep=" ", dtype=np.float64)
+    if len(result) != size or not np.isfinite(result).all():
+        raise ValueError(f"expected {size} finite values, got {value!r}")
+    return result
+
+
+def _urdf_origin_transform(origin: ET.Element | None) -> np.ndarray:
+    if origin is None:
+        return np.eye(4)
+    xyz = _parse_vector(origin.get("xyz"), 3, (0.0, 0.0, 0.0))
+    rpy = _parse_vector(origin.get("rpy"), 3, (0.0, 0.0, 0.0))
+    transform = trimesh.transformations.euler_matrix(*rpy, axes="sxyz")
+    transform[:3, 3] = xyz
+    return transform
+
+
+def _resolve_urdf_mesh_path(filename: str, urdf_path: Path) -> Path:
+    if filename.startswith("file://"):
+        path = Path(filename.removeprefix("file://"))
+    elif filename.startswith("package://"):
+        package_path = Path(filename.removeprefix("package://"))
+        if len(package_path.parts) < 2:
+            raise ValueError(f"invalid package URI: {filename}")
+        package_name, relative_parts = package_path.parts[0], package_path.parts[1:]
+        candidates = [
+            parent / package_name / Path(*relative_parts)
+            for parent in (urdf_path.parent, *urdf_path.parents)
+        ]
+        candidates.extend(
+            parent / Path(*relative_parts)
+            for parent in urdf_path.parents
+            if parent.name == package_name
+        )
+        for candidate in candidates:
+            if candidate.is_file():
+                return candidate.resolve()
+        raise FileNotFoundError(
+            f"could not resolve {filename!r} relative to {urdf_path}"
+        )
+    else:
+        path = Path(filename)
+    if not path.is_absolute():
+        path = urdf_path.parent / path
+    path = path.resolve()
+    if not path.is_file():
+        raise FileNotFoundError(path)
+    return path
+
+
+def collect_urdf_visual_points(link: ET.Element, urdf_path: Path) -> np.ndarray:
+    """Collect all visual mesh vertices in the link-local coordinate frame."""
+    point_sets: list[np.ndarray] = []
+    for visual in link.findall("visual"):
+        visual_transform = _urdf_origin_transform(visual.find("origin"))
+        geometry = visual.find("geometry")
+        mesh = geometry.find("mesh") if geometry is not None else None
+        if mesh is None:
+            continue
+        filename = mesh.get("filename")
+        if not filename:
+            raise ValueError(
+                f"visual mesh has no filename in link {link.get('name')!r}"
+            )
+        vertices = collect_mesh_file_points(
+            _resolve_urdf_mesh_path(filename, urdf_path)
+        )
+        mesh_scale = _parse_vector(mesh.get("scale"), 3, (1.0, 1.0, 1.0))
+        vertices = trimesh.transform_points(vertices * mesh_scale, visual_transform)
+        point_sets.append(vertices)
+    if not point_sets:
+        raise RuntimeError(f"no visual mesh found in link {link.get('name')!r}")
+    return np.concatenate(point_sets)
+
+
+def _format_number(value: float) -> str:
+    if abs(value) < 5e-13:
+        value = 0.0
+    return f"{value:.12g}"
+
+
+def _format_vector(values: np.ndarray | tuple[float, ...]) -> str:
+    return " ".join(_format_number(float(value)) for value in values)
+
+
+def create_urdf_collision(fit: PrimitiveFit) -> ET.Element:
+    collision = ET.Element(
+        "collision", {"name": f"{GENERATED_PREFIX}{fit.kind.upper()}"}
+    )
+    translation = fit.transform[:3, 3]
+    rpy = trimesh.transformations.euler_from_matrix(fit.transform, axes="sxyz")
+    ET.SubElement(
+        collision,
+        "origin",
+        {"xyz": _format_vector(translation), "rpy": _format_vector(rpy)},
+    )
+    geometry = ET.SubElement(collision, "geometry")
+    if fit.kind == "box":
+        ET.SubElement(geometry, "box", {"size": _format_vector(fit.dimensions)})
+    elif fit.kind == "sphere":
+        ET.SubElement(
+            geometry, "sphere", {"radius": _format_number(fit.dimensions[0])}
+        )
+    elif fit.kind == "cylinder":
+        ET.SubElement(
+            geometry,
+            "cylinder",
+            {
+                "radius": _format_number(fit.dimensions[0]),
+                "length": _format_number(fit.dimensions[1]),
+            },
+        )
+    else:
+        raise AssertionError(fit.kind)
+    return collision
+
+
+def _fit_urdf_link(
+    link: ET.Element,
+    urdf_path: Path,
+    config: dict[str, Any],
+    defaults: dict[str, Any],
+    allowed: list[str],
+) -> tuple[PrimitiveFit, int]:
+    name = link.get("name", "")
+    settings = link_settings(config, name)
+    try:
+        points = collect_urdf_visual_points(link, urdf_path)
+    except RuntimeError as error:
+        mesh_file = settings.get("mesh_file")
+        if not mesh_file:
+            raise RuntimeError(str(error)) from error
+        mesh_path = Path(mesh_file)
+        if not mesh_path.is_absolute():
+            mesh_path = Path(config["_config_dir"]) / mesh_path
+        points = collect_mesh_file_points(mesh_path.resolve())
+        print(f"FALLBACK {name}: loaded {mesh_path}")
+
+    kind = str(settings.get("primitive", "auto"))
+    if kind not in (*PRIMITIVE_TYPES, "auto"):
+        raise ValueError(f"invalid primitive for {name}: {kind}")
+    link_allowed = list(settings.get("allowed_primitives", allowed))
+    invalid = set(link_allowed) - set(PRIMITIVE_TYPES)
+    if invalid:
+        raise ValueError(f"invalid allowed_primitives for {name}: {sorted(invalid)}")
+    fit = fit_primitive(
+        points,
+        kind,
+        allowed=link_allowed,
+        padding=float(settings.get("padding", 0.0)),
+        scale=float(settings.get("scale", 1.0)),
+        cylinder_sample_count=int(
+            settings.get(
+                "cylinder_sample_count", defaults.get("cylinder_sample_count", 6)
+            )
+        ),
+        cylinder_angle_tol=float(
+            settings.get(
+                "cylinder_angle_tol", defaults.get("cylinder_angle_tol", 0.001)
+            )
+        ),
+        alignment=str(settings.get("alignment", "oriented")),
+        axis=str(settings["axis"]) if "axis" in settings else None,
+    )
+    return fit, len(points)
+
+
+def _parse_urdf(path: Path) -> ET.ElementTree:
+    parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True))
+    tree = ET.parse(path, parser=parser)
+    root = tree.getroot()
+    if root.tag != "robot":
+        raise ValueError(f"URDF root must be <robot>, found <{root.tag}>")
+    return tree
+
+
+def validate_urdf_output(path: Path, expected: dict[str, PrimitiveFit]) -> None:
+    root = _parse_urdf(path).getroot()
+    links = {link.get("name", ""): link for link in root.findall("link")}
+    missing = set(expected) - set(links)
+    if missing:
+        raise RuntimeError(f"output URDF is missing links: {sorted(missing)}")
+    for name, fit in expected.items():
+        generated = [
+            collision
+            for collision in links[name].findall("collision")
+            if collision.get("name", "").startswith(GENERATED_PREFIX)
+        ]
+        if len(generated) != 1:
+            raise RuntimeError(
+                f"expected one generated collider on {name}, found {len(generated)}"
+            )
+        geometry = generated[0].find("geometry")
+        primitive_count = (
+            sum(geometry.find(kind) is not None for kind in PRIMITIVE_TYPES)
+            if geometry is not None
+            else 0
+        )
+        if primitive_count != 1:
+            raise RuntimeError(f"invalid generated collision geometry on {name}")
+        written_transform = _urdf_origin_transform(generated[0].find("origin"))
+        if not np.allclose(written_transform, fit.transform, atol=1e-9, rtol=1e-9):
+            error = float(np.max(np.abs(written_transform - fit.transform)))
+            raise RuntimeError(
+                f"generated collision transform changed on {name}: max error {error}"
+            )
+        primitive = geometry.find(fit.kind)
+        if primitive is None:
+            raise RuntimeError(f"expected {fit.kind} collision geometry on {name}")
+        if fit.kind == "box":
+            dimensions = _parse_vector(primitive.get("size"), 3, ())
+        elif fit.kind == "sphere":
+            dimensions = np.asarray([float(primitive.get("radius", "nan"))])
+        else:
+            dimensions = np.asarray(
+                [
+                    float(primitive.get("radius", "nan")),
+                    float(primitive.get("length", "nan")),
+                ]
+            )
+        expected_dimensions = np.asarray(fit.dimensions)
+        if (
+            not np.isfinite(dimensions).all()
+            or (dimensions <= 0.0).any()
+            or not np.allclose(
+                dimensions, expected_dimensions, atol=1e-9, rtol=1e-9
+            )
+        ):
+            raise RuntimeError(
+                f"generated collision dimensions changed on {name}: "
+                f"expected {expected_dimensions}, found {dimensions}"
+            )
+
+
+def run_urdf(args: argparse.Namespace) -> int:
+    input_path = args.input.resolve()
+    if not input_path.is_file():
+        raise FileNotFoundError(input_path)
+    if not args.dry_run and args.output is None:
+        raise ValueError("--output is required unless --dry-run is used")
+
+    config = load_config(args.config.resolve() if args.config else None)
+    defaults = config.get("defaults", {})
+    allowed = list(defaults.get("allowed_primitives", PRIMITIVE_TYPES))
+    invalid = set(allowed) - set(PRIMITIVE_TYPES)
+    if invalid:
+        raise ValueError(f"invalid allowed_primitives: {sorted(invalid)}")
+
+    tree = _parse_urdf(input_path)
+    root = tree.getroot()
+    selected = set(args.only)
+    known_names = {link.get("name", "") for link in root.findall("link")}
+    unknown = selected - known_names
+    if unknown:
+        raise ValueError(f"selected links do not exist: {sorted(unknown)}")
+
+    results: list[tuple[ET.Element, PrimitiveFit]] = []
+    for link in root.findall("link"):
+        name = link.get("name", "")
+        if selected and name not in selected:
+            continue
+        settings = link_settings(config, name)
+        if not settings.get("enabled", True):
+            print(f"SKIP {name}: disabled by configuration")
+            continue
+        fit, vertex_count = _fit_urdf_link(
+            link, input_path, config, defaults, allowed
+        )
+        results.append((link, fit))
+        dimensions = ", ".join(f"{value:.6f}" for value in fit.dimensions)
+        print(
+            f"FIT  {name}: {fit.kind} ({dimensions}), "
+            f"vertices={vertex_count}, volume={fit.volume:.8f}"
+        )
+
+    if args.dry_run:
+        print(f"Dry run complete: {len(results)} collider(s) fitted")
+        return 0
+
+    output_path = args.output.resolve()
+    if output_path == input_path:
+        raise ValueError("input and output must be different files")
+    if output_path.exists() and not args.replace:
+        raise FileExistsError(f"output exists; pass --replace: {output_path}")
+
+    expected: dict[str, PrimitiveFit] = {}
+    for link, fit in results:
+        if not args.keep_existing:
+            for collision in list(link.findall("collision")):
+                link.remove(collision)
+        link.append(create_urdf_collision(fit))
+        expected[link.get("name", "")] = fit
+
+    output_path.parent.mkdir(parents=True, exist_ok=True)
+    temporary = output_path.with_name(f".{output_path.stem}.tmp{output_path.suffix}")
+    if temporary.exists():
+        temporary.unlink()
+    ET.indent(tree, space="  ")
+    tree.write(temporary, encoding="utf-8", xml_declaration=True)
+    os.replace(temporary, output_path)
+
+    if args.validate:
+        validate_urdf_output(output_path, expected)
+        print(f"Validated {len(expected)} generated collider(s)")
+    print(f"Output: {output_path}")
+    return 0
+
+
+def parse_args(argv: list[str]) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--input", required=True, type=Path, help="source URDF or USD")
+    parser.add_argument("--output", type=Path, help="generated URDF or overlay USD")
+    parser.add_argument("--config", type=Path, help="TOML fitting configuration")
+    parser.add_argument(
+        "--robot-root", help="USD robot root prim path, if auto-detection fails"
+    )
+    parser.add_argument(
+        "--only",
+        action="append",
+        default=[],
+        metavar="LINK",
+        help="generate only selected link (repeatable)",
+    )
+    parser.add_argument(
+        "--keep-existing",
+        action="store_true",
+        help="URDF only: append instead of replacing collisions on generated links",
+    )
+    parser.add_argument(
+        "--dry-run", action="store_true", help="fit and print without writing output"
+    )
+    parser.add_argument(
+        "--replace", action="store_true", help="atomically replace an existing output"
+    )
+    parser.add_argument(
+        "--validate", action="store_true", help="reopen and validate generated output"
+    )
+    return parser.parse_args(argv)
+
+
+def _asset_format(path: Path) -> str:
+    suffix = path.suffix.lower()
+    if suffix == ".urdf":
+        return "urdf"
+    if suffix in (".usd", ".usda", ".usdc"):
+        return "usd"
+    raise ValueError(
+        f"unsupported file extension {path.suffix!r}; expected .urdf, .usd, .usda, or .usdc"
+    )
+
+
+def _validate_format_options(args: argparse.Namespace, asset_format: str) -> None:
+    if args.output is not None and _asset_format(args.output) != asset_format:
+        raise ValueError("input and output formats must match")
+    if asset_format == "urdf" and args.robot_root:
+        raise ValueError("--robot-root is only valid for USD input")
+    if asset_format == "usd" and args.keep_existing:
+        raise ValueError("--keep-existing is only valid for URDF input")
+
+
+def main(argv: list[str] | None = None) -> int:
+    try:
+        args = parse_args(sys.argv[1:] if argv is None else argv)
+        asset_format = _asset_format(args.input)
+        _validate_format_options(args, asset_format)
+        if asset_format == "urdf":
+            return run_urdf(args)
+
+        from isaacsim import SimulationApp
+
+        simulation_app = SimulationApp({"headless": True})
+        try:
+            from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics
+
+            return run_usd(args, Usd, UsdGeom, UsdPhysics, Gf, Sdf)
+        finally:
+            simulation_app.close()
+    except Exception:
+        traceback.print_exc()
+        sys.stderr.flush()
+        return 1
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

scripts/meshcat/README.md

@@ -0,0 +1,80 @@
+# URDF 碰撞体查看器
+
+`view_urdf_collisions.py` 使用 MeshCat 显示 URDF 中的碰撞体。默认按 Isaac Sim 风格
+原样显示视觉 STL 和 URDF 材质，并用亮绿色线框显示 `<collision>`。它直接解析 URDF
+的 link、joint 和 origin，不依赖 Pinocchio。
+
+## 安装
+
+当前 `cmvr-es` Conda 环境已经安装 MeshCat。其他环境可执行：
+
+```bash
+python -m pip install -r scripts/meshcat/requirements.txt
+```
+
+## 运行
+
+推荐先激活 `cmvr-es` 环境，再进入项目根目录：
+
+```bash
+conda activate cmvr-es
+cd /home/lgv/cmvr/0-workspace/cmvr-es
+
+python \
+  scripts/meshcat/view_urdf_collisions.py \
+  --input model/xiaoyan_description/dual_arm_collision.urdf
+```
+
+不能在 `/home/lgv/Desktop` 等其他目录直接使用上述相对路径，否则 Python 会在当前
+目录中查找 `scripts/` 和 `model/`。另外，MeshCat 安装在 `cmvr-es` 环境中，当前提示符
+如果是 `(base)`，需要先执行 `conda activate cmvr-es`。
+
+如果需要从任意目录启动，使用完整绝对路径和 `conda run`：
+
+```bash
+conda run -n cmvr-es python \
+  /home/lgv/cmvr/0-workspace/cmvr-es/scripts/meshcat/view_urdf_collisions.py \
+  --input /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_collision.urdf
+```
+
+脚本会打开浏览器，并持续运行到按下 `Ctrl+C`。如果不希望自动打开浏览器，可增加
+`--no-browser`，然后手动打开终端输出的 `MeshCat URL`。
+
+需要关闭终端后继续查看，或者希望页面刷新后仍然保留场景时，导出独立 HTML：
+
+```bash
+conda run -n cmvr-es python \
+  scripts/meshcat/view_urdf_collisions.py \
+  --input model/xiaoyan_description/dual_arm_collision.urdf \
+  --export-html /tmp/dual_arm_collision_meshcat.html
+```
+
+独立 HTML 会嵌入视觉 STL 和碰撞体，因此文件较大，但不再依赖后台 Python 进程或
+WebSocket 连接。
+
+只显示碰撞体：
+
+```bash
+conda run -n cmvr-es python \
+  scripts/meshcat/view_urdf_collisions.py \
+  --input model/xiaoyan_description/dual_arm_collision.urdf \
+  --collision-only
+```
+
+设置关节位置，旋转关节的单位为弧度：
+
+```bash
+conda run -n cmvr-es python \
+  scripts/meshcat/view_urdf_collisions.py \
+  --input model/xiaoyan_description/dual_arm_collision.urdf \
+  --joint L_SHOULDER_P=0.5 \
+  --joint R_SHOULDER_P=-0.5
+```
+
+其他显示选项：
+
+- `--solid-collisions`：把碰撞体切换为半透明实体
+- `--visual-opacity 0.15`：需要透视内部时降低视觉模型透明度
+- `--collision-opacity 0.8`：调整碰撞体线框或实体透明度
+- `--collision-color 00ff66`：使用十六进制颜色覆盖默认绿色
+- `--collision-cylinder-lines 12`：默认 12 条轴向母线，每 30° 一条

scripts/meshcat/requirements.txt

@@ -0,0 +1,2 @@
+meshcat==0.3.2
+numpy>=1.24

scripts/meshcat/view_urdf_collisions.py

@@ -0,0 +1,721 @@
+#!/usr/bin/env python3
+"""Visualize URDF collision geometry and optional visual meshes in MeshCat."""
+
+from __future__ import annotations
+
+import argparse
+from dataclasses import dataclass
+import math
+import os
+from pathlib import Path
+import subprocess
+import sys
+import time
+import traceback
+import webbrowser
+import xml.etree.ElementTree as ET
+
+import meshcat
+import meshcat.geometry as geometry
+import numpy as np
+
+
+@dataclass(frozen=True)
+class Joint:
+    name: str
+    kind: str
+    parent: str
+    child: str
+    origin: np.ndarray
+    axis: np.ndarray
+    mimic: tuple[str, float, float] | None
+
+
+def parse_vector(
+    value: str | None, size: int, default: tuple[float, ...]
+) -> np.ndarray:
+    if value is None:
+        return np.asarray(default, dtype=np.float64)
+    parts = value.split()
+    if len(parts) != size:
+        raise ValueError(f"expected {size} values, got {value!r}")
+    result = np.asarray([float(part) for part in parts], dtype=np.float64)
+    if not np.isfinite(result).all():
+        raise ValueError(f"values contain NaN or infinity: {value!r}")
+    return result
+
+
+def rpy_rotation(rpy: np.ndarray) -> np.ndarray:
+    roll, pitch, yaw = rpy
+    cr, sr = math.cos(roll), math.sin(roll)
+    cp, sp = math.cos(pitch), math.sin(pitch)
+    cy, sy = math.cos(yaw), math.sin(yaw)
+    return np.array(
+        [
+            [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
+            [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
+            [-sp, cp * sr, cp * cr],
+        ],
+        dtype=np.float64,
+    )
+
+
+def origin_transform(origin: ET.Element | None) -> np.ndarray:
+    transform = np.eye(4)
+    if origin is None:
+        return transform
+    transform[:3, :3] = rpy_rotation(
+        parse_vector(origin.get("rpy"), 3, (0.0, 0.0, 0.0))
+    )
+    transform[:3, 3] = parse_vector(
+        origin.get("xyz"), 3, (0.0, 0.0, 0.0)
+    )
+    return transform
+
+
+def axis_angle_transform(axis: np.ndarray, angle: float) -> np.ndarray:
+    norm = float(np.linalg.norm(axis))
+    if norm < 1e-12:
+        raise ValueError("joint axis must not be zero")
+    x, y, z = axis / norm
+    c, s = math.cos(angle), math.sin(angle)
+    one_minus_c = 1.0 - c
+    transform = np.eye(4)
+    transform[:3, :3] = np.array(
+        [
+            [c + x * x * one_minus_c, x * y * one_minus_c - z * s, x * z * one_minus_c + y * s],
+            [y * x * one_minus_c + z * s, c + y * y * one_minus_c, y * z * one_minus_c - x * s],
+            [z * x * one_minus_c - y * s, z * y * one_minus_c + x * s, c + z * z * one_minus_c],
+        ],
+        dtype=np.float64,
+    )
+    return transform
+
+
+def translation_transform(offset: np.ndarray) -> np.ndarray:
+    transform = np.eye(4)
+    transform[:3, 3] = offset
+    return transform
+
+
+def parse_joint_values(values: list[str]) -> dict[str, float]:
+    result: dict[str, float] = {}
+    for assignment in values:
+        name, separator, raw_value = assignment.partition("=")
+        if not separator or not name or not raw_value:
+            raise ValueError(
+                f"invalid --joint value {assignment!r}; expected NAME=VALUE"
+            )
+        if name in result:
+            raise ValueError(f"joint value specified more than once: {name}")
+        value = float(raw_value)
+        if not math.isfinite(value):
+            raise ValueError(f"joint value must be finite: {assignment!r}")
+        result[name] = value
+    return result
+
+
+def parse_joints(root: ET.Element) -> dict[str, Joint]:
+    joints: dict[str, Joint] = {}
+    children: set[str] = set()
+    for element in root.findall("joint"):
+        name = element.get("name")
+        kind = element.get("type")
+        parent_element = element.find("parent")
+        child_element = element.find("child")
+        if not name or not kind or parent_element is None or child_element is None:
+            raise ValueError("every joint needs name, type, parent, and child")
+        parent = parent_element.get("link")
+        child = child_element.get("link")
+        if not parent or not child:
+            raise ValueError(f"joint {name!r} has an empty parent or child")
+        if name in joints:
+            raise ValueError(f"duplicate joint name: {name}")
+        if child in children:
+            raise ValueError(f"link {child!r} has more than one parent joint")
+        axis_element = element.find("axis")
+        axis = parse_vector(
+            axis_element.get("xyz") if axis_element is not None else None,
+            3,
+            (1.0, 0.0, 0.0),
+        )
+        mimic_element = element.find("mimic")
+        mimic = None
+        if mimic_element is not None:
+            source = mimic_element.get("joint")
+            if not source:
+                raise ValueError(f"mimic joint {name!r} has no source joint")
+            mimic = (
+                source,
+                float(mimic_element.get("multiplier", "1")),
+                float(mimic_element.get("offset", "0")),
+            )
+        joints[name] = Joint(
+            name=name,
+            kind=kind,
+            parent=parent,
+            child=child,
+            origin=origin_transform(element.find("origin")),
+            axis=axis,
+            mimic=mimic,
+        )
+        children.add(child)
+    return joints
+
+
+def resolve_joint_values(
+    joints: dict[str, Joint], requested: dict[str, float]
+) -> dict[str, float]:
+    unknown = set(requested) - set(joints)
+    if unknown:
+        raise ValueError(f"unknown joints: {sorted(unknown)}")
+    fixed = [name for name in requested if joints[name].kind == "fixed"]
+    if fixed:
+        raise ValueError(f"fixed joints cannot be assigned: {sorted(fixed)}")
+
+    resolved: dict[str, float] = {}
+
+    def resolve(name: str, stack: set[str]) -> float:
+        if name in resolved:
+            return resolved[name]
+        if name in stack:
+            raise ValueError(f"mimic joint cycle contains {name!r}")
+        joint = joints[name]
+        if name in requested:
+            value = requested[name]
+        elif joint.mimic is not None:
+            source, multiplier, offset = joint.mimic
+            if source not in joints:
+                raise ValueError(
+                    f"mimic joint {name!r} references unknown joint {source!r}"
+                )
+            value = multiplier * resolve(source, stack | {name}) + offset
+        else:
+            value = 0.0
+        resolved[name] = value
+        return value
+
+    for joint_name in joints:
+        resolve(joint_name, set())
+    return resolved
+
+
+def joint_motion(joint: Joint, value: float) -> np.ndarray:
+    if joint.kind == "fixed":
+        return np.eye(4)
+    if joint.kind in ("revolute", "continuous"):
+        return axis_angle_transform(joint.axis, value)
+    if joint.kind == "prismatic":
+        norm = float(np.linalg.norm(joint.axis))
+        if norm < 1e-12:
+            raise ValueError(f"joint {joint.name!r} axis must not be zero")
+        axis = joint.axis / norm
+        return translation_transform(axis * value)
+    raise ValueError(
+        f"joint {joint.name!r} uses unsupported type {joint.kind!r}; "
+        "supported types are fixed, revolute, continuous, and prismatic"
+    )
+
+
+def compute_link_transforms(
+    root: ET.Element, joints: dict[str, Joint], values: dict[str, float]
+) -> dict[str, np.ndarray]:
+    links = {element.get("name") for element in root.findall("link")}
+    if None in links:
+        raise ValueError("every link needs a name")
+    children = {joint.child for joint in joints.values()}
+    roots = links - children
+    if not roots:
+        raise ValueError("URDF has no root link")
+
+    transforms = {name: np.eye(4) for name in roots}
+    pending = list(joints.values())
+    while pending:
+        unresolved: list[Joint] = []
+        for joint in pending:
+            if joint.parent not in links or joint.child not in links:
+                raise ValueError(
+                    f"joint {joint.name!r} references a missing parent or child link"
+                )
+            if joint.parent not in transforms:
+                unresolved.append(joint)
+                continue
+            transforms[joint.child] = (
+                transforms[joint.parent]
+                @ joint.origin
+                @ joint_motion(joint, values[joint.name])
+            )
+        if len(unresolved) == len(pending):
+            names = [joint.name for joint in unresolved]
+            raise ValueError(f"joint graph is cyclic or disconnected: {names}")
+        pending = unresolved
+    return transforms
+
+
+def resolve_mesh_path(filename: str, urdf_path: Path) -> Path:
+    if filename.startswith("file://"):
+        path = Path(filename.removeprefix("file://"))
+    elif filename.startswith("package://"):
+        package_path = Path(filename.removeprefix("package://"))
+        if len(package_path.parts) < 2:
+            raise ValueError(f"invalid package URI: {filename}")
+        package_name, relative_parts = package_path.parts[0], package_path.parts[1:]
+        candidates = [
+            parent / package_name / Path(*relative_parts)
+            for parent in (urdf_path.parent, *urdf_path.parents)
+        ]
+        candidates.extend(
+            parent / Path(*relative_parts)
+            for parent in urdf_path.parents
+            if parent.name == package_name
+        )
+        for candidate in candidates:
+            if candidate.is_file():
+                return candidate.resolve()
+        raise FileNotFoundError(
+            f"could not resolve {filename!r} relative to {urdf_path}"
+        )
+    else:
+        path = Path(filename)
+    if not path.is_absolute():
+        path = urdf_path.parent / path
+    path = path.resolve()
+    if not path.is_file():
+        raise FileNotFoundError(path)
+    return path
+
+
+def load_mesh(path: Path) -> geometry.Geometry:
+    suffix = path.suffix.lower()
+    if suffix == ".stl":
+        return geometry.StlMeshGeometry.from_file(str(path))
+    if suffix == ".obj":
+        return geometry.ObjMeshGeometry.from_file(str(path))
+    if suffix == ".dae":
+        return geometry.DaeMeshGeometry.from_file(str(path))
+    raise ValueError(
+        f"unsupported mesh format {path.suffix!r}: {path}; "
+        "MeshCat viewer supports STL, OBJ, and DAE"
+    )
+
+
+def cylinder_dimensions(cylinder: ET.Element) -> tuple[float, float]:
+    radius = float(cylinder.get("radius", "nan"))
+    length = float(cylinder.get("length", "nan"))
+    if (
+        not math.isfinite(radius)
+        or not math.isfinite(length)
+        or radius <= 0.0
+        or length <= 0.0
+    ):
+        raise ValueError(
+            f"cylinder radius and length must be positive: {radius}, {length}"
+        )
+    return radius, length
+
+
+def cylinder_correction() -> np.ndarray:
+    correction = np.eye(4)
+    # Three.js cylinders use local Y; URDF cylinders use local Z.
+    correction[:3, :3] = rpy_rotation(np.array([math.pi / 2.0, 0.0, 0.0]))
+    return correction
+
+
+def cylinder_wireframe(
+    radius: float,
+    length: float,
+    generator_count: int,
+    color: int,
+    opacity_value: float,
+    ring_segments: int = 64,
+) -> geometry.LineSegments:
+    vertices: list[tuple[float, float, float]] = []
+    half_length = length / 2.0
+
+    # Smooth top and bottom rings, without cap triangulation spokes.
+    for y in (-half_length, half_length):
+        for index in range(ring_segments):
+            first = 2.0 * math.pi * index / ring_segments
+            second = 2.0 * math.pi * (index + 1) / ring_segments
+            vertices.extend(
+                [
+                    (radius * math.cos(first), y, radius * math.sin(first)),
+                    (radius * math.cos(second), y, radius * math.sin(second)),
+                ]
+            )
+
+    # Sparse axial generator lines; six means one line every 60 degrees.
+    for index in range(generator_count):
+        angle = 2.0 * math.pi * index / generator_count
+        x = radius * math.cos(angle)
+        z = radius * math.sin(angle)
+        vertices.extend([(x, -half_length, z), (x, half_length, z)])
+
+    points = np.asarray(vertices, dtype=np.float32).T
+    material = geometry.LineBasicMaterial(
+        color=color,
+        transparent=opacity_value < 1.0,
+        opacity=opacity_value,
+    )
+    return geometry.LineSegments(geometry.PointsGeometry(points), material)
+
+
+def geometry_object(
+    geometry_element: ET.Element, urdf_path: Path
+) -> tuple[geometry.Geometry, np.ndarray]:
+    box = geometry_element.find("box")
+    sphere = geometry_element.find("sphere")
+    cylinder = geometry_element.find("cylinder")
+    mesh = geometry_element.find("mesh")
+    correction = np.eye(4)
+
+    if box is not None:
+        size = parse_vector(box.get("size"), 3, ())
+        if (size <= 0.0).any():
+            raise ValueError(f"box size must be positive: {size}")
+        return geometry.Box(size), correction
+    if sphere is not None:
+        radius = float(sphere.get("radius", "nan"))
+        if not math.isfinite(radius) or radius <= 0.0:
+            raise ValueError(f"sphere radius must be positive: {radius}")
+        return geometry.Sphere(radius), correction
+    if cylinder is not None:
+        radius, length = cylinder_dimensions(cylinder)
+        return geometry.Cylinder(length, radius), cylinder_correction()
+    if mesh is not None:
+        filename = mesh.get("filename")
+        if not filename:
+            raise ValueError("mesh geometry has no filename")
+        scale = parse_vector(mesh.get("scale"), 3, (1.0, 1.0, 1.0))
+        correction[:3, :3] = np.diag(scale)
+        return load_mesh(resolve_mesh_path(filename, urdf_path)), correction
+    raise ValueError("geometry must contain box, sphere, cylinder, or mesh")
+
+
+def parse_rgba(value: str) -> tuple[float, float, float, float]:
+    rgba = parse_vector(value, 4, ())
+    if ((rgba < 0.0) | (rgba > 1.0)).any():
+        raise ValueError(f"RGBA values must be between 0 and 1: {value!r}")
+    return tuple(float(component) for component in rgba)
+
+
+def rgb_integer(rgb: tuple[float, float, float]) -> int:
+    red, green, blue = (round(component * 255.0) for component in rgb)
+    return (red << 16) | (green << 8) | blue
+
+
+def visual_rgba(
+    visual: ET.Element,
+    named_materials: dict[str, tuple[float, float, float, float]],
+) -> tuple[float, float, float, float]:
+    material = visual.find("material")
+    if material is None:
+        return (0.65, 0.68, 0.72, 1.0)
+    color = material.find("color")
+    if color is not None and color.get("rgba"):
+        return parse_rgba(color.get("rgba", ""))
+    name = material.get("name")
+    if name and name in named_materials:
+        return named_materials[name]
+    return (0.65, 0.68, 0.72, 1.0)
+
+
+def named_materials(
+    root: ET.Element,
+) -> dict[str, tuple[float, float, float, float]]:
+    result: dict[str, tuple[float, float, float, float]] = {}
+    for material in root.findall("material"):
+        name = material.get("name")
+        color = material.find("color")
+        if name and color is not None and color.get("rgba"):
+            result[name] = parse_rgba(color.get("rgba", ""))
+    return result
+
+
+def parse_color(value: str) -> int:
+    normalized = value.removeprefix("#").removeprefix("0x")
+    if len(normalized) != 6:
+        raise argparse.ArgumentTypeError("color must use RRGGBB format")
+    try:
+        result = int(normalized, 16)
+    except ValueError as error:
+        raise argparse.ArgumentTypeError("color must use RRGGBB format") from error
+    return result
+
+
+def opacity(value: str) -> float:
+    result = float(value)
+    if not 0.0 <= result <= 1.0:
+        raise argparse.ArgumentTypeError("opacity must be between 0 and 1")
+    return result
+
+
+def cylinder_lines(value: str) -> int:
+    result = int(value)
+    if result < 3:
+        raise argparse.ArgumentTypeError("cylinder line count must be at least 3")
+    return result
+
+
+def safe_name(value: str) -> str:
+    return value.replace("/", "_")
+
+
+def render_urdf(
+    viewer: meshcat.Visualizer,
+    root: ET.Element,
+    urdf_path: Path,
+    link_transforms: dict[str, np.ndarray],
+    *,
+    collision_only: bool,
+    visual_opacity: float,
+    collision_opacity: float,
+    collision_color: int,
+    wireframe: bool,
+    collision_cylinder_lines: int,
+) -> tuple[int, int]:
+    viewer.delete()
+    materials = named_materials(root)
+    visual_count = 0
+    collision_count = 0
+    collision_material = geometry.MeshPhongMaterial(
+        color=collision_color,
+        transparent=collision_opacity < 1.0,
+        opacity=collision_opacity,
+        wireframe=wireframe,
+    )
+
+    for link in root.findall("link"):
+        link_name = link.get("name", "")
+        link_transform = link_transforms[link_name]
+        if not collision_only:
+            for index, visual in enumerate(link.findall("visual")):
+                geometry_element = visual.find("geometry")
+                if geometry_element is None:
+                    raise ValueError(f"visual geometry missing on link {link_name!r}")
+                shape, correction = geometry_object(geometry_element, urdf_path)
+                rgba = visual_rgba(visual, materials)
+                alpha = visual_opacity * rgba[3]
+                material = geometry.MeshPhongMaterial(
+                    color=rgb_integer(rgba[:3]),
+                    transparent=alpha < 1.0,
+                    opacity=alpha,
+                )
+                node = viewer[
+                    f"robot/visual/{safe_name(link_name)}/visual_{index}"
+                ]
+                node.set_object(shape, material)
+                node.set_transform(
+                    link_transform
+                    @ origin_transform(visual.find("origin"))
+                    @ correction
+                )
+                visual_count += 1
+
+        for index, collision in enumerate(link.findall("collision")):
+            geometry_element = collision.find("geometry")
+            if geometry_element is None:
+                raise ValueError(f"collision geometry missing on link {link_name!r}")
+            cylinder = geometry_element.find("cylinder")
+            if wireframe and cylinder is not None:
+                radius, length = cylinder_dimensions(cylinder)
+                shape = cylinder_wireframe(
+                    radius,
+                    length,
+                    collision_cylinder_lines,
+                    collision_color,
+                    collision_opacity,
+                )
+                correction = cylinder_correction()
+                custom_line_object = True
+            else:
+                shape, correction = geometry_object(geometry_element, urdf_path)
+                custom_line_object = False
+            node = viewer[
+                f"robot/collision/{safe_name(link_name)}/collision_{index}"
+            ]
+            if custom_line_object:
+                node.set_object(shape)
+            else:
+                node.set_object(shape, collision_material)
+            node.set_transform(
+                link_transform
+                @ origin_transform(collision.find("origin"))
+                @ correction
+            )
+            collision_count += 1
+    return visual_count, collision_count
+
+
+def close_viewer(viewer: meshcat.Visualizer) -> None:
+    """Close MeshCat 0.3.x without relying on its broken Visualizer.close()."""
+    window = viewer.window
+    window.zmq_socket.close(linger=0)
+    server_process = window.server_proc
+    if server_process is None or server_process.poll() is not None:
+        return
+    server_process.terminate()
+    try:
+        server_process.wait(timeout=3.0)
+    except subprocess.TimeoutExpired:
+        server_process.kill()
+        server_process.wait(timeout=3.0)
+
+
+def export_static_html(viewer: meshcat.Visualizer, output: Path) -> None:
+    output = output.resolve()
+    if output.suffix.lower() != ".html":
+        raise ValueError(f"MeshCat snapshot must use an .html extension: {output}")
+    output.parent.mkdir(parents=True, exist_ok=True)
+    temporary = output.with_name(f".{output.stem}.tmp{output.suffix}")
+    temporary.write_text(viewer.static_html(), encoding="utf-8")
+    os.replace(temporary, output)
+
+
+def parse_args(argv: list[str]) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--input", required=True, type=Path, help="URDF to display")
+    parser.add_argument(
+        "--joint",
+        action="append",
+        default=[],
+        metavar="NAME=VALUE",
+        help="joint position in radians, or meters for prismatic joints",
+    )
+    parser.add_argument(
+        "--collision-only",
+        action="store_true",
+        help="hide visual geometry and display only collisions",
+    )
+    parser.add_argument(
+        "--visual-opacity",
+        type=opacity,
+        default=1.0,
+        help="visual geometry opacity (default: 1.0)",
+    )
+    parser.add_argument(
+        "--collision-opacity",
+        type=opacity,
+        default=1.0,
+        help="collision geometry opacity (default: 1.0)",
+    )
+    parser.add_argument(
+        "--collision-color",
+        type=parse_color,
+        default=parse_color("00ff00"),
+        metavar="RRGGBB",
+        help="collision color in hexadecimal (default: 00ff00)",
+    )
+    parser.add_argument(
+        "--collision-cylinder-lines",
+        "--collision-cylinder-segments",
+        dest="collision_cylinder_lines",
+        type=cylinder_lines,
+        default=12,
+        metavar="COUNT",
+        help="axial lines on collision cylinders (default: 12, every 30 degrees)",
+    )
+    collision_style = parser.add_mutually_exclusive_group()
+    collision_style.add_argument(
+        "--wireframe",
+        dest="wireframe",
+        action="store_true",
+        default=True,
+        help="draw collision geometry as wireframe (default)",
+    )
+    collision_style.add_argument(
+        "--solid-collisions",
+        dest="wireframe",
+        action="store_false",
+        help="draw collision geometry as translucent solids",
+    )
+    parser.add_argument(
+        "--no-browser", action="store_true", help="do not automatically open a browser"
+    )
+    parser.add_argument(
+        "--export-html",
+        type=Path,
+        help="write a standalone MeshCat HTML snapshot and exit",
+    )
+    parser.add_argument(
+        "--exit-after-load",
+        action="store_true",
+        help=argparse.SUPPRESS,
+    )
+    parser.add_argument(
+        "--zmq-url", help="connect to an existing MeshCat ZMQ server"
+    )
+    return parser.parse_args(argv)
+
+
+def run(args: argparse.Namespace) -> int:
+    urdf_path = args.input.resolve()
+    if not urdf_path.is_file():
+        raise FileNotFoundError(urdf_path)
+    if urdf_path.suffix.lower() != ".urdf":
+        raise ValueError(f"input must be a .urdf file: {urdf_path}")
+    root = ET.parse(urdf_path).getroot()
+    if root.tag != "robot":
+        raise ValueError(f"URDF root must be <robot>, found <{root.tag}>")
+
+    joints = parse_joints(root)
+    requested = parse_joint_values(args.joint)
+    values = resolve_joint_values(joints, requested)
+    link_transforms = compute_link_transforms(root, joints, values)
+
+    viewer = meshcat.Visualizer(zmq_url=args.zmq_url)
+    visual_count, collision_count = render_urdf(
+        viewer,
+        root,
+        urdf_path,
+        link_transforms,
+        collision_only=args.collision_only,
+        visual_opacity=args.visual_opacity,
+        collision_opacity=args.collision_opacity,
+        collision_color=args.collision_color,
+        wireframe=args.wireframe,
+        collision_cylinder_lines=args.collision_cylinder_lines,
+    )
+    if collision_count == 0:
+        raise RuntimeError(f"URDF contains no <collision> elements: {urdf_path}")
+
+    url = viewer.url()
+    print(f"Loaded: {urdf_path}")
+    print(f"Visual geometry: {visual_count}")
+    print(f"Collision geometry: {collision_count}")
+    print(f"MeshCat URL: {url}", flush=True)
+    if args.export_html is not None:
+        export_static_html(viewer, args.export_html)
+        snapshot_uri = args.export_html.resolve().as_uri()
+        print(f"Standalone snapshot: {snapshot_uri}", flush=True)
+        if not args.no_browser:
+            webbrowser.open(snapshot_uri, new=2)
+        close_viewer(viewer)
+        return 0
+    if not args.no_browser:
+        viewer.open()
+    if args.exit_after_load:
+        close_viewer(viewer)
+        return 0
+
+    print("Press Ctrl+C to stop the viewer.", flush=True)
+    try:
+        while True:
+            time.sleep(1.0)
+    except KeyboardInterrupt:
+        print("Stopping MeshCat viewer.")
+    finally:
+        close_viewer(viewer)
+    return 0
+
+
+def main(argv: list[str] | None = None) -> int:
+    try:
+        return run(parse_args(sys.argv[1:] if argv is None else argv))
+    except Exception:
+        traceback.print_exc()
+        return 1
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())