diff --git a/MUJOCO_LOG.TXT b/MUJOCO_LOG.TXT new file mode 100644 index 00000000..55930968 --- /dev/null +++ b/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 + diff --git a/cmvr-es/algorithms/CMakeLists.txt b/cmvr-es/algorithms/CMakeLists.txt index 53f2415c..471f0dfe 100644 --- a/cmvr-es/algorithms/CMakeLists.txt +++ b/cmvr-es/algorithms/CMakeLists.txt @@ -2,3 +2,4 @@ add_subdirectory(motion_planner) add_subdirectory(kinematics/ik_solver) add_subdirectory(perception) add_subdirectory(controllers) +add_subdirectory(collision_detection) diff --git a/cmvr-es/algorithms/collision_detection/CMakeLists.txt b/cmvr-es/algorithms/collision_detection/CMakeLists.txt new file mode 100644 index 00000000..f6401175 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/CMakeLists.txt @@ -0,0 +1,48 @@ +add_library(self_collision_checker SHARED + self_collision/src/self_collision_checker.cpp + self_collision/src/distance_sampling_policy.cpp +) + +target_include_directories(self_collision_checker PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_compile_definitions(self_collision_checker PRIVATE + PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION + PINOCCHIO_WITH_HPP_FCL + COAL_DISABLE_HPP_FCL_WARNINGS +) + +target_link_libraries(self_collision_checker PUBLIC + pinocchio_default + pinocchio_parsers + pinocchio_collision + coal +) + +add_library(cmvr_es::self_collision_checker ALIAS self_collision_checker) + +add_executable(self_collision_checker_test + self_collision/test/self_collision_checker_test.cpp +) +target_link_libraries(self_collision_checker_test PRIVATE + cmvr_es::self_collision_checker + gtest + gtest_main + pthread +) +target_compile_definitions(self_collision_checker_test PRIVATE + CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}" +) + +add_executable(self_collision_benchmark + self_collision/benchmark/self_collision_benchmark.cpp +) +target_link_libraries(self_collision_benchmark PRIVATE + cmvr_es::self_collision_checker +) +target_compile_definitions(self_collision_benchmark PRIVATE + CMVR_ES_SOURCE_DIR="${PROJECT_SOURCE_DIR}" +) + +install(TARGETS self_collision_checker LIBRARY DESTINATION lib) diff --git a/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp b/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp new file mode 100644 index 00000000..dae09d71 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/benchmark/self_collision_benchmark.cpp @@ -0,0 +1,53 @@ +#include +#include +#include +#include +#include + +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +int main() +{ + const std::string urdf_path = std::string(CMVR_ES_SOURCE_DIR) + + "/model/xiaoyan_description/dual_arm_collision.urdf"; + const std::vector joint_names{ + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", + "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R", + }; + + cmvr::SelfCollisionChecker checker; + std::string error; + if (!checker.init(urdf_path, joint_names, {}, &error)) { + std::cerr << "Initialization failed: " << error << '\n'; + return 1; + } + + constexpr std::size_t kIterations = 2000; + std::vector samples_us; + samples_us.reserve(kIterations); + std::vector q(joint_names.size(), 0.0); + for (std::size_t iteration = 0; iteration < kIterations; ++iteration) { + q[0] = 0.2 * static_cast(iteration % 100) / 100.0; + const auto begin = std::chrono::steady_clock::now(); + const auto result = checker.check(q); + const auto end = std::chrono::steady_clock::now(); + if (!result.valid) { + std::cerr << "Collision check failed: " << result.error << '\n'; + return 1; + } + samples_us.push_back(std::chrono::duration(end - begin).count()); + } + + std::sort(samples_us.begin(), samples_us.end()); + double total_us = 0.0; + for (const double sample : samples_us) { + total_us += sample; + } + const std::size_t p99_index = static_cast(0.99 * (samples_us.size() - 1)); + std::cout << "active_pairs=" << checker.activePairCount() << '\n' + << "iterations=" << samples_us.size() << '\n' + << "average_us=" << total_us / samples_us.size() << '\n' + << "p99_us=" << samples_us[p99_index] << '\n' + << "max_us=" << samples_us.back() << '\n'; + return 0; +} diff --git a/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h b/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h new file mode 100644 index 00000000..f2b022af --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/include/distance_sampling_policy.h @@ -0,0 +1,46 @@ +#ifndef CMVR_ES_DISTANCE_SAMPLING_POLICY_H +#define CMVR_ES_DISTANCE_SAMPLING_POLICY_H + +#include +#include + +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +namespace cmvr { + +struct DistanceSamplingOptions { + double max_geometry_displacement_m{0.002}; + double max_check_period_s{0.01}; +}; + +class DistanceSamplingPolicy { +public: + using Clock = std::chrono::steady_clock; + + bool configure(const DistanceSamplingOptions& options, + std::string* error = nullptr); + + bool shouldCheck(const CollisionGeometrySnapshot& current, + Clock::time_point now) const; + + void markChecked(const CollisionGeometrySnapshot& current, + Clock::time_point now); + + void reset(); + + double displacementSinceLastCheck( + const CollisionGeometrySnapshot& current) const; + + bool hasBaseline() const { return has_baseline_; } + +private: + DistanceSamplingOptions options_{}; + CollisionGeometrySnapshot last_checked_{}; + Clock::time_point last_check_time_{}; + bool configured_{false}; + bool has_baseline_{false}; +}; + +} // namespace cmvr + +#endif // CMVR_ES_DISTANCE_SAMPLING_POLICY_H diff --git a/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h b/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h new file mode 100644 index 00000000..60fae78b --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/include/self_collision_checker.h @@ -0,0 +1,82 @@ +#ifndef CMVR_ES_SELF_COLLISION_CHECKER_H +#define CMVR_ES_SELF_COLLISION_CHECKER_H + +#include +#include +#include +#include + +#include + +namespace cmvr { + +struct CollisionPair { + std::string first; + std::string second; +}; + +struct SelfCollisionOptions { + std::vector ignored_pairs; +}; + +struct CollisionObjectPose { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::size_t geometry_index{0}; + Eigen::Vector3d position{Eigen::Vector3d::Zero()}; + Eigen::Quaterniond orientation{Eigen::Quaterniond::Identity()}; + double bounding_radius_m{0.0}; +}; + +struct CollisionGeometrySnapshot { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + std::vector> objects; +}; + +struct SelfCollisionResult { + bool valid{false}; + bool in_collision{false}; + double minimum_distance_m{0.0}; + std::string first; + std::string second; + std::string error; +}; + +// Instances cache Pinocchio work data and are not thread-safe. +class SelfCollisionChecker { +public: + SelfCollisionChecker(); + ~SelfCollisionChecker(); + + SelfCollisionChecker(SelfCollisionChecker&&) noexcept; + SelfCollisionChecker& operator=(SelfCollisionChecker&&) noexcept; + + SelfCollisionChecker(const SelfCollisionChecker&) = delete; + SelfCollisionChecker& operator=(const SelfCollisionChecker&) = delete; + + bool init(const std::string& urdf_path, + const std::vector& active_joint_names, + const SelfCollisionOptions& options, + std::string* error = nullptr); + + bool makeSnapshot(const std::vector& joint_positions, + CollisionGeometrySnapshot* snapshot, + std::string* error = nullptr); + + SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot); + SelfCollisionResult check(const std::vector& joint_positions); + + bool initialized() const; + std::size_t dof() const; + std::size_t activePairCount() const; + const std::vector& jointNames() const; + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace cmvr + +#endif // CMVR_ES_SELF_COLLISION_CHECKER_H diff --git a/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp b/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp new file mode 100644 index 00000000..e45df763 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/src/distance_sampling_policy.cpp @@ -0,0 +1,100 @@ +#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h" + +#include +#include +#include + +namespace cmvr { +namespace { + +void setError(std::string* error, const std::string& message) +{ + if (error) { + *error = message; + } +} + +double rotationAngle(const Eigen::Quaterniond& first, + const Eigen::Quaterniond& second) +{ + const double dot = std::clamp( + std::abs(first.normalized().dot(second.normalized())), 0.0, 1.0); + return 2.0 * std::acos(dot); +} + +} // namespace + +bool DistanceSamplingPolicy::configure(const DistanceSamplingOptions& options, + std::string* error) +{ + if (!std::isfinite(options.max_geometry_displacement_m) || + options.max_geometry_displacement_m <= 0.0) { + setError(error, "max_geometry_displacement_m must be finite and positive"); + return false; + } + if (!std::isfinite(options.max_check_period_s) || + options.max_check_period_s <= 0.0) { + setError(error, "max_check_period_s must be finite and positive"); + return false; + } + options_ = options; + configured_ = true; + reset(); + if (error) { + error->clear(); + } + return true; +} + +bool DistanceSamplingPolicy::shouldCheck(const CollisionGeometrySnapshot& current, + const Clock::time_point now) const +{ + if (!configured_ || !has_baseline_) { + return true; + } + const double elapsed_s = std::chrono::duration(now - last_check_time_).count(); + if (elapsed_s >= options_.max_check_period_s) { + return true; + } + return displacementSinceLastCheck(current) >= options_.max_geometry_displacement_m; +} + +void DistanceSamplingPolicy::markChecked(const CollisionGeometrySnapshot& current, + const Clock::time_point now) +{ + last_checked_ = current; + last_check_time_ = now; + has_baseline_ = true; +} + +void DistanceSamplingPolicy::reset() +{ + last_checked_.objects.clear(); + last_check_time_ = Clock::time_point{}; + has_baseline_ = false; +} + +double DistanceSamplingPolicy::displacementSinceLastCheck( + const CollisionGeometrySnapshot& current) const +{ + if (!has_baseline_ || current.objects.size() != last_checked_.objects.size()) { + return std::numeric_limits::infinity(); + } + + double maximum_displacement = 0.0; + for (std::size_t index = 0; index < current.objects.size(); ++index) { + const auto& previous = last_checked_.objects[index]; + const auto& now = current.objects[index]; + if (previous.geometry_index != now.geometry_index) { + return std::numeric_limits::infinity(); + } + const double translation = (now.position - previous.position).norm(); + const double radius = std::max(previous.bounding_radius_m, now.bounding_radius_m); + const double swept_distance = + translation + radius * rotationAngle(previous.orientation, now.orientation); + maximum_displacement = std::max(maximum_displacement, swept_distance); + } + return maximum_displacement; +} + +} // namespace cmvr diff --git a/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp b/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp new file mode 100644 index 00000000..8606bd22 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/src/self_collision_checker.cpp @@ -0,0 +1,403 @@ +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr { +namespace { + +using LinkPairKey = std::pair; + +LinkPairKey canonicalPair(std::string first, std::string second) +{ + if (second < first) { + std::swap(first, second); + } + return {std::move(first), std::move(second)}; +} + +void setError(std::string* error, const std::string& message) +{ + if (error) { + *error = message; + } +} + +} // namespace + +class SelfCollisionChecker::Impl { +public: + bool init(const std::string& urdf_path, + const std::vector& active_joint_names, + const SelfCollisionOptions& options, + std::string* error) + { + reset(); + if (urdf_path.empty()) { + setError(error, "URDF path is empty"); + return false; + } + if (!std::filesystem::is_regular_file(urdf_path)) { + setError(error, "URDF file does not exist: " + urdf_path); + return false; + } + if (active_joint_names.empty()) { + setError(error, "Active joint list is empty"); + return false; + } + + try { + pinocchio::urdf::buildModel(urdf_path, model_); + pinocchio::urdf::buildGeom( + model_, urdf_path, pinocchio::COLLISION, geometry_model_); + } catch (const std::exception& exception) { + setError(error, "Failed to load collision URDF: " + std::string(exception.what())); + reset(); + return false; + } + + if (geometry_model_.ngeoms == 0) { + setError(error, "URDF contains no collision geometry: " + urdf_path); + reset(); + return false; + } + + std::unordered_set active_joint_ids; + std::unordered_set unique_joint_names; + joint_names_.reserve(active_joint_names.size()); + joint_q_indices_.reserve(active_joint_names.size()); + for (const auto& joint_name : active_joint_names) { + if (joint_name.empty() || !unique_joint_names.insert(joint_name).second) { + setError(error, "Active joint names must be non-empty and unique"); + reset(); + return false; + } + if (!model_.existJointName(joint_name)) { + setError(error, "Joint not found in URDF: " + joint_name); + reset(); + return false; + } + const pinocchio::JointIndex joint_id = model_.getJointId(joint_name); + const auto& joint = model_.joints[joint_id]; + if (joint.nq() != 1) { + setError(error, "Only one-DoF active joints are supported: " + joint_name); + reset(); + return false; + } + active_joint_ids.insert(joint_id); + joint_names_.push_back(joint_name); + joint_q_indices_.push_back(joint.idx_q()); + } + + geometry_link_names_.resize(geometry_model_.ngeoms); + std::unordered_set selected_link_names; + for (pinocchio::GeomIndex geometry_id = 0; + geometry_id < geometry_model_.ngeoms; + ++geometry_id) { + auto& geometry = geometry_model_.geometryObjects[geometry_id]; + const std::string link_name = geometry.parentFrame < model_.frames.size() + ? model_.frames[geometry.parentFrame].name + : geometry.name; + geometry_link_names_[geometry_id] = link_name; + + const bool is_static = geometry.parentJoint == 0; + const bool belongs_to_active_arm = active_joint_ids.count(geometry.parentJoint) != 0; + if (!is_static && !belongs_to_active_arm) { + continue; + } + + if (!geometry.geometry) { + setError(error, "Collision geometry is null for link: " + link_name); + reset(); + return false; + } + geometry.geometry->computeLocalAABB(); + selected_geometry_indices_.push_back(geometry_id); + selected_link_names.insert(link_name); + } + + if (selected_geometry_indices_.size() < 2) { + setError(error, "Fewer than two collision geometries remain after arm filtering"); + reset(); + return false; + } + + std::set ignored_pairs; + for (const auto& pair : options.ignored_pairs) { + if (pair.first.empty() || pair.second.empty() || pair.first == pair.second) { + setError(error, "Ignored collision pairs require two different non-empty links"); + reset(); + return false; + } + if (!selected_link_names.count(pair.first) || !selected_link_names.count(pair.second)) { + setError(error, + "Ignored collision pair references an inactive or unknown link: " + + pair.first + ", " + pair.second); + reset(); + return false; + } + ignored_pairs.insert(canonicalPair(pair.first, pair.second)); + } + + geometry_model_.removeAllCollisionPairs(); + for (std::size_t first_index = 0; + first_index < selected_geometry_indices_.size(); + ++first_index) { + const auto first_geometry_id = selected_geometry_indices_[first_index]; + const auto& first_geometry = geometry_model_.geometryObjects[first_geometry_id]; + for (std::size_t second_index = first_index + 1; + second_index < selected_geometry_indices_.size(); + ++second_index) { + const auto second_geometry_id = selected_geometry_indices_[second_index]; + const auto& second_geometry = geometry_model_.geometryObjects[second_geometry_id]; + + if (first_geometry.parentJoint == second_geometry.parentJoint) { + continue; + } + if (model_.parents[first_geometry.parentJoint] == second_geometry.parentJoint || + model_.parents[second_geometry.parentJoint] == first_geometry.parentJoint) { + continue; + } + + const auto link_pair = canonicalPair( + geometry_link_names_[first_geometry_id], + geometry_link_names_[second_geometry_id]); + if (ignored_pairs.count(link_pair)) { + continue; + } + geometry_model_.addCollisionPair( + pinocchio::CollisionPair(first_geometry_id, second_geometry_id)); + } + } + + if (geometry_model_.collisionPairs.empty()) { + setError(error, "No active collision pairs remain after filtering"); + reset(); + return false; + } + + data_ = std::make_unique(model_); + geometry_data_ = std::make_unique(geometry_model_); + for (auto& request : geometry_data_->distanceRequests) { + request.enable_signed_distance = true; + } + neutral_q_ = pinocchio::neutral(model_); + initialized_ = true; + if (error) { + error->clear(); + } + return true; + } + + bool makeSnapshot(const std::vector& joint_positions, + CollisionGeometrySnapshot* snapshot, + std::string* error) + { + if (!initialized_) { + setError(error, "SelfCollisionChecker is not initialized"); + return false; + } + if (!snapshot) { + setError(error, "Collision snapshot output is null"); + return false; + } + if (joint_positions.size() != joint_names_.size()) { + std::ostringstream stream; + stream << "Joint position size mismatch: expected " << joint_names_.size() + << ", got " << joint_positions.size(); + setError(error, stream.str()); + return false; + } + + Eigen::VectorXd q = neutral_q_; + for (std::size_t index = 0; index < joint_positions.size(); ++index) { + if (!std::isfinite(joint_positions[index])) { + setError(error, "Joint position contains a non-finite value: " + joint_names_[index]); + return false; + } + q[joint_q_indices_[index]] = joint_positions[index]; + } + + try { + pinocchio::updateGeometryPlacements( + model_, *data_, geometry_model_, *geometry_data_, q); + } catch (const std::exception& exception) { + setError(error, "Failed to update collision geometry: " + std::string(exception.what())); + return false; + } + + snapshot->objects.clear(); + snapshot->objects.reserve(selected_geometry_indices_.size()); + for (const auto geometry_id : selected_geometry_indices_) { + const auto& placement = geometry_data_->oMg[geometry_id]; + const auto& geometry = geometry_model_.geometryObjects[geometry_id]; + CollisionObjectPose pose; + pose.geometry_index = geometry_id; + pose.position = placement.translation(); + pose.orientation = Eigen::Quaterniond(placement.rotation()).normalized(); + pose.bounding_radius_m = std::max(0.0, geometry.geometry->aabb_radius); + snapshot->objects.push_back(std::move(pose)); + } + if (error) { + error->clear(); + } + return true; + } + + SelfCollisionResult check(const CollisionGeometrySnapshot& snapshot) + { + SelfCollisionResult result; + if (!initialized_) { + result.error = "SelfCollisionChecker is not initialized"; + return result; + } + if (snapshot.objects.size() != selected_geometry_indices_.size()) { + result.error = "Collision snapshot size does not match initialized geometry"; + return result; + } + + for (std::size_t index = 0; index < snapshot.objects.size(); ++index) { + const auto& pose = snapshot.objects[index]; + if (pose.geometry_index != selected_geometry_indices_[index] || + pose.geometry_index >= geometry_data_->oMg.size()) { + result.error = "Collision snapshot geometry order is invalid"; + return result; + } + if (!pose.position.allFinite() || !pose.orientation.coeffs().allFinite() || + pose.orientation.norm() <= std::numeric_limits::epsilon()) { + result.error = "Collision snapshot contains an invalid pose"; + return result; + } + geometry_data_->oMg[pose.geometry_index] = pinocchio::SE3( + pose.orientation.normalized().toRotationMatrix(), pose.position); + } + + try { + const std::size_t pair_index = + pinocchio::computeDistances(geometry_model_, *geometry_data_); + if (pair_index >= geometry_model_.collisionPairs.size()) { + result.error = "Collision distance computation returned no active pair"; + return result; + } + const auto& pair = geometry_model_.collisionPairs[pair_index]; + result.minimum_distance_m = geometry_data_->distanceResults[pair_index].min_distance; + result.first = geometry_link_names_[pair.first]; + result.second = geometry_link_names_[pair.second]; + result.in_collision = result.minimum_distance_m <= 0.0; + result.valid = std::isfinite(result.minimum_distance_m); + if (!result.valid) { + result.error = "Collision distance is not finite"; + } + } catch (const std::exception& exception) { + result.error = "Collision distance computation failed: " + std::string(exception.what()); + } + return result; + } + + SelfCollisionResult check(const std::vector& joint_positions) + { + CollisionGeometrySnapshot snapshot; + std::string error; + if (!makeSnapshot(joint_positions, &snapshot, &error)) { + SelfCollisionResult result; + result.error = std::move(error); + return result; + } + return check(snapshot); + } + + void reset() + { + initialized_ = false; + joint_names_.clear(); + joint_q_indices_.clear(); + selected_geometry_indices_.clear(); + geometry_link_names_.clear(); + geometry_data_.reset(); + data_.reset(); + model_ = pinocchio::Model{}; + geometry_model_ = pinocchio::GeometryModel{}; + neutral_q_.resize(0); + } + + bool initialized_{false}; + std::vector joint_names_; + std::vector joint_q_indices_; + std::vector selected_geometry_indices_; + std::vector geometry_link_names_; + pinocchio::Model model_; + pinocchio::GeometryModel geometry_model_; + std::unique_ptr data_; + std::unique_ptr geometry_data_; + Eigen::VectorXd neutral_q_; +}; + +SelfCollisionChecker::SelfCollisionChecker() + : impl_(std::make_unique()) +{ +} + +SelfCollisionChecker::~SelfCollisionChecker() = default; +SelfCollisionChecker::SelfCollisionChecker(SelfCollisionChecker&&) noexcept = default; +SelfCollisionChecker& SelfCollisionChecker::operator=(SelfCollisionChecker&&) noexcept = default; + +bool SelfCollisionChecker::init(const std::string& urdf_path, + const std::vector& active_joint_names, + const SelfCollisionOptions& options, + std::string* error) +{ + return impl_->init(urdf_path, active_joint_names, options, error); +} + +bool SelfCollisionChecker::makeSnapshot(const std::vector& joint_positions, + CollisionGeometrySnapshot* snapshot, + std::string* error) +{ + return impl_->makeSnapshot(joint_positions, snapshot, error); +} + +SelfCollisionResult SelfCollisionChecker::check(const CollisionGeometrySnapshot& snapshot) +{ + return impl_->check(snapshot); +} + +SelfCollisionResult SelfCollisionChecker::check(const std::vector& joint_positions) +{ + return impl_->check(joint_positions); +} + +bool SelfCollisionChecker::initialized() const +{ + return impl_->initialized_; +} + +std::size_t SelfCollisionChecker::dof() const +{ + return impl_->joint_names_.size(); +} + +std::size_t SelfCollisionChecker::activePairCount() const +{ + return impl_->geometry_model_.collisionPairs.size(); +} + +const std::vector& SelfCollisionChecker::jointNames() const +{ + return impl_->joint_names_; +} + +} // namespace cmvr diff --git a/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp b/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp new file mode 100644 index 00000000..cfe18169 --- /dev/null +++ b/cmvr-es/algorithms/collision_detection/self_collision/test/self_collision_checker_test.cpp @@ -0,0 +1,110 @@ +#include +#include +#include + +#include + +#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h" +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" + +namespace cmvr { +namespace { + +const std::vector kRightArmJoints{ + "R_SHOULDER_P", + "R_SHOULDER_R", + "R_SHOULDER_Y", + "R_ELBOW_R", + "R_WRIST_P", + "R_WRIST_Y", + "R_WRIST_R", +}; + +std::string collisionUrdfPath() +{ + return std::string(CMVR_ES_SOURCE_DIR) + + "/model/xiaoyan_description/dual_arm_collision.urdf"; +} + +CollisionGeometrySnapshot singleObjectSnapshot(double x, + double angle, + double radius) +{ + CollisionGeometrySnapshot snapshot; + CollisionObjectPose pose; + pose.geometry_index = 1; + pose.position = Eigen::Vector3d(x, 0.0, 0.0); + pose.orientation = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()); + pose.bounding_radius_m = radius; + snapshot.objects.push_back(pose); + return snapshot; +} + +TEST(SelfCollisionCheckerTest, LoadsRightArmFromDualArmUrdf) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error; + EXPECT_EQ(checker.dof(), 7U); + EXPECT_GT(checker.activePairCount(), 0U); + + CollisionGeometrySnapshot snapshot; + ASSERT_TRUE(checker.makeSnapshot(std::vector(7, 0.0), &snapshot, &error)) << error; + EXPECT_EQ(snapshot.objects.size(), 11U); + + const SelfCollisionResult result = checker.check(snapshot); + ASSERT_TRUE(result.valid) << result.error; + EXPECT_TRUE(result.first.rfind("L_", 0) != 0); + EXPECT_TRUE(result.second.rfind("L_", 0) != 0); +} + +TEST(SelfCollisionCheckerTest, RejectsWrongJointVectorSize) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error; + + CollisionGeometrySnapshot snapshot; + EXPECT_FALSE(checker.makeSnapshot(std::vector(6, 0.0), &snapshot, &error)); + EXPECT_NE(error.find("size mismatch"), std::string::npos); +} + +TEST(SelfCollisionCheckerTest, RemovesConfiguredIgnoredPair) +{ + SelfCollisionChecker baseline; + SelfCollisionChecker filtered; + std::string error; + ASSERT_TRUE(baseline.init(collisionUrdfPath(), kRightArmJoints, {}, &error)) << error; + + SelfCollisionOptions options; + options.ignored_pairs.push_back({"base_link", "R_ELBOW_R_S"}); + ASSERT_TRUE(filtered.init(collisionUrdfPath(), kRightArmJoints, options, &error)) << error; + EXPECT_EQ(filtered.activePairCount() + 1, baseline.activePairCount()); +} + +TEST(DistanceSamplingPolicyTest, SamplesByAccumulatedGeometryDisplacement) +{ + DistanceSamplingPolicy policy; + DistanceSamplingOptions options; + options.max_geometry_displacement_m = 0.002; + options.max_check_period_s = 0.01; + std::string error; + ASSERT_TRUE(policy.configure(options, &error)) << error; + + const auto start = DistanceSamplingPolicy::Clock::now(); + const auto initial = singleObjectSnapshot(0.0, 0.0, 0.2); + EXPECT_TRUE(policy.shouldCheck(initial, start)); + policy.markChecked(initial, start); + + EXPECT_FALSE(policy.shouldCheck( + singleObjectSnapshot(0.001, 0.0, 0.2), start + std::chrono::milliseconds(1))); + EXPECT_TRUE(policy.shouldCheck( + singleObjectSnapshot(0.0021, 0.0, 0.2), start + std::chrono::milliseconds(2))); + EXPECT_TRUE(policy.shouldCheck( + singleObjectSnapshot(0.0, 0.011, 0.2), start + std::chrono::milliseconds(2))); + EXPECT_TRUE(policy.shouldCheck( + initial, start + std::chrono::milliseconds(10))); +} + +} // namespace +} // namespace cmvr diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index db13dfa9..64db2085 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -14,4 +14,12 @@ task_manager { config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt" enable: true } + tasks { + id: "right_arm_self_collision" + type: TASK_TYPE_SELF_COLLISION + run_mode: TASK_RUN_MODE_PERIODIC_STEP + control_period_s: 0.002 + config_file: "tasks/self_collision_task/self_collision_task.pb.txt" + enable: true + } } diff --git a/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt b/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt new file mode 100644 index 00000000..d6c5623f --- /dev/null +++ b/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt @@ -0,0 +1,24 @@ +self_collision_task { + id: "right_arm_self_collision" + arm_id: "mujoco_right_arm" + + checker { + urdf_path: "model/xiaoyan_description/dual_arm_collision.urdf" + + # Simplified compact-wrist bodies overlap in the normal assembled pose. + ignored_pairs { + first: "R_WRIST_P_S" + second: "R_WRIST_R_S" + } + } + + sampling { + max_geometry_displacement_m: 0.002 + max_check_period_s: 0.01 + } + + safety { + warning_distance_m: 0.02 + stop_distance_m: 0.005 + } +} diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index 05410231..ac8567ab 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -36,6 +36,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type) return "TASK_TYPE_TOUCH_SCREEN"; case config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER: return "TASK_TYPE_GRPC_SERVER"; + case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION: + return "TASK_TYPE_SELF_COLLISION"; case config::TaskConfigEntry::TASK_TYPE_UNKNOWN: default: return "TASK_TYPE_UNKNOWN"; diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index c8a2960f..e16bbb5f 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/cmvr-es/task/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(task touch_screen_task/src/touch_screen_task.cpp + self_collision_task/src/self_collision_task.cpp ) target_include_directories(task PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -10,6 +11,7 @@ target_link_libraries(task cmvr_es::common cmvr_es::ik_solver cmvr_es::base_motion + cmvr_es::self_collision_checker PRIVATE cmvr_es::device_manager ) diff --git a/cmvr-es/task/self_collision_task/include/self_collision_task.h b/cmvr-es/task/self_collision_task/include/self_collision_task.h new file mode 100644 index 00000000..ae45d795 --- /dev/null +++ b/cmvr-es/task/self_collision_task/include/self_collision_task.h @@ -0,0 +1,69 @@ +#ifndef CMVR_ES_SELF_COLLISION_TASK_H +#define CMVR_ES_SELF_COLLISION_TASK_H + +#include +#include +#include + +#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h" +#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" +#include "cmvr/config/self_collision_task_config/self_collision_task_config.pb.h" +#include "devices/arm/robot_arm.h" +#include "task/task.h" + +namespace cmvr::task { + +enum class CollisionSafetyLevel { + UNKNOWN = 0, + SAFE, + WARNING, + STOP, +}; + +struct SelfCollisionTaskStatus { + CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN}; + SelfCollisionResult result; + bool stop_latched{false}; +}; + +class SelfCollisionTask final : public Task { +public: + explicit SelfCollisionTask(const config::SelfCollisionTaskConfig& config); + + const std::string& id() const override { return id_; } + TaskRunMode runMode() const override { return TaskRunMode::PERIODIC_STEP; } + + bool init() override; + bool start() override; + bool step(double dt) override; + void stop() override; + + TaskState state() const override; + bool isBusy() const override; + bool isFinished() const override; + bool isFailed() const override; + std::string stateString() const override; + std::string detailStatusString() const override; + + SelfCollisionTaskStatus latestStatus() const; + +private: + static bool validateConfig(const config::SelfCollisionTaskConfig& config, + std::string* error); + static const char* safetyLevelToString(CollisionSafetyLevel level); + + config::SelfCollisionTaskConfig config_; + std::string id_; + std::shared_ptr arm_; + SelfCollisionChecker checker_; + DistanceSamplingPolicy sampling_; + + mutable std::mutex mutex_; + TaskState state_{TaskState::UNINITIALIZED}; + SelfCollisionTaskStatus latest_status_{}; + std::string last_error_; +}; + +} // namespace cmvr::task + +#endif // CMVR_ES_SELF_COLLISION_TASK_H diff --git a/cmvr-es/task/self_collision_task/src/self_collision_task.cpp b/cmvr-es/task/self_collision_task/src/self_collision_task.cpp new file mode 100644 index 00000000..e3a94ba0 --- /dev/null +++ b/cmvr-es/task/self_collision_task/src/self_collision_task.cpp @@ -0,0 +1,304 @@ +#include "task/self_collision_task/include/self_collision_task.h" + +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::task { + +SelfCollisionTask::SelfCollisionTask(const config::SelfCollisionTaskConfig& config) + : config_(config), + id_(config.id()) +{ +} + +bool SelfCollisionTask::validateConfig(const config::SelfCollisionTaskConfig& config, + std::string* error) +{ + auto fail = [error](const std::string& message) { + if (error) { + *error = message; + } + return false; + }; + + if (config.id().empty()) { + return fail("Self-collision task id is empty"); + } + if (config.arm_id().empty()) { + return fail("Self-collision task arm_id is empty"); + } + if (config.checker().urdf_path().empty()) { + return fail("Self-collision checker URDF path is empty"); + } + const auto& sampling = config.sampling(); + if (!std::isfinite(sampling.max_geometry_displacement_m()) || + sampling.max_geometry_displacement_m() <= 0.0) { + return fail("max_geometry_displacement_m must be finite and positive"); + } + if (!std::isfinite(sampling.max_check_period_s()) || + sampling.max_check_period_s() <= 0.0) { + return fail("max_check_period_s must be finite and positive"); + } + + const auto& safety = config.safety(); + if (!std::isfinite(safety.stop_distance_m()) || safety.stop_distance_m() < 0.0) { + return fail("stop_distance_m must be finite and non-negative"); + } + if (!std::isfinite(safety.warning_distance_m()) || + safety.warning_distance_m() < safety.stop_distance_m()) { + return fail("warning_distance_m must be finite and not less than stop_distance_m"); + } + for (const auto& pair : config.checker().ignored_pairs()) { + if (pair.first().empty() || pair.second().empty() || pair.first() == pair.second()) { + return fail("ignored_pairs entries require two different non-empty links"); + } + } + if (error) { + error->clear(); + } + return true; +} + +bool SelfCollisionTask::init() +{ + std::string error; + if (!validateConfig(config_, &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + auto arm = device::DeviceManager::getInstance().getDevice( + config_.arm_id()); + if (!arm) { + std::lock_guard lock(mutex_); + last_error_ = "Robot arm not found: " + config_.arm_id(); + state_ = TaskState::FAILED; + return false; + } + + const auto model = arm->getRobotModel(); + if (!model.valid()) { + std::lock_guard lock(mutex_); + last_error_ = "Robot arm model is invalid: " + config_.arm_id(); + state_ = TaskState::FAILED; + return false; + } + + SelfCollisionOptions checker_options; + checker_options.ignored_pairs.reserve(config_.checker().ignored_pairs_size()); + for (const auto& pair : config_.checker().ignored_pairs()) { + checker_options.ignored_pairs.push_back({pair.first(), pair.second()}); + } + if (!checker_.init(config_.checker().urdf_path(), + model.joint_names, + checker_options, + &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + const DistanceSamplingOptions sampling_options{ + config_.sampling().max_geometry_displacement_m(), + config_.sampling().max_check_period_s(), + }; + if (!sampling_.configure(sampling_options, &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + { + std::lock_guard lock(mutex_); + arm_ = std::move(arm); + latest_status_ = {}; + last_error_.clear(); + state_ = TaskState::IDLE; + } + CMVR_LOG(INFO) << "[SelfCollisionTask] Initialized id=" << id_ + << ", arm=" << config_.arm_id() + << ", dof=" << checker_.dof() + << ", active_pairs=" << checker_.activePairCount(); + return true; +} + +bool SelfCollisionTask::start() +{ + std::lock_guard lock(mutex_); + if (state_ == TaskState::RUNNING) { + return true; + } + if (state_ != TaskState::IDLE && state_ != TaskState::STOPPED) { + last_error_ = "Self-collision task is not initialized"; + state_ = TaskState::FAILED; + return false; + } + sampling_.reset(); + latest_status_ = {}; + last_error_.clear(); + state_ = TaskState::RUNNING; + return true; +} + +bool SelfCollisionTask::step(const double dt) +{ + (void)dt; + std::shared_ptr arm; + { + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING) { + return false; + } + arm = arm_; + } + + const auto joint_state = arm->getJointState(); + CollisionGeometrySnapshot snapshot; + std::string error; + if (!checker_.makeSnapshot(joint_state.position, &snapshot, &error)) { + std::lock_guard lock(mutex_); + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + const auto now = DistanceSamplingPolicy::Clock::now(); + if (!sampling_.shouldCheck(snapshot, now)) { + return true; + } + + SelfCollisionResult result = checker_.check(snapshot); + if (!result.valid) { + std::lock_guard lock(mutex_); + last_error_ = result.error.empty() ? "Self-collision distance check failed" : result.error; + state_ = TaskState::FAILED; + return false; + } + sampling_.markChecked(snapshot, now); + + CollisionSafetyLevel level = CollisionSafetyLevel::SAFE; + if (result.minimum_distance_m <= config_.safety().stop_distance_m()) { + level = CollisionSafetyLevel::STOP; + } else if (result.minimum_distance_m <= config_.safety().warning_distance_m()) { + level = CollisionSafetyLevel::WARNING; + } + + bool trigger_stop = false; + CollisionSafetyLevel previous_level = CollisionSafetyLevel::UNKNOWN; + { + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING) { + return true; + } + previous_level = latest_status_.level; + latest_status_.level = level; + latest_status_.result = result; + if (level == CollisionSafetyLevel::STOP && !latest_status_.stop_latched) { + latest_status_.stop_latched = true; + trigger_stop = true; + } + } + + if (level != previous_level) { + if (level == CollisionSafetyLevel::SAFE) { + CMVR_LOG(INFO) << "[SelfCollisionTask] level=" << safetyLevelToString(level) + << ", distance_m=" << result.minimum_distance_m + << ", pair=" << result.first << "/" << result.second; + } else { + CMVR_LOG(WARNING) << "[SelfCollisionTask] level=" << safetyLevelToString(level) + << ", distance_m=" << result.minimum_distance_m + << ", pair=" << result.first << "/" << result.second; + } + } + + if (trigger_stop) { + const auto stop_result = arm->protectiveStop(); + if (!stop_result.ok()) { + std::lock_guard lock(mutex_); + last_error_ = "Protective stop failed: " + stop_result.message; + state_ = TaskState::FAILED; + return false; + } + } + return true; +} + +void SelfCollisionTask::stop() +{ + std::lock_guard lock(mutex_); + if (state_ != TaskState::FAILED) { + state_ = TaskState::STOPPED; + } +} + +TaskState SelfCollisionTask::state() const +{ + std::lock_guard lock(mutex_); + return state_; +} + +bool SelfCollisionTask::isBusy() const +{ + return state() == TaskState::RUNNING; +} + +bool SelfCollisionTask::isFinished() const +{ + return state() == TaskState::STOPPED; +} + +bool SelfCollisionTask::isFailed() const +{ + return state() == TaskState::FAILED; +} + +std::string SelfCollisionTask::stateString() const +{ + return taskStateToString(state()); +} + +std::string SelfCollisionTask::detailStatusString() const +{ + std::lock_guard lock(mutex_); + if (!last_error_.empty()) { + return std::string(taskStateToString(state_)) + " " + last_error_; + } + std::ostringstream stream; + stream << taskStateToString(state_) + << " level=" << safetyLevelToString(latest_status_.level); + if (latest_status_.result.valid) { + stream << " distance_m=" << std::setprecision(6) + << latest_status_.result.minimum_distance_m + << " pair=" << latest_status_.result.first + << "/" << latest_status_.result.second; + } + return stream.str(); +} + +SelfCollisionTaskStatus SelfCollisionTask::latestStatus() const +{ + std::lock_guard lock(mutex_); + return latest_status_; +} + +const char* SelfCollisionTask::safetyLevelToString(const CollisionSafetyLevel level) +{ + switch (level) { + case CollisionSafetyLevel::UNKNOWN: return "UNKNOWN"; + case CollisionSafetyLevel::SAFE: return "SAFE"; + case CollisionSafetyLevel::WARNING: return "WARNING"; + case CollisionSafetyLevel::STOP: return "STOP"; + } + return "UNKNOWN"; +} + +} // namespace cmvr::task diff --git a/cmvr-es/task/task_factory.h b/cmvr-es/task/task_factory.h index e3efe733..dfb1ce62 100644 --- a/cmvr-es/task/task_factory.h +++ b/cmvr-es/task/task_factory.h @@ -14,6 +14,8 @@ #include "common/config/config_files.h" #include "task/task.h" #include "task/touch_screen_task/include/touch_screen_task.h" +#include "task/self_collision_task/include/self_collision_task.h" +#include "cmvr/config/self_collision_task_config/self_collision_task_config.pb.h" namespace cmvr::task { @@ -52,6 +54,28 @@ inline std::shared_ptr createTouchScreenTask(const config::TaskConfigEntry return std::make_shared(cfg); } +inline std::shared_ptr createSelfCollisionTask(const config::TaskConfigEntry& entry) +{ + if (entry.id().empty() || entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[TaskFactory] Invalid SelfCollision task entry: " << entry.id(); + return nullptr; + } + + config::SelfCollisionTaskRootConfig root_cfg; + if (!ConfigHelper::loadConfigFile(entry.config_file(), root_cfg)) { + CMVR_LOG(ERROR) << "[TaskFactory] Failed to load SelfCollision config: " + << entry.config_file(); + return nullptr; + } + const auto& cfg = root_cfg.self_collision_task(); + if (cfg.id().empty() || cfg.id() != entry.id()) { + CMVR_LOG(ERROR) << "[TaskFactory] SelfCollision task ID mismatch: manager id=" + << entry.id() << ", config id=" << cfg.id(); + return nullptr; + } + return std::make_shared(cfg); +} + } // namespace task_factory_detail class TaskFactory { @@ -70,6 +94,8 @@ public: switch (entry.type()) { case config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN: return task_factory_detail::createTouchScreenTask(entry); + case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION: + return task_factory_detail::createSelfCollisionTask(entry); default: break; } diff --git a/model/xiaoyan_description/dual_arm.urdf b/model/xiaoyan_description/dual_arm.urdf index f1cc20d0..7e7ab687 100644 --- a/model/xiaoyan_description/dual_arm.urdf +++ b/model/xiaoyan_description/dual_arm.urdf @@ -8,44 +8,30 @@ - - - - - - - - - - + + + + + + + + + + - - - - - - + + + + + + + - - - - - - - - - - - - - - - - - - - - + + + + + @@ -68,12 +54,6 @@ - - - - - - @@ -97,12 +77,6 @@ - - - - - - @@ -134,12 +108,6 @@ - - - - - - @@ -172,12 +140,6 @@ - - - - - - @@ -209,12 +171,6 @@ - - - - - - @@ -246,12 +202,6 @@ - - - - - - @@ -283,12 +233,6 @@ - - - - - - @@ -320,12 +264,6 @@ - - - - - - @@ -357,12 +295,6 @@ - - - - - - @@ -394,12 +326,6 @@ - - - - - - @@ -432,12 +358,6 @@ - - - - - - @@ -469,12 +389,6 @@ - - - - - - @@ -506,12 +420,6 @@ - - - - - - @@ -543,12 +451,6 @@ - - - - - - @@ -581,12 +483,6 @@ - - - - - - diff --git a/model/xiaoyan_description/dual_arm/dual_arm.usda b/model/xiaoyan_description/dual_arm/dual_arm.usda new file mode 100644 index 00000000..8da4d998 --- /dev/null +++ b/model/xiaoyan_description/dual_arm/dual_arm.usda @@ -0,0 +1,52 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend references = @./payloads/base.usda@ + variants = { + string Physics = "physx" + } + append variantSets = "Physics" +) +{ + variantSet "Physics" = { + "mujoco" ( + prepend payload = @./payloads/Physics/mujoco.usda@ + ) { + + } + "none" { + + } + "physics" ( + prepend payload = @./payloads/Physics/physics.usda@ + ) { + + } + "physx" ( + prepend payload = @./payloads/Physics/physx.usda@ + ) { + + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/Physics/mujoco.usda b/model/xiaoyan_description/dual_arm/payloads/Physics/mujoco.usda new file mode 100644 index 00000000..dc5e41fe --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/Physics/mujoco.usda @@ -0,0 +1,444 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + def MjcActuator "L_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + over "root_joint" + { + } + + over "L_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_FINGER_TIP_FIXED" + { + } + + over "R_CAM_FIXED" + { + } + } + + over "Geometry" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" + { + over "L_WRIST_R_S" + { + } + + over "L_WRIST_R_S_1" + { + } + } + + over "L_WRIST_Y_S" + { + } + + over "L_WRIST_Y_S_1" + { + } + } + + over "L_WRIST_P_S" + { + } + + over "L_WRIST_P_S_1" + { + } + } + + over "L_ELBOW_R_S" + { + } + + over "L_ELBOW_R_S_1" + { + } + } + + over "L_SHOULDER_Y_S" + { + } + + over "L_SHOULDER_Y_S_1" + { + } + } + + over "L_SHOULDER_R_S" + { + } + + over "L_SHOULDER_R_S_1" + { + } + } + + over "L_SHOULDER_P_S" + { + } + + over "L_SHOULDER_P_S_1" + { + } + } + + over "R_SHOULDER_P_S" + { + over "R_SHOULDER_R_S" + { + over "R_SHOULDER_Y_S" + { + over "R_ELBOW_R_S" + { + over "R_WRIST_P_S" + { + over "R_WRIST_Y_S" + { + over "R_WRIST_R_S" + { + over "R_FINGER_TIP" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_CAM" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_WRIST_R_S" + { + } + + over "R_WRIST_R_S_1" + { + } + } + + over "R_WRIST_Y_S" + { + } + + over "R_WRIST_Y_S_1" + { + } + } + + over "R_WRIST_P_S" + { + } + + over "R_WRIST_P_S_1" + { + } + } + + over "R_ELBOW_R_S" + { + } + + over "R_ELBOW_R_S_1" + { + } + } + + over "R_SHOULDER_Y_S" + { + } + + over "R_SHOULDER_Y_S_1" + { + } + } + + over "R_SHOULDER_R_S" + { + } + + over "R_SHOULDER_R_S_1" + { + } + } + + over "R_SHOULDER_P_S" + { + } + + over "R_SHOULDER_P_S_1" + { + } + } + + over "PELVIS_S" + { + } + + over "PELVIS_S_1" + { + } + } + } + + over "Materials" + { + over "material_16" + { + } + + over "material_17" + { + } + } + + over "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/Physics/physics.usda b/model/xiaoyan_description/dual_arm/payloads/Physics/physics.usda new file mode 100644 index 00000000..ece6b107 --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/Physics/physics.usda @@ -0,0 +1,530 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "PELVIS_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI", "PhysicsMassAPI"] + ) + { + bool newton:selfCollisionEnabled = 0 + point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627) + float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747) + float physics:mass = 2.106246 + quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252) + + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823) + + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278) + + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022) + + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766) + + over "L_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155) + + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "L_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925) + float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117) + float physics:mass = 0.50489414 + quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542) + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165) + + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957) + + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771) + + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363) + + over "R_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155) + + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "R_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534) + float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028) + float physics:mass = 0.50436604 + quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874) + + over "R_FINGER_TIP" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + + over "R_CAM" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + def PhysicsRevoluteJoint "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, 0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -114.59156 + float physics:upperLimit = 114.59156 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, 0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -124.9048 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "L_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, 0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -117.456345 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, 0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "L_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.0258, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 14.896903 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, -0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, -0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, -0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 117.456345 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, -0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "R_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.03, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -32.658596 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsFixedJoint "root_joint" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_FINGER_TIP_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_CAM_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677) + quatf physics:localRot1 = (1, 0, 0, 0) + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/Physics/physx.usda b/model/xiaoyan_description/dual_arm/payloads/Physics/physx.usda new file mode 100644 index 00000000..f65b957d --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/Physics/physx.usda @@ -0,0 +1,129 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + over "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/base.usda b/model/xiaoyan_description/dual_arm/payloads/base.usda new file mode 100644 index 00000000..2d9719a2 --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/base.usda @@ -0,0 +1,594 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./robot.usda@ + ] + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend apiSchemas = ["GeomModelAPI"] + assetInfo = { + string name = "dual_arm" + } + kind = "component" +) +{ + float3[] extentsHint = [(-0.043500002, -0.958077, -0.011133737), (0.080300845, 0.9075668, 0.12106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.043500002, -0.959077, -0.011133737), (0.080300845, 0.9075668, 0.12206)] + + def Scope "Materials" + { + def Material "material_16" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_17" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + + def Scope "Geometry" + { + def Xform "PELVIS_S" + { + def Xform "PELVIS_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, 0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, 0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, 0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, 0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.0258, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S_1" ( + displayName = "L_WRIST_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_WRIST_Y_S_1" ( + displayName = "L_WRIST_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_WRIST_P_S_1" ( + displayName = "L_WRIST_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_ELBOW_R_S_1" ( + displayName = "L_ELBOW_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_SHOULDER_Y_S_1" ( + displayName = "L_SHOULDER_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_SHOULDER_R_S_1" ( + displayName = "L_SHOULDER_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "L_SHOULDER_P_S_1" ( + displayName = "L_SHOULDER_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, -0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, -0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, -0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, -0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.03, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_FINGER_TIP" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_CAM" + { + quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.01212, -0.17655, 0.07506) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_WRIST_R_S_1" ( + displayName = "R_WRIST_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_WRIST_Y_S_1" ( + displayName = "R_WRIST_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_WRIST_P_S_1" ( + displayName = "R_WRIST_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_ELBOW_R_S_1" ( + displayName = "R_ELBOW_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_Y_S_1" ( + displayName = "R_SHOULDER_Y_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_R_S_1" ( + displayName = "R_SHOULDER_R_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_SHOULDER_P_S_1" ( + displayName = "R_SHOULDER_P_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "PELVIS_S_1" ( + displayName = "PELVIS_S" + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + + def Scope "Physics" + { + } + + def Scope "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/geometries.usd b/model/xiaoyan_description/dual_arm/payloads/geometries.usd new file mode 100644 index 00000000..8aeaa987 Binary files /dev/null and b/model/xiaoyan_description/dual_arm/payloads/geometries.usd differ diff --git a/model/xiaoyan_description/dual_arm/payloads/instances.usda b/model/xiaoyan_description/dual_arm/payloads/instances.usda new file mode 100644 index 00000000..54238edb --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/instances.usda @@ -0,0 +1,564 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Instances" +{ + def Xform "PELVIS_S" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_1" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "PELVIS_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_2" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_3" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_4" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_5" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_ELBOW_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_6" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_7" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "L_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_8" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_9" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_10" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_11" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_12" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_ELBOW_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_13" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_P_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_14" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_Y_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } + + def Xform "R_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_15" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_R_S_1" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["PhysicsCollisionAPI", "NewtonCollisionAPI", "PhysicsMeshCollisionAPI", "NewtonMeshCollisionAPI"] + ) + { + token physics:approximation = "convexHull" + token purpose = "guide" + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/materials.usda b/model/xiaoyan_description/dual_arm/payloads/materials.usda new file mode 100644 index 00000000..5801264d --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/materials.usda @@ -0,0 +1,222 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Materials" +{ + def Material "material_16" + { + color3f inputs:diffuseColor = (0, 1, 1) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_17" + { + color3f inputs:diffuseColor = (0, 1, 0) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_1" + { + color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_2" + { + color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_3" + { + color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_7" + { + color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } +} + diff --git a/model/xiaoyan_description/dual_arm/payloads/robot.usda b/model/xiaoyan_description/dual_arm/payloads/robot.usda new file mode 100644 index 00000000..5ebf95bb --- /dev/null +++ b/model/xiaoyan_description/dual_arm/payloads/robot.usda @@ -0,0 +1,260 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpg9rhrzku/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_5_54i39n/temp_dual_arm/dual_arm.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" ( + prepend apiSchemas = ["IsaacRobotAPI"] +) +{ + prepend rel isaac:physics:robotJoints = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + prepend rel isaac:physics:robotLinks = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + token isaac:robotType = "Default" + + over "Geometry" + { + over "PELVIS_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_FINGER_TIP" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + + over "R_CAM" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + over "root_joint" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_FINGER_TIP_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_CAM_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/dual_arm.usda b/model/xiaoyan_description/dual_arm_1/dual_arm.usda new file mode 100644 index 00000000..6b9fc6f6 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/dual_arm.usda @@ -0,0 +1,52 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend references = @./payloads/base.usda@ + variants = { + string Physics = "physx" + } + append variantSets = "Physics" +) +{ + variantSet "Physics" = { + "mujoco" ( + prepend payload = @./payloads/Physics/mujoco.usda@ + ) { + + } + "none" { + + } + "physics" ( + prepend payload = @./payloads/Physics/physics.usda@ + ) { + + } + "physx" ( + prepend payload = @./payloads/Physics/physx.usda@ + ) { + + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/Physics/mujoco.usda b/model/xiaoyan_description/dual_arm_1/payloads/Physics/mujoco.usda new file mode 100644 index 00000000..a6382414 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/Physics/mujoco.usda @@ -0,0 +1,400 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + def MjcActuator "L_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + over "root_joint" + { + } + + over "base_fixed" + { + } + + over "L_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_FINGER_TIP_FIXED" + { + } + + over "R_CAM_FIXED" + { + } + } + + over "Geometry" + { + over "base_link" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" + { + } + + over "L_WRIST_Y_S" + { + } + } + + over "L_WRIST_P_S" + { + } + } + + over "L_ELBOW_R_S" + { + } + } + + over "L_SHOULDER_Y_S" + { + } + } + + over "L_SHOULDER_R_S" + { + } + } + + over "L_SHOULDER_P_S" + { + } + } + + over "R_SHOULDER_P_S" + { + over "R_SHOULDER_R_S" + { + over "R_SHOULDER_Y_S" + { + over "R_ELBOW_R_S" + { + over "R_WRIST_P_S" + { + over "R_WRIST_Y_S" + { + over "R_WRIST_R_S" + { + over "R_FINGER_TIP" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_CAM" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_WRIST_R_S" + { + } + } + + over "R_WRIST_Y_S" + { + } + } + + over "R_WRIST_P_S" + { + } + } + + over "R_ELBOW_R_S" + { + } + } + + over "R_SHOULDER_Y_S" + { + } + } + + over "R_SHOULDER_R_S" + { + } + } + + over "R_SHOULDER_P_S" + { + } + } + + over "PELVIS_S" + { + } + } + + over "cylinder" + { + } + + over "base_column" + { + } + } + } + + over "Materials" + { + over "gray" + { + } + + over "material_16" + { + } + + over "material_17" + { + } + } + + over "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/Physics/physics.usda b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physics.usda new file mode 100644 index 00000000..ccac6fc0 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physics.usda @@ -0,0 +1,554 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI"] + ) + { + bool newton:selfCollisionEnabled = 0 + + over "PELVIS_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627) + float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747) + float physics:mass = 2.106246 + quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252) + + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823) + + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278) + + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022) + + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766) + + over "L_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155) + + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "L_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925) + float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117) + float physics:mass = 0.50489414 + quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542) + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165) + + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957) + + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771) + + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363) + + over "R_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155) + + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "R_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534) + float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028) + float physics:mass = 0.50436604 + quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874) + + over "R_FINGER_TIP" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + + over "R_CAM" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + + over "base_column" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + + over "Physics" + { + def PhysicsRevoluteJoint "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, 0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -114.59156 + float physics:upperLimit = 114.59156 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, 0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -124.9048 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "L_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, 0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -117.456345 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, 0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "L_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.0258, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 14.896903 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, -0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, -0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, -0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 117.456345 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, -0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "R_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.03, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -32.658596 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsFixedJoint "root_joint" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "base_fixed" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 1.2) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_FINGER_TIP_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_CAM_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677) + quatf physics:localRot1 = (1, 0, 0, 0) + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/Physics/physx.usda b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physx.usda new file mode 100644 index 00000000..638500a6 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/Physics/physx.usda @@ -0,0 +1,129 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + over "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/base.usda b/model/xiaoyan_description/dual_arm_1/payloads/base.usda new file mode 100644 index 00000000..dee6c966 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/base.usda @@ -0,0 +1,451 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./robot.usda@ + ] + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend apiSchemas = ["GeomModelAPI"] + assetInfo = { + string name = "dual_arm" + } + kind = "component" +) +{ + float3[] extentsHint = [(-0.05, -0.958077, -2.3841858e-8), (0.080300845, 0.9075668, 1.32106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.05, -0.959077, -2.3841858e-8), (0.05, 0.05, 1.32206)] + + def Scope "Materials" + { + def Material "gray" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_16" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_17" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + + def Scope "Geometry" + { + def Xform "base_link" + { + def Cylinder "cylinder" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + rel material:binding = + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "PELVIS_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 1.2) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "PELVIS_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, 0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, 0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, 0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, 0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.0258, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + + def Xform "R_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, -0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, -0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, -0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, -0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.03, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_FINGER_TIP" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_CAM" + { + quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.01212, -0.17655, 0.07506) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + } + } + } + + def Cylinder "base_column" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + uniform token purpose = "guide" + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + + def Scope "Physics" + { + } + + def Scope "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/geometries.usd b/model/xiaoyan_description/dual_arm_1/payloads/geometries.usd new file mode 100644 index 00000000..399b49af Binary files /dev/null and b/model/xiaoyan_description/dual_arm_1/payloads/geometries.usd differ diff --git a/model/xiaoyan_description/dual_arm_1/payloads/instances.usda b/model/xiaoyan_description/dual_arm_1/payloads/instances.usda new file mode 100644 index 00000000..c2e7a10a --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/instances.usda @@ -0,0 +1,369 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Instances" +{ + def Xform "PELVIS_S" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_1" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_2" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_3" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_4" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_5" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_6" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_7" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_8" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_9" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_10" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_11" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_12" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_13" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_14" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_15" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/materials.usda b/model/xiaoyan_description/dual_arm_1/payloads/materials.usda new file mode 100644 index 00000000..79051996 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/materials.usda @@ -0,0 +1,255 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_1/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Materials" +{ + def Material "gray" + { + color3f inputs:diffuseColor = (0.21404114, 0.21404114, 0.21404114) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_16" + { + color3f inputs:diffuseColor = (0, 1, 1) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_17" + { + color3f inputs:diffuseColor = (0, 1, 0) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_1" + { + color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_2" + { + color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_3" + { + color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_7" + { + color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_1/payloads/robot.usda b/model/xiaoyan_description/dual_arm_1/payloads/robot.usda new file mode 100644 index 00000000..14a8afe8 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_1/payloads/robot.usda @@ -0,0 +1,273 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpya9pt0m3/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm_9daxnqq8/temp_dual_arm/dual_arm.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" ( + prepend apiSchemas = ["IsaacRobotAPI"] +) +{ + prepend rel isaac:physics:robotJoints = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + prepend rel isaac:physics:robotLinks = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + token isaac:robotType = "Manipulator" + + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "PELVIS_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_FINGER_TIP" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + + over "R_CAM" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + over "root_joint" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "base_fixed" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_FINGER_TIP_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_CAM_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/dual_arm.usda b/model/xiaoyan_description/dual_arm_2/dual_arm.usda new file mode 100644 index 00000000..b6ed6a5b --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/dual_arm.usda @@ -0,0 +1,52 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend references = @./payloads/base.usda@ + variants = { + string Physics = "physx" + } + append variantSets = "Physics" +) +{ + variantSet "Physics" = { + "mujoco" ( + prepend payload = @./payloads/Physics/mujoco.usda@ + ) { + + } + "none" { + + } + "physics" ( + prepend payload = @./payloads/Physics/physics.usda@ + ) { + + } + "physx" ( + prepend payload = @./payloads/Physics/physx.usda@ + ) { + + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/Physics/mujoco.usda b/model/xiaoyan_description/dual_arm_2/payloads/Physics/mujoco.usda new file mode 100644 index 00000000..23566fce --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/Physics/mujoco.usda @@ -0,0 +1,400 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + def MjcActuator "L_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "L_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_P_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_R_actuator" + { + uniform double mjc:forceRange:max = 120 + uniform double mjc:forceRange:min = -120 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_SHOULDER_Y_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_ELBOW_R_actuator" + { + uniform double mjc:forceRange:max = 80 + uniform double mjc:forceRange:min = -80 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_P_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_Y_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + def MjcActuator "R_WRIST_R_actuator" + { + uniform double mjc:forceRange:max = 50 + uniform double mjc:forceRange:min = -50 + custom rel mjc:target + prepend rel mjc:target = + } + + over "root_joint" + { + } + + over "base_fixed" + { + } + + over "L_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "L_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_SHOULDER_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_ELBOW_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_P" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_Y" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_WRIST_R" ( + delete apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + } + + over "R_FINGER_TIP_FIXED" + { + } + + over "R_CAM_FIXED" + { + } + } + + over "Geometry" + { + over "base_link" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" + { + } + + over "L_WRIST_Y_S" + { + } + } + + over "L_WRIST_P_S" + { + } + } + + over "L_ELBOW_R_S" + { + } + } + + over "L_SHOULDER_Y_S" + { + } + } + + over "L_SHOULDER_R_S" + { + } + } + + over "L_SHOULDER_P_S" + { + } + } + + over "R_SHOULDER_P_S" + { + over "R_SHOULDER_R_S" + { + over "R_SHOULDER_Y_S" + { + over "R_ELBOW_R_S" + { + over "R_WRIST_P_S" + { + over "R_WRIST_Y_S" + { + over "R_WRIST_R_S" + { + over "R_FINGER_TIP" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_CAM" + { + over "sphere" + { + } + + over "sphere_1" + { + } + } + + over "R_WRIST_R_S" + { + } + } + + over "R_WRIST_Y_S" + { + } + } + + over "R_WRIST_P_S" + { + } + } + + over "R_ELBOW_R_S" + { + } + } + + over "R_SHOULDER_Y_S" + { + } + } + + over "R_SHOULDER_R_S" + { + } + } + + over "R_SHOULDER_P_S" + { + } + } + + over "PELVIS_S" + { + } + } + + over "cylinder" + { + } + + over "base_column" + { + } + } + } + + over "Materials" + { + over "gray" + { + } + + over "material_16" + { + } + + over "material_17" + { + } + } + + over "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/Physics/physics.usda b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physics.usda new file mode 100644 index 00000000..b0444420 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physics.usda @@ -0,0 +1,554 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsArticulationRootAPI", "NewtonArticulationRootAPI"] + ) + { + bool newton:selfCollisionEnabled = 0 + + over "PELVIS_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0.000037852908, 3.8178143e-7, 0.038639627) + float3 physics:diagonalInertia = (0.0013673676, 0.0016570506, 0.0016829747) + float physics:mass = 2.106246 + quatf physics:principalAxes = (0.009335395, 0.7070413, 0.707049, -0.009335252) + + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, 0.070459306, 0.0000011526188) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (0.52669495, -0.5265165, -0.4719702, -0.471823) + + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, 0.09173933, -1.6708507e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (-0.000014568957, 0.50742036, 0.8616986, 0.00003184278) + + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, 0.08636205, 9.507486e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (0.53709006, -0.5370771, -0.45993194, -0.45994022) + + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, 0.060319997, 2.996559e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.32768953, 0.32742363, 0.62656015, 0.626766) + + over "L_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965917e-10, 0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (0.044508155, 0.7057046, 0.7057046, 0.044508155) + + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.064268e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "L_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.016147736, 0.09550466, -0.004993925) + float3 physics:diagonalInertia = (0.00017805478, 0.00018857485, 0.00028092117) + float physics:mass = 0.50489414 + quatf physics:principalAxes = (-0.020479547, 0.8394515, 0.54304194, -0.00269542) + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0098225875, -0.070459306, -0.0000011506992) + float3 physics:diagonalInertia = (0.0004438491, 0.00046564807, 0.00058487366) + float physics:mass = 0.88073754 + quatf physics:principalAxes = (-0.4719702, 0.471823, 0.52669495, 0.5265165) + + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.034602527, -0.09173933, 1.8628064e-8) + float3 physics:diagonalInertia = (0.00029429645, 0.0004076361, 0.00041477106) + float physics:mass = 0.59478843 + quatf physics:principalAxes = (0.00003184278, 0.8616986, 0.50742036, -0.000014568957) + + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0044097686, -0.08636205, -7.587919e-9) + float3 physics:diagonalInertia = (0.00021101937, 0.00029734103, 0.00032981517) + float physics:mass = 0.56340605 + quatf physics:principalAxes = (-0.45993194, 0.45994022, 0.53709006, 0.5370771) + + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.033562426, -0.060319997, -2.9773634e-7) + float3 physics:diagonalInertia = (0.00013903381, 0.00018104233, 0.00018907762) + float physics:mass = 0.3935719 + quatf physics:principalAxes = (-0.62656015, 0.626766, 0.32768953, 0.32742363) + + over "R_WRIST_P_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-1.3965658e-10, -0.06759726, 0.019200552) + float3 physics:diagonalInertia = (0.00009728105, 0.00047675407, 0.00048914185) + float physics:mass = 0.44233248 + quatf physics:principalAxes = (-0.044508155, 0.7057046, 0.7057046, -0.044508155) + + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.0046413587, -5.0642646e-10, -0.03412538) + float3 physics:diagonalInertia = (0.000045789966, 0.00005849702, 0.0000600636) + float physics:mass = 0.23573847 + quatf physics:principalAxes = (-0.6641875, 0.6641875, 0.24260041, -0.24260041) + + over "R_WRIST_R_S" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (-0.020164223, -0.110749684, -0.0059895534) + float3 physics:diagonalInertia = (0.00013062927, 0.00018608647, 0.00027189028) + float physics:mass = 0.50436604 + quatf physics:principalAxes = (0.00645075, 0.6693525, 0.7427797, -0.014283874) + + over "R_FINGER_TIP" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + + over "R_CAM" ( + prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] + ) + { + point3f physics:centerOfMass = (0, 0, 0) + float3 physics:diagonalInertia = (0.000001, 0.000001, 0.000001) + float physics:mass = 0 + quatf physics:principalAxes = (1, 0, 0, 0) + + over "sphere_1" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + + over "base_column" ( + prepend apiSchemas = ["NewtonCollisionAPI"] + ) + { + } + } + } + + over "Physics" + { + def PhysicsRevoluteJoint "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, 0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -114.59156 + float physics:upperLimit = 114.59156 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, 0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -124.9048 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "L_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, 0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -117.456345 + float physics:upperLimit = 0 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, 0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "L_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "L_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.0258, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -89.95438 + float physics:upperLimit = 14.896903 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.0945, 0.042) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 120 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.035, -0.0765, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 120 + custom float urdf:limit:velocity = 3.351 + } + + def PhysicsRevoluteJoint "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.035, -0.1475, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_ELBOW_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 80 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.034, -0.1025, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = 0 + float physics:upperLimit = 117.456345 + custom float urdf:limit:effort = 80 + custom float urdf:limit:velocity = 3.8758 + } + + def PhysicsRevoluteJoint "R_WRIST_P" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Y" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.034, -0.0965, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0, -1, 0, 0) + quatf physics:localRot1 = (0, -1, 0, 0) + float physics:lowerLimit = -179.90875 + float physics:upperLimit = 179.90875 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsRevoluteJoint "R_WRIST_Y" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "Z" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, -0.1525, 0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -44.69071 + float physics:upperLimit = 44.69071 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 0.79 + } + + def PhysicsRevoluteJoint "R_WRIST_R" ( + prepend apiSchemas = ["PhysicsDriveAPI:angular", "PhysicsJointStateAPI:angular"] + ) + { + float drive:angular:physics:maxForce = 50 + uniform token physics:axis = "X" + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.03, 0, -0.039) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -32.658596 + float physics:upperLimit = 89.95438 + custom float urdf:limit:effort = 50 + custom float urdf:limit:velocity = 4.71 + } + + def PhysicsFixedJoint "root_joint" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 0) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "base_fixed" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 1.2) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_FINGER_TIP_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (0.00684256, -0.284077, 0.00801525) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (1, 0, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + } + + def PhysicsFixedJoint "R_CAM_FIXED" + { + custom rel physics:body0 + prepend rel physics:body0 = + custom rel physics:body1 + prepend rel physics:body1 = + point3f physics:localPos0 = (-0.01212, -0.17655, 0.07506) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (-3.1746543e-11, 3.174666e-11, 0.70710677, -0.70710677) + quatf physics:localRot1 = (1, 0, 0, 0) + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/Physics/physx.usda b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physx.usda new file mode 100644 index 00000000..778984fd --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/Physics/physx.usda @@ -0,0 +1,129 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./physics.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Physics" + { + over "L_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 191.99817 + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 222.06699 + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 45.263668 + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["PhysxJointAPI"] + ) + { + float physxJoint:maxJointVelocity = 269.86313 + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/base.usda b/model/xiaoyan_description/dual_arm_2/payloads/base.usda new file mode 100644 index 00000000..58f9e8ef --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/base.usda @@ -0,0 +1,451 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @./robot.usda@ + ] + upAxis = "Z" +) + +def Xform "dual_arm" ( + prepend apiSchemas = ["GeomModelAPI"] + assetInfo = { + string name = "dual_arm" + } + kind = "component" +) +{ + float3[] extentsHint = [(-0.05, -0.958077, -2.3841858e-8), (0.080300845, 0.9075668, 1.32106), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (3.4028235e38, 3.4028235e38, 3.4028235e38), (-3.4028235e38, -3.4028235e38, -3.4028235e38), (-0.05, -0.959077, -2.3841858e-8), (0.05, 0.05, 1.32206)] + + def Scope "Materials" + { + def Material "gray" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_16" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + + def Material "material_17" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + + def Scope "Geometry" + { + def Xform "base_link" + { + def Cylinder "cylinder" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + rel material:binding = + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "PELVIS_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 1.2) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "PELVIS_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, 0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, 0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, 0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, 0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "L_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "L_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.0258, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + + def Xform "R_SHOULDER_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.0945, 0.042) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.035, -0.0765, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_SHOULDER_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.035, -0.1475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_SHOULDER_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_ELBOW_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.034, -0.1025, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_ELBOW_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_P_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.034, -0.0965, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_P_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_Y_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, -0.1525, 0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_Y_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_WRIST_R_S" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.03, 0, -0.039) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Xform "R_WRIST_R_S" ( + instanceable = true + prepend references = @./instances.usda@ + ) + { + quatd xformOp:orient = (1, 0, 0, 0) + double3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Xform "R_FINGER_TIP" + { + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0.00684256, -0.284077, 0.00801525) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Xform "R_CAM" + { + quatf xformOp:orient = (-3.1746543e-11, 3.174668e-11, 0.70710677, -0.70710677) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (-0.01212, -0.17655, 0.07506) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + + def Sphere "sphere" ( + prepend apiSchemas = ["MaterialBindingAPI"] + ) + { + float3[] extent = [(-0.004, -0.004, -0.004), (0.004, 0.004, 0.004)] + rel material:binding = + double radius = 0.004 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + def Sphere "sphere_1" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + displayName = "sphere" + ) + { + float3[] extent = [(-0.005, -0.005, -0.005), (0.005, 0.005, 0.005)] + uniform token purpose = "guide" + double radius = 0.005 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + } + } + } + + def Cylinder "base_column" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + ) + { + uniform token axis = "Z" + float3[] extent = [(-0.05, -0.05, -0.6), (0.05, 0.05, 0.6)] + double height = 1.2 + uniform token purpose = "guide" + double radius = 0.05 + quatf xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (1, 1, 1) + double3 xformOp:translate = (0, 0, 0.6) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + + def Scope "Physics" + { + } + + def Scope "VisualMaterials" + { + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/geometries.usd b/model/xiaoyan_description/dual_arm_2/payloads/geometries.usd new file mode 100644 index 00000000..8015a4a5 Binary files /dev/null and b/model/xiaoyan_description/dual_arm_2/payloads/geometries.usd differ diff --git a/model/xiaoyan_description/dual_arm_2/payloads/instances.usda b/model/xiaoyan_description/dual_arm_2/payloads/instances.usda new file mode 100644 index 00000000..dfc8b3f8 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/instances.usda @@ -0,0 +1,369 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Instances" +{ + def Xform "PELVIS_S" ( + prepend references = @./geometries.usd@ + ) + { + over "PELVIS_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_1" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_2" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_3" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_4" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_5" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_6" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_7" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "L_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "L_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_8" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_9" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_10" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_SHOULDER_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_SHOULDER_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_11" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_ELBOW_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_ELBOW_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_12" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_P_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_P_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_13" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_Y_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_Y_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_14" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } + + def Xform "R_WRIST_R_S" ( + prepend references = @./geometries.usd@ + ) + { + over "R_WRIST_R_S" ( + apiSchemas = ["MaterialBindingAPI"] + ) + { + custom rel material:binding + prepend rel material:binding = + } + + def Scope "VisualMaterials" + { + def Material "material_15" ( + instanceable = true + prepend references = @./materials.usda@ + ) + { + } + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/materials.usda b/model/xiaoyan_description/dual_arm_2/payloads/materials.usda new file mode 100644 index 00000000..b404b460 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/materials.usda @@ -0,0 +1,255 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd + + +Generated from Composed Stage of root layer /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_2/payloads/base.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +def Scope "Materials" +{ + def Material "gray" + { + color3f inputs:diffuseColor = (0.21404114, 0.21404114, 0.21404114) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_16" + { + color3f inputs:diffuseColor = (0, 1, 1) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_17" + { + color3f inputs:diffuseColor = (0, 1, 0) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_1" + { + color3f inputs:diffuseColor = (0.44520125, 0.44520125, 0.44520125) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_2" + { + color3f inputs:diffuseColor = (0.7835379, 0.82278585, 0.8468733) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_3" + { + color3f inputs:diffuseColor = (0.7681513, 0.7681513, 0.8148467) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } + + def Material "material_7" + { + color3f inputs:diffuseColor = (0.37626222, 0.34191445, 0.30498737) + float inputs:metallic = 0 + float inputs:opacity = 1 + float inputs:roughness = 0.5 + token inputs:wrapMode = "repeat" + token outputs:displacement ( + displayGroup = "Outputs" + ) + prepend token outputs:displacement.connect = + token outputs:surface ( + displayGroup = "Outputs" + ) + prepend token outputs:surface.connect = + token outputs:volume ( + displayGroup = "Outputs" + ) + + def Shader "PreviewSurface" ( + apiSchemas = ["NodeDefAPI"] + ) + { + token info:id = "UsdPreviewSurface" + prepend color3f inputs:diffuseColor.connect = + prepend float inputs:metallic.connect = + prepend float inputs:opacity.connect = + prepend float inputs:roughness.connect = + token outputs:displacement + token outputs:surface + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_2/payloads/robot.usda b/model/xiaoyan_description/dual_arm_2/payloads/robot.usda new file mode 100644 index 00000000..fd7f6ff7 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_2/payloads/robot.usda @@ -0,0 +1,273 @@ +#usda 1.0 +( + customLayerData = { + string creator = "URDF USD Converter v0.1.3" + } + defaultPrim = "dual_arm" + doc = """Generated from Composed Stage of root layer /tmp/tmpa433zx62/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/usdex_dual_arm/dual_arm.usdc + + +Generated from Composed Stage of root layer /tmp/urdf_import_dual_arm__0kvb3li/temp_dual_arm/dual_arm.usd +""" + kilogramsPerUnit = 1 + metersPerUnit = 1 + upAxis = "Z" +) + +over "dual_arm" ( + prepend apiSchemas = ["IsaacRobotAPI"] +) +{ + prepend rel isaac:physics:robotJoints = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + prepend rel isaac:physics:robotLinks = [ + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + , + ] + token isaac:robotType = "Manipulator" + + over "Geometry" + { + over "base_link" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "PELVIS_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "L_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + + over "R_SHOULDER_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_SHOULDER_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_ELBOW_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_P_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_Y_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_WRIST_R_S" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + over "R_FINGER_TIP" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + + over "R_CAM" ( + prepend apiSchemas = ["IsaacLinkAPI"] + ) + { + } + } + } + } + } + } + } + } + } + } + } + + over "Physics" + { + over "root_joint" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "base_fixed" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "L_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_SHOULDER_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_ELBOW_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_P" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_Y" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_WRIST_R" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_FINGER_TIP_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + + over "R_CAM_FIXED" ( + prepend apiSchemas = ["IsaacJointAPI"] + ) + { + } + } +} + diff --git a/model/xiaoyan_description/dual_arm_collision.urdf b/model/xiaoyan_description/dual_arm_collision.urdf new file mode 100644 index 00000000..1ce06c46 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_collision.urdf @@ -0,0 +1,517 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/model/xiaoyan_description/dual_arm_collision.usda b/model/xiaoyan_description/dual_arm_collision.usda new file mode 100644 index 00000000..08a8e167 --- /dev/null +++ b/model/xiaoyan_description/dual_arm_collision.usda @@ -0,0 +1,324 @@ +#usda 1.0 +( + defaultPrim = "dual_arm" + kilogramsPerUnit = 1 + metersPerUnit = 1 + subLayers = [ + @dual_arm_2/dual_arm.usda@ + ] + upAxis = "Z" +) + +over "dual_arm" +{ + over "Geometry" + { + over "base_link" + { + over "PELVIS_S" + { + over "L_SHOULDER_P_S" + { + over "L_SHOULDER_R_S" + { + over "L_SHOULDER_Y_S" + { + over "L_ELBOW_R_S" + { + over "L_WRIST_P_S" + { + over "L_WRIST_Y_S" + { + over "L_WRIST_R_S" ( + instanceable = false + ) + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.11902186, 0.25356677, 0.069732234) + double3 xformOp:translate = (-0.0050100889056921005, 0.11078338418155909, -0.01045313011854887) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.5000000000000001, -0.5000000000000001, -0.5000000000000001, 0.5000000000000001) + float3 xformOp:scale = (0.0409999, 0.053493902, 0.0595) + double3 xformOp:translate = (-0.0037500001490116098, -5.408977040638672e-19, -0.03274694923311472) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cylinder "AUTO_COLLISION_CYLINDER" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + uniform token axis = "Z" + custom token collision:primitiveType = "cylinder" + double height = 0.16749374149367213 + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double radius = 0.03765367431379833 + quatd xformOp:orient = (0.7071067811865475, -0.7071067811865475, 0, 0) + double3 xformOp:translate = (3.469446951953614e-18, 0.08874687016941607, 0.012211145890315668) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.069, 0.1285, 0.058) + double3 xformOp:translate = (-0.034500000427457156, 0.03925000037997961, 1.862645149230957e-9) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.060999997, 0.14400001, 0.062992066) + double3 xformOp:translate = (-0.0008999994024634361, 0.062000001315027475, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.0785, 0.1725, 0.065) + double3 xformOp:translate = (-0.03925000161955211, 0.056249999441206455, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.000040139854472664035, -0.00004013985447262077, 0.7071067800472512, 0.7071067800472514) + float3 xformOp:scale = (0.07170313, 0.07299983, 0.104499996) + double3 xformOp:translate = (-0.004348363594192078, 0.06074999878183007, 3.992215372663401e-8) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.07699999, 0.216, 0.0805) + double3 xformOp:translate = (0, 0, 0.04024999989568795) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_SHOULDER_P_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.7071067800472514, 0.7071067800472515, -0.0000401398544727094, 0.00004013985447257145) + float3 xformOp:scale = (0.07170313, 0.07299983, 0.104499996) + double3 xformOp:translate = (-0.004348363594192072, -0.06074999924749136, 3.992215372663644e-8) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_SHOULDER_R_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.0785, 0.1725, 0.065) + double3 xformOp:translate = (-0.03925000161954845, -0.056249999441206455, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_SHOULDER_Y_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.060999997, 0.14400001, 0.062992066) + double3 xformOp:translate = (-0.0008999994024634361, -0.06200000178068876, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_ELBOW_R_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.069, 0.1285, 0.058) + double3 xformOp:translate = (-0.03450000042745616, -0.03925000037997961, 1.862645149230957e-9) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_WRIST_P_S" + { + def Cylinder "AUTO_COLLISION_CYLINDER" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + uniform token axis = "Z" + custom token collision:primitiveType = "cylinder" + double height = 0.16749374056234956 + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double radius = 0.03765373180894358 + quatd xformOp:orient = (0.7071067811865475, -0.7071067811865475, 0, 0) + double3 xformOp:translate = (3.469446951953614e-18, -0.08874687063507736, 0.012211077787740575) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient"] + } + + over "R_WRIST_Y_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (0.5000000000000001, -0.5000000000000001, -0.5000000000000001, 0.5000000000000001) + float3 xformOp:scale = (0.0409999, 0.053493902, 0.0595) + double3 xformOp:translate = (-0.0037500001490116098, -5.408977040638672e-19, -0.03274694923311472) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + + over "R_WRIST_R_S" + { + def Cube "AUTO_COLLISION_BOX" ( + prepend apiSchemas = ["PhysicsCollisionAPI"] + customData = { + string collisionGenerator = "trimesh-primitives-v1" + } + ) + { + custom token collision:primitiveType = "box" + bool physics:collisionEnabled = 1 + uniform token purpose = "guide" + double size = 1 + quatd xformOp:orient = (1, 0, 0, 0) + float3 xformOp:scale = (0.11152348, 0.25179133, 0.0779598) + double3 xformOp:translate = (-0.01363389752805233, -0.1093915430828929, -0.014153839088976383) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + } + } + } + } + } + } + } + } + } + } + } +} + diff --git a/protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto b/protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto new file mode 100644 index 00000000..ded6d8cd --- /dev/null +++ b/protos/cmvr/config/self_collision_task_config/self_collision_task_config.proto @@ -0,0 +1,35 @@ +syntax = "proto3"; + +package cmvr.config; + +message CollisionPairConfig { + string first = 1; + string second = 2; +} + +message SelfCollisionCheckerConfig { + string urdf_path = 1; + repeated CollisionPairConfig ignored_pairs = 2; +} + +message DistanceSamplingConfig { + double max_geometry_displacement_m = 1; + double max_check_period_s = 2; +} + +message CollisionSafetyConfig { + double warning_distance_m = 1; + double stop_distance_m = 2; +} + +message SelfCollisionTaskConfig { + string id = 1; + string arm_id = 2; + SelfCollisionCheckerConfig checker = 10; + DistanceSamplingConfig sampling = 11; + CollisionSafetyConfig safety = 12; +} + +message SelfCollisionTaskRootConfig { + SelfCollisionTaskConfig self_collision_task = 1; +} diff --git a/protos/cmvr/config/task_manager_config/task_manager_config.proto b/protos/cmvr/config/task_manager_config/task_manager_config.proto index 6c13c845..28d148c1 100644 --- a/protos/cmvr/config/task_manager_config/task_manager_config.proto +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -6,6 +6,7 @@ message TaskConfigEntry { TASK_TYPE_UNKNOWN = 0; TASK_TYPE_TOUCH_SCREEN = 1; TASK_TYPE_GRPC_SERVER = 3; + TASK_TYPE_SELF_COLLISION = 4; reserved 2; reserved "TASK_TYPE_ARM_CONTROL"; } diff --git a/scripts/isaac_sim/README.md b/scripts/isaac_sim/README.md new file mode 100644 index 00000000..04e81ab5 --- /dev/null +++ b/scripts/isaac_sim/README.md @@ -0,0 +1,145 @@ +# 简化碰撞体生成器 + +统一入口 `generate_collision_primitives.py` 根据输入和输出扩展名自动处理 URDF 或 +USD。它读取机器人各个 link 的可视网格，并拟合为 box、sphere 或 cylinder；自动 +模式会选择包围体积最小的几何体。 + +URDF 输入会生成一份包含 `` 的新 URDF；USD 输入会生成引用原 USD 的 +overlay。两种流程都不会修改源文件。USD 生成器还会把长度单位、质量单位和 up axis +复制到 overlay 根层，避免使用默认的厘米制和 Y-up 坐标系。当前双臂模型使用米制、 +Z-up 坐标系。 + +## 预览拟合结果 + +使用 `--dry-run` 只计算和打印拟合结果，不生成输出文件： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --dry-run +``` + +调试配置时，可使用 `--only R_ELBOW_R_S` 只拟合一个 link。 + +## 生成并验证 overlay USD + +先生成到 `/tmp` 进行测试： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \ + --output /tmp/dual_arm_collision_test.usda \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --replace \ + --validate +``` + +在 Isaac Sim 中打开 `/tmp/dual_arm_collision_test.usda`，并在 Viewport 中启用 +Guide Geometry，即可查看碰撞体。默认情况下，overlay 会停用输入 USD 中已有的 +网格碰撞实例，避免旧碰撞体和新碰撞体同时生效。 + +确认结果后，可以生成到项目目录： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm_2/dual_arm.usda \ + --output model/xiaoyan_description/dual_arm_collision.usda \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --replace \ + --validate +``` + +## 配置单个 link + +使用 link 名称编写单独配置： + +```toml +[links.R_ELBOW_R_S] +primitive = "cylinder" +axis = "y" +padding = 0.002 +scale = 1.0 +``` + +`primitive` 支持以下取值： + +- `auto`：自动选择包围体积最小的几何体 +- `box`：盒体 +- `sphere`：球体 +- `cylinder`：圆柱体 + +盒体设置 `alignment = "link"` 后，会使用与 link 局部 XYZ 轴平行的 AABB， +不会产生自由旋转的斜包围盒。圆柱体设置 `axis = "x"`、`"y"` 或 `"z"` 后， +圆柱轴会固定到对应的 link 局部轴。 + +其他常用参数： + +- `padding`：在碰撞体外侧增加的绝对尺寸，单位为米 +- `scale`：以碰撞体中心为基准进行整体缩放 +- `enabled = false`：跳过该 link + +## USD 中缺少可视网格时回退到 STL + +如果导入后的 USD 中缺少某个 link 的可视网格，可以使用 `mesh_file` 指向 URDF +使用的原始 STL： + +```toml +[links.L_WRIST_R_S] +mesh_file = "../../model/xiaoyan_description/meshes/L_WRIST_R_S.STL" +primitive = "box" +alignment = "link" +``` + +相对路径以 TOML 配置文件所在目录为基准。STL 顶点必须使用该 link 的局部坐标系。 + +## 直接生成带简化碰撞体的 URDF + +对于 URDF，统一入口会读取每个 link 的 visual mesh，应用 `` 和 +`` 后，把拟合结果写成新的 ``。原始 URDF 不会被修改。 + +先只预览拟合结果： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm.urdf \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --dry-run +``` + +生成并验证新的 URDF： + +```bash +/home/lgv/app/isaacsim/python.sh \ + scripts/isaac_sim/generate_collision_primitives.py \ + --input model/xiaoyan_description/dual_arm.urdf \ + --output model/xiaoyan_description/dual_arm_collision.urdf \ + --config scripts/isaac_sim/dual_arm_collision.toml \ + --replace \ + --validate +``` + +默认行为是：只对实际生成碰撞体的 link 删除旧 ``，然后写入一个名为 +`AUTO_COLLISION_BOX`、`AUTO_COLLISION_SPHERE` 或 `AUTO_COLLISION_CYLINDER` 的新碰撞体。 +配置为 `enabled = false` 的 link 完全不改动，所以当前配置会保留 `base_link` 的底座 +圆柱，以及 `R_FINGER_TIP`、`R_CAM` 的原有球体。 + +调试时可用 `--only L_WRIST_P_S` 只处理一个 link。需要保留某个已存在碰撞体并在其后 +追加自动碰撞体时，使用 `--keep-existing`。 + +## 使用 MeshCat 查看 URDF 碰撞体 + +生成后可直接启动 MeshCat 查看器。STL 按 URDF 原始材质显示，亮绿色线框是 URDF 的 +``： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf +``` + +完整选项和关节位置设置方法见 `scripts/meshcat/README.md`。 diff --git a/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-312.pyc b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-312.pyc new file mode 100644 index 00000000..55da4544 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-312.pyc differ diff --git a/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-313.pyc b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-313.pyc new file mode 100644 index 00000000..d44724f3 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/generate_collision_primitives.cpython-313.pyc differ diff --git a/scripts/isaac_sim/__pycache__/generate_urdf_collision_primitives.cpython-312.pyc b/scripts/isaac_sim/__pycache__/generate_urdf_collision_primitives.cpython-312.pyc new file mode 100644 index 00000000..d2ca3891 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/generate_urdf_collision_primitives.cpython-312.pyc differ diff --git a/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-312.pyc b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-312.pyc new file mode 100644 index 00000000..59bbcfd2 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-312.pyc differ diff --git a/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-313.pyc b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-313.pyc new file mode 100644 index 00000000..117859e4 Binary files /dev/null and b/scripts/isaac_sim/__pycache__/test_collision_primitives.cpython-313.pyc differ diff --git a/scripts/isaac_sim/dual_arm_collision.toml b/scripts/isaac_sim/dual_arm_collision.toml new file mode 100644 index 00000000..12f73d69 --- /dev/null +++ b/scripts/isaac_sim/dual_arm_collision.toml @@ -0,0 +1,84 @@ +# 默认配置会应用到所有未单独覆盖的 link。 +[defaults] +# auto 会分别拟合 box、sphere、cylinder，并选择包围体积最小的一种。 +primitive = "auto" +# auto 模式允许参与比较的碰撞体类型。 +allowed_primitives = ["box", "sphere", "cylinder"] +# 碰撞体向外扩张的绝对尺寸，单位为米。0 表示不扩张。 +padding = 0.0 +# 以拟合中心为基准缩放碰撞体。1.0 表示保持原始拟合尺寸。 +scale = 1.0 +# trimesh 搜索最小包围圆柱时的方向采样密度；越大越精确，但计算越慢。 +cylinder_sample_count = 6 +# 圆柱方向优化的角度容差。 +cylinder_angle_tol = 0.001 +# 仅用于 USD：如果输入中仍有旧的 STL 网格碰撞体，在 overlay 中将其停用。 +# URDF 默认替换生成 link 上的旧碰撞体；传入 --keep-existing 可改为追加。 +disable_existing_mesh_collisions = true + +# base_link 已在 dual_arm.urdf 中明确配置了固定圆柱碰撞体 base_column， +# 不需要根据可视模型重复生成。 +[links.base_link] +enabled = false + +# 指尖和相机在 dual_arm.urdf 中已经有明确的 sphere 碰撞体，跳过自动拟合。 +[links.R_FINGER_TIP] +enabled = false + +[links.R_CAM] +enabled = false + +# 躯干使用与 PELVIS_S 局部 XYZ 轴平行的长方体。 +# alignment="link" 表示使用 link-local AABB，不允许盒体自由倾斜。 +[links.PELVIS_S] +primitive = "box" +alignment = "link" + +# 左右长连杆在各自 link 局部坐标中主要沿 Y 轴延伸。 +# 这些 link 使用轴对齐长方体，避免最小体积 OBB 出现肉眼可见的倾斜。 +[links.L_SHOULDER_R_S] +primitive = "box" +alignment = "link" + +[links.R_SHOULDER_R_S] +primitive = "box" +alignment = "link" + +[links.L_SHOULDER_Y_S] +primitive = "box" +alignment = "link" + +[links.R_SHOULDER_Y_S] +primitive = "box" +alignment = "link" + +[links.L_ELBOW_R_S] +primitive = "box" +alignment = "link" + +[links.R_ELBOW_R_S] +primitive = "box" +alignment = "link" + +# 左右前臂使用圆柱体，圆柱轴固定为 link 局部 Y 轴。 +# 如果更重视减少自碰撞误报，也可以改为 primitive="box"、alignment="link"。 +[links.L_WRIST_P_S] +primitive = "cylinder" +axis = "y" + +[links.R_WRIST_P_S] +primitive = "cylinder" +axis = "y" + +# 最新导入的 USD 没有提供左右末端腕部的可视网格点， +# 因此回退到 URDF 使用的原始 STL。mesh_file 相对本 TOML 文件解析。 +# 末端腕部外形不规则且截面不圆，使用 link 轴对齐长方体。 +[links.L_WRIST_R_S] +mesh_file = "../../model/xiaoyan_description/meshes/L_WRIST_R_S.STL" +primitive = "box" +alignment = "link" + +[links.R_WRIST_R_S] +mesh_file = "../../model/xiaoyan_description/meshes/R_WRIST_R_S.STL" +primitive = "box" +alignment = "link" diff --git a/scripts/isaac_sim/generate_collision_primitives.py b/scripts/isaac_sim/generate_collision_primitives.py new file mode 100644 index 00000000..1174ed71 --- /dev/null +++ b/scripts/isaac_sim/generate_collision_primitives.py @@ -0,0 +1,900 @@ +#!/usr/bin/env python3 +"""Fit primitive colliders to a robot's visual meshes and write URDF or USD. + +The input and output formats are selected from their file extensions. URDF +output is a new complete document; USD output is an overlay of the input USD. +The source asset is never modified. +""" + +from __future__ import annotations + +import argparse +import os +from dataclasses import dataclass +from pathlib import Path +import sys +import tomllib +import traceback +import xml.etree.ElementTree as ET +from typing import Any + +import numpy as np +import trimesh + + +GENERATOR_TAG = "trimesh-primitives-v1" +GENERATED_PREFIX = "AUTO_COLLISION_" +PRIMITIVE_TYPES = ("box", "sphere", "cylinder") + + +@dataclass(frozen=True) +class PrimitiveFit: + kind: str + transform: np.ndarray + dimensions: tuple[float, ...] + volume: float + + +def _points_array(points: np.ndarray) -> np.ndarray: + result = np.asarray(points, dtype=np.float64) + if result.ndim != 2 or result.shape[1] != 3 or len(result) < 4: + raise ValueError("at least four 3D points are required") + if not np.isfinite(result).all(): + raise ValueError("points contain NaN or infinity") + return result + + +def fit_box(points: np.ndarray, padding: float = 0.0, scale: float = 1.0) -> PrimitiveFit: + points = _points_array(points) + to_box, extents = trimesh.bounds.oriented_bounds(points) + extents = np.asarray(extents, dtype=np.float64) * scale + 2.0 * padding + transform = np.linalg.inv(np.asarray(to_box, dtype=np.float64)) + return PrimitiveFit("box", transform, tuple(extents), float(np.prod(extents))) + + +def fit_link_aligned_box( + points: np.ndarray, padding: float = 0.0, scale: float = 1.0 +) -> PrimitiveFit: + points = _points_array(points) + lower = points.min(axis=0) + upper = points.max(axis=0) + extents = (upper - lower) * scale + 2.0 * padding + transform = np.eye(4) + transform[:3, 3] = (lower + upper) / 2.0 + return PrimitiveFit("box", transform, tuple(extents), float(np.prod(extents))) + + +def fit_sphere(points: np.ndarray, padding: float = 0.0, scale: float = 1.0) -> PrimitiveFit: + points = _points_array(points) + center, radius = trimesh.nsphere.minimum_nsphere(points) + radius = float(radius) * scale + padding + transform = np.eye(4) + transform[:3, 3] = center + return PrimitiveFit("sphere", transform, (radius,), float(4.0 * np.pi * radius**3 / 3.0)) + + +def fit_cylinder( + points: np.ndarray, + padding: float = 0.0, + scale: float = 1.0, + sample_count: int = 6, + angle_tol: float = 0.001, +) -> PrimitiveFit: + points = _points_array(points) + result = trimesh.bounds.minimum_cylinder( + points, sample_count=sample_count, angle_tol=angle_tol + ) + radius = float(result["radius"]) * scale + padding + height = float(result["height"]) * scale + 2.0 * padding + transform = np.asarray(result["transform"], dtype=np.float64) + volume = float(np.pi * radius**2 * height) + return PrimitiveFit("cylinder", transform, (radius, height), volume) + + +def fit_axis_aligned_cylinder( + points: np.ndarray, + axis: str, + padding: float = 0.0, + scale: float = 1.0, +) -> PrimitiveFit: + points = _points_array(points) + axis = axis.lower() + if axis not in "xyz": + raise ValueError(f"cylinder axis must be x, y, or z: {axis}") + axis_index = "xyz".index(axis) + radial_indices = [index for index in range(3) if index != axis_index] + radial_center, radius = trimesh.nsphere.minimum_nsphere( + points[:, radial_indices] + ) + axial_min = float(points[:, axis_index].min()) + axial_max = float(points[:, axis_index].max()) + + center = np.zeros(3) + center[axis_index] = (axial_min + axial_max) / 2.0 + center[radial_indices] = radial_center + radius = float(radius) * scale + padding + height = (axial_max - axial_min) * scale + 2.0 * padding + + transform = np.eye(4) + if axis == "x": + transform[:3, :3] = np.array( + [[0.0, 0.0, 1.0], [0.0, 1.0, 0.0], [-1.0, 0.0, 0.0]] + ) + elif axis == "y": + transform[:3, :3] = np.array( + [[1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, -1.0, 0.0]] + ) + transform[:3, 3] = center + volume = float(np.pi * radius**2 * height) + return PrimitiveFit("cylinder", transform, (radius, height), volume) + + +def fit_primitive( + points: np.ndarray, + kind: str, + *, + allowed: list[str], + padding: float, + scale: float, + cylinder_sample_count: int, + cylinder_angle_tol: float, + alignment: str = "oriented", + axis: str | None = None, +) -> PrimitiveFit: + def fit(candidate: str) -> PrimitiveFit: + if candidate == "box": + if alignment == "link": + return fit_link_aligned_box(points, padding, scale) + return fit_box(points, padding, scale) + if candidate == "sphere": + return fit_sphere(points, padding, scale) + if candidate == "cylinder": + if axis: + return fit_axis_aligned_cylinder(points, axis, padding, scale) + return fit_cylinder( + points, + padding, + scale, + cylinder_sample_count, + cylinder_angle_tol, + ) + raise ValueError(f"unsupported primitive type: {candidate}") + + if kind != "auto": + return fit(kind) + + candidates: list[PrimitiveFit] = [] + failures: list[str] = [] + for candidate in allowed: + try: + candidates.append(fit(candidate)) + except Exception as exc: # A degenerate mesh may fail one fitter only. + failures.append(f"{candidate}: {exc}") + if not candidates: + raise RuntimeError("all primitive fits failed: " + "; ".join(failures)) + return min(candidates, key=lambda candidate: candidate.volume) + + +def load_config(path: Path | None) -> dict[str, Any]: + if path is None: + return {} + with path.open("rb") as stream: + config = tomllib.load(stream) + config["_config_dir"] = str(path.parent.resolve()) + return config + + +def link_settings(config: dict[str, Any], link_name: str) -> dict[str, Any]: + settings = dict(config.get("defaults", {})) + settings.update(config.get("links", {}).get(link_name, {})) + return settings + + +def _find_robot_root(stage: Any, requested_path: str | None) -> Any: + if requested_path: + prim = stage.GetPrimAtPath(requested_path) + if not prim: + raise ValueError(f"robot root does not exist: {requested_path}") + return prim + + default_prim = stage.GetDefaultPrim() + if default_prim and default_prim.GetRelationship("isaac:physics:robotLinks").IsValid(): + return default_prim + + for prim in stage.Traverse(): + if prim.GetRelationship("isaac:physics:robotLinks").IsValid(): + return prim + raise RuntimeError("could not find an Isaac robotLinks relationship; pass --robot-root") + + +def find_robot_links(stage: Any, robot_root_path: str | None) -> list[Any]: + root = _find_robot_root(stage, robot_root_path) + targets = root.GetRelationship("isaac:physics:robotLinks").GetTargets() + links = [stage.GetPrimAtPath(path) for path in targets] + links = [prim for prim in links if prim] + if not links: + raise RuntimeError(f"robot has no resolved links: {root.GetPath()}") + return links + + +def _computed_purpose(prim: Any, UsdGeom: Any) -> str: + imageable = UsdGeom.Imageable(prim) + if not imageable: + return "" + return str(imageable.ComputePurpose()) + + +def collect_visual_points(link: Any, link_paths: set[str], Usd: Any, UsdGeom: Any, UsdPhysics: Any) -> tuple[np.ndarray, set[str]]: + """Return visual vertices in link coordinates and direct mesh-collider roots.""" + cache = UsdGeom.XformCache() + link_to_world = cache.GetLocalToWorldTransform(link) + world_to_link = np.asarray(link_to_world.GetInverse(), dtype=np.float64) + point_sets: list[np.ndarray] = [] + mesh_collision_roots: set[str] = set() + + for child in link.GetChildren(): + if str(child.GetPath()) in link_paths or child.GetName().startswith(GENERATED_PREFIX): + continue + + child_has_mesh_collision = False + for prim in Usd.PrimRange(child, Usd.TraverseInstanceProxies()): + if not prim.IsA(UsdGeom.Mesh): + continue + purpose = _computed_purpose(prim, UsdGeom) + is_collision = prim.HasAPI(UsdPhysics.CollisionAPI) or purpose == str(UsdGeom.Tokens.guide) + if is_collision: + child_has_mesh_collision = True + continue + if purpose not in ("", str(UsdGeom.Tokens.default_), str(UsdGeom.Tokens.render)): + continue + + points = np.asarray(UsdGeom.Mesh(prim).GetPointsAttr().Get(), dtype=np.float64) + if not len(points): + continue + mesh_to_world = np.asarray(cache.GetLocalToWorldTransform(prim), dtype=np.float64) + mesh_to_link = mesh_to_world @ world_to_link + local_points = points @ mesh_to_link[:3, :3] + mesh_to_link[3, :3] + point_sets.append(local_points) + + if child_has_mesh_collision: + mesh_collision_roots.add(str(child.GetPath())) + + if not point_sets: + raise RuntimeError(f"no visual mesh vertices found below {link.GetPath()}") + return np.concatenate(point_sets), mesh_collision_roots + + +def collect_mesh_file_points(path: Path) -> np.ndarray: + """Load a URDF visual mesh whose vertices are already link-local.""" + if not path.is_file(): + raise FileNotFoundError(path) + loaded = trimesh.load(path, force="mesh") + if isinstance(loaded, trimesh.Scene): + meshes = tuple(loaded.geometry.values()) + if not meshes: + raise RuntimeError(f"mesh file has no geometry: {path}") + loaded = trimesh.util.concatenate(meshes) + return _points_array(np.asarray(loaded.vertices, dtype=np.float64)) + + +def _set_transform(prim: Any, transform: np.ndarray, scale: tuple[float, float, float] | None, Gf: Any, UsdGeom: Any) -> None: + xformable = UsdGeom.Xformable(prim) + translation = transform[:3, 3] + quaternion = trimesh.transformations.quaternion_from_matrix(transform) + xformable.AddTranslateOp().Set(Gf.Vec3d(*translation.tolist())) + xformable.AddOrientOp(UsdGeom.XformOp.PrecisionDouble).Set( + Gf.Quatd(float(quaternion[0]), Gf.Vec3d(*quaternion[1:4].tolist())) + ) + if scale is not None: + xformable.AddScaleOp().Set(Gf.Vec3d(*scale)) + + +def prepare_authoring_links(stage: Any, link_paths: list[str]) -> None: + """De-instance only branches that contain a link requiring a new child.""" + instance_roots: set[str] = set() + for link_path in link_paths: + link = stage.GetPrimAtPath(link_path) + if not link: + continue + if link.IsInstance(): + instance_roots.add(link_path) + continue + if not link.IsInstanceProxy(): + continue + instance_root = link + while instance_root.IsInstanceProxy(): + instance_root = instance_root.GetParent() + if not instance_root or not instance_root.IsInstance(): + raise RuntimeError(f"could not find an instance root for collision link: {link_path}") + instance_roots.add(str(instance_root.GetPath())) + + if not instance_roots: + return + for instance_root in instance_roots: + stage.OverridePrim(instance_root).SetInstanceable(False) + stage.GetRootLayer().Save() + stage.Reload() + + still_proxies = [ + link_path + for link_path in link_paths + if stage.GetPrimAtPath(link_path).IsInstanceProxy() + ] + if still_proxies: + raise RuntimeError(f"links remained instance proxies: {still_proxies}") + + +def author_primitive(stage: Any, link_path: str, fit: PrimitiveFit, Gf: Any, Sdf: Any, UsdGeom: Any, UsdPhysics: Any) -> str: + prim_path = f"{link_path}/{GENERATED_PREFIX}{fit.kind.upper()}" + if fit.kind == "box": + shape = UsdGeom.Cube.Define(stage, prim_path) + shape.CreateSizeAttr(1.0) + scale = tuple(float(value) for value in fit.dimensions) + elif fit.kind == "sphere": + shape = UsdGeom.Sphere.Define(stage, prim_path) + shape.CreateRadiusAttr(float(fit.dimensions[0])) + scale = None + elif fit.kind == "cylinder": + shape = UsdGeom.Cylinder.Define(stage, prim_path) + shape.CreateAxisAttr(UsdGeom.Tokens.z) + shape.CreateRadiusAttr(float(fit.dimensions[0])) + shape.CreateHeightAttr(float(fit.dimensions[1])) + scale = None + else: + raise AssertionError(fit.kind) + + prim = shape.GetPrim() + _set_transform(prim, fit.transform, scale, Gf, UsdGeom) + UsdPhysics.CollisionAPI.Apply(prim).CreateCollisionEnabledAttr(True) + UsdGeom.Imageable(prim).CreatePurposeAttr(UsdGeom.Tokens.guide) + prim.SetCustomDataByKey("collisionGenerator", GENERATOR_TAG) + prim.CreateAttribute("collision:primitiveType", Sdf.ValueTypeNames.Token, custom=True).Set(fit.kind) + return prim_path + + +def create_overlay_stage( + input_path: Path, + temporary_output: Path, + default_prim_path: str, + source_stage: Any, + Usd: Any, +) -> Any: + stage = Usd.Stage.CreateNew(str(temporary_output)) + relative_input = os.path.relpath(input_path, temporary_output.parent) + stage.GetRootLayer().subLayerPaths = [relative_input] + for metadata_key in ("upAxis", "metersPerUnit", "kilogramsPerUnit"): + metadata_value = source_stage.GetMetadata(metadata_key) + if metadata_value is not None: + stage.SetMetadata(metadata_key, metadata_value) + source_default = stage.GetPrimAtPath(default_prim_path) + if not source_default: + raise RuntimeError(f"default prim did not compose into overlay: {default_prim_path}") + stage.SetDefaultPrim(source_default) + return stage + + +def validate_usd_output(path: Path, expected_count: int, Usd: Any, UsdGeom: Any, UsdPhysics: Any) -> None: + stage = Usd.Stage.Open(str(path)) + if not stage.GetDefaultPrim(): + raise RuntimeError("output USD has no default prim") + generated = [ + prim + for prim in stage.Traverse() + if prim.GetCustomDataByKey("collisionGenerator") == GENERATOR_TAG + ] + if len(generated) != expected_count: + raise RuntimeError(f"expected {expected_count} generated colliders, found {len(generated)}") + for prim in generated: + if prim.GetTypeName() not in ("Cube", "Sphere", "Cylinder"): + raise RuntimeError(f"generated collider is not a primitive: {prim.GetPath()}") + if not prim.HasAPI(UsdPhysics.CollisionAPI): + raise RuntimeError(f"CollisionAPI missing: {prim.GetPath()}") + if _computed_purpose(prim, UsdGeom) != str(UsdGeom.Tokens.guide): + raise RuntimeError(f"guide purpose missing: {prim.GetPath()}") + + +def run_usd(args: argparse.Namespace, Usd: Any, UsdGeom: Any, UsdPhysics: Any, Gf: Any, Sdf: Any) -> int: + input_path = args.input.resolve() + if not input_path.is_file(): + raise FileNotFoundError(input_path) + if not args.dry_run and args.output is None: + raise ValueError("--output is required unless --dry-run is used") + + config = load_config(args.config.resolve() if args.config else None) + defaults = config.get("defaults", {}) + allowed = list(defaults.get("allowed_primitives", PRIMITIVE_TYPES)) + invalid = set(allowed) - set(PRIMITIVE_TYPES) + if invalid: + raise ValueError(f"invalid allowed_primitives: {sorted(invalid)}") + + source_stage = Usd.Stage.Open(str(input_path)) + if not source_stage: + raise RuntimeError(f"could not open USD: {input_path}") + links = find_robot_links(source_stage, args.robot_root) + source_default = source_stage.GetDefaultPrim() + if not source_default: + raise RuntimeError("input USD has no default prim") + link_paths = {str(link.GetPath()) for link in links} + selected = set(args.only) + results: list[tuple[str, PrimitiveFit, set[str]]] = [] + + for link in links: + name = link.GetName() + if selected and name not in selected: + continue + settings = link_settings(config, name) + if not settings.get("enabled", True): + print(f"SKIP {name}: disabled by configuration") + continue + try: + points, collision_roots = collect_visual_points( + link, link_paths, Usd, UsdGeom, UsdPhysics + ) + except RuntimeError as error: + mesh_file = settings.get("mesh_file") + if not mesh_file: + raise RuntimeError( + f"{error}; no mesh_file configured for link name {name!r}" + ) from error + mesh_path = Path(mesh_file) + if not mesh_path.is_absolute(): + mesh_path = Path(config["_config_dir"]) / mesh_path + points = collect_mesh_file_points(mesh_path.resolve()) + collision_roots = set() + print(f"FALLBACK {name}: loaded {mesh_path}") + kind = str(settings.get("primitive", "auto")) + if kind not in (*PRIMITIVE_TYPES, "auto"): + raise ValueError(f"invalid primitive for {name}: {kind}") + fit = fit_primitive( + points, + kind, + allowed=list(settings.get("allowed_primitives", allowed)), + padding=float(settings.get("padding", 0.0)), + scale=float(settings.get("scale", 1.0)), + cylinder_sample_count=int(settings.get("cylinder_sample_count", defaults.get("cylinder_sample_count", 6))), + cylinder_angle_tol=float(settings.get("cylinder_angle_tol", defaults.get("cylinder_angle_tol", 0.001))), + alignment=str(settings.get("alignment", "oriented")), + axis=str(settings["axis"]) if "axis" in settings else None, + ) + results.append((str(link.GetPath()), fit, collision_roots)) + dimensions = ", ".join(f"{value:.6f}" for value in fit.dimensions) + print(f"FIT {name}: {fit.kind} ({dimensions}), vertices={len(points)}, volume={fit.volume:.8f}") + + if selected: + found = {Path(path).name for path, _, _ in results} + missing = selected - found + if missing: + raise ValueError(f"selected links were not generated: {sorted(missing)}") + if args.dry_run: + print(f"Dry run complete: {len(results)} collider(s) fitted") + return 0 + + output_path = args.output.resolve() + if output_path == input_path: + raise ValueError("input and output must be different files") + if output_path.exists() and not args.replace: + raise FileExistsError(f"output exists; pass --replace: {output_path}") + output_path.parent.mkdir(parents=True, exist_ok=True) + temporary = output_path.with_name(f".{output_path.stem}.tmp{output_path.suffix}") + if temporary.exists(): + temporary.unlink() + + print(f"CREATE overlay: {temporary}", flush=True) + output_stage = create_overlay_stage( + input_path, + temporary, + str(source_default.GetPath()), + source_stage, + Usd, + ) + print("CREATE overlay: composed", flush=True) + prepare_authoring_links(output_stage, [link_path for link_path, _, _ in results]) + disable_meshes = bool(defaults.get("disable_existing_mesh_collisions", True)) + for link_path, fit, collision_roots in results: + if disable_meshes: + for collision_root in collision_roots: + output_stage.OverridePrim(collision_root).SetActive(False) + authored = author_primitive( + output_stage, link_path, fit, Gf, Sdf, UsdGeom, UsdPhysics + ) + print(f"WRITE {authored}") + output_stage.GetRootLayer().Save() + del output_stage + os.replace(temporary, output_path) + + if args.validate: + validate_usd_output(output_path, len(results), Usd, UsdGeom, UsdPhysics) + print(f"Validated {len(results)} generated collider(s)") + print(f"Output: {output_path}") + return 0 + + +def _parse_vector( + value: str | None, size: int, default: tuple[float, ...] +) -> np.ndarray: + if value is None: + return np.asarray(default, dtype=np.float64) + result = np.fromstring(value, sep=" ", dtype=np.float64) + if len(result) != size or not np.isfinite(result).all(): + raise ValueError(f"expected {size} finite values, got {value!r}") + return result + + +def _urdf_origin_transform(origin: ET.Element | None) -> np.ndarray: + if origin is None: + return np.eye(4) + xyz = _parse_vector(origin.get("xyz"), 3, (0.0, 0.0, 0.0)) + rpy = _parse_vector(origin.get("rpy"), 3, (0.0, 0.0, 0.0)) + transform = trimesh.transformations.euler_matrix(*rpy, axes="sxyz") + transform[:3, 3] = xyz + return transform + + +def _resolve_urdf_mesh_path(filename: str, urdf_path: Path) -> Path: + if filename.startswith("file://"): + path = Path(filename.removeprefix("file://")) + elif filename.startswith("package://"): + package_path = Path(filename.removeprefix("package://")) + if len(package_path.parts) < 2: + raise ValueError(f"invalid package URI: {filename}") + package_name, relative_parts = package_path.parts[0], package_path.parts[1:] + candidates = [ + parent / package_name / Path(*relative_parts) + for parent in (urdf_path.parent, *urdf_path.parents) + ] + candidates.extend( + parent / Path(*relative_parts) + for parent in urdf_path.parents + if parent.name == package_name + ) + for candidate in candidates: + if candidate.is_file(): + return candidate.resolve() + raise FileNotFoundError( + f"could not resolve {filename!r} relative to {urdf_path}" + ) + else: + path = Path(filename) + if not path.is_absolute(): + path = urdf_path.parent / path + path = path.resolve() + if not path.is_file(): + raise FileNotFoundError(path) + return path + + +def collect_urdf_visual_points(link: ET.Element, urdf_path: Path) -> np.ndarray: + """Collect all visual mesh vertices in the link-local coordinate frame.""" + point_sets: list[np.ndarray] = [] + for visual in link.findall("visual"): + visual_transform = _urdf_origin_transform(visual.find("origin")) + geometry = visual.find("geometry") + mesh = geometry.find("mesh") if geometry is not None else None + if mesh is None: + continue + filename = mesh.get("filename") + if not filename: + raise ValueError( + f"visual mesh has no filename in link {link.get('name')!r}" + ) + vertices = collect_mesh_file_points( + _resolve_urdf_mesh_path(filename, urdf_path) + ) + mesh_scale = _parse_vector(mesh.get("scale"), 3, (1.0, 1.0, 1.0)) + vertices = trimesh.transform_points(vertices * mesh_scale, visual_transform) + point_sets.append(vertices) + if not point_sets: + raise RuntimeError(f"no visual mesh found in link {link.get('name')!r}") + return np.concatenate(point_sets) + + +def _format_number(value: float) -> str: + if abs(value) < 5e-13: + value = 0.0 + return f"{value:.12g}" + + +def _format_vector(values: np.ndarray | tuple[float, ...]) -> str: + return " ".join(_format_number(float(value)) for value in values) + + +def create_urdf_collision(fit: PrimitiveFit) -> ET.Element: + collision = ET.Element( + "collision", {"name": f"{GENERATED_PREFIX}{fit.kind.upper()}"} + ) + translation = fit.transform[:3, 3] + rpy = trimesh.transformations.euler_from_matrix(fit.transform, axes="sxyz") + ET.SubElement( + collision, + "origin", + {"xyz": _format_vector(translation), "rpy": _format_vector(rpy)}, + ) + geometry = ET.SubElement(collision, "geometry") + if fit.kind == "box": + ET.SubElement(geometry, "box", {"size": _format_vector(fit.dimensions)}) + elif fit.kind == "sphere": + ET.SubElement( + geometry, "sphere", {"radius": _format_number(fit.dimensions[0])} + ) + elif fit.kind == "cylinder": + ET.SubElement( + geometry, + "cylinder", + { + "radius": _format_number(fit.dimensions[0]), + "length": _format_number(fit.dimensions[1]), + }, + ) + else: + raise AssertionError(fit.kind) + return collision + + +def _fit_urdf_link( + link: ET.Element, + urdf_path: Path, + config: dict[str, Any], + defaults: dict[str, Any], + allowed: list[str], +) -> tuple[PrimitiveFit, int]: + name = link.get("name", "") + settings = link_settings(config, name) + try: + points = collect_urdf_visual_points(link, urdf_path) + except RuntimeError as error: + mesh_file = settings.get("mesh_file") + if not mesh_file: + raise RuntimeError(str(error)) from error + mesh_path = Path(mesh_file) + if not mesh_path.is_absolute(): + mesh_path = Path(config["_config_dir"]) / mesh_path + points = collect_mesh_file_points(mesh_path.resolve()) + print(f"FALLBACK {name}: loaded {mesh_path}") + + kind = str(settings.get("primitive", "auto")) + if kind not in (*PRIMITIVE_TYPES, "auto"): + raise ValueError(f"invalid primitive for {name}: {kind}") + link_allowed = list(settings.get("allowed_primitives", allowed)) + invalid = set(link_allowed) - set(PRIMITIVE_TYPES) + if invalid: + raise ValueError(f"invalid allowed_primitives for {name}: {sorted(invalid)}") + fit = fit_primitive( + points, + kind, + allowed=link_allowed, + padding=float(settings.get("padding", 0.0)), + scale=float(settings.get("scale", 1.0)), + cylinder_sample_count=int( + settings.get( + "cylinder_sample_count", defaults.get("cylinder_sample_count", 6) + ) + ), + cylinder_angle_tol=float( + settings.get( + "cylinder_angle_tol", defaults.get("cylinder_angle_tol", 0.001) + ) + ), + alignment=str(settings.get("alignment", "oriented")), + axis=str(settings["axis"]) if "axis" in settings else None, + ) + return fit, len(points) + + +def _parse_urdf(path: Path) -> ET.ElementTree: + parser = ET.XMLParser(target=ET.TreeBuilder(insert_comments=True)) + tree = ET.parse(path, parser=parser) + root = tree.getroot() + if root.tag != "robot": + raise ValueError(f"URDF root must be , found <{root.tag}>") + return tree + + +def validate_urdf_output(path: Path, expected: dict[str, PrimitiveFit]) -> None: + root = _parse_urdf(path).getroot() + links = {link.get("name", ""): link for link in root.findall("link")} + missing = set(expected) - set(links) + if missing: + raise RuntimeError(f"output URDF is missing links: {sorted(missing)}") + for name, fit in expected.items(): + generated = [ + collision + for collision in links[name].findall("collision") + if collision.get("name", "").startswith(GENERATED_PREFIX) + ] + if len(generated) != 1: + raise RuntimeError( + f"expected one generated collider on {name}, found {len(generated)}" + ) + geometry = generated[0].find("geometry") + primitive_count = ( + sum(geometry.find(kind) is not None for kind in PRIMITIVE_TYPES) + if geometry is not None + else 0 + ) + if primitive_count != 1: + raise RuntimeError(f"invalid generated collision geometry on {name}") + written_transform = _urdf_origin_transform(generated[0].find("origin")) + if not np.allclose(written_transform, fit.transform, atol=1e-9, rtol=1e-9): + error = float(np.max(np.abs(written_transform - fit.transform))) + raise RuntimeError( + f"generated collision transform changed on {name}: max error {error}" + ) + primitive = geometry.find(fit.kind) + if primitive is None: + raise RuntimeError(f"expected {fit.kind} collision geometry on {name}") + if fit.kind == "box": + dimensions = _parse_vector(primitive.get("size"), 3, ()) + elif fit.kind == "sphere": + dimensions = np.asarray([float(primitive.get("radius", "nan"))]) + else: + dimensions = np.asarray( + [ + float(primitive.get("radius", "nan")), + float(primitive.get("length", "nan")), + ] + ) + expected_dimensions = np.asarray(fit.dimensions) + if ( + not np.isfinite(dimensions).all() + or (dimensions <= 0.0).any() + or not np.allclose( + dimensions, expected_dimensions, atol=1e-9, rtol=1e-9 + ) + ): + raise RuntimeError( + f"generated collision dimensions changed on {name}: " + f"expected {expected_dimensions}, found {dimensions}" + ) + + +def run_urdf(args: argparse.Namespace) -> int: + input_path = args.input.resolve() + if not input_path.is_file(): + raise FileNotFoundError(input_path) + if not args.dry_run and args.output is None: + raise ValueError("--output is required unless --dry-run is used") + + config = load_config(args.config.resolve() if args.config else None) + defaults = config.get("defaults", {}) + allowed = list(defaults.get("allowed_primitives", PRIMITIVE_TYPES)) + invalid = set(allowed) - set(PRIMITIVE_TYPES) + if invalid: + raise ValueError(f"invalid allowed_primitives: {sorted(invalid)}") + + tree = _parse_urdf(input_path) + root = tree.getroot() + selected = set(args.only) + known_names = {link.get("name", "") for link in root.findall("link")} + unknown = selected - known_names + if unknown: + raise ValueError(f"selected links do not exist: {sorted(unknown)}") + + results: list[tuple[ET.Element, PrimitiveFit]] = [] + for link in root.findall("link"): + name = link.get("name", "") + if selected and name not in selected: + continue + settings = link_settings(config, name) + if not settings.get("enabled", True): + print(f"SKIP {name}: disabled by configuration") + continue + fit, vertex_count = _fit_urdf_link( + link, input_path, config, defaults, allowed + ) + results.append((link, fit)) + dimensions = ", ".join(f"{value:.6f}" for value in fit.dimensions) + print( + f"FIT {name}: {fit.kind} ({dimensions}), " + f"vertices={vertex_count}, volume={fit.volume:.8f}" + ) + + if args.dry_run: + print(f"Dry run complete: {len(results)} collider(s) fitted") + return 0 + + output_path = args.output.resolve() + if output_path == input_path: + raise ValueError("input and output must be different files") + if output_path.exists() and not args.replace: + raise FileExistsError(f"output exists; pass --replace: {output_path}") + + expected: dict[str, PrimitiveFit] = {} + for link, fit in results: + if not args.keep_existing: + for collision in list(link.findall("collision")): + link.remove(collision) + link.append(create_urdf_collision(fit)) + expected[link.get("name", "")] = fit + + output_path.parent.mkdir(parents=True, exist_ok=True) + temporary = output_path.with_name(f".{output_path.stem}.tmp{output_path.suffix}") + if temporary.exists(): + temporary.unlink() + ET.indent(tree, space=" ") + tree.write(temporary, encoding="utf-8", xml_declaration=True) + os.replace(temporary, output_path) + + if args.validate: + validate_urdf_output(output_path, expected) + print(f"Validated {len(expected)} generated collider(s)") + print(f"Output: {output_path}") + return 0 + + +def parse_args(argv: list[str]) -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--input", required=True, type=Path, help="source URDF or USD") + parser.add_argument("--output", type=Path, help="generated URDF or overlay USD") + parser.add_argument("--config", type=Path, help="TOML fitting configuration") + parser.add_argument( + "--robot-root", help="USD robot root prim path, if auto-detection fails" + ) + parser.add_argument( + "--only", + action="append", + default=[], + metavar="LINK", + help="generate only selected link (repeatable)", + ) + parser.add_argument( + "--keep-existing", + action="store_true", + help="URDF only: append instead of replacing collisions on generated links", + ) + parser.add_argument( + "--dry-run", action="store_true", help="fit and print without writing output" + ) + parser.add_argument( + "--replace", action="store_true", help="atomically replace an existing output" + ) + parser.add_argument( + "--validate", action="store_true", help="reopen and validate generated output" + ) + return parser.parse_args(argv) + + +def _asset_format(path: Path) -> str: + suffix = path.suffix.lower() + if suffix == ".urdf": + return "urdf" + if suffix in (".usd", ".usda", ".usdc"): + return "usd" + raise ValueError( + f"unsupported file extension {path.suffix!r}; expected .urdf, .usd, .usda, or .usdc" + ) + + +def _validate_format_options(args: argparse.Namespace, asset_format: str) -> None: + if args.output is not None and _asset_format(args.output) != asset_format: + raise ValueError("input and output formats must match") + if asset_format == "urdf" and args.robot_root: + raise ValueError("--robot-root is only valid for USD input") + if asset_format == "usd" and args.keep_existing: + raise ValueError("--keep-existing is only valid for URDF input") + + +def main(argv: list[str] | None = None) -> int: + try: + args = parse_args(sys.argv[1:] if argv is None else argv) + asset_format = _asset_format(args.input) + _validate_format_options(args, asset_format) + if asset_format == "urdf": + return run_urdf(args) + + from isaacsim import SimulationApp + + simulation_app = SimulationApp({"headless": True}) + try: + from pxr import Gf, Sdf, Usd, UsdGeom, UsdPhysics + + return run_usd(args, Usd, UsdGeom, UsdPhysics, Gf, Sdf) + finally: + simulation_app.close() + except Exception: + traceback.print_exc() + sys.stderr.flush() + return 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/meshcat/README.md b/scripts/meshcat/README.md new file mode 100644 index 00000000..09a52583 --- /dev/null +++ b/scripts/meshcat/README.md @@ -0,0 +1,80 @@ +# URDF 碰撞体查看器 + +`view_urdf_collisions.py` 使用 MeshCat 显示 URDF 中的碰撞体。默认按 Isaac Sim 风格 +原样显示视觉 STL 和 URDF 材质，并用亮绿色线框显示 ``。它直接解析 URDF +的 link、joint 和 origin，不依赖 Pinocchio。 + +## 安装 + +当前 `cmvr-es` Conda 环境已经安装 MeshCat。其他环境可执行： + +```bash +python -m pip install -r scripts/meshcat/requirements.txt +``` + +## 运行 + +推荐先激活 `cmvr-es` 环境，再进入项目根目录： + +```bash +conda activate cmvr-es +cd /home/lgv/cmvr/0-workspace/cmvr-es + +python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf +``` + +不能在 `/home/lgv/Desktop` 等其他目录直接使用上述相对路径，否则 Python 会在当前 +目录中查找 `scripts/` 和 `model/`。另外，MeshCat 安装在 `cmvr-es` 环境中，当前提示符 +如果是 `(base)`，需要先执行 `conda activate cmvr-es`。 + +如果需要从任意目录启动，使用完整绝对路径和 `conda run`： + +```bash +conda run -n cmvr-es python \ + /home/lgv/cmvr/0-workspace/cmvr-es/scripts/meshcat/view_urdf_collisions.py \ + --input /home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm_collision.urdf +``` + +脚本会打开浏览器，并持续运行到按下 `Ctrl+C`。如果不希望自动打开浏览器，可增加 +`--no-browser`，然后手动打开终端输出的 `MeshCat URL`。 + +需要关闭终端后继续查看，或者希望页面刷新后仍然保留场景时，导出独立 HTML： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf \ + --export-html /tmp/dual_arm_collision_meshcat.html +``` + +独立 HTML 会嵌入视觉 STL 和碰撞体，因此文件较大，但不再依赖后台 Python 进程或 +WebSocket 连接。 + +只显示碰撞体： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf \ + --collision-only +``` + +设置关节位置，旋转关节的单位为弧度： + +```bash +conda run -n cmvr-es python \ + scripts/meshcat/view_urdf_collisions.py \ + --input model/xiaoyan_description/dual_arm_collision.urdf \ + --joint L_SHOULDER_P=0.5 \ + --joint R_SHOULDER_P=-0.5 +``` + +其他显示选项： + +- `--solid-collisions`：把碰撞体切换为半透明实体 +- `--visual-opacity 0.15`：需要透视内部时降低视觉模型透明度 +- `--collision-opacity 0.8`：调整碰撞体线框或实体透明度 +- `--collision-color 00ff66`：使用十六进制颜色覆盖默认绿色 +- `--collision-cylinder-lines 12`：默认 12 条轴向母线，每 30° 一条 diff --git a/scripts/meshcat/__pycache__/view_urdf_collisions.cpython-313.pyc b/scripts/meshcat/__pycache__/view_urdf_collisions.cpython-313.pyc new file mode 100644 index 00000000..c64f7e7a Binary files /dev/null and b/scripts/meshcat/__pycache__/view_urdf_collisions.cpython-313.pyc differ diff --git a/scripts/meshcat/requirements.txt b/scripts/meshcat/requirements.txt new file mode 100644 index 00000000..a8ef6068 --- /dev/null +++ b/scripts/meshcat/requirements.txt @@ -0,0 +1,2 @@ +meshcat==0.3.2 +numpy>=1.24 diff --git a/scripts/meshcat/view_urdf_collisions.py b/scripts/meshcat/view_urdf_collisions.py new file mode 100644 index 00000000..c8f81964 --- /dev/null +++ b/scripts/meshcat/view_urdf_collisions.py @@ -0,0 +1,721 @@ +#!/usr/bin/env python3 +"""Visualize URDF collision geometry and optional visual meshes in MeshCat.""" + +from __future__ import annotations + +import argparse +from dataclasses import dataclass +import math +import os +from pathlib import Path +import subprocess +import sys +import time +import traceback +import webbrowser +import xml.etree.ElementTree as ET + +import meshcat +import meshcat.geometry as geometry +import numpy as np + + +@dataclass(frozen=True) +class Joint: + name: str + kind: str + parent: str + child: str + origin: np.ndarray + axis: np.ndarray + mimic: tuple[str, float, float] | None + + +def parse_vector( + value: str | None, size: int, default: tuple[float, ...] +) -> np.ndarray: + if value is None: + return np.asarray(default, dtype=np.float64) + parts = value.split() + if len(parts) != size: + raise ValueError(f"expected {size} values, got {value!r}") + result = np.asarray([float(part) for part in parts], dtype=np.float64) + if not np.isfinite(result).all(): + raise ValueError(f"values contain NaN or infinity: {value!r}") + return result + + +def rpy_rotation(rpy: np.ndarray) -> np.ndarray: + roll, pitch, yaw = rpy + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + return np.array( + [ + [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], + [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], + [-sp, cp * sr, cp * cr], + ], + dtype=np.float64, + ) + + +def origin_transform(origin: ET.Element | None) -> np.ndarray: + transform = np.eye(4) + if origin is None: + return transform + transform[:3, :3] = rpy_rotation( + parse_vector(origin.get("rpy"), 3, (0.0, 0.0, 0.0)) + ) + transform[:3, 3] = parse_vector( + origin.get("xyz"), 3, (0.0, 0.0, 0.0) + ) + return transform + + +def axis_angle_transform(axis: np.ndarray, angle: float) -> np.ndarray: + norm = float(np.linalg.norm(axis)) + if norm < 1e-12: + raise ValueError("joint axis must not be zero") + x, y, z = axis / norm + c, s = math.cos(angle), math.sin(angle) + one_minus_c = 1.0 - c + transform = np.eye(4) + transform[:3, :3] = np.array( + [ + [c + x * x * one_minus_c, x * y * one_minus_c - z * s, x * z * one_minus_c + y * s], + [y * x * one_minus_c + z * s, c + y * y * one_minus_c, y * z * one_minus_c - x * s], + [z * x * one_minus_c - y * s, z * y * one_minus_c + x * s, c + z * z * one_minus_c], + ], + dtype=np.float64, + ) + return transform + + +def translation_transform(offset: np.ndarray) -> np.ndarray: + transform = np.eye(4) + transform[:3, 3] = offset + return transform + + +def parse_joint_values(values: list[str]) -> dict[str, float]: + result: dict[str, float] = {} + for assignment in values: + name, separator, raw_value = assignment.partition("=") + if not separator or not name or not raw_value: + raise ValueError( + f"invalid --joint value {assignment!r}; expected NAME=VALUE" + ) + if name in result: + raise ValueError(f"joint value specified more than once: {name}") + value = float(raw_value) + if not math.isfinite(value): + raise ValueError(f"joint value must be finite: {assignment!r}") + result[name] = value + return result + + +def parse_joints(root: ET.Element) -> dict[str, Joint]: + joints: dict[str, Joint] = {} + children: set[str] = set() + for element in root.findall("joint"): + name = element.get("name") + kind = element.get("type") + parent_element = element.find("parent") + child_element = element.find("child") + if not name or not kind or parent_element is None or child_element is None: + raise ValueError("every joint needs name, type, parent, and child") + parent = parent_element.get("link") + child = child_element.get("link") + if not parent or not child: + raise ValueError(f"joint {name!r} has an empty parent or child") + if name in joints: + raise ValueError(f"duplicate joint name: {name}") + if child in children: + raise ValueError(f"link {child!r} has more than one parent joint") + axis_element = element.find("axis") + axis = parse_vector( + axis_element.get("xyz") if axis_element is not None else None, + 3, + (1.0, 0.0, 0.0), + ) + mimic_element = element.find("mimic") + mimic = None + if mimic_element is not None: + source = mimic_element.get("joint") + if not source: + raise ValueError(f"mimic joint {name!r} has no source joint") + mimic = ( + source, + float(mimic_element.get("multiplier", "1")), + float(mimic_element.get("offset", "0")), + ) + joints[name] = Joint( + name=name, + kind=kind, + parent=parent, + child=child, + origin=origin_transform(element.find("origin")), + axis=axis, + mimic=mimic, + ) + children.add(child) + return joints + + +def resolve_joint_values( + joints: dict[str, Joint], requested: dict[str, float] +) -> dict[str, float]: + unknown = set(requested) - set(joints) + if unknown: + raise ValueError(f"unknown joints: {sorted(unknown)}") + fixed = [name for name in requested if joints[name].kind == "fixed"] + if fixed: + raise ValueError(f"fixed joints cannot be assigned: {sorted(fixed)}") + + resolved: dict[str, float] = {} + + def resolve(name: str, stack: set[str]) -> float: + if name in resolved: + return resolved[name] + if name in stack: + raise ValueError(f"mimic joint cycle contains {name!r}") + joint = joints[name] + if name in requested: + value = requested[name] + elif joint.mimic is not None: + source, multiplier, offset = joint.mimic + if source not in joints: + raise ValueError( + f"mimic joint {name!r} references unknown joint {source!r}" + ) + value = multiplier * resolve(source, stack | {name}) + offset + else: + value = 0.0 + resolved[name] = value + return value + + for joint_name in joints: + resolve(joint_name, set()) + return resolved + + +def joint_motion(joint: Joint, value: float) -> np.ndarray: + if joint.kind == "fixed": + return np.eye(4) + if joint.kind in ("revolute", "continuous"): + return axis_angle_transform(joint.axis, value) + if joint.kind == "prismatic": + norm = float(np.linalg.norm(joint.axis)) + if norm < 1e-12: + raise ValueError(f"joint {joint.name!r} axis must not be zero") + axis = joint.axis / norm + return translation_transform(axis * value) + raise ValueError( + f"joint {joint.name!r} uses unsupported type {joint.kind!r}; " + "supported types are fixed, revolute, continuous, and prismatic" + ) + + +def compute_link_transforms( + root: ET.Element, joints: dict[str, Joint], values: dict[str, float] +) -> dict[str, np.ndarray]: + links = {element.get("name") for element in root.findall("link")} + if None in links: + raise ValueError("every link needs a name") + children = {joint.child for joint in joints.values()} + roots = links - children + if not roots: + raise ValueError("URDF has no root link") + + transforms = {name: np.eye(4) for name in roots} + pending = list(joints.values()) + while pending: + unresolved: list[Joint] = [] + for joint in pending: + if joint.parent not in links or joint.child not in links: + raise ValueError( + f"joint {joint.name!r} references a missing parent or child link" + ) + if joint.parent not in transforms: + unresolved.append(joint) + continue + transforms[joint.child] = ( + transforms[joint.parent] + @ joint.origin + @ joint_motion(joint, values[joint.name]) + ) + if len(unresolved) == len(pending): + names = [joint.name for joint in unresolved] + raise ValueError(f"joint graph is cyclic or disconnected: {names}") + pending = unresolved + return transforms + + +def resolve_mesh_path(filename: str, urdf_path: Path) -> Path: + if filename.startswith("file://"): + path = Path(filename.removeprefix("file://")) + elif filename.startswith("package://"): + package_path = Path(filename.removeprefix("package://")) + if len(package_path.parts) < 2: + raise ValueError(f"invalid package URI: {filename}") + package_name, relative_parts = package_path.parts[0], package_path.parts[1:] + candidates = [ + parent / package_name / Path(*relative_parts) + for parent in (urdf_path.parent, *urdf_path.parents) + ] + candidates.extend( + parent / Path(*relative_parts) + for parent in urdf_path.parents + if parent.name == package_name + ) + for candidate in candidates: + if candidate.is_file(): + return candidate.resolve() + raise FileNotFoundError( + f"could not resolve {filename!r} relative to {urdf_path}" + ) + else: + path = Path(filename) + if not path.is_absolute(): + path = urdf_path.parent / path + path = path.resolve() + if not path.is_file(): + raise FileNotFoundError(path) + return path + + +def load_mesh(path: Path) -> geometry.Geometry: + suffix = path.suffix.lower() + if suffix == ".stl": + return geometry.StlMeshGeometry.from_file(str(path)) + if suffix == ".obj": + return geometry.ObjMeshGeometry.from_file(str(path)) + if suffix == ".dae": + return geometry.DaeMeshGeometry.from_file(str(path)) + raise ValueError( + f"unsupported mesh format {path.suffix!r}: {path}; " + "MeshCat viewer supports STL, OBJ, and DAE" + ) + + +def cylinder_dimensions(cylinder: ET.Element) -> tuple[float, float]: + radius = float(cylinder.get("radius", "nan")) + length = float(cylinder.get("length", "nan")) + if ( + not math.isfinite(radius) + or not math.isfinite(length) + or radius <= 0.0 + or length <= 0.0 + ): + raise ValueError( + f"cylinder radius and length must be positive: {radius}, {length}" + ) + return radius, length + + +def cylinder_correction() -> np.ndarray: + correction = np.eye(4) + # Three.js cylinders use local Y; URDF cylinders use local Z. + correction[:3, :3] = rpy_rotation(np.array([math.pi / 2.0, 0.0, 0.0])) + return correction + + +def cylinder_wireframe( + radius: float, + length: float, + generator_count: int, + color: int, + opacity_value: float, + ring_segments: int = 64, +) -> geometry.LineSegments: + vertices: list[tuple[float, float, float]] = [] + half_length = length / 2.0 + + # Smooth top and bottom rings, without cap triangulation spokes. + for y in (-half_length, half_length): + for index in range(ring_segments): + first = 2.0 * math.pi * index / ring_segments + second = 2.0 * math.pi * (index + 1) / ring_segments + vertices.extend( + [ + (radius * math.cos(first), y, radius * math.sin(first)), + (radius * math.cos(second), y, radius * math.sin(second)), + ] + ) + + # Sparse axial generator lines; six means one line every 60 degrees. + for index in range(generator_count): + angle = 2.0 * math.pi * index / generator_count + x = radius * math.cos(angle) + z = radius * math.sin(angle) + vertices.extend([(x, -half_length, z), (x, half_length, z)]) + + points = np.asarray(vertices, dtype=np.float32).T + material = geometry.LineBasicMaterial( + color=color, + transparent=opacity_value < 1.0, + opacity=opacity_value, + ) + return geometry.LineSegments(geometry.PointsGeometry(points), material) + + +def geometry_object( + geometry_element: ET.Element, urdf_path: Path +) -> tuple[geometry.Geometry, np.ndarray]: + box = geometry_element.find("box") + sphere = geometry_element.find("sphere") + cylinder = geometry_element.find("cylinder") + mesh = geometry_element.find("mesh") + correction = np.eye(4) + + if box is not None: + size = parse_vector(box.get("size"), 3, ()) + if (size <= 0.0).any(): + raise ValueError(f"box size must be positive: {size}") + return geometry.Box(size), correction + if sphere is not None: + radius = float(sphere.get("radius", "nan")) + if not math.isfinite(radius) or radius <= 0.0: + raise ValueError(f"sphere radius must be positive: {radius}") + return geometry.Sphere(radius), correction + if cylinder is not None: + radius, length = cylinder_dimensions(cylinder) + return geometry.Cylinder(length, radius), cylinder_correction() + if mesh is not None: + filename = mesh.get("filename") + if not filename: + raise ValueError("mesh geometry has no filename") + scale = parse_vector(mesh.get("scale"), 3, (1.0, 1.0, 1.0)) + correction[:3, :3] = np.diag(scale) + return load_mesh(resolve_mesh_path(filename, urdf_path)), correction + raise ValueError("geometry must contain box, sphere, cylinder, or mesh") + + +def parse_rgba(value: str) -> tuple[float, float, float, float]: + rgba = parse_vector(value, 4, ()) + if ((rgba < 0.0) | (rgba > 1.0)).any(): + raise ValueError(f"RGBA values must be between 0 and 1: {value!r}") + return tuple(float(component) for component in rgba) + + +def rgb_integer(rgb: tuple[float, float, float]) -> int: + red, green, blue = (round(component * 255.0) for component in rgb) + return (red << 16) | (green << 8) | blue + + +def visual_rgba( + visual: ET.Element, + named_materials: dict[str, tuple[float, float, float, float]], +) -> tuple[float, float, float, float]: + material = visual.find("material") + if material is None: + return (0.65, 0.68, 0.72, 1.0) + color = material.find("color") + if color is not None and color.get("rgba"): + return parse_rgba(color.get("rgba", "")) + name = material.get("name") + if name and name in named_materials: + return named_materials[name] + return (0.65, 0.68, 0.72, 1.0) + + +def named_materials( + root: ET.Element, +) -> dict[str, tuple[float, float, float, float]]: + result: dict[str, tuple[float, float, float, float]] = {} + for material in root.findall("material"): + name = material.get("name") + color = material.find("color") + if name and color is not None and color.get("rgba"): + result[name] = parse_rgba(color.get("rgba", "")) + return result + + +def parse_color(value: str) -> int: + normalized = value.removeprefix("#").removeprefix("0x") + if len(normalized) != 6: + raise argparse.ArgumentTypeError("color must use RRGGBB format") + try: + result = int(normalized, 16) + except ValueError as error: + raise argparse.ArgumentTypeError("color must use RRGGBB format") from error + return result + + +def opacity(value: str) -> float: + result = float(value) + if not 0.0 <= result <= 1.0: + raise argparse.ArgumentTypeError("opacity must be between 0 and 1") + return result + + +def cylinder_lines(value: str) -> int: + result = int(value) + if result < 3: + raise argparse.ArgumentTypeError("cylinder line count must be at least 3") + return result + + +def safe_name(value: str) -> str: + return value.replace("/", "_") + + +def render_urdf( + viewer: meshcat.Visualizer, + root: ET.Element, + urdf_path: Path, + link_transforms: dict[str, np.ndarray], + *, + collision_only: bool, + visual_opacity: float, + collision_opacity: float, + collision_color: int, + wireframe: bool, + collision_cylinder_lines: int, +) -> tuple[int, int]: + viewer.delete() + materials = named_materials(root) + visual_count = 0 + collision_count = 0 + collision_material = geometry.MeshPhongMaterial( + color=collision_color, + transparent=collision_opacity < 1.0, + opacity=collision_opacity, + wireframe=wireframe, + ) + + for link in root.findall("link"): + link_name = link.get("name", "") + link_transform = link_transforms[link_name] + if not collision_only: + for index, visual in enumerate(link.findall("visual")): + geometry_element = visual.find("geometry") + if geometry_element is None: + raise ValueError(f"visual geometry missing on link {link_name!r}") + shape, correction = geometry_object(geometry_element, urdf_path) + rgba = visual_rgba(visual, materials) + alpha = visual_opacity * rgba[3] + material = geometry.MeshPhongMaterial( + color=rgb_integer(rgba[:3]), + transparent=alpha < 1.0, + opacity=alpha, + ) + node = viewer[ + f"robot/visual/{safe_name(link_name)}/visual_{index}" + ] + node.set_object(shape, material) + node.set_transform( + link_transform + @ origin_transform(visual.find("origin")) + @ correction + ) + visual_count += 1 + + for index, collision in enumerate(link.findall("collision")): + geometry_element = collision.find("geometry") + if geometry_element is None: + raise ValueError(f"collision geometry missing on link {link_name!r}") + cylinder = geometry_element.find("cylinder") + if wireframe and cylinder is not None: + radius, length = cylinder_dimensions(cylinder) + shape = cylinder_wireframe( + radius, + length, + collision_cylinder_lines, + collision_color, + collision_opacity, + ) + correction = cylinder_correction() + custom_line_object = True + else: + shape, correction = geometry_object(geometry_element, urdf_path) + custom_line_object = False + node = viewer[ + f"robot/collision/{safe_name(link_name)}/collision_{index}" + ] + if custom_line_object: + node.set_object(shape) + else: + node.set_object(shape, collision_material) + node.set_transform( + link_transform + @ origin_transform(collision.find("origin")) + @ correction + ) + collision_count += 1 + return visual_count, collision_count + + +def close_viewer(viewer: meshcat.Visualizer) -> None: + """Close MeshCat 0.3.x without relying on its broken Visualizer.close().""" + window = viewer.window + window.zmq_socket.close(linger=0) + server_process = window.server_proc + if server_process is None or server_process.poll() is not None: + return + server_process.terminate() + try: + server_process.wait(timeout=3.0) + except subprocess.TimeoutExpired: + server_process.kill() + server_process.wait(timeout=3.0) + + +def export_static_html(viewer: meshcat.Visualizer, output: Path) -> None: + output = output.resolve() + if output.suffix.lower() != ".html": + raise ValueError(f"MeshCat snapshot must use an .html extension: {output}") + output.parent.mkdir(parents=True, exist_ok=True) + temporary = output.with_name(f".{output.stem}.tmp{output.suffix}") + temporary.write_text(viewer.static_html(), encoding="utf-8") + os.replace(temporary, output) + + +def parse_args(argv: list[str]) -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--input", required=True, type=Path, help="URDF to display") + parser.add_argument( + "--joint", + action="append", + default=[], + metavar="NAME=VALUE", + help="joint position in radians, or meters for prismatic joints", + ) + parser.add_argument( + "--collision-only", + action="store_true", + help="hide visual geometry and display only collisions", + ) + parser.add_argument( + "--visual-opacity", + type=opacity, + default=1.0, + help="visual geometry opacity (default: 1.0)", + ) + parser.add_argument( + "--collision-opacity", + type=opacity, + default=1.0, + help="collision geometry opacity (default: 1.0)", + ) + parser.add_argument( + "--collision-color", + type=parse_color, + default=parse_color("00ff00"), + metavar="RRGGBB", + help="collision color in hexadecimal (default: 00ff00)", + ) + parser.add_argument( + "--collision-cylinder-lines", + "--collision-cylinder-segments", + dest="collision_cylinder_lines", + type=cylinder_lines, + default=12, + metavar="COUNT", + help="axial lines on collision cylinders (default: 12, every 30 degrees)", + ) + collision_style = parser.add_mutually_exclusive_group() + collision_style.add_argument( + "--wireframe", + dest="wireframe", + action="store_true", + default=True, + help="draw collision geometry as wireframe (default)", + ) + collision_style.add_argument( + "--solid-collisions", + dest="wireframe", + action="store_false", + help="draw collision geometry as translucent solids", + ) + parser.add_argument( + "--no-browser", action="store_true", help="do not automatically open a browser" + ) + parser.add_argument( + "--export-html", + type=Path, + help="write a standalone MeshCat HTML snapshot and exit", + ) + parser.add_argument( + "--exit-after-load", + action="store_true", + help=argparse.SUPPRESS, + ) + parser.add_argument( + "--zmq-url", help="connect to an existing MeshCat ZMQ server" + ) + return parser.parse_args(argv) + + +def run(args: argparse.Namespace) -> int: + urdf_path = args.input.resolve() + if not urdf_path.is_file(): + raise FileNotFoundError(urdf_path) + if urdf_path.suffix.lower() != ".urdf": + raise ValueError(f"input must be a .urdf file: {urdf_path}") + root = ET.parse(urdf_path).getroot() + if root.tag != "robot": + raise ValueError(f"URDF root must be , found <{root.tag}>") + + joints = parse_joints(root) + requested = parse_joint_values(args.joint) + values = resolve_joint_values(joints, requested) + link_transforms = compute_link_transforms(root, joints, values) + + viewer = meshcat.Visualizer(zmq_url=args.zmq_url) + visual_count, collision_count = render_urdf( + viewer, + root, + urdf_path, + link_transforms, + collision_only=args.collision_only, + visual_opacity=args.visual_opacity, + collision_opacity=args.collision_opacity, + collision_color=args.collision_color, + wireframe=args.wireframe, + collision_cylinder_lines=args.collision_cylinder_lines, + ) + if collision_count == 0: + raise RuntimeError(f"URDF contains no elements: {urdf_path}") + + url = viewer.url() + print(f"Loaded: {urdf_path}") + print(f"Visual geometry: {visual_count}") + print(f"Collision geometry: {collision_count}") + print(f"MeshCat URL: {url}", flush=True) + if args.export_html is not None: + export_static_html(viewer, args.export_html) + snapshot_uri = args.export_html.resolve().as_uri() + print(f"Standalone snapshot: {snapshot_uri}", flush=True) + if not args.no_browser: + webbrowser.open(snapshot_uri, new=2) + close_viewer(viewer) + return 0 + if not args.no_browser: + viewer.open() + if args.exit_after_load: + close_viewer(viewer) + return 0 + + print("Press Ctrl+C to stop the viewer.", flush=True) + try: + while True: + time.sleep(1.0) + except KeyboardInterrupt: + print("Stopping MeshCat viewer.") + finally: + close_viewer(viewer) + return 0 + + +def main(argv: list[str] | None = None) -> int: + try: + return run(parse_args(sys.argv[1:] if argv is None else argv)) + except Exception: + traceback.print_exc() + return 1 + + +if __name__ == "__main__": + raise SystemExit(main())