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 index cfe18169..5c5d75b4 100644 --- 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 @@ -20,12 +20,80 @@ const std::vector kRightArmJoints{ "R_WRIST_R", }; +const std::vector kGen2RightArmJoints{ + "right_arm_J1", + "right_arm_J2", + "right_arm_J3", + "right_arm_J4", + "right_arm_J5", + "right_arm_J6", + "right_arm_J7", +}; + +const std::vector kGen2SetupPose{ + 0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0, +}; + +const std::vector kGen2WarningPose{ + 2.45028525340088, + 0.413065394330014, + -1.78610031118294, + 2.3232081721811, + -2.96828882895788, + -1.59350098130002, + 0.582912411114367, +}; + +const std::vector kGen2StopPose{ + 2.13758633436379, + 1.61835160165575, + -2.3836142221041, + 0.964538527544213, + -0.00382525077004825, + 1.74586899135531, + -0.336868659266887, +}; + +const std::vector kGen2CollisionPose{ + -0.42656969579233, + 1.41426471041774, + -2.67949400419915, + 2.45814854129954, + -2.35907388079205, + 1.14125209449898, + 1.53232912981414, +}; + +const std::vector kGen2TorsoCollisionPose{ + 1.57607137794121, + 2.06613762981425, + -1.76915077905899, + 0.959251437141443, + -0.725973209527894, + 1.79390262120717, + 0.2223354372144, +}; + std::string collisionUrdfPath() { return std::string(CMVR_ES_SOURCE_DIR) + "/model/xiaoyan_description/dual_arm_collision.urdf"; } +std::string gen2CollisionUrdfPath() +{ + return std::string(CMVR_ES_SOURCE_DIR) + + "/model/gen2/collision/robot_collision.urdf"; +} + +SelfCollisionOptions gen2CollisionOptions() +{ + SelfCollisionOptions options; + options.ignored_pairs.push_back({"arm_link_5_2", "arm_link_7_2"}); + options.ignored_pairs.push_back({"body_link", "arm_link_2_2"}); + return options; +} + CollisionGeometrySnapshot singleObjectSnapshot(double x, double angle, double radius) @@ -82,6 +150,64 @@ TEST(SelfCollisionCheckerTest, RemovesConfiguredIgnoredPair) EXPECT_EQ(filtered.activePairCount() + 1, baseline.activePairCount()); } +TEST(SelfCollisionCheckerTest, LoadsGen2RightArmCollisionModel) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init( + gen2CollisionUrdfPath(), + kGen2RightArmJoints, + gen2CollisionOptions(), + &error)) << error; + EXPECT_EQ(checker.dof(), 7U); + EXPECT_EQ(checker.activePairCount(), 19U); + + CollisionGeometrySnapshot snapshot; + ASSERT_TRUE(checker.makeSnapshot(kGen2SetupPose, &snapshot, &error)) << error; + EXPECT_EQ(snapshot.objects.size(), 8U); + + const SelfCollisionResult setup_result = checker.check(snapshot); + ASSERT_TRUE(setup_result.valid) << setup_result.error; + EXPECT_FALSE(setup_result.in_collision); + EXPECT_GT(setup_result.minimum_distance_m, 0.02); +} + +TEST(SelfCollisionCheckerTest, ClassifiesGen2SafetyDistances) +{ + SelfCollisionChecker checker; + std::string error; + ASSERT_TRUE(checker.init( + gen2CollisionUrdfPath(), + kGen2RightArmJoints, + gen2CollisionOptions(), + &error)) << error; + + const SelfCollisionResult warning_result = checker.check(kGen2WarningPose); + ASSERT_TRUE(warning_result.valid) << warning_result.error; + EXPECT_FALSE(warning_result.in_collision); + EXPECT_GT(warning_result.minimum_distance_m, 0.005); + EXPECT_LE(warning_result.minimum_distance_m, 0.02); + + const SelfCollisionResult stop_result = checker.check(kGen2StopPose); + ASSERT_TRUE(stop_result.valid) << stop_result.error; + EXPECT_FALSE(stop_result.in_collision); + EXPECT_GT(stop_result.minimum_distance_m, 0.0); + EXPECT_LE(stop_result.minimum_distance_m, 0.005); + + const SelfCollisionResult collision_result = checker.check(kGen2CollisionPose); + ASSERT_TRUE(collision_result.valid) << collision_result.error; + EXPECT_TRUE(collision_result.in_collision); + EXPECT_LE(collision_result.minimum_distance_m, 0.0); + + const SelfCollisionResult torso_result = + checker.check(kGen2TorsoCollisionPose); + ASSERT_TRUE(torso_result.valid) << torso_result.error; + EXPECT_TRUE(torso_result.in_collision); + EXPECT_LE(torso_result.minimum_distance_m, 0.0); + EXPECT_TRUE(torso_result.first == "body_link" || + torso_result.second == "body_link"); +} + TEST(DistanceSamplingPolicyTest, SamplesByAccumulatedGeometryDisplacement) { DistanceSamplingPolicy policy; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt index eb26409f..33026787 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt @@ -14,3 +14,14 @@ target_link_libraries(arm_motion add_library(cmvr_es::arm_motion ALIAS arm_motion) add_library(cmvr_es::algorithms::arm_motion ALIAS arm_motion) install(TARGETS arm_motion LIBRARY DESTINATION lib) + +add_executable(toppra_joint_motion_planner_test + joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp +) + +target_link_libraries(toppra_joint_motion_planner_test + PRIVATE + cmvr_es::algorithms::arm_motion + gtest + gtest_main +) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h index ea500ea4..a2f77c51 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/joint_motion_planner.h @@ -1,18 +1,15 @@ #ifndef CMVR_ES_JOINT_MOTION_PLANNER_H #define CMVR_ES_JOINT_MOTION_PLANNER_H +#include +#include #include +#include "common/base/logging/logger.h" #include "common/types/arm/arm_types.h" namespace cmvr::device { -struct JointTrajectorySample { - double t{0.0}; - std::vector position; - std::vector velocity; -}; - class JointMotionPlanner { public: virtual ~JointMotionPlanner() = default; @@ -23,9 +20,137 @@ public: const JointPositionCommand& target, const MotionOptions& options, double speed_scaling, - std::vector& samples) = 0; + JointTrajectory& trajectory) = 0; + + virtual bool planReplay(const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + JointTrajectory& replay_trajectory) = 0; + + bool validateJointTrajectory(const JointTrajectory& trajectory, + std::size_t expected_dof, + const MotionOptions& limits) const; }; +inline bool JointMotionPlanner::validateJointTrajectory( + const JointTrajectory& trajectory, + const std::size_t expected_dof, + const MotionOptions& limits) const +{ + if (trajectory.size() < 2 || expected_dof == 0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory must contain at least " + "two points and have a non-zero DOF"; + return false; + } + if (!std::isfinite(limits.velocity) || limits.velocity <= 0.0 || + !std::isfinite(limits.acceleration) || limits.acceleration <= 0.0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity or acceleration limit is invalid"; + return false; + } + if (!limits.joint_velocity_limits.empty() && + limits.joint_velocity_limits.size() != expected_dof) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] joint velocity limit count does not match DOF"; + return false; + } + + constexpr double kVelocityTolerance = 1e-6; + constexpr double kAccelerationTolerance = 1e-3; + double maximum_velocity = 0.0; + double maximum_acceleration = 0.0; + double maximum_position_velocity = 0.0; + double maximum_position_acceleration = 0.0; + double maximum_jerk = 0.0; + std::vector previous_position_velocity(expected_dof, 0.0); + std::vector previous_acceleration(expected_dof, 0.0); + + for (std::size_t i = 0; i < trajectory.size(); ++i) { + const auto& point = trajectory[i]; + if (!std::isfinite(point.time_s) || + point.position.size() != expected_dof || + point.velocity.size() != expected_dof) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid trajectory point at index=" << i; + return false; + } + + double dt = 0.0; + if (i > 0) { + dt = point.time_s - trajectory[i - 1].time_s; + if (!std::isfinite(dt) || dt <= 0.0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] trajectory time is not increasing at index=" + << i; + return false; + } + } + + for (std::size_t joint = 0; joint < expected_dof; ++joint) { + if (!std::isfinite(point.position[joint]) || + !std::isfinite(point.velocity[joint])) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] non-finite trajectory value at point=" + << i << ", joint=" << joint; + return false; + } + + const double velocity = std::abs(point.velocity[joint]); + const double velocity_limit = limits.joint_velocity_limits.empty() + ? limits.velocity + : limits.joint_velocity_limits[joint]; + if (!std::isfinite(velocity_limit) || velocity_limit <= 0.0) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] invalid velocity limit for joint=" + << joint; + return false; + } + maximum_velocity = std::max(maximum_velocity, velocity); + if (velocity > velocity_limit + kVelocityTolerance) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] velocity limit exceeded at point=" + << i << ", joint=" << joint + << ", actual=" << velocity + << ", limit=" << velocity_limit; + return false; + } + + if (i > 0) { + const double position_velocity = + (point.position[joint] - trajectory[i - 1].position[joint]) / dt; + const double acceleration = + (point.velocity[joint] - trajectory[i - 1].velocity[joint]) / dt; + maximum_position_velocity = std::max( + maximum_position_velocity, std::abs(position_velocity)); + maximum_acceleration = std::max( + maximum_acceleration, std::abs(acceleration)); + if (std::abs(acceleration) > + limits.acceleration + kAccelerationTolerance) { + CMVR_LOG(ERROR) << "[JointMotionPlanner] acceleration limit exceeded at point=" + << i << ", joint=" << joint + << ", actual=" << std::abs(acceleration) + << ", limit=" << limits.acceleration; + return false; + } + if (i > 1) { + maximum_position_acceleration = std::max( + maximum_position_acceleration, + std::abs(position_velocity - + previous_position_velocity[joint]) / dt); + maximum_jerk = std::max( + maximum_jerk, + std::abs(acceleration - previous_acceleration[joint]) / dt); + } + previous_position_velocity[joint] = position_velocity; + previous_acceleration[joint] = acceleration; + } + } + } + + CMVR_LOG(INFO) << "[JointMotionPlanner] trajectory validated" + << ", points=" << trajectory.size() + << ", max_velocity_rad_s=" << maximum_velocity + << ", max_discrete_acceleration_rad_s2=" << maximum_acceleration + << ", max_position_velocity_rad_s=" << maximum_position_velocity + << ", max_position_acceleration_rad_s2=" + << maximum_position_acceleration + << ", max_discrete_jerk_rad_s3=" << maximum_jerk; + return true; +} + } // namespace cmvr::device #endif // CMVR_ES_JOINT_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h index 8c6db577..7aadd085 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h @@ -21,9 +21,19 @@ public: const JointPositionCommand& target, const MotionOptions& options, double speed_scaling, - std::vector& samples) override; + JointTrajectory& trajectory) override; + + bool planReplay(const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + JointTrajectory& replay_trajectory) override; private: + bool sampleTrajectory_( + const std::shared_ptr& planner, + const cmvr::TrajPtr& raw_trajectory, + JointTrajectory& trajectory) const; + std::shared_ptr planner_; cmvr::PathType path_type_{cmvr::PathType::Quintic}; double sample_period_s_{0.001}; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp index 8ee1e8e2..1fe882fa 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/src/toppra_joint_motion_planner.cpp @@ -1,6 +1,9 @@ #include "algorithms/motion_planner/arm_motion/joint_motion/toppra/include/toppra_joint_motion_planner.h" +#include + #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" +#include "common/base/logging/logger.h" namespace cmvr::device { @@ -39,36 +42,215 @@ bool ToppraJointMotionPlanner::init() return true; } +bool ToppraJointMotionPlanner::sampleTrajectory_( + const std::shared_ptr& planner, + const cmvr::TrajPtr& raw_trajectory, + JointTrajectory& trajectory) const +{ + const auto raw_samples = planner->sampleTrajectory( + raw_trajectory, sample_period_s_); + if (raw_samples.size() < 2) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] trajectory sampling returned fewer than " + "two points: count=" + << raw_samples.size(); + return false; + } + trajectory.clear(); + trajectory.reserve(raw_samples.size()); + for (std::size_t i = 0; i < raw_samples.size(); ++i) { + const auto& sample = raw_samples[i]; + if (!std::isfinite(sample.t) || !sample.q.allFinite() || + !sample.qd.allFinite()) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner] sampled trajectory contains " + "a non-finite value at point=" + << i; + trajectory.clear(); + return false; + } + JointTrajectoryPoint point; + point.time_s = sample.t; + point.position = toStdVector(sample.q); + point.velocity = toStdVector(sample.qd); + trajectory.push_back(std::move(point)); + } + return true; +} + bool ToppraJointMotionPlanner::planMoveJ(const std::vector& start, const JointPositionCommand& target, const MotionOptions& options, const double speed_scaling, - std::vector& samples) + JointTrajectory& trajectory) { - samples.clear(); + trajectory.clear(); if (!planner_ || start.empty() || start.size() != target.position.size() || options.velocity <= 0.0 || options.acceleration <= 0.0) { return false; } - cmvr::TrajPtr trajectory; + cmvr::TrajPtr raw_trajectory; planner_->setPathType(path_type_); planner_->setGridSizes(grid_size_, high_grid_size_); planner_->setSymmetricLimits( std::vector(start.size(), options.velocity * speed_scaling), std::vector(start.size(), options.acceleration)); - if (!planner_->plan(start, target.position, trajectory)) { + if (!planner_->plan(start, target.position, raw_trajectory)) { return false; } - const auto raw_samples = planner_->sampleTrajectory(trajectory, sample_period_s_); - samples.reserve(raw_samples.size()); - for (const auto& sample : raw_samples) { - JointTrajectorySample dst; - dst.t = sample.t; - dst.position = toStdVector(sample.q); - dst.velocity = toStdVector(sample.qd); - samples.push_back(std::move(dst)); + return sampleTrajectory_(planner_, raw_trajectory, trajectory); +} + +bool ToppraJointMotionPlanner::planReplay( + const std::vector& current_position, + const JointTrajectory& recorded_trajectory, + const MotionOptions& options, + JointTrajectory& replay_trajectory) +{ + replay_trajectory.clear(); + if (current_position.empty() || recorded_trajectory.size() < 2 || + options.velocity <= 0.0 || + options.acceleration <= 0.0 || !std::isfinite(options.velocity) || + !std::isfinite(options.acceleration)) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid trajectory or options"; + return false; + } + + const std::size_t dof = current_position.size(); + if (!options.joint_velocity_limits.empty() && + options.joint_velocity_limits.size() != dof) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid DOF or joint limits"; + return false; + } + for (const double position : current_position) { + if (!std::isfinite(position)) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite current position"; + return false; + } + } + for (std::size_t i = 0; i < recorded_trajectory.size(); ++i) { + const auto& point = recorded_trajectory[i]; + if (!std::isfinite(point.time_s) || point.position.size() != dof || + (i > 0 && point.time_s <= recorded_trajectory[i - 1].time_s)) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid recorded point: " + << i; + return false; + } + for (std::size_t joint = 0; joint < dof; ++joint) { + if (!std::isfinite(point.position[joint])) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] non-finite recorded point: " + << i; + return false; + } + } + } + + std::vector velocity_limits = options.joint_velocity_limits; + if (velocity_limits.empty()) { + velocity_limits.assign(dof, options.velocity); + } + for (std::size_t joint = 0; joint < velocity_limits.size(); ++joint) { + const double limit = velocity_limits[joint]; + if (!std::isfinite(limit) || limit <= 0.0) { + CMVR_LOG(ERROR) << "[ToppraJointMotionPlanner][planReplay] invalid joint velocity limit"; + return false; + } + } + + const double ramp_duration_s = std::max( + sample_period_s_, options.velocity / options.acceleration); + replay_trajectory.reserve(recorded_trajectory.size() + 2); + replay_trajectory.push_back(JointTrajectoryPoint{ + 0.0, current_position, std::vector(dof, 0.0)}); + + double replay_time_s = ramp_duration_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory.back().position, + std::vector(dof, 0.0)}); + for (std::size_t i = recorded_trajectory.size() - 1; i > 0; --i) { + replay_time_s += recorded_trajectory[i].time_s - + recorded_trajectory[i - 1].time_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory[i - 1].position, + std::vector(dof, 0.0)}); + } + replay_time_s += ramp_duration_s; + replay_trajectory.push_back(JointTrajectoryPoint{ + replay_time_s, + recorded_trajectory.front().position, + std::vector(dof, 0.0)}); + + const auto update_velocities = [&] { + for (auto& point : replay_trajectory) { + std::fill(point.velocity.begin(), point.velocity.end(), 0.0); + } + for (std::size_t i = 1; i + 1 < replay_trajectory.size(); ++i) { + const double dt = replay_trajectory[i + 1].time_s - + replay_trajectory[i - 1].time_s; + for (std::size_t joint = 0; joint < dof; ++joint) { + replay_trajectory[i].velocity[joint] = + (replay_trajectory[i + 1].position[joint] - + replay_trajectory[i - 1].position[joint]) / dt; + } + } + }; + + for (int iteration = 0; iteration < 3; ++iteration) { + update_velocities(); + double required_scale = 1.0; + std::vector previous_position_velocity(dof, 0.0); + for (std::size_t i = 0; i < replay_trajectory.size(); ++i) { + const auto& point = replay_trajectory[i]; + for (std::size_t joint = 0; joint < dof; ++joint) { + required_scale = std::max( + required_scale, + std::abs(point.velocity[joint]) / velocity_limits[joint]); + if (i == 0) { + continue; + } + + const double dt = point.time_s - + replay_trajectory[i - 1].time_s; + const double position_velocity = + (point.position[joint] - + replay_trajectory[i - 1].position[joint]) / dt; + const double acceleration = + (point.velocity[joint] - + replay_trajectory[i - 1].velocity[joint]) / dt; + required_scale = std::max( + required_scale, + std::abs(position_velocity) / velocity_limits[joint]); + required_scale = std::max( + required_scale, + std::sqrt(std::abs(acceleration) / + options.acceleration)); + if (i > 1) { + const double position_acceleration = + (position_velocity - + previous_position_velocity[joint]) / dt; + required_scale = std::max( + required_scale, + std::sqrt(std::abs(position_acceleration) / + options.acceleration)); + } + previous_position_velocity[joint] = position_velocity; + } + } + + if (required_scale <= 1.0 + 1e-9) { + break; + } + required_scale *= 1.001; + for (auto& point : replay_trajectory) { + point.time_s *= required_scale; + } + } + update_velocities(); + if (!validateJointTrajectory(replay_trajectory, dof, options)) { + replay_trajectory.clear(); + return false; } return true; } diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp new file mode 100644 index 00000000..40ad8414 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/joint_motion/toppra/test/toppra_joint_motion_planner_test.cpp @@ -0,0 +1,139 @@ +#include +#include +#include +#include +#include + +#include + +#include "joint_motion/toppra/include/toppra_joint_motion_planner.h" + +namespace cmvr::device { +namespace { + +constexpr std::size_t kDof = 7; + +JointTrajectory makeRecordedTrajectory(const std::size_t point_count) +{ + JointTrajectory trajectory; + trajectory.reserve(point_count); + for (std::size_t i = 0; i < point_count; ++i) { + const double s = static_cast(i) / + static_cast(point_count - 1); + JointTrajectoryPoint point; + point.time_s = static_cast(i) * 0.002; + point.position = { + 0.40 * s, + -0.25 * s + 0.03 * std::sin(3.141592653589793 * s), + 0.20 * s * s, + 0.30 * std::sin(1.5707963267948966 * s), + -0.12 * s, + 0.15 * s, + -0.08 * std::sin(3.141592653589793 * s), + }; + point.velocity.assign(kDof, 0.0); + trajectory.push_back(std::move(point)); + } + return trajectory; +} + +double maximumPositionError(const std::vector& lhs, + const std::vector& rhs) +{ + if (lhs.size() != rhs.size()) { + return std::numeric_limits::infinity(); + } + double maximum = 0.0; + for (std::size_t i = 0; i < lhs.size(); ++i) { + maximum = std::max(maximum, std::abs(lhs[i] - rhs[i])); + } + return maximum; +} + +TEST(ToppraJointMotionPlannerTest, PlansBoundedReverseReplay) +{ + ToppraJointMotionPlanner planner( + cmvr::PathType::Quintic, 0.001, 150, 300); + ASSERT_TRUE(planner.init()); + + const JointTrajectory recorded = makeRecordedTrajectory(300); + MotionOptions options; + options.velocity = 0.15; + options.acceleration = 5.0; + + JointTrajectory replay; + ASSERT_TRUE(planner.planReplay( + recorded.back().position, recorded, options, replay)); + ASSERT_EQ(replay.size(), recorded.size() + 2); + EXPECT_LT(maximumPositionError( + replay.front().position, recorded.back().position), + 1e-9); + EXPECT_LT(maximumPositionError( + replay.back().position, recorded.front().position), + 1e-9); + for (std::size_t i = 0; i < recorded.size(); ++i) { + EXPECT_LT(maximumPositionError( + replay[i + 1].position, + recorded[recorded.size() - 1 - i].position), + 1e-9); + } + + double maximum_velocity = 0.0; + double maximum_acceleration = 0.0; + for (std::size_t i = 0; i < replay.size(); ++i) { + ASSERT_EQ(replay[i].position.size(), kDof); + ASSERT_EQ(replay[i].velocity.size(), kDof); + for (std::size_t joint = 0; joint < kDof; ++joint) { + maximum_velocity = std::max( + maximum_velocity, std::abs(replay[i].velocity[joint])); + if (i > 0) { + const double dt = replay[i].time_s - replay[i - 1].time_s; + ASSERT_GT(dt, 0.0); + maximum_acceleration = std::max( + maximum_acceleration, + std::abs(replay[i].velocity[joint] - + replay[i - 1].velocity[joint]) / dt); + } + } + } + EXPECT_LE(maximum_velocity, options.velocity + 1e-6); + EXPECT_LE(maximum_acceleration, options.acceleration + 1e-3); +} + +TEST(ToppraJointMotionPlannerTest, RejectsNonIncreasingRecordedTime) +{ + ToppraJointMotionPlanner planner( + cmvr::PathType::Quintic, 0.001, 150, 300); + ASSERT_TRUE(planner.init()); + + JointTrajectory recorded = makeRecordedTrajectory(10); + recorded[5].time_s = recorded[4].time_s; + MotionOptions options; + options.velocity = 0.15; + options.acceleration = 5.0; + + JointTrajectory replay; + EXPECT_FALSE(planner.planReplay( + recorded.back().position, recorded, options, replay)); + EXPECT_TRUE(replay.empty()); +} + +TEST(ToppraJointMotionPlannerTest, ValidationRejectsInvalidOutputTrajectory) +{ + ToppraJointMotionPlanner planner( + cmvr::PathType::Quintic, 0.001, 150, 300); + MotionOptions options; + options.velocity = 0.15; + options.acceleration = 5.0; + + JointTrajectory trajectory = makeRecordedTrajectory(10); + trajectory[5].velocity[2] = options.velocity + 0.01; + EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options)); + + trajectory[5].velocity[2] = 0.0; + trajectory[5].time_s = trajectory[4].time_s; + EXPECT_FALSE(planner.validateJointTrajectory(trajectory, kDof, options)); +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt index bd84d4b9..62fb599c 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -20,4 +20,15 @@ target_link_libraries(base_motion PUBLIC ) add_library(cmvr_es::base_motion ALIAS base_motion) -install(TARGETS base_motion LIBRARY DESTINATION lib) \ No newline at end of file +install(TARGETS base_motion LIBRARY DESTINATION lib) + +add_executable(toppra_multi_waypoint_test + joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp +) + +target_link_libraries(toppra_multi_waypoint_test + PRIVATE + cmvr_es::base_motion + gtest + gtest_main +) diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h index 39865020..f344fc1e 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h @@ -109,37 +109,18 @@ namespace cmvr { static void sanitizeVsq(toppra::Vector &v); - // centripetal 弦长（alpha=0.5），生成严格递增 S + // Joint-space chord length keeps the parameterization independent of + // how densely the same geometric path is sampled. static std::vector - makeS_centripetal(const std::vector &q) { + makeSChordLength(const std::vector &q) { const size_t M = q.size(); std::vector S(M, 0.0); - auto chord = [](const Eigen::VectorXd &a, const Eigen::VectorXd &b) { - double d = (a - b).norm(); - return std::pow(std::max(d, 1e-16), 0.5); - }; for (size_t i = 1; i < M; ++i) { - S[i] = S[i - 1] + chord(q[i], q[i - 1]); - if (S[i] <= S[i - 1]) S[i] = S[i - 1] + 1e-12; + S[i] = S[i - 1] + (q[i] - q[i - 1]).norm(); } return S; } - // 等距参数（简单稳妥） - static inline std::vector makeS_equal(size_t M) { - std::vector S(M); - for (size_t i = 0; i < M; ++i) S[i] = static_cast(i); - return S; - } - - // 或：先用centripetal，再整体归一化到跨度≈(M-1)，并设置每段最小ds - static inline void normalize_and_floor_S(std::vector &S, double ds_min = 0.2) { - for (size_t i = 1; i < S.size(); ++i) S[i] -= S[0]; - double L = S.back(); - if (L > 0) for (auto &x: S) x *= (S.size() - 1) / L; - for (size_t i = 1; i < S.size(); ++i) if (S[i] - S[i - 1] < ds_min) S[i] = S[i - 1] + ds_min; - } - // Catmull–Rom（centripetal）估计结点几何速度 v（端点=0） static std::vector estimateVelsCatmull(const std::vector &q, @@ -159,14 +140,16 @@ namespace cmvr { // 对内点几何速度限幅，抑制过冲（k∈[0.5,1.0]） static void clampNodeVels(std::vector &v, const std::vector &q, + const std::vector &S, double k = 1.0) { const size_t M = q.size(); if (M <= 2) return; for (size_t i = 1; i + 1 < M; ++i) { - double d0 = (q[i] - q[i - 1]).norm(); - double d1 = (q[i + 1] - q[i]).norm(); - double d = std::max(std::min(d0, d1), 1e-12); - double vmax = k * d; + const double ds0 = std::max(S[i] - S[i - 1], 1e-12); + const double ds1 = std::max(S[i + 1] - S[i], 1e-12); + const double slope0 = (q[i] - q[i - 1]).norm() / ds0; + const double slope1 = (q[i + 1] - q[i]).norm() / ds1; + const double vmax = k * std::min(slope0, slope1); double n = v[i].norm(); if (n > vmax) v[i] *= (vmax / n); } diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp index e3637b8b..040b59e1 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner.cpp @@ -5,10 +5,117 @@ #include #include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" +#include +#include #include #include namespace cmvr { + namespace { + + class TimeScaledTrajectory final : public ITrajectory { + public: + TimeScaledTrajectory(TrajPtr source, const double scale) + : source_(std::move(source)), scale_(scale), source_interval_(source_->timeInterval()) + { + } + + toppra::Bound timeInterval() const override + { + toppra::Bound interval; + interval << source_interval_[0], + source_interval_[0] + + (source_interval_[1] - source_interval_[0]) * scale_; + return interval; + } + + Eigen::VectorXd q(const double t) const override + { + return source_->q(sourceTime_(t)); + } + + Eigen::VectorXd qd(const double t) const override + { + return source_->qd(sourceTime_(t)) / scale_; + } + + Eigen::VectorXd qdd(const double t) const override + { + return source_->qdd(sourceTime_(t)) / (scale_ * scale_); + } + + private: + double sourceTime_(const double output_time) const + { + return std::clamp( + source_interval_[0] + + (output_time - source_interval_[0]) / scale_, + source_interval_[0], + source_interval_[1]); + } + + TrajPtr source_; + double scale_{1.0}; + toppra::Bound source_interval_; + }; + + bool enforceSampledLimits(const TrajPtr& source, + const std::vector& velocity_limits, + const std::vector& acceleration_limits, + const std::size_t waypoint_count, + TrajPtr& output) + { + if (!source || velocity_limits.empty() || + velocity_limits.size() != acceleration_limits.size()) { + return false; + } + const auto interval = source->timeInterval(); + const double duration = interval[1] - interval[0]; + if (!std::isfinite(duration) || duration <= 0.0) { + return false; + } + + const std::size_t time_samples = static_cast( + std::ceil(duration / 0.001)) + 1; + const std::size_t path_samples = waypoint_count * 20; + const std::size_t sample_count = std::clamp( + std::max({std::size_t{1000}, time_samples, path_samples}), + std::size_t{1000}, + std::size_t{200000}); + + double required_scale = 1.0; + for (std::size_t sample = 0; sample < sample_count; ++sample) { + const double ratio = static_cast(sample) / + static_cast(sample_count - 1); + const double time = interval[0] + duration * ratio; + const Eigen::VectorXd velocity = source->qd(time); + const Eigen::VectorXd acceleration = source->qdd(time); + if (!velocity.allFinite() || !acceleration.allFinite() || + velocity.size() != static_cast(velocity_limits.size()) || + acceleration.size() != + static_cast(acceleration_limits.size())) { + return false; + } + for (Eigen::Index joint = 0; joint < velocity.size(); ++joint) { + const std::size_t index = static_cast(joint); + required_scale = std::max( + required_scale, + std::abs(velocity[joint]) / velocity_limits[index]); + required_scale = std::max( + required_scale, + std::sqrt(std::abs(acceleration[joint]) / + acceleration_limits[index])); + } + } + + constexpr double kNumericalMargin = 1.001; + output = std::make_shared( + source, required_scale * kNumericalMargin); + return true; + } + + } // namespace + // ===== ConstAccelTraj ===== ConstAccelTraj::ConstAccelTraj(std::shared_ptr p) : impl_(std::move(p)) { @@ -54,22 +161,39 @@ namespace cmvr { bool ToppraJointTrajectoryPlanner::plan(const std::vector>& waypoints, TrajPtr& traj_out) { traj_out.reset(); - const size_t M = waypoints.size(); - if (M < 2) return false; + if (waypoints.size() < 2) return false; const size_t DoF = waypoints.front().size(); - for (const auto& w : waypoints) if (w.size()!=DoF) return false; + if (DoF == 0) return false; + for (const auto& w : waypoints) { + if (w.size() != DoF) return false; + for (const double value : w) { + if (!std::isfinite(value)) return false; + } + } if (!ensureLimitsSized(DoF)) return false; + for (size_t joint = 0; joint < DoF; ++joint) { + if (!std::isfinite(v_max_[joint]) || v_max_[joint] <= 0.0 || + !std::isfinite(a_max_[joint]) || a_max_[joint] <= 0.0) { + return false; + } + } - // 组装 - std::vector q; q.reserve(M); - for (const auto& w : waypoints) - q.emplace_back(Eigen::Map(w.data(), DoF)); + std::vector q; + q.reserve(waypoints.size()); + constexpr double kDuplicateDistance = 1e-10; + for (const auto& waypoint : waypoints) { + Eigen::VectorXd value = Eigen::Map( + waypoint.data(), static_cast(DoF)); + if (q.empty() || (value - q.back()).norm() > kDuplicateDistance) { + q.push_back(std::move(value)); + } + } + if (q.size() < 2) return false; + const size_t M = q.size(); - // 生成 S -// std::vector S = (M==2) ? std::vector{0.0,1.0} -// : makeS_centripetal(q); - std::vector S = (M==2) ? std::vector{0.0,1.0} - : makeS_equal(M); + const std::vector S = M == 2 + ? std::vector{0.0, 1.0} + : makeSChordLength(q); // 几何路径 auto path = buildPathUnified(q, S); @@ -86,8 +210,23 @@ namespace cmvr { // TOPPRA toppra::algorithm::TOPPRA algo{constraints, path}; - auto solve_once = [&](int N)->bool{ - algo.setN(N); + auto solve_once = [&](const int requested_intervals)->bool{ + const int segment_count = static_cast(M - 1); + const int subdivisions = std::max( + 1, (requested_intervals + segment_count - 1) / segment_count); + toppra::Vector grid(segment_count * subdivisions + 1); + Eigen::Index index = 0; + for (int segment = 0; segment < segment_count; ++segment) { + const double start = S[static_cast(segment)]; + const double length = S[static_cast(segment + 1)] - start; + for (int subdivision = 0; subdivision < subdivisions; ++subdivision) { + grid[index++] = start + length * + static_cast(subdivision) / + static_cast(subdivisions); + } + } + grid[index] = S.back(); + algo.setGridpoints(grid); algo.solver(std::make_shared()); return algo.computePathParametrization(0.0, 0.0) == toppra::ReturnCode::OK; }; @@ -99,19 +238,20 @@ namespace cmvr { toppra::Vector grid = data.gridpoints; toppra::Vector vsq = data.parametrization; + TrajPtr candidate; auto ca = std::make_shared(path, grid, vsq); if (ca->validate()) { - traj_out = std::make_shared(std::move(ca)); - return true; - } - sanitizeVsq(vsq); - try { - traj_out = std::make_shared(path, grid, vsq); - (void) traj_out->timeInterval(); - return true; - } catch (...) { - return false; + candidate = std::make_shared(std::move(ca)); + } else { + sanitizeVsq(vsq); + try { + candidate = std::make_shared(path, grid, vsq); + (void) candidate->timeInterval(); + } catch (...) { + return false; + } } + return enforceSampledLimits(candidate, v_max_, a_max_, M, traj_out); } @@ -225,7 +365,7 @@ namespace cmvr { double ds = std::max(S[k+1]-S[k], 1e-12); toppra::Matrix seg(2, DoF); Eigen::RowVectorXd A1 = ((q[k+1]-q[k])/ds).transpose(); - Eigen::RowVectorXd A0 = (q[k] - A1.transpose()*S[k]).transpose(); + Eigen::RowVectorXd A0 = q[k].transpose(); seg.row(0)=A1; seg.row(1)=A0; segs.emplace_back(std::move(seg)); } @@ -237,7 +377,7 @@ namespace cmvr { ToppraJointTrajectoryPlanner::buildCubicHermiteMulti(const std::vector& q, const std::vector& S) { auto v = estimateVelsCatmull(q, S); - clampNodeVels(v, q, /*k=*/1.0); + clampNodeVels(v, q, S, /*k=*/1.0); toppra::Vectors pos(q.begin(), q.end()); toppra::Vectors vel(v.begin(), v.end()); auto herm = toppra::PiecewisePolyPath::CubicHermiteSpline(pos, vel, S); @@ -282,7 +422,7 @@ namespace cmvr { const std::vector& S) { const size_t M = q.size(), DoF = q[0].size(); auto v = estimateVelsCatmull(q, S); - clampNodeVels(v, q, /*k=*/1.0); + clampNodeVels(v, q, S, /*k=*/1.0); auto a = estimateAccelsSecondDiff(q, S); toppra::Matrices segs; segs.reserve(M-1); @@ -301,11 +441,11 @@ namespace cmvr { Eigen::VectorXd C5 = ( 6.0*dq - (3.0*A1 + 0.5*(a0*ds*ds)) - (3.0*(v1*ds) - 0.5*(a1*ds*ds)) ); toppra::Matrix seg(6, DoF); - seg.row(0)=C5.transpose(); - seg.row(1)=C4.transpose(); - seg.row(2)=C3.transpose(); - seg.row(3)=A2.transpose(); - seg.row(4)=A1.transpose(); + seg.row(0)=(C5 / std::pow(ds, 5)).transpose(); + seg.row(1)=(C4 / std::pow(ds, 4)).transpose(); + seg.row(2)=(C3 / std::pow(ds, 3)).transpose(); + seg.row(3)=(a0 / 2.0).transpose(); + seg.row(4)=v0.transpose(); seg.row(5)=A0.transpose(); segs.emplace_back(std::move(seg)); } @@ -384,4 +524,4 @@ namespace cmvr { } return true; } -} // namespace cmvr \ No newline at end of file +} // namespace cmvr diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp new file mode 100644 index 00000000..87c6f3cb --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/test/toppra_multi_waypoint_test.cpp @@ -0,0 +1,228 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" + +namespace cmvr { +namespace { + +constexpr std::size_t kDof = 7; +constexpr double kVelocityLimit = 0.15; +constexpr double kAccelerationLimit = 0.3; +constexpr double kSamplePeriodS = 0.002; + +std::vector> makeSmoothWaypoints(const std::size_t count) +{ + constexpr double kPi = 3.14159265358979323846; + std::vector> waypoints; + waypoints.reserve(count); + for (std::size_t i = 0; i < count; ++i) { + const double s = static_cast(i) / + static_cast(count - 1); + std::vector q(kDof, 0.0); + q[0] = 0.40 * s + 0.03 * std::sin(2.0 * kPi * s); + q[1] = -0.25 * s + 0.04 * std::sin(kPi * s); + q[2] = 0.20 * s * s; + q[3] = 0.30 * std::sin(0.5 * kPi * s); + q[4] = -0.12 * s + 0.02 * std::sin(3.0 * kPi * s); + q[5] = 0.15 * s; + q[6] = -0.08 * std::sin(kPi * s); + waypoints.push_back(std::move(q)); + } + return waypoints; +} + +double maxAbs(const Eigen::VectorXd& value) +{ + double result = 0.0; + for (Eigen::Index i = 0; i < value.size(); ++i) { + result = std::max(result, std::abs(value[i])); + } + return result; +} + +double positionError(const Eigen::VectorXd& actual, + const std::vector& expected) +{ + if (actual.size() != static_cast(expected.size())) { + return std::numeric_limits::infinity(); + } + double squared_error = 0.0; + for (Eigen::Index i = 0; i < actual.size(); ++i) { + const double error = actual[i] - expected[static_cast(i)]; + squared_error += error * error; + } + return std::sqrt(squared_error); +} + +struct PlanMetrics { + bool success{false}; + double planning_ms{0.0}; + double duration_s{0.0}; + double max_velocity{0.0}; + double max_acceleration{0.0}; + double max_waypoint_error{0.0}; + double start_error{0.0}; + double end_error{0.0}; + std::size_t sample_count{0}; +}; + +PlanMetrics planAndMeasure(const std::vector>& waypoints, + const PathType path_type = PathType::Linear) +{ + PlanMetrics metrics; + ToppraJointTrajectoryPlanner planner(path_type); + planner.setSymmetricLimits( + std::vector(kDof, kVelocityLimit), + std::vector(kDof, kAccelerationLimit)); + planner.setGridSizes(150, 300); + + TrajPtr trajectory; + const auto start = std::chrono::steady_clock::now(); + metrics.success = planner.plan(waypoints, trajectory); + metrics.planning_ms = std::chrono::duration( + std::chrono::steady_clock::now() - start).count(); + if (!metrics.success || !trajectory) { + return metrics; + } + + const auto interval = trajectory->timeInterval(); + metrics.duration_s = interval[1] - interval[0]; + const auto samples = planner.sampleTrajectory(trajectory, kSamplePeriodS); + metrics.sample_count = samples.size(); + if (samples.empty()) { + metrics.success = false; + return metrics; + } + metrics.start_error = positionError(samples.front().q, waypoints.front()); + metrics.end_error = positionError(samples.back().q, waypoints.back()); + + for (const auto& sample : samples) { + if (!std::isfinite(sample.t) || !sample.q.allFinite() || + !sample.qd.allFinite() || !sample.qdd.allFinite()) { + metrics.success = false; + return metrics; + } + metrics.max_velocity = std::max(metrics.max_velocity, maxAbs(sample.qd)); + metrics.max_acceleration = std::max( + metrics.max_acceleration, maxAbs(sample.qdd)); + } + + std::size_t sample_index = 0; + for (const auto& waypoint : waypoints) { + while (sample_index + 1 < samples.size() && + positionError(samples[sample_index + 1].q, waypoint) <= + positionError(samples[sample_index].q, waypoint)) { + ++sample_index; + } + metrics.max_waypoint_error = std::max( + metrics.max_waypoint_error, + positionError(samples[sample_index].q, waypoint)); + } + return metrics; +} + +const char* pathTypeName(const PathType path_type) +{ + switch (path_type) { + case PathType::Linear: return "Linear"; + case PathType::CubicHermite: return "CubicHermite"; + case PathType::Quintic: return "Quintic"; + case PathType::Natural: return "Natural"; + } + return "Unknown"; +} + +void printMetrics(const std::size_t waypoint_count, const PlanMetrics& metrics) +{ + std::cout << "[ToppraMultiWaypointTest] waypoints=" << waypoint_count + << ", success=" << metrics.success + << ", planning_ms=" << metrics.planning_ms + << ", duration_s=" << metrics.duration_s + << ", samples=" << metrics.sample_count + << ", max_qd=" << metrics.max_velocity + << ", max_qdd=" << metrics.max_acceleration + << ", max_waypoint_error=" << metrics.max_waypoint_error + << ", start_error=" << metrics.start_error + << ", end_error=" << metrics.end_error + << std::endl; +} + +TEST(ToppraMultiWaypointTest, SmoothSevenDofPathScalesToThousandsOfWaypoints) +{ + double reference_duration_s = 0.0; + for (const std::size_t count : {10U, 100U, 300U, 1000U, 3000U}) { + const auto metrics = planAndMeasure(makeSmoothWaypoints(count)); + printMetrics(count, metrics); + ASSERT_TRUE(metrics.success) << "waypoint_count=" << count; + EXPECT_GT(metrics.duration_s, 0.0) << "waypoint_count=" << count; + EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6) + << "waypoint_count=" << count; + EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5) + << "waypoint_count=" << count; + EXPECT_LT(metrics.max_waypoint_error, 0.002) + << "waypoint_count=" << count; + if (reference_duration_s == 0.0) { + reference_duration_s = metrics.duration_s; + } else { + EXPECT_NEAR(metrics.duration_s, reference_duration_s, + reference_duration_s * 0.10) + << "waypoint_count=" << count; + } + } +} + +TEST(ToppraMultiWaypointTest, RepeatedWaypointsRemainPlannable) +{ + const auto smooth = makeSmoothWaypoints(300); + std::vector> repeated; + repeated.reserve(smooth.size() * 2); + for (const auto& waypoint : smooth) { + repeated.push_back(waypoint); + repeated.push_back(waypoint); + } + + const auto metrics = planAndMeasure(repeated); + printMetrics(repeated.size(), metrics); + EXPECT_TRUE(metrics.success); + if (metrics.success) { + EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6); + EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5); + EXPECT_LT(metrics.max_waypoint_error, 0.002); + } +} + +TEST(ToppraMultiWaypointTest, CompareInterpolationModesAtThreeHundredWaypoints) +{ + const auto waypoints = makeSmoothWaypoints(300); + for (const auto path_type : { + PathType::CubicHermite, + PathType::Quintic, + PathType::Natural}) { + const auto metrics = planAndMeasure(waypoints, path_type); + std::cout << "[ToppraMultiWaypointTest] path_type=" + << pathTypeName(path_type) << std::endl; + printMetrics(waypoints.size(), metrics); + EXPECT_TRUE(metrics.success) << pathTypeName(path_type); + if (metrics.success) { + EXPECT_LE(metrics.max_velocity, kVelocityLimit + 1e-6) + << pathTypeName(path_type); + EXPECT_LE(metrics.max_acceleration, kAccelerationLimit + 1e-5) + << pathTypeName(path_type); + EXPECT_LT(metrics.max_waypoint_error, 0.002) + << pathTypeName(path_type); + EXPECT_LT(metrics.start_error, 1e-9) << pathTypeName(path_type); + EXPECT_LT(metrics.end_error, 1e-9) << pathTypeName(path_type); + } + } +} + +} // namespace +} // namespace cmvr diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 567b503c..4728236c 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -112,6 +112,14 @@ struct JointGroupState { } }; +struct JointTrajectoryPoint { + double time_s{0.0}; + std::vector position; + std::vector velocity; +}; + +using JointTrajectory = std::vector; + struct JointPositionCommand { std::vector position; diff --git a/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt new file mode 100644 index 00000000..f8c0a3b8 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt @@ -0,0 +1,151 @@ +arm { + robot_arms { + id: "mujoco_right_arm" + + motor { + motor_system_id: "mujoco_motors" + motor_group_ids: "mujoco_right_arm" + dof: 7 + joint_names: "right_arm_J1" + joint_names: "right_arm_J2" + joint_names: "right_arm_J3" + joint_names: "right_arm_J4" + joint_names: "right_arm_J5" + joint_names: "right_arm_J6" + joint_names: "right_arm_J7" + upd_freq: 1000 + buffer_size: 50 + default_vel: 0.6 + default_acc: 2.0 + } + + kinematics { + pinocchio_dls_ik_solver { + urdf_path: "model/gen2/robot.urdf" + base_frame_name: "body_link" + flange_frame_name: "arm_link_7_2" + max_iters: 200 + pos_eps: 1e-6 + rot_eps: 1e-6 + damping: 1e-5 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + } + soft_limit { + enable: true + margin_ratio: 0.01 + min_margin_rad: 0.01 + } + avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + weight: 2.0 + } + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.005 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 4.0 + max_joint_acceleration_rad_s2: 100.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 10.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 10.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + } + + speed_l { + pinocchio_cartesian_motion_planner { + linear_velocity_max: 0.5 + linear_acceleration_max: 2.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + enforce_joint_acceleration_limits: true + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.005 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 4.0 + max_joint_acceleration_rad_s2: 100.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 10.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 10.0 + min_desired_linear_speed: 0.01 + min_desired_angular_speed: 1e-4 + } + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + stop_acceleration: 2.0 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt new file mode 100644 index 00000000..8bd88329 --- /dev/null +++ b/cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt @@ -0,0 +1,35 @@ +motor { + id: "mujoco_motors" + + motor_groups { + id: "mujoco_right_arm" + bus_type: MOTOR_BUS_MUJOCO + vendor: MOTOR_VENDOR_MUJOCO + protocol: MOTOR_PROTOCOL_MUJOCO + mujoco { + world_id: "mujoco_world" + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 } + } + + motors { + motors { id: 1 joint_name: "right_arm_J1" } + motors { id: 2 joint_name: "right_arm_J2" } + motors { id: 3 joint_name: "right_arm_J3" } + motors { id: 4 joint_name: "right_arm_J4" } + motors { id: 5 joint_name: "right_arm_J5" } + motors { id: 6 joint_name: "right_arm_J6" } + motors { id: 7 joint_name: "right_arm_J7" } + } + } +} diff --git a/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt b/cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt index d6c5623f..db9fe0e8 100644 --- 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 @@ -21,4 +21,14 @@ self_collision_task { warning_distance_m: 0.02 stop_distance_m: 0.005 } + + + recovery { + clear_distance_m: 0.025 + stable_period_s: 0.1 + max_joint_velocity_rad_s: 0.15 + max_joint_acceleration_rad_s2: 0.3 + history_duration_s: 10.0 + max_distance_regression_m: 0.001 + } } diff --git a/cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt b/cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt new file mode 100644 index 00000000..49f5cfb3 --- /dev/null +++ b/cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt @@ -0,0 +1,37 @@ +self_collision_task { + id: "gen2_right_arm_self_collision" + arm_id: "mujoco_right_arm" + + checker { + urdf_path: "model/gen2/collision/robot_collision.urdf" + + # These second-neighbor mounting bodies overlap in normal assembled poses. + ignored_pairs { + first: "arm_link_5_2" + second: "arm_link_7_2" + } + ignored_pairs { + first: "body_link" + second: "arm_link_2_2" + } + } + + sampling { + max_geometry_displacement_m: 0.002 + max_check_period_s: 0.01 + } + + safety { + warning_distance_m: 0.02 + stop_distance_m: 0.005 + } + + recovery { + clear_distance_m: 0.05 + stable_period_s: 0.1 + max_joint_velocity_rad_s: 0.3 + max_joint_acceleration_rad_s2: 5.0 + history_duration_s: 10.0 + max_distance_regression_m: 0.001 + } +} diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 20049b6f..a4d4dd6f 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -36,6 +36,14 @@ public: Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; Result protectiveStop() override { return emergencyStop(); } + Result recoverProtectiveStop( + const JointTrajectory&, + const MotionOptions&) override + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "protective recovery is not implemented for AuboArm"); + } Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } bool isProtectiveStopped() const override { return false; } diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 92a3d1a3..5db631fa 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -42,6 +42,14 @@ public: Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; Result protectiveStop() override { return emergencyStop(); } + Result recoverProtectiveStop( + const JointTrajectory&, + const MotionOptions&) override + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "protective recovery is not implemented for HuayanRobot"); + } Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } bool isProtectiveStopped() const override; @@ -140,4 +148,4 @@ private: #endif // CMVR_ES_HUAYAN_ROBOT_H -#endif //CMVR_ES_HUAYAN_ARM_H \ No newline at end of file +#endif //CMVR_ES_HUAYAN_ARM_H diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt index c0b7d99b..a194fa73 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -34,3 +34,21 @@ target_link_libraries(motor_robot_arm_mujoco_test gtest_main pthread ) + +add_executable(motor_robot_arm_gen2_mujoco_test + src/motor_robot_arm_gen2_mujoco_test.cpp +) + +target_link_libraries(motor_robot_arm_gen2_mujoco_test + PRIVATE + cmvr_es::device::motor_robot_arm + cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver + cmvr_es::device_manager + cmvr_es::mujoco_viewer + cmvr_es::proto + cmvr_es::task + gtest + gtest_main + pthread +) diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 35471e29..73be9284 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -41,11 +41,14 @@ public: Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override { return emergencyStop(); } + Result protectiveStop() override; + Result recoverProtectiveStop( + const JointTrajectory& path, + const MotionOptions& options) override; Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } - bool isProtectiveStopped() const override { return false; } - bool isEmergencyStopped() const override { return emergency_stopped_; } + bool isProtectiveStopped() const override { return protective_stopped_.load(); } + bool isEmergencyStopped() const override { return emergency_stopped_.load(); } bool isFault() const override { return false; } Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; @@ -76,7 +79,7 @@ public: Result brakeRelease() override; Result shutdown() override; Result clearFault() override { return Result::success(); } - Result unlockProtectiveStop() override { return Result::success(); } + Result unlockProtectiveStop() override; Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -94,6 +97,10 @@ public: private: bool containsJoint_(const std::string& joint_name) const; + bool safetyStopRequested_() const; + std::optional safetyStopResult_(const std::string& command, + bool interrupted = false) const; + Result quickStopMotors_(); bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const; bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const; std::shared_ptr getMotor_(const std::string& joint_name) const; @@ -126,7 +133,10 @@ private: mutable std::mutex mutex_; std::atomic busy_{false}; double speed_scaling_{1.0}; - bool emergency_stopped_{false}; + std::atomic protective_stopped_{false}; + std::atomic emergency_stopped_{false}; + std::atomic protective_recovery_active_{false}; + std::atomic protective_recovery_cancel_requested_{false}; ServoOptions servo_options_; }; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 26f22d14..5acc3f39 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,11 @@ struct BusyGuard { ~BusyGuard() { busy.store(false); } }; +struct AtomicFlagGuard { + std::atomic& flag; + ~AtomicFlagGuard() { flag.store(false); } +}; + } // namespace MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) @@ -133,12 +139,15 @@ bool MotorRobotArm::stop() ArmState MotorRobotArm::getRobotState() const { + const bool protective_stopped = protective_stopped_.load(); + const bool emergency_stopped = emergency_stopped_.load(); ArmState state; state.connected = motor_manager_ != nullptr; state.powered_on = true; - state.brake_released = !emergency_stopped_; + state.brake_released = !emergency_stopped; state.moving = busy(); - state.emergency_stopped = emergency_stopped_; + state.protective_stopped = protective_stopped; + state.emergency_stopped = emergency_stopped; state.speed_scaling = speed_scaling_; state.robot_mode = RobotMode::Idle; state.safety_mode = getSafetyMode(); @@ -187,7 +196,13 @@ CartesianPose MotorRobotArm::getTcpPose(const FrameType frame) const SafetyMode MotorRobotArm::getSafetyMode() const { - return emergency_stopped_ ? SafetyMode::EmergencyStop : SafetyMode::Normal; + if (emergency_stopped_.load()) { + return SafetyMode::EmergencyStop; + } + if (protective_stopped_.load()) { + return SafetyMode::ProtectiveStop; + } + return SafetyMode::Normal; } Result MotorRobotArm::torqueOn() @@ -202,7 +217,7 @@ Result MotorRobotArm::torqueOn() "failed to torque on motor for joint: " + joint_name); } } - emergency_stopped_ = false; + emergency_stopped_.store(false); return Result::success(); } @@ -256,6 +271,194 @@ Result MotorRobotArm::emergencyStop() if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } + protective_recovery_cancel_requested_.store(true); + protective_stopped_.store(false); + emergency_stopped_.store(true); + return quickStopMotors_(); +} + +Result MotorRobotArm::protectiveStop() +{ + if (emergency_stopped_.load()) { + return Result::success(); + } + if (cartesian_velocity_controller_) { + cartesian_velocity_controller_->shutdown(); + } + protective_recovery_cancel_requested_.store(true); + protective_stopped_.store(true); + return quickStopMotors_(); +} + +Result MotorRobotArm::recoverProtectiveStop( + const JointTrajectory& path, + const MotionOptions& options) +{ + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "protective recovery rejected: arm is in emergency stop"); + } + if (!protective_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery rejected: arm is not protective stopped"); + } + if (path.size() < 2 || + !std::isfinite(options.velocity) || options.velocity <= 0.0 || + !std::isfinite(options.acceleration) || options.acceleration <= 0.0) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "protective recovery path or options are invalid"); + } + + for (std::size_t i = 0; i < path.size(); ++i) { + const auto& sample = path[i]; + if (!std::isfinite(sample.time_s) || + sample.position.size() != joint_names_.size() || + sample.velocity.size() != joint_names_.size() || + (i > 0 && sample.time_s <= path[i - 1].time_s)) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "protective recovery sample shape or time is invalid"); + } + for (std::size_t joint = 0; joint < sample.position.size(); ++joint) { + if (!std::isfinite(sample.position[joint]) || + !std::isfinite(sample.velocity[joint])) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "protective recovery sample contains a non-finite value"); + } + } + } + + if (busy_.exchange(true)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery rejected: arm is busy"); + } + BusyGuard busy_guard{busy_}; + + bool expected = false; + if (!protective_recovery_active_.compare_exchange_strong(expected, true)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery is already active"); + } + AtomicFlagGuard recovery_guard{protective_recovery_active_}; + protective_recovery_cancel_requested_.store(false); + + if (!joint_planner_) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "protective recovery planner is not initialized"); + } + + JointTrajectory recovery_trajectory; + const auto planning_start = std::chrono::steady_clock::now(); + if (!joint_planner_->planReplay( + readJointPosition_(), path, options, recovery_trajectory)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to plan protective recovery replay trajectory"); + } + const double planning_ms = std::chrono::duration( + std::chrono::steady_clock::now() - planning_start).count(); + CMVR_LOG(INFO) << "[MotorRobotArm] protective recovery planned" + << ", input_samples=" << path.size() + << ", command_samples=" << recovery_trajectory.size() + << ", planning_ms=" << planning_ms + << ", trajectory_duration_s=" + << recovery_trajectory.back().time_s; + + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "protective recovery interrupted by emergency stop during planning"); + } + if (protective_recovery_cancel_requested_.load()) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "protective recovery aborted by collision monitor during planning"); + } + + std::lock_guard lock(mutex_); + std::vector> motors; + motors.reserve(joint_names_.size()); + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_name); + } + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION && + !motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to set recovery position mode for joint: " + joint_name); + } + motors.push_back(std::move(motor)); + } + + const auto trajectory_start = std::chrono::steady_clock::now(); + for (std::size_t i = 0; i < recovery_trajectory.size(); ++i) { + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "protective recovery interrupted by emergency stop"); + } + if (protective_recovery_cancel_requested_.load()) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "protective recovery aborted by collision monitor"); + } + + const auto& sample = recovery_trajectory[i]; + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, sample.position, sample.velocity)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to submit protective recovery sample"); + } + + if (i + 1 < recovery_trajectory.size()) { + std::this_thread::sleep_until( + trajectory_start + + std::chrono::duration_cast( + std::chrono::duration( + recovery_trajectory[i + 1].time_s))); + } + } + + const std::vector zero_velocity(joint_names_.size(), 0.0); + if (!motor_manager_->commandCyclicPositionsAtomic( + motors, path.front().position, zero_velocity)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "failed to hold final protective recovery position"); + } + return Result::success(); +} + +Result MotorRobotArm::unlockProtectiveStop() +{ + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "cannot unlock protective stop while arm is emergency stopped"); + } + if (protective_recovery_active_.load()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "cannot unlock protective stop while recovery is active"); + } + protective_stopped_.store(false); + return Result::success(); +} + +Result MotorRobotArm::quickStopMotors_() +{ for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { @@ -266,10 +469,32 @@ Result MotorRobotArm::emergencyStop() "failed to quick stop motor for joint: " + joint_name); } } - emergency_stopped_ = true; return Result::success(); } +bool MotorRobotArm::safetyStopRequested_() const +{ + return emergency_stopped_.load() || protective_stopped_.load(); +} + +std::optional MotorRobotArm::safetyStopResult_( + const std::string& command, + const bool interrupted) const +{ + const char* action = interrupted ? " interrupted by " : " rejected: arm is in "; + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + command + action + "emergency stop"); + } + if (protective_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + command + action + "protective stop"); + } + return std::nullopt; +} + Result MotorRobotArm::setSpeedScaling(const double scaling) { if (scaling < 0.0 || scaling > 1.0) { @@ -281,6 +506,9 @@ Result MotorRobotArm::setSpeedScaling(const double scaling) Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options) { + if (const auto stopped = safetyStopResult_("moveJ")) { + return *stopped; + } std::string error; if (!validatePositionCommand_(target, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); @@ -294,7 +522,7 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti BusyGuard busy_guard{busy_}; std::lock_guard lock(mutex_); - std::vector samples; + JointTrajectory samples; if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) { return Result::failure(ArmErrorCode::CommandFailed, "[MotorRobotArm] moveJ planner failed: " + id_); } @@ -323,6 +551,9 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti constexpr double fallback_dt = 0.001; std::vector command_velocity(motors.size(), 0.0); for (std::size_t k = 1; k < samples.size(); ++k) { + if (const auto stopped = safetyStopResult_("moveJ", true)) { + return *stopped; + } const auto& sample = samples[k]; if (sample.position.size() != motors.size()) { return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch"); @@ -337,7 +568,8 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti "failed to submit atomic cyclic position command"); } if (k + 1 < samples.size()) { - const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t + const double next_t = samples[k + 1].time_s > 0.0 + ? samples[k + 1].time_s : static_cast(k + 1) * fallback_dt; std::this_thread::sleep_until(t0 + std::chrono::duration_cast( std::chrono::duration(next_t))); @@ -351,6 +583,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, const double duration) { (void)acceleration; + if (const auto stopped = safetyStopResult_("speedJ")) { + return *stopped; + } std::string error; if (!validateVelocityCommand_(velocity, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); @@ -388,6 +623,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, Result MotorRobotArm::stopJ(const double acceleration) { + if (safetyStopRequested_()) { + return Result::success(); + } JointVelocityCommand zero; zero.velocity.assign(joint_names_.size(), 0.0); return speedJ(zero, acceleration, 0.0); @@ -397,6 +635,9 @@ Result MotorRobotArm::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame) { + if (const auto stopped = safetyStopResult_("moveL")) { + return *stopped; + } if (cartesian_velocity_controller_) { cartesian_velocity_controller_->shutdown(); } @@ -438,8 +679,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target, << ", executable_path_m=" << trajectory.executable_path_length; } - return executeMoveLTrajectory_(trajectory) ? Result::success() - : Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); + if (executeMoveLTrajectory_(trajectory)) { + return Result::success(); + } + if (const auto stopped = safetyStopResult_("moveL", true)) { + return *stopped; + } + return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed"); } Result MotorRobotArm::speedL(const CartesianVelocity& velocity, @@ -447,6 +693,9 @@ Result MotorRobotArm::speedL(const CartesianVelocity& velocity, const double duration, const FrameType frame) { + if (const auto stopped = safetyStopResult_("speedL")) { + return *stopped; + } if (busy_.load()) { return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); } @@ -486,6 +735,9 @@ Result MotorRobotArm::startServoMode(const ServoOptions& options) Result MotorRobotArm::servoJ(const JointPositionCommand& target) { + if (const auto stopped = safetyStopResult_("servoJ")) { + return *stopped; + } std::string error; if (!validatePositionCommand_(target, error)) { return Result::failure(ArmErrorCode::InvalidArgument, error); @@ -790,6 +1042,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj auto next_deadline = std::chrono::steady_clock::now(); for (std::size_t i = 1; i < trajectory.position.size(); ++i) { + if (safetyStopRequested_()) { + return false; + } const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]); const auto& position = trajectory.position[i]; const auto& velocity = trajectory.velocity[i]; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp new file mode 100644 index 00000000..ea3fd7aa --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp @@ -0,0 +1,881 @@ +#include "arm/motor_robot_arm/include/motor_robot_arm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "common/io/proto_file_io.h" +#include "common/math/transform_math.h" +#include "manager/device_manager/include/device_manager.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" +#include "task/self_collision_task/include/self_collision_task.h" + +namespace cmvr::device { +namespace { + +constexpr std::size_t kDof = 7; +constexpr std::array kJointNames = { + "right_arm_J1", "right_arm_J2", "right_arm_J3", "right_arm_J4", + "right_arm_J5", "right_arm_J6", "right_arm_J7" +}; + +const std::vector kSetupPose{ + 0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0 +}; + +const std::vector kTorsoCollisionPose{ + 1.57607137794121, + 2.06613762981425, + -1.76915077905899, + 0.959251437141443, + -0.725973209527894, + 1.79390262120717, + 0.2223354372144, +}; + +std::filesystem::path findProjectRoot() +{ + const std::filesystem::path marker = "model/gen2/gen2_fixed.xml"; + const auto search = [&](std::filesystem::path current) { + while (!current.empty()) { + if (std::filesystem::exists(current / marker)) { + return current; + } + const auto parent = current.parent_path(); + if (parent == current) { + break; + } + current = parent; + } + return std::filesystem::path{}; + }; + + auto root = search(std::filesystem::current_path()); + if (!root.empty()) { + return root; + } + return search(std::filesystem::path(__FILE__).parent_path()); +} + +double maxPositionError(const std::vector& actual, + const std::vector& expected) +{ + if (actual.size() != expected.size()) { + return std::numeric_limits::infinity(); + } + double error = 0.0; + for (std::size_t i = 0; i < actual.size(); ++i) { + error = std::max(error, std::abs(actual[i] - expected[i])); + } + return error; +} + +double translationError(const CartesianPose& lhs, const CartesianPose& rhs) +{ + return std::sqrt(std::pow(lhs.x - rhs.x, 2.0) + + std::pow(lhs.y - rhs.y, 2.0) + + std::pow(lhs.z - rhs.z, 2.0)); +} + +double rotationError(const CartesianPose& lhs, const CartesianPose& rhs) +{ + const Eigen::Matrix3d lhs_rotation = + common::math::poseToMatrix(lhs).block<3, 3>(0, 0); + const Eigen::Matrix3d rhs_rotation = + common::math::poseToMatrix(rhs).block<3, 3>(0, 0); + return std::abs(Eigen::AngleAxisd(lhs_rotation.transpose() * rhs_rotation).angle()); +} + +Eigen::Vector3d baseRotationDelta(const CartesianPose& start, const CartesianPose& end) +{ + const Eigen::Matrix3d start_rotation = + common::math::poseToMatrix(start).block<3, 3>(0, 0); + const Eigen::Matrix3d end_rotation = + common::math::poseToMatrix(end).block<3, 3>(0, 0); + const Eigen::AngleAxisd delta(end_rotation * start_rotation.transpose()); + return delta.axis() * delta.angle(); +} + +template +void waitFor(Predicate predicate, const std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (!predicate() && std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +struct ScenarioOutcome { + Result move_j{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result move_l{Result::failure(ArmErrorCode::UnknownError, "not run")}; + double move_j_error{std::numeric_limits::infinity()}; + double move_l_error{std::numeric_limits::infinity()}; + double move_l_rotation_error{std::numeric_limits::infinity()}; + std::string worker_error; +}; + +class MotorRobotArmGen2MujocoTest : public ::testing::Test { +protected: + void SetUp() override + { + DeviceManager::destroyInstance(); + project_root_ = findProjectRoot(); + ASSERT_FALSE(project_root_.empty()); + + config::MujocoWorldRootConfig world_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(), + &world_root_config)); + ASSERT_GT(world_root_config.worlds_size(), 0); + + auto world_config = world_root_config.worlds(0); + world_config.set_model_path( + (project_root_ / "model/gen2/gen2_fixed.xml").string()); + world_device_ = std::make_shared(world_config); + ASSERT_TRUE(world_device_->init()); + ASSERT_TRUE(world_device_->start()); + + config::MotorRootConfig motor_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / + "cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt").string(), + &motor_root_config)); + + std::unordered_set right_arm_joints; + for (const auto* joint_name : kJointNames) { + right_arm_joints.insert(joint_name); + } + MotorManager::clearActiveJoints(); + MotorManager::setActiveJoints( + "mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}}); + + motor_system_ = std::make_shared( + "mujoco_motors", motor_root_config.motor()); + ASSERT_TRUE(motor_system_->init()); + world_ = MotorManager::mujocoWorldFor("mujoco_motors"); + ASSERT_TRUE(world_); + ASSERT_TRUE(world_->isLoaded()); + + config::ArmRootConfig root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / + "cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt").string(), + &root_config)); + ASSERT_GT(root_config.arm().robot_arms_size(), 0); + + auto arm_config = root_config.arm().robot_arms(0); + arm_config.mutable_kinematics() + ->mutable_pinocchio_dls_ik_solver() + ->set_urdf_path((project_root_ / "model/gen2/robot.urdf").string()); + + arm_ = std::make_shared(arm_config); + ASSERT_TRUE(arm_->init()); + const Result torque_result = arm_->torqueOn(); + ASSERT_TRUE(torque_result.ok()) << torque_result.message; + + config::DeviceManagerConfig device_manager_config; + device_manager_config.set_name("gen2_collision_mujoco_test"); + DeviceManager::getInstance(device_manager_config).registerDevice(arm_); + } + + void TearDown() override + { + if (arm_) { + arm_->stop(); + } + if (motor_system_) { + motor_system_->stop(); + } + if (world_device_) { + world_device_->stop(); + } + DeviceManager::destroyInstance(); + MotorManager::clearActiveJoints(); + } + + std::filesystem::path project_root_; + std::shared_ptr world_device_; + std::shared_ptr motor_system_; + std::shared_ptr world_; + std::shared_ptr arm_; +}; + +TEST_F(MotorRobotArmGen2MujocoTest, HoldsInitialPosition) +{ + const auto start = arm_->getJointState().position; + ASSERT_EQ(start.size(), kDof); + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + const auto end = arm_->getJointState().position; + const double drift = maxPositionError(end, start); + + std::cout << "[MotorRobotArmGen2MujocoTest] hold max drift: " + << drift << std::endl; + EXPECT_LT(drift, 0.02); +} + +TEST_F(MotorRobotArmGen2MujocoTest, ProtectiveAndEmergencyStopAreDistinct) +{ + ASSERT_TRUE(arm_->protectiveStop().ok()); + EXPECT_TRUE(arm_->isProtectiveStopped()); + EXPECT_FALSE(arm_->isEmergencyStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::ProtectiveStop); + const auto protective_state = arm_->getRobotState(); + EXPECT_TRUE(protective_state.protective_stopped); + EXPECT_FALSE(protective_state.emergency_stopped); + + MotionOptions options; + options.velocity = 0.6; + options.acceleration = 2.0; + const Result protected_move = arm_->moveJ( + JointPositionCommand{kSetupPose}, options); + EXPECT_EQ(protected_move.code, ArmErrorCode::RobotInProtectiveStop); + + ASSERT_TRUE(arm_->unlockProtectiveStop().ok()); + EXPECT_FALSE(arm_->isProtectiveStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal); + + ASSERT_TRUE(arm_->emergencyStop().ok()); + EXPECT_FALSE(arm_->isProtectiveStopped()); + EXPECT_TRUE(arm_->isEmergencyStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::EmergencyStop); + const auto emergency_state = arm_->getRobotState(); + EXPECT_FALSE(emergency_state.protective_stopped); + EXPECT_TRUE(emergency_state.emergency_stopped); + + const Result rejected_unlock = arm_->unlockProtectiveStop(); + EXPECT_EQ(rejected_unlock.code, ArmErrorCode::RobotInEmergencyStop); + ASSERT_TRUE(arm_->torqueOn().ok()); + EXPECT_FALSE(arm_->isEmergencyStopped()); + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal); +} + +TEST_F(MotorRobotArmGen2MujocoTest, MoveJ) +{ + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + ScenarioOutcome outcome; + + std::thread scenario([&] { + try { + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + MotionOptions options; + options.velocity = 0.6; + options.acceleration = 2.0; + + outcome.move_j = arm_->moveJ(JointPositionCommand{kSetupPose}, options); + waitFor([&] { + return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + outcome.move_j_error = maxPositionError( + arm_->getJointState().position, kSetupPose); + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(2)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + std::cout << "[MotorRobotArmGen2MujocoTest] moveJ max error: " + << outcome.move_j_error << std::endl; + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message; + EXPECT_LT(outcome.move_j_error, 0.08); +} + +TEST_F(MotorRobotArmGen2MujocoTest, MoveL) +{ + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + ScenarioOutcome outcome; + + std::thread scenario([&] { + try { + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + MotionOptions joint_options; + joint_options.velocity = 1.6; + joint_options.acceleration = 12.0; + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + waitFor([&] { + return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + outcome.move_j_error = maxPositionError( + arm_->getJointState().position, kSetupPose); + if (!outcome.move_j.ok()) { + throw std::runtime_error(outcome.move_j.message); + } + + MotionOptions cartesian_options; + cartesian_options.velocity = 0.08; + cartesian_options.acceleration = 0.4; + cartesian_options.jerk = 1.0; + + MotionOptions rotation_options; + rotation_options.velocity = 0.15; + rotation_options.acceleration = 0.5; + rotation_options.jerk = 2.0; + + const auto return_to_setup = [&](const char* step_name) { + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + if (!outcome.move_j.ok()) { + throw std::runtime_error( + std::string("moveJ before moveL ") + step_name + + ": " + outcome.move_j.message); + } + waitFor([&] { + return maxPositionError( + arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + }; + + struct CartesianStep { + const char* name; + double dx; + double dy; + double dz; + }; + const std::array translation_steps{{ + {"+X", 0.15, 0.0, 0.0}, + {"+Y", 0.0, 0.15, 0.0}, + {"+Z", 0.0, 0.0, 0.15}, + }}; + + struct RotationStep { + const char* name; + double drx; + double dry; + double drz; + }; + constexpr double kRotationStep = + 20.0 * 3.14159265358979323846 / 180.0; + const std::array rotation_steps{{ + {"+RX", kRotationStep, 0.0, 0.0}, + {"+RY", 0.0, kRotationStep, 0.0}, + {"+RZ", 0.0, 0.0, kRotationStep}, + }}; + + outcome.move_l_error = 0.0; + for (std::size_t i = 0; i < translation_steps.size(); ++i) { + const auto& step = translation_steps[i]; + if (i > 0) { + return_to_setup(step.name); + } + CartesianPose target = arm_->getTcpPose(); + target.x += step.dx; + target.y += step.dy; + target.z += step.dz; + + outcome.move_l = arm_->moveL( + target, cartesian_options, FrameType::Base); + if (!outcome.move_l.ok()) { + throw std::runtime_error( + std::string("moveL ") + step.name + ": " + + outcome.move_l.message); + } + waitFor([&] { + return translationError(arm_->getTcpPose(), target) < 0.005; + }, std::chrono::seconds(5)); + const double error = translationError(arm_->getTcpPose(), target); + outcome.move_l_error = std::max(outcome.move_l_error, error); + std::cout << "[MotorRobotArmGen2MujocoTest] moveL " + << step.name << " translation error: " << error + << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + outcome.move_l_rotation_error = 0.0; + for (const auto& step : rotation_steps) { + return_to_setup(step.name); + CartesianPose target = arm_->getTcpPose(); + target.rx += step.drx; + target.ry += step.dry; + target.rz += step.drz; + + outcome.move_l = arm_->moveL( + target, rotation_options, FrameType::Base); + if (!outcome.move_l.ok()) { + throw std::runtime_error( + std::string("moveL ") + step.name + ": " + + outcome.move_l.message); + } + waitFor([&] { + return rotationError(arm_->getTcpPose(), target) < 0.01; + }, std::chrono::seconds(7)); + const double error = rotationError(arm_->getTcpPose(), target); + outcome.move_l_rotation_error = std::max( + outcome.move_l_rotation_error, error); + std::cout << "[MotorRobotArmGen2MujocoTest] moveL " + << step.name << " rotation error: " << error + << " rad" << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(3)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + std::cout << "[MotorRobotArmGen2MujocoTest] moveJ setup max error: " + << outcome.move_j_error + << ", moveL translation error: " << outcome.move_l_error + << ", moveL rotation error: " + << outcome.move_l_rotation_error << " rad" << std::endl; + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message; + EXPECT_LT(outcome.move_j_error, 0.08); + EXPECT_TRUE(outcome.move_l.ok()) << outcome.move_l.message; + EXPECT_LT(outcome.move_l_error, 0.01); + EXPECT_LT(outcome.move_l_rotation_error, 0.02); +} + +TEST_F(MotorRobotArmGen2MujocoTest, SelfCollisionProtectiveStopMoveJMoveLSpeedL) +{ + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + + struct CollisionCaseOutcome { + std::string name; + Result setup_move{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result collision_move{Result::failure(ArmErrorCode::UnknownError, "not run")}; + Result recovery{Result::failure(ArmErrorCode::UnknownError, "not run")}; + task::SelfCollisionTaskStatus initial_status; + task::SelfCollisionTaskStatus stop_status; + task::SelfCollisionTaskStatus recovered_status; + CartesianPose start_tcp; + CartesianPose final_tcp; + std::vector final_position; + bool task_initialized{false}; + bool task_started{false}; + bool stop_seen{false}; + bool recovery_succeeded{false}; + bool monitor_ok{true}; + bool protective_stopped{false}; + bool emergency_stopped{false}; + SafetyMode safety_mode{SafetyMode::Unknown}; + }; + + CollisionCaseOutcome move_j_outcome; + CollisionCaseOutcome move_l_outcome; + CollisionCaseOutcome speed_l_outcome; + CartesianPose move_l_target; + CartesianPose collision_tcp_target; + std::string worker_error; + + std::thread scenario([&] { + try { + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + config::SelfCollisionTaskRootConfig root_config; + const auto config_path = project_root_ / + "cmvr-es/config/tasks/self_collision_task/" + "self_collision_task_gen2.pb.txt"; + if (!ProtoMessageIo::getProtoFromAsciiFile( + config_path.string(), &root_config)) { + throw std::runtime_error( + "failed to load self-collision config: " + config_path.string()); + } + auto collision_config = root_config.self_collision_task(); + collision_config.mutable_checker()->set_urdf_path( + (project_root_ / + "model/gen2/collision/robot_collision.urdf").string()); + + Eigen::Matrix4d collision_tcp_transform = Eigen::Matrix4d::Identity(); + const auto solver = arm_->kinematicsSolver(); + if (!solver || + !solver->fk(kTorsoCollisionPose, collision_tcp_transform, true)) { + throw std::runtime_error("failed to calculate collision TCP target"); + } + collision_tcp_target = + common::math::matrixToPose(collision_tcp_transform); + + const auto run_collision_case = [&]( + const std::string& name, + const std::function& start_motion, + const std::chrono::milliseconds stop_timeout) { + CollisionCaseOutcome outcome; + outcome.name = name; + + const Result torque_result = arm_->torqueOn(); + if (!torque_result.ok()) { + throw std::runtime_error( + name + " torqueOn: " + torque_result.message); + } + + MotionOptions setup_options; + setup_options.velocity = 0.6; + setup_options.acceleration = 2.0; + outcome.setup_move = arm_->moveJ( + JointPositionCommand{kSetupPose}, setup_options); + if (!outcome.setup_move.ok()) { + throw std::runtime_error( + name + " setup moveJ: " + outcome.setup_move.message); + } + + task::SelfCollisionTask collision_task(collision_config); + outcome.task_initialized = collision_task.init(); + if (!outcome.task_initialized) { + throw std::runtime_error( + name + " init: " + collision_task.detailStatusString()); + } + outcome.task_started = collision_task.start(); + if (!outcome.task_started || !collision_task.step(0.002)) { + throw std::runtime_error( + name + " start: " + collision_task.detailStatusString()); + } + outcome.initial_status = collision_task.latestStatus(); + if (outcome.initial_status.level != task::CollisionSafetyLevel::SAFE) { + throw std::runtime_error( + name + " setup pose is not SAFE: " + + collision_task.detailStatusString()); + } + outcome.start_tcp = arm_->getTcpPose(); + + std::atomic_bool monitor_running{true}; + std::atomic_bool monitor_ok{true}; + std::atomic_bool stop_seen{false}; + std::thread monitor([&] { + while (monitor_running.load()) { + if (!collision_task.step(0.002)) { + monitor_ok = false; + break; + } + const auto status = collision_task.latestStatus(); + if (status.stop_latched && !stop_seen.load()) { + outcome.stop_status = status; + stop_seen.store(true); + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + }); + + outcome.collision_move = start_motion(); + waitFor([&] { + return stop_seen.load() || !monitor_ok.load(); + }, stop_timeout); + if (!stop_seen.load()) { + arm_->stopMotion(); + monitor_running = false; + monitor.join(); + collision_task.stop(); + throw std::runtime_error(name + " did not trigger protective stop"); + } + + outcome.recovery = collision_task.requestRecovery( + outcome.stop_status.event_id); + outcome.recovered_status = collision_task.latestStatus(); + outcome.recovery_succeeded = outcome.recovery.ok(); + + monitor_running = false; + monitor.join(); + collision_task.stop(); + outcome.stop_seen = stop_seen.load(); + outcome.monitor_ok = monitor_ok.load(); + outcome.final_position = arm_->getJointState().position; + outcome.final_tcp = arm_->getTcpPose(); + outcome.protective_stopped = arm_->isProtectiveStopped(); + outcome.emergency_stopped = arm_->isEmergencyStopped(); + outcome.safety_mode = arm_->getSafetyMode(); + + std::cout << "[MotorRobotArmGen2MujocoTest] collision " + << name + << " stop_seen=" << outcome.stop_seen + << ", distance_m=" + << outcome.stop_status.result.minimum_distance_m + << ", pair=" << outcome.stop_status.result.first + << "/" << outcome.stop_status.result.second + << ", event_id=" << outcome.stop_status.event_id + << ", recovery=" << outcome.recovery.message + << ", recovered_distance_m=" + << outcome.recovered_status.result.minimum_distance_m + << std::endl; + return outcome; + }; + + move_j_outcome = run_collision_case( + "MoveJ", + [&] { + MotionOptions options; + options.velocity = 0.45; + options.acceleration = 1.0; + return arm_->moveJ( + JointPositionCommand{kTorsoCollisionPose}, options); + }, + std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + move_l_outcome = run_collision_case( + "MoveL", + [&] { + move_l_target = collision_tcp_target; + MotionOptions options; + options.velocity = 0.12; + options.acceleration = 0.5; + options.jerk = 2.0; + return arm_->moveL( + move_l_target, options, FrameType::Base); + }, + std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::seconds(1)); + + speed_l_outcome = run_collision_case( + "SpeedL", + [&] { + const CartesianPose start = arm_->getTcpPose(); + Eigen::Vector3d linear_direction{ + collision_tcp_target.x - start.x, + collision_tcp_target.y - start.y, + collision_tcp_target.z - start.z, + }; + Eigen::Vector3d angular_direction = + baseRotationDelta(start, collision_tcp_target); + const double command_duration_s = std::max( + linear_direction.norm() / 0.05, + angular_direction.norm() / 0.20); + if (command_duration_s <= 0.0) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "SpeedL collision target has zero displacement"); + } + linear_direction /= command_duration_s; + angular_direction /= command_duration_s; + std::cout + << "[MotorRobotArmGen2MujocoTest] collision SpeedL target_time=" + << command_duration_s << " s" << std::endl; + return arm_->speedL( + CartesianVelocity{ + linear_direction.x(), + linear_direction.y(), + linear_direction.z(), + angular_direction.x(), + angular_direction.y(), + angular_direction.z(), + }, + 0.5, + 0.0, + FrameType::Base); + }, + std::chrono::seconds(15)); + } catch (const std::exception& error) { + worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(3)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + EXPECT_TRUE(worker_error.empty()) << worker_error; + const auto expect_protective_stop = [&](const CollisionCaseOutcome& outcome) { + EXPECT_TRUE(outcome.setup_move.ok()) + << outcome.name << ": " << outcome.setup_move.message; + EXPECT_TRUE(outcome.task_initialized) << outcome.name; + EXPECT_TRUE(outcome.task_started) << outcome.name; + EXPECT_EQ(outcome.initial_status.level, task::CollisionSafetyLevel::SAFE) + << outcome.name; + EXPECT_TRUE(outcome.monitor_ok) << outcome.name; + EXPECT_TRUE(outcome.stop_seen) << outcome.name; + EXPECT_EQ(outcome.stop_status.level, task::CollisionSafetyLevel::STOP) + << outcome.name; + EXPECT_TRUE(outcome.stop_status.stop_latched) << outcome.name; + EXPECT_NE(outcome.stop_status.event_id, 0U) << outcome.name; + EXPECT_EQ(outcome.stop_status.recovery_state, + task::ProtectiveRecoveryState::AVAILABLE) + << outcome.name; + EXPECT_GE(outcome.stop_status.recovery_sample_count, 2U) + << outcome.name; + EXPECT_LE(outcome.stop_status.result.minimum_distance_m, 0.005) + << outcome.name; + EXPECT_TRUE(outcome.stop_status.result.first == "body_link" || + outcome.stop_status.result.second == "body_link") + << outcome.name; + EXPECT_TRUE(outcome.recovery_succeeded) + << outcome.name << ": " << outcome.recovery.message; + EXPECT_FALSE(outcome.recovered_status.stop_latched) << outcome.name; + EXPECT_EQ(outcome.recovered_status.recovery_state, + task::ProtectiveRecoveryState::SUCCEEDED) + << outcome.name; + EXPECT_GE(outcome.recovered_status.result.minimum_distance_m, 0.025) + << outcome.name; + EXPECT_FALSE(outcome.protective_stopped) << outcome.name; + EXPECT_FALSE(outcome.emergency_stopped) << outcome.name; + EXPECT_EQ(outcome.safety_mode, SafetyMode::Normal) + << outcome.name; + }; + expect_protective_stop(move_j_outcome); + expect_protective_stop(move_l_outcome); + expect_protective_stop(speed_l_outcome); + + EXPECT_EQ(move_j_outcome.collision_move.code, + ArmErrorCode::RobotInProtectiveStop); + EXPECT_GT(maxPositionError(move_j_outcome.final_position, kTorsoCollisionPose), 0.02); + EXPECT_EQ(move_l_outcome.collision_move.code, + ArmErrorCode::RobotInProtectiveStop); + EXPECT_GT(translationError(move_l_outcome.final_tcp, move_l_target), 0.01); + EXPECT_TRUE(speed_l_outcome.collision_move.ok()) + << speed_l_outcome.collision_move.message; + EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal); +} + +TEST_F(MotorRobotArmGen2MujocoTest, SpeedL) +{ + constexpr auto kCommandDuration = std::chrono::seconds(2); + MuJocoViewer viewer(world_); + viewer.setupCamera(2.5, -160.0, -20.0); + ScenarioOutcome outcome; + std::array measured_deltas{}; + + std::thread scenario([&] { + try { + if (!world_ || !world_->isRunning()) { + throw std::runtime_error("MuJoCo world is not running"); + } + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + MotionOptions joint_options; + joint_options.velocity = 0.6; + joint_options.acceleration = 2.0; + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + waitFor([&] { + return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + outcome.move_j_error = maxPositionError( + arm_->getJointState().position, kSetupPose); + if (!outcome.move_j.ok()) { + throw std::runtime_error(outcome.move_j.message); + } + + struct SpeedStep { + const char* name; + CartesianVelocity command; + bool angular; + std::size_t axis; + }; + const std::array steps{{ + {"+X", CartesianVelocity{0.05, 0.0, 0.0, 0.0, 0.0, 0.0}, false, 0}, + {"+Y", CartesianVelocity{0.0, 0.05, 0.0, 0.0, 0.0, 0.0}, false, 1}, + {"+Z", CartesianVelocity{0.0, 0.0, 0.05, 0.0, 0.0, 0.0}, false, 2}, + {"+RX", CartesianVelocity{0.0, 0.0, 0.0, 0.30, 0.0, 0.0}, true, 0}, + {"+RY", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.30, 0.0}, true, 1}, + {"+RZ", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.0, 0.30}, true, 2}, + }}; + + for (std::size_t i = 0; i < steps.size(); ++i) { + const auto& step = steps[i]; + if (i > 0) { + outcome.move_j = arm_->moveJ( + JointPositionCommand{kSetupPose}, joint_options); + if (!outcome.move_j.ok()) { + throw std::runtime_error( + std::string("moveJ before speedL ") + step.name + + ": " + outcome.move_j.message); + } + waitFor([&] { + return maxPositionError( + arm_->getJointState().position, kSetupPose) < 0.04; + }, std::chrono::seconds(3)); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + } + + const CartesianPose start = arm_->getTcpPose(); + const Result speed_result = arm_->speedL( + step.command, 0.5, 0.0, FrameType::Base); + if (!speed_result.ok()) { + throw std::runtime_error( + std::string("speedL ") + step.name + ": " + + speed_result.message); + } + + std::this_thread::sleep_for(kCommandDuration); + const Result stop_result = arm_->stopL(0.5); + if (!stop_result.ok()) { + throw std::runtime_error( + std::string("stopL ") + step.name + ": " + + stop_result.message); + } + waitFor([&] { return !arm_->busy(); }, std::chrono::seconds(3)); + + const CartesianPose end = arm_->getTcpPose(); + if (step.angular) { + measured_deltas[i] = baseRotationDelta(start, end)[step.axis]; + std::cout << "[MotorRobotArmGen2MujocoTest] speedL " + << step.name << " rotation delta: " + << measured_deltas[i] << " rad" << std::endl; + } else { + const Eigen::Vector3d translation_delta{ + end.x - start.x, end.y - start.y, end.z - start.z}; + measured_deltas[i] = translation_delta[step.axis]; + std::cout << "[MotorRobotArmGen2MujocoTest] speedL " + << step.name << " translation delta: " + << measured_deltas[i] << " m" << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + } catch (const std::exception& error) { + outcome.worker_error = error.what(); + } + + std::this_thread::sleep_for(std::chrono::seconds(3)); + viewer.requestStop(); + }); + + viewer.setRunning(true); + viewer.run(); + scenario.join(); + + EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error; + EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message; + EXPECT_LT(outcome.move_j_error, 0.08); + for (std::size_t i = 0; i < 3; ++i) { + EXPECT_GT(measured_deltas[i], 0.02); + } + for (std::size_t i = 3; i < measured_deltas.size(); ++i) { + EXPECT_GT(measured_deltas[i], 0.10); + } +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 6ab24556..f3b927d2 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -34,6 +34,9 @@ public: virtual Result emergencyStop() = 0; virtual Result protectiveStop() = 0; + virtual Result recoverProtectiveStop( + const JointTrajectory& path, + const MotionOptions& options) = 0; virtual Result setSpeedScaling(double scaling) = 0; virtual double getSpeedScaling() const = 0; virtual bool isProtectiveStopped() const = 0; 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 index ae45d795..938207e8 100644 --- a/cmvr-es/task/self_collision_task/include/self_collision_task.h +++ b/cmvr-es/task/self_collision_task/include/self_collision_task.h @@ -1,9 +1,14 @@ #ifndef CMVR_ES_SELF_COLLISION_TASK_H #define CMVR_ES_SELF_COLLISION_TASK_H +#include +#include +#include #include #include +#include #include +#include #include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h" #include "algorithms/collision_detection/self_collision/include/self_collision_checker.h" @@ -20,10 +25,22 @@ enum class CollisionSafetyLevel { STOP, }; +enum class ProtectiveRecoveryState { + IDLE = 0, + AVAILABLE, + RECOVERING, + SUCCEEDED, + FAILED, +}; + struct SelfCollisionTaskStatus { CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN}; SelfCollisionResult result; bool stop_latched{false}; + std::uint64_t event_id{0}; + ProtectiveRecoveryState recovery_state{ProtectiveRecoveryState::IDLE}; + std::size_t recovery_sample_count{0}; + std::string recovery_error; }; class SelfCollisionTask final : public Task { @@ -46,11 +63,15 @@ public: std::string detailStatusString() const override; SelfCollisionTaskStatus latestStatus() const; + device::Result requestRecovery(std::uint64_t event_id); private: static bool validateConfig(const config::SelfCollisionTaskConfig& config, std::string* error); static const char* safetyLevelToString(CollisionSafetyLevel level); + static const char* recoveryStateToString(ProtectiveRecoveryState state); + void recordJointSample_(const device::JointGroupState& joint_state, + DistanceSamplingPolicy::Clock::time_point now); config::SelfCollisionTaskConfig config_; std::string id_; @@ -59,8 +80,16 @@ private: DistanceSamplingPolicy sampling_; mutable std::mutex mutex_; + std::condition_variable recovery_cv_; TaskState state_{TaskState::UNINITIALIZED}; SelfCollisionTaskStatus latest_status_{}; + std::deque joint_history_; + device::JointTrajectory recovery_path_; + DistanceSamplingPolicy::Clock::time_point history_epoch_{}; + std::optional recovery_clear_since_; + double recovery_best_distance_m_{0.0}; + bool recovery_clear_confirmed_{false}; + std::uint64_t next_event_id_{1}; std::string last_error_; }; 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 index e3a94ba0..00464c60 100644 --- a/cmvr-es/task/self_collision_task/src/self_collision_task.cpp +++ b/cmvr-es/task/self_collision_task/src/self_collision_task.cpp @@ -1,5 +1,7 @@ #include "task/self_collision_task/include/self_collision_task.h" +#include +#include #include #include #include @@ -53,6 +55,31 @@ bool SelfCollisionTask::validateConfig(const config::SelfCollisionTaskConfig& co safety.warning_distance_m() < safety.stop_distance_m()) { return fail("warning_distance_m must be finite and not less than stop_distance_m"); } + const auto& recovery = config.recovery(); + if (!std::isfinite(recovery.clear_distance_m()) || + recovery.clear_distance_m() <= safety.warning_distance_m()) { + return fail("recovery clear_distance_m must be finite and greater than warning_distance_m"); + } + if (!std::isfinite(recovery.stable_period_s()) || + recovery.stable_period_s() <= 0.0) { + return fail("recovery stable_period_s must be finite and positive"); + } + if (!std::isfinite(recovery.max_joint_velocity_rad_s()) || + recovery.max_joint_velocity_rad_s() <= 0.0) { + return fail("recovery max_joint_velocity_rad_s must be finite and positive"); + } + if (!std::isfinite(recovery.max_joint_acceleration_rad_s2()) || + recovery.max_joint_acceleration_rad_s2() <= 0.0) { + return fail("recovery max_joint_acceleration_rad_s2 must be finite and positive"); + } + if (!std::isfinite(recovery.history_duration_s()) || + recovery.history_duration_s() <= 0.0) { + return fail("recovery history_duration_s must be finite and positive"); + } + if (!std::isfinite(recovery.max_distance_regression_m()) || + recovery.max_distance_regression_m() < 0.0) { + return fail("recovery max_distance_regression_m must be finite and non-negative"); + } for (const auto& pair : config.checker().ignored_pairs()) { if (pair.first().empty() || pair.second().empty() || pair.first() == pair.second()) { return fail("ignored_pairs entries require two different non-empty links"); @@ -121,6 +148,13 @@ bool SelfCollisionTask::init() std::lock_guard lock(mutex_); arm_ = std::move(arm); latest_status_ = {}; + joint_history_.clear(); + recovery_path_.clear(); + history_epoch_ = DistanceSamplingPolicy::Clock::now(); + recovery_clear_since_.reset(); + recovery_best_distance_m_ = 0.0; + recovery_clear_confirmed_ = false; + next_event_id_ = 1; last_error_.clear(); state_ = TaskState::IDLE; } @@ -144,6 +178,12 @@ bool SelfCollisionTask::start() } sampling_.reset(); latest_status_ = {}; + joint_history_.clear(); + recovery_path_.clear(); + history_epoch_ = DistanceSamplingPolicy::Clock::now(); + recovery_clear_since_.reset(); + recovery_best_distance_m_ = 0.0; + recovery_clear_confirmed_ = false; last_error_.clear(); state_ = TaskState::RUNNING; return true; @@ -161,27 +201,49 @@ bool SelfCollisionTask::step(const double dt) 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; + const auto fail_monitoring = [&](std::string error) { + bool cancel_recovery = false; + { + std::lock_guard lock(mutex_); + cancel_recovery = + latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING; + if (cancel_recovery) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = error; + } + last_error_ = std::move(error); + state_ = TaskState::FAILED; + recovery_cv_.notify_all(); + } + if (cancel_recovery) { + (void)arm->protectiveStop(); + } return false; + }; + + const auto joint_state = arm->getJointState(); + const auto model = arm->getRobotModel(); + if (!joint_state.validForModel(model)) { + return fail_monitoring("Robot arm returned an invalid joint state"); } const auto now = DistanceSamplingPolicy::Clock::now(); + recordJointSample_(joint_state, now); + + CollisionGeometrySnapshot snapshot; + std::string error; + if (!checker_.makeSnapshot(joint_state.position, &snapshot, &error)) { + return fail_monitoring(std::move(error)); + } + if (!sampling_.shouldCheck(snapshot, now)) { return true; } SelfCollisionResult result = checker_.check(snapshot); if (!result.valid) { - std::lock_guard lock(mutex_); - last_error_ = result.error.empty() ? "Self-collision distance check failed" : result.error; - state_ = TaskState::FAILED; - return false; + return fail_monitoring( + result.error.empty() ? "Self-collision distance check failed" : result.error); } sampling_.markChecked(snapshot, now); @@ -193,6 +255,7 @@ bool SelfCollisionTask::step(const double dt) } bool trigger_stop = false; + bool abort_recovery = false; CollisionSafetyLevel previous_level = CollisionSafetyLevel::UNKNOWN; { std::lock_guard lock(mutex_); @@ -202,9 +265,63 @@ bool SelfCollisionTask::step(const double dt) previous_level = latest_status_.level; latest_status_.level = level; latest_status_.result = result; - if (level == CollisionSafetyLevel::STOP && !latest_status_.stop_latched) { + + if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) { + if (arm->isEmergencyStopped()) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = + "Protective recovery interrupted by emergency stop"; + recovery_clear_since_.reset(); + abort_recovery = true; + recovery_cv_.notify_all(); + } else if (result.minimum_distance_m + + config_.recovery().max_distance_regression_m() < + recovery_best_distance_m_) { + std::ostringstream stream; + stream << "Protective recovery distance regressed from " + << recovery_best_distance_m_ << " m to " + << result.minimum_distance_m << " m"; + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = stream.str(); + recovery_clear_since_.reset(); + abort_recovery = true; + recovery_cv_.notify_all(); + } else { + recovery_best_distance_m_ = std::max( + recovery_best_distance_m_, result.minimum_distance_m); + if (result.minimum_distance_m >= + config_.recovery().clear_distance_m()) { + if (!recovery_clear_since_) { + recovery_clear_since_ = now; + } else if (std::chrono::duration( + now - *recovery_clear_since_).count() >= + config_.recovery().stable_period_s()) { + recovery_clear_confirmed_ = true; + recovery_cv_.notify_all(); + } + } else { + recovery_clear_since_.reset(); + recovery_clear_confirmed_ = false; + } + } + } else if (level == CollisionSafetyLevel::STOP && + !latest_status_.stop_latched) { latest_status_.stop_latched = true; + latest_status_.event_id = next_event_id_++; + latest_status_.recovery_state = ProtectiveRecoveryState::AVAILABLE; + latest_status_.recovery_error.clear(); + recovery_path_.assign(joint_history_.begin(), joint_history_.end()); + latest_status_.recovery_sample_count = recovery_path_.size(); trigger_stop = true; + } else if (!latest_status_.stop_latched && + result.minimum_distance_m >= + config_.recovery().clear_distance_m()) { + joint_history_.clear(); + device::JointTrajectoryPoint sample; + sample.time_s = std::chrono::duration(now - history_epoch_).count(); + sample.position = joint_state.position; + sample.velocity = joint_state.velocity; + joint_history_.push_back(std::move(sample)); } } @@ -220,7 +337,9 @@ bool SelfCollisionTask::step(const double dt) } } - if (trigger_stop) { + if (abort_recovery) { + (void)arm->protectiveStop(); + } else if (trigger_stop) { const auto stop_result = arm->protectiveStop(); if (!stop_result.ok()) { std::lock_guard lock(mutex_); @@ -232,11 +351,165 @@ bool SelfCollisionTask::step(const double dt) return true; } -void SelfCollisionTask::stop() +void SelfCollisionTask::recordJointSample_( + const device::JointGroupState& joint_state, + const DistanceSamplingPolicy::Clock::time_point now) { std::lock_guard lock(mutex_); - if (state_ != TaskState::FAILED) { - state_ = TaskState::STOPPED; + if (state_ != TaskState::RUNNING || latest_status_.stop_latched) { + return; + } + + device::JointTrajectoryPoint sample; + sample.time_s = std::chrono::duration(now - history_epoch_).count(); + sample.position = joint_state.position; + sample.velocity = joint_state.velocity; + if (!joint_history_.empty() && sample.time_s <= joint_history_.back().time_s) { + return; + } + joint_history_.push_back(std::move(sample)); + + const double oldest_time_s = joint_history_.back().time_s - + config_.recovery().history_duration_s(); + while (joint_history_.size() > 1 && + joint_history_.front().time_s < oldest_time_s) { + joint_history_.pop_front(); + } +} + +device::Result SelfCollisionTask::requestRecovery(const std::uint64_t event_id) +{ + std::shared_ptr arm; + device::JointTrajectory path; + device::MotionOptions options; + { + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING) { + return device::Result::failure( + device::ArmErrorCode::RobotNotReady, + "Protective recovery rejected: collision task is not running"); + } + if (!latest_status_.stop_latched || + latest_status_.recovery_state != ProtectiveRecoveryState::AVAILABLE) { + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + "Protective recovery rejected: no recoverable collision stop is available"); + } + if (event_id == 0 || event_id != latest_status_.event_id) { + return device::Result::failure( + device::ArmErrorCode::InvalidArgument, + "Protective recovery rejected: event_id does not match the active stop"); + } + if (!arm_ || !arm_->isProtectiveStopped() || arm_->isEmergencyStopped()) { + return device::Result::failure( + arm_ && arm_->isEmergencyStopped() + ? device::ArmErrorCode::RobotInEmergencyStop + : device::ArmErrorCode::RobotNotReady, + "Protective recovery rejected: arm safety state is invalid"); + } + if (recovery_path_.size() < 2) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = + "Protective recovery path contains fewer than two samples"; + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + latest_status_.recovery_error); + } + + arm = arm_; + path = recovery_path_; + options.velocity = config_.recovery().max_joint_velocity_rad_s(); + options.acceleration = + config_.recovery().max_joint_acceleration_rad_s2(); + latest_status_.recovery_state = ProtectiveRecoveryState::RECOVERING; + latest_status_.recovery_error.clear(); + recovery_best_distance_m_ = latest_status_.result.minimum_distance_m; + recovery_clear_since_.reset(); + recovery_clear_confirmed_ = false; + } + + CMVR_LOG(INFO) << "[SelfCollisionTask] recovery requested event_id=" + << event_id << ", samples=" << path.size() + << ", max_joint_velocity_rad_s=" + << options.velocity + << ", max_joint_acceleration_rad_s2=" + << options.acceleration; + const auto playback_result = arm->recoverProtectiveStop(path, options); + if (!playback_result.ok()) { + std::lock_guard lock(mutex_); + if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = playback_result.message; + } + const std::string recovery_error = latest_status_.recovery_error; + recovery_cv_.notify_all(); + return device::Result::failure(playback_result.code, recovery_error); + } + + { + std::unique_lock lock(mutex_); + const auto clear_timeout = std::chrono::duration( + config_.recovery().stable_period_s() + 2.0); + const bool completed = recovery_cv_.wait_for(lock, clear_timeout, [&] { + return state_ != TaskState::RUNNING || recovery_clear_confirmed_ || + latest_status_.recovery_state != ProtectiveRecoveryState::RECOVERING; + }); + if (!completed || !recovery_clear_confirmed_) { + if (latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING) { + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = completed + ? "Protective recovery ended before the clear distance was confirmed" + : "Protective recovery clear-distance confirmation timed out"; + } + const std::string error = latest_status_.recovery_error; + lock.unlock(); + (void)arm->protectiveStop(); + return device::Result::failure( + device::ArmErrorCode::CommandFailed, error); + } + + latest_status_.stop_latched = false; + latest_status_.recovery_state = ProtectiveRecoveryState::SUCCEEDED; + latest_status_.recovery_error.clear(); + joint_history_.clear(); + joint_history_.push_back(path.front()); + recovery_path_.clear(); + latest_status_.recovery_sample_count = 0; + } + + const auto unlock_result = arm->unlockProtectiveStop(); + if (!unlock_result.ok()) { + std::lock_guard lock(mutex_); + latest_status_.stop_latched = true; + latest_status_.recovery_state = ProtectiveRecoveryState::FAILED; + latest_status_.recovery_error = + "Failed to unlock protective stop: " + unlock_result.message; + return device::Result::failure( + unlock_result.code, latest_status_.recovery_error); + } + + CMVR_LOG(INFO) << "[SelfCollisionTask] recovery completed event_id=" + << event_id << ", clear_distance_m=" + << config_.recovery().clear_distance_m(); + return device::Result::success(); +} + +void SelfCollisionTask::stop() +{ + std::shared_ptr arm; + bool cancel_recovery = false; + { + std::lock_guard lock(mutex_); + cancel_recovery = + latest_status_.recovery_state == ProtectiveRecoveryState::RECOVERING; + arm = arm_; + if (state_ != TaskState::FAILED) { + state_ = TaskState::STOPPED; + } + recovery_cv_.notify_all(); + } + if (cancel_recovery && arm) { + (void)arm->protectiveStop(); } } @@ -274,13 +547,20 @@ std::string SelfCollisionTask::detailStatusString() const } std::ostringstream stream; stream << taskStateToString(state_) - << " level=" << safetyLevelToString(latest_status_.level); + << " level=" << safetyLevelToString(latest_status_.level) + << " stop_latched=" << latest_status_.stop_latched + << " event_id=" << latest_status_.event_id + << " recovery=" + << recoveryStateToString(latest_status_.recovery_state); if (latest_status_.result.valid) { stream << " distance_m=" << std::setprecision(6) << latest_status_.result.minimum_distance_m << " pair=" << latest_status_.result.first << "/" << latest_status_.result.second; } + if (!latest_status_.recovery_error.empty()) { + stream << " recovery_error=" << latest_status_.recovery_error; + } return stream.str(); } @@ -301,4 +581,17 @@ const char* SelfCollisionTask::safetyLevelToString(const CollisionSafetyLevel le return "UNKNOWN"; } +const char* SelfCollisionTask::recoveryStateToString( + const ProtectiveRecoveryState state) +{ + switch (state) { + case ProtectiveRecoveryState::IDLE: return "IDLE"; + case ProtectiveRecoveryState::AVAILABLE: return "AVAILABLE"; + case ProtectiveRecoveryState::RECOVERING: return "RECOVERING"; + case ProtectiveRecoveryState::SUCCEEDED: return "SUCCEEDED"; + case ProtectiveRecoveryState::FAILED: return "FAILED"; + } + return "UNKNOWN"; +} + } // namespace cmvr::task diff --git a/model/gen2/assets/10100.part b/model/gen2/assets/10100.part new file mode 100644 index 00000000..a5799f3a --- /dev/null +++ b/model/gen2/assets/10100.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M4Rmvs8/9nOQASQoz", + "isStandardContent": false, + "name": "10100 <1>", + "partId": "JyD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/10100.stl b/model/gen2/assets/10100.stl new file mode 100644 index 00000000..f7c27440 Binary files /dev/null and b/model/gen2/assets/10100.stl differ diff --git a/model/gen2/assets/10100__2.part b/model/gen2/assets/10100__2.part new file mode 100644 index 00000000..b79e9263 --- /dev/null +++ b/model/gen2/assets/10100__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "MHaJriwfEEKCaNP0z", + "isStandardContent": false, + "name": "10100 <2>", + "partId": "J5D", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/10100__2.stl b/model/gen2/assets/10100__2.stl new file mode 100644 index 00000000..5814b233 Binary files /dev/null and b/model/gen2/assets/10100__2.stl differ diff --git a/model/gen2/assets/10100__3.part b/model/gen2/assets/10100__3.part new file mode 100644 index 00000000..fe8f6c01 --- /dev/null +++ b/model/gen2/assets/10100__3.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "8073bb18db19f7fa7997ad99", + "documentMicroversion": "428a1bfe2c594dd041131bcd", + "documentVersion": "102e666e83d6b78c25c9ce78", + "elementId": "5decfde3994a7031e4d53265", + "fullConfiguration": "default", + "id": "MMSYOwzMqbgilS4dL", + "isStandardContent": false, + "name": "10100 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/10100__3.stl b/model/gen2/assets/10100__3.stl new file mode 100644 index 00000000..2f81fa0e Binary files /dev/null and b/model/gen2/assets/10100__3.stl differ diff --git a/model/gen2/assets/1020001.part b/model/gen2/assets/1020001.part new file mode 100644 index 00000000..38a2e880 --- /dev/null +++ b/model/gen2/assets/1020001.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "387522184bef9b82eac1fdfc", + "documentMicroversion": "caf63b0301e581fe867e46ba", + "documentVersion": "961b145da2fcbaf35ddb6766", + "elementId": "58a93d37d32edd9f1f80c4a6", + "fullConfiguration": "default", + "id": "MtGpd/bL/Q+stBW6B", + "isStandardContent": false, + "name": "1020001 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/1020001.stl b/model/gen2/assets/1020001.stl new file mode 100644 index 00000000..7f21adc5 Binary files /dev/null and b/model/gen2/assets/1020001.stl differ diff --git a/model/gen2/assets/arm_link_1.part b/model/gen2/assets/arm_link_1.part new file mode 100644 index 00000000..52a02296 --- /dev/null +++ b/model/gen2/assets/arm_link_1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "M77c5kvnmFYX3+E1/", + "isStandardContent": false, + "name": "arm_link_1 <2>", + "partId": "RHDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_1.stl b/model/gen2/assets/arm_link_1.stl new file mode 100644 index 00000000..c5221527 Binary files /dev/null and b/model/gen2/assets/arm_link_1.stl differ diff --git a/model/gen2/assets/arm_link_2.part b/model/gen2/assets/arm_link_2.part new file mode 100644 index 00000000..466137f5 --- /dev/null +++ b/model/gen2/assets/arm_link_2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MAagFsEi4vOqUYBnE", + "isStandardContent": false, + "name": "arm_link_2 <2>", + "partId": "JvD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_2.stl b/model/gen2/assets/arm_link_2.stl new file mode 100644 index 00000000..e0f07b54 Binary files /dev/null and b/model/gen2/assets/arm_link_2.stl differ diff --git a/model/gen2/assets/arm_link_3.part b/model/gen2/assets/arm_link_3.part new file mode 100644 index 00000000..9020c9d7 --- /dev/null +++ b/model/gen2/assets/arm_link_3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MODZ2heuMlDmDK6JN", + "isStandardContent": false, + "name": "arm_link_3 <2>", + "partId": "RRBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_3.stl b/model/gen2/assets/arm_link_3.stl new file mode 100644 index 00000000..ec969414 Binary files /dev/null and b/model/gen2/assets/arm_link_3.stl differ diff --git a/model/gen2/assets/arm_link_4.part b/model/gen2/assets/arm_link_4.part new file mode 100644 index 00000000..b6e91f6c --- /dev/null +++ b/model/gen2/assets/arm_link_4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MmEi+ZosIdG9Wp+iP", + "isStandardContent": false, + "name": "arm_link_4 <2>", + "partId": "RwCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_4.stl b/model/gen2/assets/arm_link_4.stl new file mode 100644 index 00000000..b7c392c1 Binary files /dev/null and b/model/gen2/assets/arm_link_4.stl differ diff --git a/model/gen2/assets/arm_link_5.part b/model/gen2/assets/arm_link_5.part new file mode 100644 index 00000000..7e3ea789 --- /dev/null +++ b/model/gen2/assets/arm_link_5.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MBPP2k1l9Xc6L3QvC", + "isStandardContent": false, + "name": "arm_link_5 <2>", + "partId": "RxCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_5.stl b/model/gen2/assets/arm_link_5.stl new file mode 100644 index 00000000..7aec226c Binary files /dev/null and b/model/gen2/assets/arm_link_5.stl differ diff --git a/model/gen2/assets/arm_link_6.part b/model/gen2/assets/arm_link_6.part new file mode 100644 index 00000000..96e7540e --- /dev/null +++ b/model/gen2/assets/arm_link_6.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "MIcvuqnQVxIZF1ql0", + "isStandardContent": false, + "name": "arm_link_6 <2>", + "partId": "RyCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_6.stl b/model/gen2/assets/arm_link_6.stl new file mode 100644 index 00000000..a019833c Binary files /dev/null and b/model/gen2/assets/arm_link_6.stl differ diff --git a/model/gen2/assets/arm_link_7.part b/model/gen2/assets/arm_link_7.part new file mode 100644 index 00000000..13512a37 --- /dev/null +++ b/model/gen2/assets/arm_link_7.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8c088a46f41b8780aa1cf914", + "fullConfiguration": "default", + "id": "Mv//Etw9ckikOO5Dz", + "isStandardContent": false, + "name": "arm_link_7 <2>", + "partId": "RrCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/arm_link_7.stl b/model/gen2/assets/arm_link_7.stl new file mode 100644 index 00000000..6af8cf87 Binary files /dev/null and b/model/gen2/assets/arm_link_7.stl differ diff --git a/model/gen2/assets/body_link.part b/model/gen2/assets/body_link.part new file mode 100644 index 00000000..d9c1dc62 --- /dev/null +++ b/model/gen2/assets/body_link.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "0384359606fafa70e61c4a9c", + "fullConfiguration": "default", + "id": "MdJXCUi9nHtYjnfh4", + "isStandardContent": false, + "name": "body_link <1>", + "partId": "RgQD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/body_link.stl b/model/gen2/assets/body_link.stl new file mode 100644 index 00000000..bccfeeff Binary files /dev/null and b/model/gen2/assets/body_link.stl differ diff --git a/model/gen2/assets/ethercat挂杆_上.part b/model/gen2/assets/ethercat挂杆_上.part new file mode 100644 index 00000000..07e4fef2 --- /dev/null +++ b/model/gen2/assets/ethercat挂杆_上.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M6a8atbQgryvAh+2G", + "isStandardContent": false, + "name": "ethercat\u6302\u6746-\u4e0a <1>", + "partId": "RMDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ethercat挂杆_上.stl b/model/gen2/assets/ethercat挂杆_上.stl new file mode 100644 index 00000000..f3214420 Binary files /dev/null and b/model/gen2/assets/ethercat挂杆_上.stl differ diff --git a/model/gen2/assets/ethercat挂杆_下.part b/model/gen2/assets/ethercat挂杆_下.part new file mode 100644 index 00000000..576f381e --- /dev/null +++ b/model/gen2/assets/ethercat挂杆_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M6IPrfKh9NKHG6KNc", + "isStandardContent": false, + "name": "ethercat\u6302\u6746-\u4e0b <1>", + "partId": "RMDH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ethercat挂杆_下.stl b/model/gen2/assets/ethercat挂杆_下.stl new file mode 100644 index 00000000..04e843f0 Binary files /dev/null and b/model/gen2/assets/ethercat挂杆_下.stl differ diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.part b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.part new file mode 100644 index 00000000..d87c155c --- /dev/null +++ b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.part @@ -0,0 +1,14 @@ +{ + "configuration": "JTQwc2NwPU9ITmFJVkFLb3VXVUNGQnNjWjhrOE1zRG1nMFNRSU9JOUxqWXlZc1dGZEklM0Q7QXZlcmFnZURpYW1ldGVyPTAuMDAzNjAwMDAwMDAwMDAwMDAwMyttZXRlcjtCYXNpY0RpYW1ldGVyPTAuMDAzK21ldGVyO0hlYWREaWFtZXRlcj0wLjAwNTUrbWV0ZXI7SGVhZEZpbGxldD0zLjBFLTQrbWV0ZXI7SGVhZEhlaWdodD0wLjAwMyttZXRlcjtIZXhEZXB0aD0wLjAwMTMwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMjUrbWV0ZXI7TGVuZ3RoPTAuMDI1K21ldGVyO1BpdGNoPTUuMEUtNCttZXRlcjtUaHJlYWRMZW5ndGg9MC4wMTgwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7VHJhbnNpdGlvbkxlbmd0aD01LjFFLTQrbWV0ZXI7VHJpYW5nbGVIZWlnaHQ9NC4zMzAxMjdFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTEuMEUtNCttZXRlcg", + "documentId": "da5fe16b33cc63bf8b9e7e78", + "documentMicroversion": "e36ce5d010b02c39f1a32b51", + "documentVersion": "dc1a15dae669d740ea2d555e", + "elementId": "5b44a050e0b24df3e47c76dc", + "fullConfiguration": "JTQwc2NwPU9ITmFJVkFLb3VXVUNGQnNjWjhrOE1zRG1nMFNRSU9JOUxqWXlZc1dGZEklM0Q7QXZlcmFnZURpYW1ldGVyPTAuMDAzNjAwMDAwMDAwMDAwMDAwMyttZXRlcjtCYXNpY0RpYW1ldGVyPTAuMDAzK21ldGVyO0hlYWREaWFtZXRlcj0wLjAwNTUrbWV0ZXI7SGVhZEZpbGxldD0zLjBFLTQrbWV0ZXI7SGVhZEhlaWdodD0wLjAwMyttZXRlcjtIZXhEZXB0aD0wLjAwMTMwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMjUrbWV0ZXI7TGVuZ3RoPTAuMDI1K21ldGVyO1BpdGNoPTUuMEUtNCttZXRlcjtUaHJlYWRMZW5ndGg9MC4wMTgwMDAwMDAwMDAwMDAwMDIrbWV0ZXI7VHJhbnNpdGlvbkxlbmd0aD01LjFFLTQrbWV0ZXI7VHJpYW5nbGVIZWlnaHQ9NC4zMzAxMjdFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTEuMEUtNCttZXRlcg", + "id": "Mz77l4SnYYxPYDxga", + "isStandardContent": true, + "name": "Hex socket head cap screw M3x0.50 x 25 x 18 <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.stl b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.stl new file mode 100644 index 00000000..26cbae94 Binary files /dev/null and b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_25_x_18__e477b94a6da3e3efbf1606183756b785.stl differ diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.part b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.part new file mode 100644 index 00000000..a31206cb --- /dev/null +++ b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.part @@ -0,0 +1,14 @@ +{ + "configuration": "JTQwc2NwPXZaeDNQUzV0NWU2YlI2R3BJR25ZZ1ZoczdzREJNOWpvdmNXYiUyQjVIU1JnOCUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDM2MDAwMDAwMDAwMDAwMDAzK21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDMrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA1NSttZXRlcjtIZWFkRmlsbGV0PTMuMEUtNCttZXRlcjtIZWFkSGVpZ2h0PTAuMDAzK21ldGVyO0hleERlcHRoPTAuMDAxMzAwMDAwMDAwMDAwMDAwMittZXRlcjtIZXhTaXplPTAuMDAyNSttZXRlcjtMZW5ndGg9MC4wNSttZXRlcjtQaXRjaD01LjBFLTQrbWV0ZXI7VGhyZWFkTGVuZ3RoPTAuMDE4MDAwMDAwMDAwMDAwMDAyK21ldGVyO1RyYW5zaXRpb25MZW5ndGg9NS4xRS00K21ldGVyO1RyaWFuZ2xlSGVpZ2h0PTQuMzMwMTI3RS00K21ldGVyO1VuZGVySGVhZEZpbGxldD0xLjBFLTQrbWV0ZXI", + "documentId": "da5fe16b33cc63bf8b9e7e78", + "documentMicroversion": "e36ce5d010b02c39f1a32b51", + "documentVersion": "dc1a15dae669d740ea2d555e", + "elementId": "5b44a050e0b24df3e47c76dc", + "fullConfiguration": "JTQwc2NwPXZaeDNQUzV0NWU2YlI2R3BJR25ZZ1ZoczdzREJNOWpvdmNXYiUyQjVIU1JnOCUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDM2MDAwMDAwMDAwMDAwMDAzK21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDMrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA1NSttZXRlcjtIZWFkRmlsbGV0PTMuMEUtNCttZXRlcjtIZWFkSGVpZ2h0PTAuMDAzK21ldGVyO0hleERlcHRoPTAuMDAxMzAwMDAwMDAwMDAwMDAwMittZXRlcjtIZXhTaXplPTAuMDAyNSttZXRlcjtMZW5ndGg9MC4wNSttZXRlcjtQaXRjaD01LjBFLTQrbWV0ZXI7VGhyZWFkTGVuZ3RoPTAuMDE4MDAwMDAwMDAwMDAwMDAyK21ldGVyO1RyYW5zaXRpb25MZW5ndGg9NS4xRS00K21ldGVyO1RyaWFuZ2xlSGVpZ2h0PTQuMzMwMTI3RS00K21ldGVyO1VuZGVySGVhZEZpbGxldD0xLjBFLTQrbWV0ZXI", + "id": "MzS3hV4uHEP51PCyW", + "isStandardContent": true, + "name": "Hex socket head cap screw M3x0.50 x 50 x 18 <5>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.stl b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.stl new file mode 100644 index 00000000..203ef536 Binary files /dev/null and b/model/gen2/assets/hex_socket_head_cap_screw_m3x0_50_x_50_x_18__4dd82fe21c6c50a3e9c04ef8ceda3dc7.stl differ diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.part b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.part new file mode 100644 index 00000000..948acca9 --- /dev/null +++ b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.part @@ -0,0 +1,14 @@ +{ + "configuration": "JTQwc2NwPWxOSElNUXhVSTdPY3d2VXJqaDNJcVhHZHV3elVUZWYlMkY1N3cydklXNTZ1ZyUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDQ3K21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDQrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA3K21ldGVyO0hlYWRGaWxsZXQ9NC4wRS00K21ldGVyO0hlYWRIZWlnaHQ9MC4wMDQrbWV0ZXI7SGV4RGVwdGg9MC4wMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMyttZXRlcjtMZW5ndGg9MC4wMDgrbWV0ZXI7UGl0Y2g9Ny4wRS00K21ldGVyO1RocmVhZExlbmd0aD0wLjAwOCttZXRlcjtUcmFuc2l0aW9uTGVuZ3RoPTYuMEUtNCttZXRlcjtUcmlhbmdsZUhlaWdodD02LjA2MjE3NzhFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTIuMEUtNCttZXRlcg", + "documentId": "da5fe16b33cc63bf8b9e7e78", + "documentMicroversion": "e36ce5d010b02c39f1a32b51", + "documentVersion": "dc1a15dae669d740ea2d555e", + "elementId": "5b44a050e0b24df3e47c76dc", + "fullConfiguration": "JTQwc2NwPWxOSElNUXhVSTdPY3d2VXJqaDNJcVhHZHV3elVUZWYlMkY1N3cydklXNTZ1ZyUzRDtBdmVyYWdlRGlhbWV0ZXI9MC4wMDQ3K21ldGVyO0Jhc2ljRGlhbWV0ZXI9MC4wMDQrbWV0ZXI7SGVhZERpYW1ldGVyPTAuMDA3K21ldGVyO0hlYWRGaWxsZXQ9NC4wRS00K21ldGVyO0hlYWRIZWlnaHQ9MC4wMDQrbWV0ZXI7SGV4RGVwdGg9MC4wMDIrbWV0ZXI7SGV4U2l6ZT0wLjAwMyttZXRlcjtMZW5ndGg9MC4wMDgrbWV0ZXI7UGl0Y2g9Ny4wRS00K21ldGVyO1RocmVhZExlbmd0aD0wLjAwOCttZXRlcjtUcmFuc2l0aW9uTGVuZ3RoPTYuMEUtNCttZXRlcjtUcmlhbmdsZUhlaWdodD02LjA2MjE3NzhFLTQrbWV0ZXI7VW5kZXJIZWFkRmlsbGV0PTIuMEUtNCttZXRlcg", + "id": "Mwr1cbw7PCKF93OWT", + "isStandardContent": true, + "name": "Hex socket head cap screw M4x0.70 x 8 <7>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.stl b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.stl new file mode 100644 index 00000000..568a174b Binary files /dev/null and b/model/gen2/assets/hex_socket_head_cap_screw_m4x0_70_x_8__a416dc2a8d778a4ef6150c0739aa89d1.stl differ diff --git a/model/gen2/assets/j2关节.part b/model/gen2/assets/j2关节.part new file mode 100644 index 00000000..095fb6d6 --- /dev/null +++ b/model/gen2/assets/j2关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "8b7e66f798cbb082509293d8", + "documentMicroversion": "887ff4907089063547dfb683", + "documentVersion": "05b6b05241fd0e72cdcb2a58", + "elementId": "a4d6cca76f08dc255e08b999", + "fullConfiguration": "default", + "id": "MPjW04ncnuOxOzkZ0", + "isStandardContent": false, + "name": "J2\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j2关节.stl b/model/gen2/assets/j2关节.stl new file mode 100644 index 00000000..0848ee6d Binary files /dev/null and b/model/gen2/assets/j2关节.stl differ diff --git a/model/gen2/assets/j2关节盖板.part b/model/gen2/assets/j2关节盖板.part new file mode 100644 index 00000000..1e36dd10 --- /dev/null +++ b/model/gen2/assets/j2关节盖板.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "639734872365a5c6ab1e6dc0", + "documentMicroversion": "87a57a1cf4702ec8bbc72b83", + "documentVersion": "bc5b603bcba394f302ef13b6", + "elementId": "2a770d046c759ce59d9fb447", + "fullConfiguration": "default", + "id": "M3lE/jcPA4MBiXRl+", + "isStandardContent": false, + "name": "J2\u5173\u8282\u76d6\u677f <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j2关节盖板.stl b/model/gen2/assets/j2关节盖板.stl new file mode 100644 index 00000000..e6fc0a4b Binary files /dev/null and b/model/gen2/assets/j2关节盖板.stl differ diff --git a/model/gen2/assets/j3关节.part b/model/gen2/assets/j3关节.part new file mode 100644 index 00000000..05d24dcc --- /dev/null +++ b/model/gen2/assets/j3关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "15fc539038cd4e87718802cf", + "documentMicroversion": "dd0168517fb915b74ab9f650", + "documentVersion": "c78f7c31e667644647771463", + "elementId": "cce50e82bbd0f737c1086291", + "fullConfiguration": "default", + "id": "ML+TSApNzCC0D52rC", + "isStandardContent": false, + "name": "J3\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j3关节.stl b/model/gen2/assets/j3关节.stl new file mode 100644 index 00000000..aba09b4f Binary files /dev/null and b/model/gen2/assets/j3关节.stl differ diff --git a/model/gen2/assets/j4关节.part b/model/gen2/assets/j4关节.part new file mode 100644 index 00000000..8d11e737 --- /dev/null +++ b/model/gen2/assets/j4关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "7b3f5b2e877e0152778a9ca2", + "documentMicroversion": "0f0440eac84e1ad6c9e4d950", + "documentVersion": "6d55eb8af3364ff18181e064", + "elementId": "deecc12ddde91edb0deb20c9", + "fullConfiguration": "default", + "id": "MGEe7vYxsi98IEyH5", + "isStandardContent": false, + "name": "J4\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4关节.stl b/model/gen2/assets/j4关节.stl new file mode 100644 index 00000000..1940bc4d Binary files /dev/null and b/model/gen2/assets/j4关节.stl differ diff --git a/model/gen2/assets/j4轴承支撑.part b/model/gen2/assets/j4轴承支撑.part new file mode 100644 index 00000000..74ca7542 --- /dev/null +++ b/model/gen2/assets/j4轴承支撑.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "38be5a8344ea08045e614079", + "fullConfiguration": "default", + "id": "MMhyQcMwjwq7kE227", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u652f\u6491 <1>", + "partId": "RWBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承支撑.stl b/model/gen2/assets/j4轴承支撑.stl new file mode 100644 index 00000000..c14cdd08 Binary files /dev/null and b/model/gen2/assets/j4轴承支撑.stl differ diff --git a/model/gen2/assets/j4轴承支撑__2.part b/model/gen2/assets/j4轴承支撑__2.part new file mode 100644 index 00000000..7375e897 --- /dev/null +++ b/model/gen2/assets/j4轴承支撑__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "a51a941965828d24d7f9ab81", + "fullConfiguration": "default", + "id": "MsSXN2rPmtY0dur6R", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u652f\u6491 <2>", + "partId": "RMCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承支撑__2.stl b/model/gen2/assets/j4轴承支撑__2.stl new file mode 100644 index 00000000..1f917818 Binary files /dev/null and b/model/gen2/assets/j4轴承支撑__2.stl differ diff --git a/model/gen2/assets/j4轴承盖板.part b/model/gen2/assets/j4轴承盖板.part new file mode 100644 index 00000000..c60543b2 --- /dev/null +++ b/model/gen2/assets/j4轴承盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "38be5a8344ea08045e614079", + "fullConfiguration": "default", + "id": "MKrYD2pl4tMWf+WQQ", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u76d6\u677f <1>", + "partId": "RjBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承盖板.stl b/model/gen2/assets/j4轴承盖板.stl new file mode 100644 index 00000000..a56635ee Binary files /dev/null and b/model/gen2/assets/j4轴承盖板.stl differ diff --git a/model/gen2/assets/j4轴承盖板__2.part b/model/gen2/assets/j4轴承盖板__2.part new file mode 100644 index 00000000..50afd215 --- /dev/null +++ b/model/gen2/assets/j4轴承盖板__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "a51a941965828d24d7f9ab81", + "fullConfiguration": "default", + "id": "MqrQMlBzm6clwBYhN", + "isStandardContent": false, + "name": "J4\u8f74\u627f\u76d6\u677f <2>", + "partId": "RLCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j4轴承盖板__2.stl b/model/gen2/assets/j4轴承盖板__2.stl new file mode 100644 index 00000000..e40d5610 Binary files /dev/null and b/model/gen2/assets/j4轴承盖板__2.stl differ diff --git a/model/gen2/assets/j5主动法兰.part b/model/gen2/assets/j5主动法兰.part new file mode 100644 index 00000000..ba4cb08d --- /dev/null +++ b/model/gen2/assets/j5主动法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "95a669e868ab89618b756342", + "fullConfiguration": "default", + "id": "MkPc/e6vvvzfReU1g", + "isStandardContent": false, + "name": "J5\u4e3b\u52a8\u6cd5\u5170 <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5主动法兰.stl b/model/gen2/assets/j5主动法兰.stl new file mode 100644 index 00000000..f873e14e Binary files /dev/null and b/model/gen2/assets/j5主动法兰.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板.part b/model/gen2/assets/j5从动轴心盖板.part new file mode 100644 index 00000000..c7178894 --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "M++0c8F/OA1VcSYPL", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <2>", + "partId": "R1BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板.stl b/model/gen2/assets/j5从动轴心盖板.stl new file mode 100644 index 00000000..eec1e308 Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板__2.part b/model/gen2/assets/j5从动轴心盖板__2.part new file mode 100644 index 00000000..793e5578 --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "M+lDhmwR1TmaWXh/y", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <1>", + "partId": "R0BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板__2.stl b/model/gen2/assets/j5从动轴心盖板__2.stl new file mode 100644 index 00000000..efe21ac5 Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板__2.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板__3.part b/model/gen2/assets/j5从动轴心盖板__3.part new file mode 100644 index 00000000..2e4eb6fc --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板__3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "Mb3FWJ4KHbCKAG26B", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <3>", + "partId": "RDBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板__3.stl b/model/gen2/assets/j5从动轴心盖板__3.stl new file mode 100644 index 00000000..e8375f6c Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板__3.stl differ diff --git a/model/gen2/assets/j5从动轴心盖板__4.part b/model/gen2/assets/j5从动轴心盖板__4.part new file mode 100644 index 00000000..5f1c4005 --- /dev/null +++ b/model/gen2/assets/j5从动轴心盖板__4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "MjEKCYXkYWHg0gqf+", + "isStandardContent": false, + "name": "J5\u4ece\u52a8\u8f74\u5fc3\u76d6\u677f <4>", + "partId": "RSBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5从动轴心盖板__4.stl b/model/gen2/assets/j5从动轴心盖板__4.stl new file mode 100644 index 00000000..8600d762 Binary files /dev/null and b/model/gen2/assets/j5从动轴心盖板__4.stl differ diff --git a/model/gen2/assets/j5关节.part b/model/gen2/assets/j5关节.part new file mode 100644 index 00000000..ef2b4497 --- /dev/null +++ b/model/gen2/assets/j5关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "6738eff574c7bac74278c035", + "documentMicroversion": "7b5e69869d9b02be1ce0581d", + "documentVersion": "a0db86e78c7d2dc110a9791b", + "elementId": "60347bdca2a24e7714baa4d6", + "fullConfiguration": "default", + "id": "MjQlv+qNUV6RZuD6P", + "isStandardContent": false, + "name": "J5\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j5关节.stl b/model/gen2/assets/j5关节.stl new file mode 100644 index 00000000..3d5e4944 Binary files /dev/null and b/model/gen2/assets/j5关节.stl differ diff --git a/model/gen2/assets/j6轴承支撑.part b/model/gen2/assets/j6轴承支撑.part new file mode 100644 index 00000000..2f161626 --- /dev/null +++ b/model/gen2/assets/j6轴承支撑.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "ec3810b640410692b6d75e6d", + "documentMicroversion": "2677b5c61e0ce625e61f2de1", + "documentVersion": "260d5320b9d212f7376901b9", + "elementId": "9b8fab3d8fa54635da3b9281", + "fullConfiguration": "default", + "id": "M1oxqvy7AYCLJRufY", + "isStandardContent": false, + "name": "J6\u8f74\u627f\u652f\u6491 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j6轴承支撑.stl b/model/gen2/assets/j6轴承支撑.stl new file mode 100644 index 00000000..54b5846e Binary files /dev/null and b/model/gen2/assets/j6轴承支撑.stl differ diff --git a/model/gen2/assets/j6轴承支撑1.part b/model/gen2/assets/j6轴承支撑1.part new file mode 100644 index 00000000..b88940bc --- /dev/null +++ b/model/gen2/assets/j6轴承支撑1.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "aab3081dc8a1b17396b04ac8", + "documentMicroversion": "989f9da17530022533d4b1b5", + "documentVersion": "431a46f24ed967bcf94385d0", + "elementId": "55a51320ef47236b54856bc1", + "fullConfiguration": "default", + "id": "MG/AppKFXBSV39O6i", + "isStandardContent": false, + "name": "J6\u8f74\u627f\u652f\u64911 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j6轴承支撑1.stl b/model/gen2/assets/j6轴承支撑1.stl new file mode 100644 index 00000000..96d0b61d Binary files /dev/null and b/model/gen2/assets/j6轴承支撑1.stl differ diff --git a/model/gen2/assets/j7关节a.part b/model/gen2/assets/j7关节a.part new file mode 100644 index 00000000..7b2f9539 --- /dev/null +++ b/model/gen2/assets/j7关节a.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "41e7681508301de01c2f4658", + "documentMicroversion": "e0b55cb820aadaa0f053d95b", + "documentVersion": "9b9bafefdd3dbfa2715fbe98", + "elementId": "28e76f0bd60c1f60bb95ed8c", + "fullConfiguration": "default", + "id": "MjmlvjYKU6BQqrtfC", + "isStandardContent": false, + "name": "J7\u5173\u8282A <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/j7关节a.stl b/model/gen2/assets/j7关节a.stl new file mode 100644 index 00000000..7ffc5f5a Binary files /dev/null and b/model/gen2/assets/j7关节a.stl differ diff --git a/model/gen2/assets/jetson安装杆_右.part b/model/gen2/assets/jetson安装杆_右.part new file mode 100644 index 00000000..dc9d6425 --- /dev/null +++ b/model/gen2/assets/jetson安装杆_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MK0IY8wljRAjD4RsR", + "isStandardContent": false, + "name": "jetson\u5b89\u88c5\u6746-\u53f3 <1>", + "partId": "RwLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/jetson安装杆_右.stl b/model/gen2/assets/jetson安装杆_右.stl new file mode 100644 index 00000000..a072d0ad Binary files /dev/null and b/model/gen2/assets/jetson安装杆_右.stl differ diff --git a/model/gen2/assets/jetson安装杆_左.part b/model/gen2/assets/jetson安装杆_左.part new file mode 100644 index 00000000..d8f085d0 --- /dev/null +++ b/model/gen2/assets/jetson安装杆_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mmwlrnd+kZGc2EiJK", + "isStandardContent": false, + "name": "jetson\u5b89\u88c5\u6746-\u5de6 <1>", + "partId": "R3LD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/jetson安装杆_左.stl b/model/gen2/assets/jetson安装杆_左.stl new file mode 100644 index 00000000..8905eb74 Binary files /dev/null and b/model/gen2/assets/jetson安装杆_左.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_2_collision.stl b/model/gen2/assets/merged/arm_link_1_2_collision.stl new file mode 100644 index 00000000..593caf4f Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_2_visual.stl b/model/gen2/assets/merged/arm_link_1_2_visual.stl new file mode 100644 index 00000000..87509b29 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_collision.stl b/model/gen2/assets/merged/arm_link_1_collision.stl new file mode 100644 index 00000000..2be06fc6 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_1_visual.stl b/model/gen2/assets/merged/arm_link_1_visual.stl new file mode 100644 index 00000000..7b8bba96 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_1_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_2_collision.stl b/model/gen2/assets/merged/arm_link_2_2_collision.stl new file mode 100644 index 00000000..f63fde64 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_2_visual.stl b/model/gen2/assets/merged/arm_link_2_2_visual.stl new file mode 100644 index 00000000..07b07c92 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_collision.stl b/model/gen2/assets/merged/arm_link_2_collision.stl new file mode 100644 index 00000000..0ad57d21 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_2_visual.stl b/model/gen2/assets/merged/arm_link_2_visual.stl new file mode 100644 index 00000000..ce6ea378 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_2_collision.stl b/model/gen2/assets/merged/arm_link_3_2_collision.stl new file mode 100644 index 00000000..dbf4fa53 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_2_visual.stl b/model/gen2/assets/merged/arm_link_3_2_visual.stl new file mode 100644 index 00000000..a01d9ba6 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_collision.stl b/model/gen2/assets/merged/arm_link_3_collision.stl new file mode 100644 index 00000000..d6c0abbf Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_3_visual.stl b/model/gen2/assets/merged/arm_link_3_visual.stl new file mode 100644 index 00000000..4a3b6785 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_3_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_2_collision.stl b/model/gen2/assets/merged/arm_link_4_2_collision.stl new file mode 100644 index 00000000..4569f480 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_2_visual.stl b/model/gen2/assets/merged/arm_link_4_2_visual.stl new file mode 100644 index 00000000..4569f480 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_collision.stl b/model/gen2/assets/merged/arm_link_4_collision.stl new file mode 100644 index 00000000..a2cc8b37 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_4_visual.stl b/model/gen2/assets/merged/arm_link_4_visual.stl new file mode 100644 index 00000000..a2cc8b37 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_4_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_2_collision.stl b/model/gen2/assets/merged/arm_link_5_2_collision.stl new file mode 100644 index 00000000..196c86a3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_2_visual.stl b/model/gen2/assets/merged/arm_link_5_2_visual.stl new file mode 100644 index 00000000..196c86a3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_collision.stl b/model/gen2/assets/merged/arm_link_5_collision.stl new file mode 100644 index 00000000..92156354 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_5_visual.stl b/model/gen2/assets/merged/arm_link_5_visual.stl new file mode 100644 index 00000000..92156354 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_5_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_2_collision.stl b/model/gen2/assets/merged/arm_link_6_2_collision.stl new file mode 100644 index 00000000..875d6cc5 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_2_visual.stl b/model/gen2/assets/merged/arm_link_6_2_visual.stl new file mode 100644 index 00000000..875d6cc5 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_collision.stl b/model/gen2/assets/merged/arm_link_6_collision.stl new file mode 100644 index 00000000..38249bf3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_6_visual.stl b/model/gen2/assets/merged/arm_link_6_visual.stl new file mode 100644 index 00000000..38249bf3 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_6_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_2_collision.stl b/model/gen2/assets/merged/arm_link_7_2_collision.stl new file mode 100644 index 00000000..9343e212 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_2_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_2_visual.stl b/model/gen2/assets/merged/arm_link_7_2_visual.stl new file mode 100644 index 00000000..9343e212 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_2_visual.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_collision.stl b/model/gen2/assets/merged/arm_link_7_collision.stl new file mode 100644 index 00000000..e71dab05 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_collision.stl differ diff --git a/model/gen2/assets/merged/arm_link_7_visual.stl b/model/gen2/assets/merged/arm_link_7_visual.stl new file mode 100644 index 00000000..e71dab05 Binary files /dev/null and b/model/gen2/assets/merged/arm_link_7_visual.stl differ diff --git a/model/gen2/assets/merged/body_link_collision.stl b/model/gen2/assets/merged/body_link_collision.stl new file mode 100644 index 00000000..094fdf95 Binary files /dev/null and b/model/gen2/assets/merged/body_link_collision.stl differ diff --git a/model/gen2/assets/merged/body_link_visual.stl b/model/gen2/assets/merged/body_link_visual.stl new file mode 100644 index 00000000..ddb3442e Binary files /dev/null and b/model/gen2/assets/merged/body_link_visual.stl differ diff --git a/model/gen2/assets/merged/j5从动轴心盖板_collision.stl b/model/gen2/assets/merged/j5从动轴心盖板_collision.stl new file mode 100644 index 00000000..6175d231 Binary files /dev/null and b/model/gen2/assets/merged/j5从动轴心盖板_collision.stl differ diff --git a/model/gen2/assets/merged/j5从动轴心盖板_visual.stl b/model/gen2/assets/merged/j5从动轴心盖板_visual.stl new file mode 100644 index 00000000..6175d231 Binary files /dev/null and b/model/gen2/assets/merged/j5从动轴心盖板_visual.stl differ diff --git a/model/gen2/assets/merged/part_1_2_collision.stl b/model/gen2/assets/merged/part_1_2_collision.stl new file mode 100644 index 00000000..5b28dbf9 Binary files /dev/null and b/model/gen2/assets/merged/part_1_2_collision.stl differ diff --git a/model/gen2/assets/merged/part_1_2_visual.stl b/model/gen2/assets/merged/part_1_2_visual.stl new file mode 100644 index 00000000..5b28dbf9 Binary files /dev/null and b/model/gen2/assets/merged/part_1_2_visual.stl differ diff --git a/model/gen2/assets/merged/part_1_collision.stl b/model/gen2/assets/merged/part_1_collision.stl new file mode 100644 index 00000000..5dbe4db7 Binary files /dev/null and b/model/gen2/assets/merged/part_1_collision.stl differ diff --git a/model/gen2/assets/merged/part_1_visual.stl b/model/gen2/assets/merged/part_1_visual.stl new file mode 100644 index 00000000..5dbe4db7 Binary files /dev/null and b/model/gen2/assets/merged/part_1_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_2_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_2_collision.stl new file mode 100644 index 00000000..8dc088dd Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_2_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_2_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_2_visual.stl new file mode 100644 index 00000000..b1300cdb Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_2_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_3_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_3_collision.stl new file mode 100644 index 00000000..5601939c Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_3_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_3_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_3_visual.stl new file mode 100644 index 00000000..79da68c0 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_3_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_4_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_4_collision.stl new file mode 100644 index 00000000..98ea5646 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_4_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_4_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_4_visual.stl new file mode 100644 index 00000000..78feead0 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_4_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_collision.stl b/model/gen2/assets/merged/rmd_x12_p20_320_collision.stl new file mode 100644 index 00000000..571394bb Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x12_p20_320_visual.stl b/model/gen2/assets/merged/rmd_x12_p20_320_visual.stl new file mode 100644 index 00000000..c0d3fdfd Binary files /dev/null and b/model/gen2/assets/merged/rmd_x12_p20_320_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_2_collision.stl b/model/gen2/assets/merged/rmd_x15_p20_450_2_collision.stl new file mode 100644 index 00000000..341d220d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_2_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_2_visual.stl b/model/gen2/assets/merged/rmd_x15_p20_450_2_visual.stl new file mode 100644 index 00000000..f0bbbb3b Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_2_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_collision.stl b/model/gen2/assets/merged/rmd_x15_p20_450_collision.stl new file mode 100644 index 00000000..7eeb1460 Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x15_p20_450_visual.stl b/model/gen2/assets/merged/rmd_x15_p20_450_visual.stl new file mode 100644 index 00000000..1ccc820c Binary files /dev/null and b/model/gen2/assets/merged/rmd_x15_p20_450_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x6_60_collision.stl b/model/gen2/assets/merged/rmd_x6_60_collision.stl new file mode 100644 index 00000000..8cd4da1d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x6_60_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x6_60_visual.stl b/model/gen2/assets/merged/rmd_x6_60_visual.stl new file mode 100644 index 00000000..8cd4da1d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x6_60_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_2_collision.stl b/model/gen2/assets/merged/rmd_x8_120_2_collision.stl new file mode 100644 index 00000000..179f15ab Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_2_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_2_visual.stl b/model/gen2/assets/merged/rmd_x8_120_2_visual.stl new file mode 100644 index 00000000..179f15ab Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_2_visual.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_collision.stl b/model/gen2/assets/merged/rmd_x8_120_collision.stl new file mode 100644 index 00000000..a9462a5d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_collision.stl differ diff --git a/model/gen2/assets/merged/rmd_x8_120_visual.stl b/model/gen2/assets/merged/rmd_x8_120_visual.stl new file mode 100644 index 00000000..a9462a5d Binary files /dev/null and b/model/gen2/assets/merged/rmd_x8_120_visual.stl differ diff --git a/model/gen2/assets/merged/waist_link_1_collision.stl b/model/gen2/assets/merged/waist_link_1_collision.stl new file mode 100644 index 00000000..a10076ce Binary files /dev/null and b/model/gen2/assets/merged/waist_link_1_collision.stl differ diff --git a/model/gen2/assets/merged/waist_link_1_visual.stl b/model/gen2/assets/merged/waist_link_1_visual.stl new file mode 100644 index 00000000..d1fbb1ca Binary files /dev/null and b/model/gen2/assets/merged/waist_link_1_visual.stl differ diff --git a/model/gen2/assets/merged/waist_link_2_collision.stl b/model/gen2/assets/merged/waist_link_2_collision.stl new file mode 100644 index 00000000..2d0419f1 Binary files /dev/null and b/model/gen2/assets/merged/waist_link_2_collision.stl differ diff --git a/model/gen2/assets/merged/waist_link_2_visual.stl b/model/gen2/assets/merged/waist_link_2_visual.stl new file mode 100644 index 00000000..6222cb48 Binary files /dev/null and b/model/gen2/assets/merged/waist_link_2_visual.stl differ diff --git a/model/gen2/assets/part3.part b/model/gen2/assets/part3.part new file mode 100644 index 00000000..ef9a84f2 --- /dev/null +++ b/model/gen2/assets/part3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "MZbolAETzbjTWQxGh", + "isStandardContent": false, + "name": "part3 <3>", + "partId": "SfEHJ", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part3.stl b/model/gen2/assets/part3.stl new file mode 100644 index 00000000..9d10b000 Binary files /dev/null and b/model/gen2/assets/part3.stl differ diff --git a/model/gen2/assets/part_1.part b/model/gen2/assets/part_1.part new file mode 100644 index 00000000..e2b20911 --- /dev/null +++ b/model/gen2/assets/part_1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "8e795471a63f9aeeb3f38bc9", + "fullConfiguration": "default", + "id": "M/fsWjcYigJevNTSo", + "isStandardContent": false, + "name": "Part 1 <10>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1.stl b/model/gen2/assets/part_1.stl new file mode 100644 index 00000000..02f96fca Binary files /dev/null and b/model/gen2/assets/part_1.stl differ diff --git a/model/gen2/assets/part_1__2.part b/model/gen2/assets/part_1__2.part new file mode 100644 index 00000000..921fbf70 --- /dev/null +++ b/model/gen2/assets/part_1__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "b62d5cd0a79626536433550c", + "fullConfiguration": "default", + "id": "Ml8S+i6AJBUQrvKPI", + "isStandardContent": false, + "name": "Part 1 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__2.stl b/model/gen2/assets/part_1__2.stl new file mode 100644 index 00000000..416c6253 Binary files /dev/null and b/model/gen2/assets/part_1__2.stl differ diff --git a/model/gen2/assets/part_1__3.part b/model/gen2/assets/part_1__3.part new file mode 100644 index 00000000..cf5897ac --- /dev/null +++ b/model/gen2/assets/part_1__3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "75bf03a59d086af969a33c01", + "fullConfiguration": "default", + "id": "MMSWppHq6Yjq0A4GJ", + "isStandardContent": false, + "name": "Part 1 <7>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__3.stl b/model/gen2/assets/part_1__3.stl new file mode 100644 index 00000000..86db7640 Binary files /dev/null and b/model/gen2/assets/part_1__3.stl differ diff --git a/model/gen2/assets/part_1__4.part b/model/gen2/assets/part_1__4.part new file mode 100644 index 00000000..c6d767db --- /dev/null +++ b/model/gen2/assets/part_1__4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "ea07b7966a3a2629b98a8575", + "fullConfiguration": "default", + "id": "MHhqsoHbvZI16AyiH", + "isStandardContent": false, + "name": "Part 1 <8>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__4.stl b/model/gen2/assets/part_1__4.stl new file mode 100644 index 00000000..fa42f58e Binary files /dev/null and b/model/gen2/assets/part_1__4.stl differ diff --git a/model/gen2/assets/part_1__5.part b/model/gen2/assets/part_1__5.part new file mode 100644 index 00000000..215e2ff0 --- /dev/null +++ b/model/gen2/assets/part_1__5.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "4efa01c02aae2d87545c9f16", + "fullConfiguration": "default", + "id": "MniQ8B1Rcr+eQTXJ6", + "isStandardContent": false, + "name": "Part 1 <9>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__5.stl b/model/gen2/assets/part_1__5.stl new file mode 100644 index 00000000..15d3e282 Binary files /dev/null and b/model/gen2/assets/part_1__5.stl differ diff --git a/model/gen2/assets/part_1__6.part b/model/gen2/assets/part_1__6.part new file mode 100644 index 00000000..712007df --- /dev/null +++ b/model/gen2/assets/part_1__6.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "6d51c3e57aab39664d6b1921", + "fullConfiguration": "default", + "id": "MxXxHbGlThzhJ469V", + "isStandardContent": false, + "name": "Part 1 <6>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__6.stl b/model/gen2/assets/part_1__6.stl new file mode 100644 index 00000000..fee8b326 Binary files /dev/null and b/model/gen2/assets/part_1__6.stl differ diff --git a/model/gen2/assets/part_1__7.part b/model/gen2/assets/part_1__7.part new file mode 100644 index 00000000..ba1bce44 --- /dev/null +++ b/model/gen2/assets/part_1__7.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M6WjFDvOl7wQzdTgE", + "isStandardContent": false, + "name": "Part 1 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__7.stl b/model/gen2/assets/part_1__7.stl new file mode 100644 index 00000000..ece3c67e Binary files /dev/null and b/model/gen2/assets/part_1__7.stl differ diff --git a/model/gen2/assets/part_1__8.part b/model/gen2/assets/part_1__8.part new file mode 100644 index 00000000..bbb9208c --- /dev/null +++ b/model/gen2/assets/part_1__8.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M7sFILMoD0PvVpFyz", + "isStandardContent": false, + "name": "Part 1 <2>", + "partId": "SWBXB", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_1__8.stl b/model/gen2/assets/part_1__8.stl new file mode 100644 index 00000000..f4620b67 Binary files /dev/null and b/model/gen2/assets/part_1__8.stl differ diff --git a/model/gen2/assets/part_2.part b/model/gen2/assets/part_2.part new file mode 100644 index 00000000..578cbc90 --- /dev/null +++ b/model/gen2/assets/part_2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "ea07b7966a3a2629b98a8575", + "fullConfiguration": "default", + "id": "MJXJUqhkZxXZYFfWJ", + "isStandardContent": false, + "name": "Part 2 <3>", + "partId": "JTD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_2.stl b/model/gen2/assets/part_2.stl new file mode 100644 index 00000000..3aab8358 Binary files /dev/null and b/model/gen2/assets/part_2.stl differ diff --git a/model/gen2/assets/part_2__2.part b/model/gen2/assets/part_2__2.part new file mode 100644 index 00000000..f3b4534a --- /dev/null +++ b/model/gen2/assets/part_2__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "4efa01c02aae2d87545c9f16", + "fullConfiguration": "default", + "id": "MvgANl+gNFugUkrMM", + "isStandardContent": false, + "name": "Part 2 <4>", + "partId": "JQD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_2__2.stl b/model/gen2/assets/part_2__2.stl new file mode 100644 index 00000000..46bc8f08 Binary files /dev/null and b/model/gen2/assets/part_2__2.stl differ diff --git a/model/gen2/assets/part_3.part b/model/gen2/assets/part_3.part new file mode 100644 index 00000000..e6cbda9f --- /dev/null +++ b/model/gen2/assets/part_3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "ea07b7966a3a2629b98a8575", + "fullConfiguration": "default", + "id": "M3lQHqrq4hZJcX0rS", + "isStandardContent": false, + "name": "Part 3 <3>", + "partId": "JmD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_3.stl b/model/gen2/assets/part_3.stl new file mode 100644 index 00000000..172ce3aa Binary files /dev/null and b/model/gen2/assets/part_3.stl differ diff --git a/model/gen2/assets/part_3__2.part b/model/gen2/assets/part_3__2.part new file mode 100644 index 00000000..743fb1b2 --- /dev/null +++ b/model/gen2/assets/part_3__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "4efa01c02aae2d87545c9f16", + "fullConfiguration": "default", + "id": "MHE2bGvITgPh7wTVN", + "isStandardContent": false, + "name": "Part 3 <4>", + "partId": "JfD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/part_3__2.stl b/model/gen2/assets/part_3__2.stl new file mode 100644 index 00000000..b0f1d141 Binary files /dev/null and b/model/gen2/assets/part_3__2.stl differ diff --git a/model/gen2/assets/ph11_n_51_101_e.part b/model/gen2/assets/ph11_n_51_101_e.part new file mode 100644 index 00000000..89c42643 --- /dev/null +++ b/model/gen2/assets/ph11_n_51_101_e.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "6a3e0b943524913fea5a4822", + "documentMicroversion": "cf8035afbce324bdc7a9e006", + "documentVersion": "351c8cbd1b321620e615819b", + "elementId": "f7464a1f93ca23cb11010a6f", + "fullConfiguration": "default", + "id": "MvgDf/uZU1+Rtwreu", + "isStandardContent": false, + "name": "PH11-N-51&101-E <3>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ph11_n_51_101_e.stl b/model/gen2/assets/ph11_n_51_101_e.stl new file mode 100644 index 00000000..a5850d84 Binary files /dev/null and b/model/gen2/assets/ph11_n_51_101_e.stl differ diff --git a/model/gen2/assets/ph17_2_2.part b/model/gen2/assets/ph17_2_2.part new file mode 100644 index 00000000..570907cb --- /dev/null +++ b/model/gen2/assets/ph17_2_2.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "e255f9ff9fda9450c77eb8f1", + "documentMicroversion": "20b63849a1768546e32fe1c1", + "documentVersion": "7c38174c464a84fcb345a027", + "elementId": "36ec63486326e8a1e99f315c", + "fullConfiguration": "default", + "id": "MIcouqN6LN1fhDLKv", + "isStandardContent": false, + "name": "PH17-2_2 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/ph17_2_2.stl b/model/gen2/assets/ph17_2_2.stl new file mode 100644 index 00000000..e0a05a7c Binary files /dev/null and b/model/gen2/assets/ph17_2_2.stl differ diff --git a/model/gen2/assets/rmd_x12_p20_320.part b/model/gen2/assets/rmd_x12_p20_320.part new file mode 100644 index 00000000..66dbd7b3 --- /dev/null +++ b/model/gen2/assets/rmd_x12_p20_320.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "62d7fd99f6f8bc18d006f61d", + "documentMicroversion": "dbf97b0b2f1de35e36aa78e5", + "documentVersion": "947d0353c559b9c880806e11", + "elementId": "e1b8602fc8f4029943e2295e", + "fullConfiguration": "default", + "id": "M4evZkgyz6yFULD1S", + "isStandardContent": false, + "name": "RMD-X12-P20-320 <4>", + "partId": "JGD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x12_p20_320.stl b/model/gen2/assets/rmd_x12_p20_320.stl new file mode 100644 index 00000000..e7476420 Binary files /dev/null and b/model/gen2/assets/rmd_x12_p20_320.stl differ diff --git a/model/gen2/assets/rmd_x15_p20_450.part b/model/gen2/assets/rmd_x15_p20_450.part new file mode 100644 index 00000000..1021d5f6 --- /dev/null +++ b/model/gen2/assets/rmd_x15_p20_450.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "ec857d60fcb70103cc12860c", + "documentMicroversion": "1a2acfb65995cbdcea466261", + "documentVersion": "16d2b0eab42ae6c11ddb97f7", + "elementId": "6ba9083deb449364f7d336a0", + "fullConfiguration": "default", + "id": "MYOenBISeP8bZc8Qo", + "isStandardContent": false, + "name": "RMD-X15-P20-450 <2>", + "partId": "JGD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x15_p20_450.stl b/model/gen2/assets/rmd_x15_p20_450.stl new file mode 100644 index 00000000..e6ac780b Binary files /dev/null and b/model/gen2/assets/rmd_x15_p20_450.stl differ diff --git a/model/gen2/assets/rmd_x6_60.part b/model/gen2/assets/rmd_x6_60.part new file mode 100644 index 00000000..859e38e4 --- /dev/null +++ b/model/gen2/assets/rmd_x6_60.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "b60b2b1541b6caba79879c87", + "documentMicroversion": "e89aeab6208ada7cddc14963", + "documentVersion": "e6dd4bc54af7df350f2b61f5", + "elementId": "4210d66ddc6c74d4a3225701", + "fullConfiguration": "default", + "id": "MJXQun7eeHfI/CBG4", + "isStandardContent": false, + "name": "RMD-X6-60 <2>", + "partId": "JJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x6_60.stl b/model/gen2/assets/rmd_x6_60.stl new file mode 100644 index 00000000..cc6a9970 Binary files /dev/null and b/model/gen2/assets/rmd_x6_60.stl differ diff --git a/model/gen2/assets/rmd_x8_120.part b/model/gen2/assets/rmd_x8_120.part new file mode 100644 index 00000000..17ff104f --- /dev/null +++ b/model/gen2/assets/rmd_x8_120.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "7ab7b9fa4ac0bac59e4b7f31", + "documentMicroversion": "3f29b3a24d3d3bc9d28f8291", + "documentVersion": "f7cb5e3afb025dafb27bbb69", + "elementId": "d519a1f4dd46602465d25754", + "fullConfiguration": "default", + "id": "MGNRNjyq5zHPCRaGM", + "isStandardContent": false, + "name": "RMD-X8-120 <3>", + "partId": "JJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/rmd_x8_120.stl b/model/gen2/assets/rmd_x8_120.stl new file mode 100644 index 00000000..459cde68 Binary files /dev/null and b/model/gen2/assets/rmd_x8_120.stl differ diff --git a/model/gen2/assets/waist_link_1.part b/model/gen2/assets/waist_link_1.part new file mode 100644 index 00000000..59d28b59 --- /dev/null +++ b/model/gen2/assets/waist_link_1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "MaMIM7xMUAfPCsjBw", + "isStandardContent": false, + "name": "waist_link_1 <1>", + "partId": "R6ED", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/waist_link_1.stl b/model/gen2/assets/waist_link_1.stl new file mode 100644 index 00000000..a5c12747 Binary files /dev/null and b/model/gen2/assets/waist_link_1.stl differ diff --git a/model/gen2/assets/waist_link_2.part b/model/gen2/assets/waist_link_2.part new file mode 100644 index 00000000..86a753f1 --- /dev/null +++ b/model/gen2/assets/waist_link_2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "384bd645b0ca4a89ac420dd3", + "fullConfiguration": "default", + "id": "M751ldLqHrvZy5x87", + "isStandardContent": false, + "name": "waist_link_2 <1>", + "partId": "RaFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/waist_link_2.stl b/model/gen2/assets/waist_link_2.stl new file mode 100644 index 00000000..b6000c4c Binary files /dev/null and b/model/gen2/assets/waist_link_2.stl differ diff --git a/model/gen2/assets/不锈钢推拉杆.part b/model/gen2/assets/不锈钢推拉杆.part new file mode 100644 index 00000000..dc5a6fd1 --- /dev/null +++ b/model/gen2/assets/不锈钢推拉杆.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "be59b5b3fc36c192f76143e2", + "fullConfiguration": "default", + "id": "Mt9kcvd/03CDMTgIs", + "isStandardContent": false, + "name": "\u4e0d\u9508\u94a2\u63a8\u62c9\u6746 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/不锈钢推拉杆.stl b/model/gen2/assets/不锈钢推拉杆.stl new file mode 100644 index 00000000..13ad6356 Binary files /dev/null and b/model/gen2/assets/不锈钢推拉杆.stl differ diff --git a/model/gen2/assets/关节轴承__外.part b/model/gen2/assets/关节轴承__外.part new file mode 100644 index 00000000..5b9fc3e1 --- /dev/null +++ b/model/gen2/assets/关节轴承__外.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "ebe12e2f11c612cfba1a391d", + "fullConfiguration": "default", + "id": "Mpi9R8DS1HtWAKAPT", + "isStandardContent": false, + "name": "\u5173\u8282\u8f74\u627f -\u5916 <4>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/关节轴承__外.stl b/model/gen2/assets/关节轴承__外.stl new file mode 100644 index 00000000..17755efe Binary files /dev/null and b/model/gen2/assets/关节轴承__外.stl differ diff --git a/model/gen2/assets/右侧板_下.part b/model/gen2/assets/右侧板_下.part new file mode 100644 index 00000000..a74ec8de --- /dev/null +++ b/model/gen2/assets/右侧板_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M3XYgvzxX84GAVmSg", + "isStandardContent": false, + "name": "\u53f3\u4fa7\u677f\uff08\u4e0b\uff09 <1>", + "partId": "JYD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右侧板_下.stl b/model/gen2/assets/右侧板_下.stl new file mode 100644 index 00000000..1cafe0b4 Binary files /dev/null and b/model/gen2/assets/右侧板_下.stl differ diff --git a/model/gen2/assets/右滑槽.part b/model/gen2/assets/右滑槽.part new file mode 100644 index 00000000..13d731df --- /dev/null +++ b/model/gen2/assets/右滑槽.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MZVXkKtoaUSIqtlGn", + "isStandardContent": false, + "name": "\u53f3\u6ed1\u69fd <1>", + "partId": "JxD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右滑槽.stl b/model/gen2/assets/右滑槽.stl new file mode 100644 index 00000000..71f7f65d Binary files /dev/null and b/model/gen2/assets/右滑槽.stl differ diff --git a/model/gen2/assets/右滑盖.part b/model/gen2/assets/右滑盖.part new file mode 100644 index 00000000..03d91a19 --- /dev/null +++ b/model/gen2/assets/右滑盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "M80GcYpLl6V+aHmLK", + "isStandardContent": false, + "name": "\u53f3\u6ed1\u76d6 <1>", + "partId": "RTBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右滑盖.stl b/model/gen2/assets/右滑盖.stl new file mode 100644 index 00000000..71c1b73e Binary files /dev/null and b/model/gen2/assets/右滑盖.stl differ diff --git a/model/gen2/assets/右臂法兰.part b/model/gen2/assets/右臂法兰.part new file mode 100644 index 00000000..142c1140 --- /dev/null +++ b/model/gen2/assets/右臂法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M/96q4NYqfHK8laya", + "isStandardContent": false, + "name": "\u53f3\u81c2\u6cd5\u5170 <1>", + "partId": "RiBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右臂法兰.stl b/model/gen2/assets/右臂法兰.stl new file mode 100644 index 00000000..e5d0d417 Binary files /dev/null and b/model/gen2/assets/右臂法兰.stl differ diff --git a/model/gen2/assets/右臂电机.part b/model/gen2/assets/右臂电机.part new file mode 100644 index 00000000..4876bb78 --- /dev/null +++ b/model/gen2/assets/右臂电机.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MP8GXDKWNCto3hS8V", + "isStandardContent": false, + "name": "\u53f3\u81c2\u7535\u673a <1>", + "partId": "R/BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/右臂电机.stl b/model/gen2/assets/右臂电机.stl new file mode 100644 index 00000000..a9f70a89 Binary files /dev/null and b/model/gen2/assets/右臂电机.stl differ diff --git a/model/gen2/assets/吊装_右.part b/model/gen2/assets/吊装_右.part new file mode 100644 index 00000000..0ceb2f3c --- /dev/null +++ b/model/gen2/assets/吊装_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MQBanFl24uiIqwBsw", + "isStandardContent": false, + "name": "\u540a\u88c5-\u53f3 <1>", + "partId": "RzHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/吊装_右.stl b/model/gen2/assets/吊装_右.stl new file mode 100644 index 00000000..79518b87 Binary files /dev/null and b/model/gen2/assets/吊装_右.stl differ diff --git a/model/gen2/assets/吊装_左.part b/model/gen2/assets/吊装_左.part new file mode 100644 index 00000000..40c72253 --- /dev/null +++ b/model/gen2/assets/吊装_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MvYDW8Ut5YsNBqjh/", + "isStandardContent": false, + "name": "\u540a\u88c5-\u5de6 <1>", + "partId": "R5HD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/吊装_左.stl b/model/gen2/assets/吊装_左.stl new file mode 100644 index 00000000..38d8f9a4 Binary files /dev/null and b/model/gen2/assets/吊装_左.stl differ diff --git a/model/gen2/assets/外壳1.part b/model/gen2/assets/外壳1.part new file mode 100644 index 00000000..56f7ad0c --- /dev/null +++ b/model/gen2/assets/外壳1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MTqNjRvBwQ+HQVcHs", + "isStandardContent": false, + "name": "\u5916\u58f31 <1>", + "partId": "JKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳1.stl b/model/gen2/assets/外壳1.stl new file mode 100644 index 00000000..ef49e798 Binary files /dev/null and b/model/gen2/assets/外壳1.stl differ diff --git a/model/gen2/assets/外壳2.part b/model/gen2/assets/外壳2.part new file mode 100644 index 00000000..58f28146 --- /dev/null +++ b/model/gen2/assets/外壳2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MC4nkIik1bp1s556v", + "isStandardContent": false, + "name": "\u5916\u58f32 <1>", + "partId": "JZD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳2.stl b/model/gen2/assets/外壳2.stl new file mode 100644 index 00000000..905fc07a Binary files /dev/null and b/model/gen2/assets/外壳2.stl differ diff --git a/model/gen2/assets/外壳_侧盖_右.part b/model/gen2/assets/外壳_侧盖_右.part new file mode 100644 index 00000000..6e911c9d --- /dev/null +++ b/model/gen2/assets/外壳_侧盖_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MKQXi3/0fXRuXGG40", + "isStandardContent": false, + "name": "\u5916\u58f3-\u4fa7\u76d6-\u53f3 <1>", + "partId": "RSMH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_侧盖_右.stl b/model/gen2/assets/外壳_侧盖_右.stl new file mode 100644 index 00000000..5bd255d2 Binary files /dev/null and b/model/gen2/assets/外壳_侧盖_右.stl differ diff --git a/model/gen2/assets/外壳_侧盖_左.part b/model/gen2/assets/外壳_侧盖_左.part new file mode 100644 index 00000000..8418e394 --- /dev/null +++ b/model/gen2/assets/外壳_侧盖_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "ML9n/y47JVCppqO4O", + "isStandardContent": false, + "name": "\u5916\u58f3-\u4fa7\u76d6-\u5de6 <1>", + "partId": "RSMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_侧盖_左.stl b/model/gen2/assets/外壳_侧盖_左.stl new file mode 100644 index 00000000..41027c9e Binary files /dev/null and b/model/gen2/assets/外壳_侧盖_左.stl differ diff --git a/model/gen2/assets/外壳_前盖.part b/model/gen2/assets/外壳_前盖.part new file mode 100644 index 00000000..99e26e9d --- /dev/null +++ b/model/gen2/assets/外壳_前盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MjC0LlUXkhLEI3+sQ", + "isStandardContent": false, + "name": "\u5916\u58f3-\u524d\u76d6 <1>", + "partId": "RjJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_前盖.stl b/model/gen2/assets/外壳_前盖.stl new file mode 100644 index 00000000..ed0012d9 Binary files /dev/null and b/model/gen2/assets/外壳_前盖.stl differ diff --git a/model/gen2/assets/外壳_前盖_下.part b/model/gen2/assets/外壳_前盖_下.part new file mode 100644 index 00000000..9a119be9 --- /dev/null +++ b/model/gen2/assets/外壳_前盖_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MXzWnzC8eVA1K9AOp", + "isStandardContent": false, + "name": "\u5916\u58f3-\u524d\u76d6-\u4e0b <1>", + "partId": "RpJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_前盖_下.stl b/model/gen2/assets/外壳_前盖_下.stl new file mode 100644 index 00000000..fa4ff1e4 Binary files /dev/null and b/model/gen2/assets/外壳_前盖_下.stl differ diff --git a/model/gen2/assets/外壳_后盖.part b/model/gen2/assets/外壳_后盖.part new file mode 100644 index 00000000..25a78225 --- /dev/null +++ b/model/gen2/assets/外壳_后盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MEpXtt7oMgb6bd4K7", + "isStandardContent": false, + "name": "\u5916\u58f3-\u540e\u76d6 <1>", + "partId": "R6MD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_后盖.stl b/model/gen2/assets/外壳_后盖.stl new file mode 100644 index 00000000..f236debb Binary files /dev/null and b/model/gen2/assets/外壳_后盖.stl differ diff --git a/model/gen2/assets/外壳_后盖_下.part b/model/gen2/assets/外壳_后盖_下.part new file mode 100644 index 00000000..06a6f04a --- /dev/null +++ b/model/gen2/assets/外壳_后盖_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "M00Wo3f4AHVleGN4f", + "isStandardContent": false, + "name": "\u5916\u58f3-\u540e\u76d6-\u4e0b <1>", + "partId": "RSML", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/外壳_后盖_下.stl b/model/gen2/assets/外壳_后盖_下.stl new file mode 100644 index 00000000..8b432a9b Binary files /dev/null and b/model/gen2/assets/外壳_后盖_下.stl differ diff --git a/model/gen2/assets/小腿.part b/model/gen2/assets/小腿.part new file mode 100644 index 00000000..e23ebc77 --- /dev/null +++ b/model/gen2/assets/小腿.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "38be5a8344ea08045e614079", + "fullConfiguration": "default", + "id": "M+9mGjSFXYjvXOSGj", + "isStandardContent": false, + "name": "\u5c0f\u817f <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/小腿.stl b/model/gen2/assets/小腿.stl new file mode 100644 index 00000000..6320ed25 Binary files /dev/null and b/model/gen2/assets/小腿.stl differ diff --git a/model/gen2/assets/小腿__2.part b/model/gen2/assets/小腿__2.part new file mode 100644 index 00000000..fc30f489 --- /dev/null +++ b/model/gen2/assets/小腿__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "a51a941965828d24d7f9ab81", + "fullConfiguration": "default", + "id": "MwioXAIZK7wvJewBS", + "isStandardContent": false, + "name": "\u5c0f\u817f <2>", + "partId": "RKCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/小腿__2.stl b/model/gen2/assets/小腿__2.stl new file mode 100644 index 00000000..a6ae2ebb Binary files /dev/null and b/model/gen2/assets/小腿__2.stl differ diff --git a/model/gen2/assets/左侧板_下.part b/model/gen2/assets/左侧板_下.part new file mode 100644 index 00000000..f99f3177 --- /dev/null +++ b/model/gen2/assets/左侧板_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MGrb7N3ttkchxFr+R", + "isStandardContent": false, + "name": "\u5de6\u4fa7\u677f\uff08\u4e0b\uff09 <1>", + "partId": "RQCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左侧板_下.stl b/model/gen2/assets/左侧板_下.stl new file mode 100644 index 00000000..4e6e6f3f Binary files /dev/null and b/model/gen2/assets/左侧板_下.stl differ diff --git a/model/gen2/assets/左滑槽.part b/model/gen2/assets/左滑槽.part new file mode 100644 index 00000000..677955da --- /dev/null +++ b/model/gen2/assets/左滑槽.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MtSjvzM/U6qF2/2vK", + "isStandardContent": false, + "name": "\u5de6\u6ed1\u69fd <1>", + "partId": "JkD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左滑槽.stl b/model/gen2/assets/左滑槽.stl new file mode 100644 index 00000000..1851e191 Binary files /dev/null and b/model/gen2/assets/左滑槽.stl differ diff --git a/model/gen2/assets/左滑盖.part b/model/gen2/assets/左滑盖.part new file mode 100644 index 00000000..08f1633b --- /dev/null +++ b/model/gen2/assets/左滑盖.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MqVnOcEE4czuCcGrq", + "isStandardContent": false, + "name": "\u5de6\u6ed1\u76d6 <1>", + "partId": "RBBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左滑盖.stl b/model/gen2/assets/左滑盖.stl new file mode 100644 index 00000000..3439abd0 Binary files /dev/null and b/model/gen2/assets/左滑盖.stl differ diff --git a/model/gen2/assets/左臂法兰.part b/model/gen2/assets/左臂法兰.part new file mode 100644 index 00000000..a0ccc976 --- /dev/null +++ b/model/gen2/assets/左臂法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mv50nGvmKTJai12AS", + "isStandardContent": false, + "name": "\u5de6\u81c2\u6cd5\u5170 <1>", + "partId": "RSCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左臂法兰.stl b/model/gen2/assets/左臂法兰.stl new file mode 100644 index 00000000..eae8893d Binary files /dev/null and b/model/gen2/assets/左臂法兰.stl differ diff --git a/model/gen2/assets/左臂电机.part b/model/gen2/assets/左臂电机.part new file mode 100644 index 00000000..12667db8 --- /dev/null +++ b/model/gen2/assets/左臂电机.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MJ40cA8u6yvxua234", + "isStandardContent": false, + "name": "\u5de6\u81c2\u7535\u673a <1>", + "partId": "RRCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/左臂电机.stl b/model/gen2/assets/左臂电机.stl new file mode 100644 index 00000000..eae14638 Binary files /dev/null and b/model/gen2/assets/左臂电机.stl differ diff --git a/model/gen2/assets/底座_6.part b/model/gen2/assets/底座_6.part new file mode 100644 index 00000000..6ca56176 --- /dev/null +++ b/model/gen2/assets/底座_6.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "0354ef7293b27d43612c5d0d", + "fullConfiguration": "default", + "id": "MtDL1ISqaj7ZwQD10", + "isStandardContent": false, + "name": "\u5e95\u5ea7-6 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底座_6.stl b/model/gen2/assets/底座_6.stl new file mode 100644 index 00000000..74886465 Binary files /dev/null and b/model/gen2/assets/底座_6.stl differ diff --git a/model/gen2/assets/底板.part b/model/gen2/assets/底板.part new file mode 100644 index 00000000..c4e6b71e --- /dev/null +++ b/model/gen2/assets/底板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MkFlrF8wJBcmivM2P", + "isStandardContent": false, + "name": "\u5e95\u677f <1>", + "partId": "RMBX", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底板.stl b/model/gen2/assets/底板.stl new file mode 100644 index 00000000..3198f6c4 Binary files /dev/null and b/model/gen2/assets/底板.stl differ diff --git a/model/gen2/assets/底部外壳.part b/model/gen2/assets/底部外壳.part new file mode 100644 index 00000000..4be02a59 --- /dev/null +++ b/model/gen2/assets/底部外壳.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MPmUT9gi4bKLP++VU", + "isStandardContent": false, + "name": "\u5e95\u90e8\u5916\u58f3 <1>", + "partId": "RRKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底部外壳.stl b/model/gen2/assets/底部外壳.stl new file mode 100644 index 00000000..8441ccc3 Binary files /dev/null and b/model/gen2/assets/底部外壳.stl differ diff --git a/model/gen2/assets/底部法兰.part b/model/gen2/assets/底部法兰.part new file mode 100644 index 00000000..ec4978a8 --- /dev/null +++ b/model/gen2/assets/底部法兰.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MmuoU/+5K/tn1Uuuz", + "isStandardContent": false, + "name": "\u5e95\u90e8\u6cd5\u5170 <1>", + "partId": "RxJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/底部法兰.stl b/model/gen2/assets/底部法兰.stl new file mode 100644 index 00000000..aba996b5 Binary files /dev/null and b/model/gen2/assets/底部法兰.stl differ diff --git a/model/gen2/assets/开关按钮.part b/model/gen2/assets/开关按钮.part new file mode 100644 index 00000000..635248ef --- /dev/null +++ b/model/gen2/assets/开关按钮.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mv0YjhCiv6JK7HE7+", + "isStandardContent": false, + "name": "\u5f00\u5173\u6309\u94ae <1>", + "partId": "R6MH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/开关按钮.stl b/model/gen2/assets/开关按钮.stl new file mode 100644 index 00000000..f7c4692a Binary files /dev/null and b/model/gen2/assets/开关按钮.stl differ diff --git a/model/gen2/assets/开关按钮盖板.part b/model/gen2/assets/开关按钮盖板.part new file mode 100644 index 00000000..cbef0ff8 --- /dev/null +++ b/model/gen2/assets/开关按钮盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MvoOTXWzNfTNyzNg1", + "isStandardContent": false, + "name": "\u5f00\u5173\u6309\u94ae\u76d6\u677f <1>", + "partId": "RJND", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/开关按钮盖板.stl b/model/gen2/assets/开关按钮盖板.stl new file mode 100644 index 00000000..328e8ffc Binary files /dev/null and b/model/gen2/assets/开关按钮盖板.stl differ diff --git a/model/gen2/assets/手掌骨架_a.part b/model/gen2/assets/手掌骨架_a.part new file mode 100644 index 00000000..49871eab --- /dev/null +++ b/model/gen2/assets/手掌骨架_a.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "899055c6cd6af50017fdf8fa", + "fullConfiguration": "default", + "id": "MdjPNuoKJPr1LXuLe", + "isStandardContent": false, + "name": "\u624b\u638c\u9aa8\u67b6-A <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/手掌骨架_a.stl b/model/gen2/assets/手掌骨架_a.stl new file mode 100644 index 00000000..8481c61c Binary files /dev/null and b/model/gen2/assets/手掌骨架_a.stl differ diff --git a/model/gen2/assets/折叠驱动器v3.part b/model/gen2/assets/折叠驱动器v3.part new file mode 100644 index 00000000..59d55321 --- /dev/null +++ b/model/gen2/assets/折叠驱动器v3.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "2d4e9f935bf8977ffd5ac0d5", + "fullConfiguration": "default", + "id": "MBoO2CX+KIuRAoE1a", + "isStandardContent": false, + "name": "\u6298\u53e0\u9a71\u52a8\u5668V3 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/折叠驱动器v3.stl b/model/gen2/assets/折叠驱动器v3.stl new file mode 100644 index 00000000..788b3e64 Binary files /dev/null and b/model/gen2/assets/折叠驱动器v3.stl differ diff --git a/model/gen2/assets/指尖_金属部分b.part b/model/gen2/assets/指尖_金属部分b.part new file mode 100644 index 00000000..052f44af --- /dev/null +++ b/model/gen2/assets/指尖_金属部分b.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "c06c3339cd9e4fa2c01e2e2d", + "fullConfiguration": "default", + "id": "MY80To9lh13ublyqF", + "isStandardContent": false, + "name": "\u6307\u5c16-\u91d1\u5c5e\u90e8\u5206B <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指尖_金属部分b.stl b/model/gen2/assets/指尖_金属部分b.stl new file mode 100644 index 00000000..8ac7cf89 Binary files /dev/null and b/model/gen2/assets/指尖_金属部分b.stl differ diff --git a/model/gen2/assets/指尖短_金属部分b.part b/model/gen2/assets/指尖短_金属部分b.part new file mode 100644 index 00000000..8bb283e2 --- /dev/null +++ b/model/gen2/assets/指尖短_金属部分b.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "b0d8e8ffedfa915e327340f4", + "fullConfiguration": "default", + "id": "MmminKSlDg1YFLKmR", + "isStandardContent": false, + "name": "\u6307\u5c16\u77ed-\u91d1\u5c5e\u90e8\u5206B <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指尖短_金属部分b.stl b/model/gen2/assets/指尖短_金属部分b.stl new file mode 100644 index 00000000..86072233 Binary files /dev/null and b/model/gen2/assets/指尖短_金属部分b.stl differ diff --git a/model/gen2/assets/指尖短_金属部分b__2.part b/model/gen2/assets/指尖短_金属部分b__2.part new file mode 100644 index 00000000..f5837f98 --- /dev/null +++ b/model/gen2/assets/指尖短_金属部分b__2.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "ac4bb6bd62c1ad7b487a0ee3", + "documentMicroversion": "034d2f4cc6593a6848f3ae5d", + "documentVersion": "8c333a8c296052edbafa6c2a", + "elementId": "894c7841ae7eb4bf59bb7d60", + "fullConfiguration": "default", + "id": "Mtdn25hbOsANtFGGl", + "isStandardContent": false, + "name": "\u6307\u5c16\u77ed-\u91d1\u5c5e\u90e8\u5206B <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指尖短_金属部分b__2.stl b/model/gen2/assets/指尖短_金属部分b__2.stl new file mode 100644 index 00000000..ae172e13 Binary files /dev/null and b/model/gen2/assets/指尖短_金属部分b__2.stl differ diff --git a/model/gen2/assets/指节_金属b.part b/model/gen2/assets/指节_金属b.part new file mode 100644 index 00000000..017e15f1 --- /dev/null +++ b/model/gen2/assets/指节_金属b.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "9118cb28ca995c46f4e34f4b", + "fullConfiguration": "default", + "id": "MnshumUO1YlD+Ko9U", + "isStandardContent": false, + "name": "\u6307\u8282-\u91d1\u5c5eB <4>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/指节_金属b.stl b/model/gen2/assets/指节_金属b.stl new file mode 100644 index 00000000..75e6e7ad Binary files /dev/null and b/model/gen2/assets/指节_金属b.stl differ diff --git a/model/gen2/assets/控制器挂杆_上.part b/model/gen2/assets/控制器挂杆_上.part new file mode 100644 index 00000000..6006a515 --- /dev/null +++ b/model/gen2/assets/控制器挂杆_上.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mh5IYfyGrF6wvmGzK", + "isStandardContent": false, + "name": "\u63a7\u5236\u5668\u6302\u6746-\u4e0a <1>", + "partId": "R3CD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/控制器挂杆_上.stl b/model/gen2/assets/控制器挂杆_上.stl new file mode 100644 index 00000000..396d0c64 Binary files /dev/null and b/model/gen2/assets/控制器挂杆_上.stl differ diff --git a/model/gen2/assets/控制器挂杆_下.part b/model/gen2/assets/控制器挂杆_下.part new file mode 100644 index 00000000..0a98c366 --- /dev/null +++ b/model/gen2/assets/控制器挂杆_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MgLDlq/TavYbYmODE", + "isStandardContent": false, + "name": "\u63a7\u5236\u5668\u6302\u6746-\u4e0b <1>", + "partId": "R3CH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/控制器挂杆_下.stl b/model/gen2/assets/控制器挂杆_下.stl new file mode 100644 index 00000000..91b20963 Binary files /dev/null and b/model/gen2/assets/控制器挂杆_下.stl differ diff --git a/model/gen2/assets/末端关节.part b/model/gen2/assets/末端关节.part new file mode 100644 index 00000000..1d65e974 --- /dev/null +++ b/model/gen2/assets/末端关节.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "fa23ed383daf64f447915c2b", + "documentMicroversion": "293169235e93895e758df8bb", + "documentVersion": "0369694b643f0badb7315400", + "elementId": "0da12e30653684e6b7330282", + "fullConfiguration": "default", + "id": "MeCqnhCcxfa176sxX", + "isStandardContent": false, + "name": "\u672b\u7aef\u5173\u8282 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/末端关节.stl b/model/gen2/assets/末端关节.stl new file mode 100644 index 00000000..1bd0336f Binary files /dev/null and b/model/gen2/assets/末端关节.stl differ diff --git a/model/gen2/assets/末端关节电机安装组件.part b/model/gen2/assets/末端关节电机安装组件.part new file mode 100644 index 00000000..32b418cd --- /dev/null +++ b/model/gen2/assets/末端关节电机安装组件.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "551d2d195f1f472f8549e41c", + "documentMicroversion": "228c31412604fbd205c7c7f0", + "documentVersion": "5b355140b1fda9df977e21e8", + "elementId": "915704befc3a0a3894e0594e", + "fullConfiguration": "default", + "id": "MJQBjMB33DiZ2dP41", + "isStandardContent": false, + "name": "\u672b\u7aef\u5173\u8282\u7535\u673a\u5b89\u88c5\u7ec4\u4ef6 <2>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/末端关节电机安装组件.stl b/model/gen2/assets/末端关节电机安装组件.stl new file mode 100644 index 00000000..78de11c1 Binary files /dev/null and b/model/gen2/assets/末端关节电机安装组件.stl differ diff --git a/model/gen2/assets/横杆2.part b/model/gen2/assets/横杆2.part new file mode 100644 index 00000000..e5bbcb96 --- /dev/null +++ b/model/gen2/assets/横杆2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MFmdDenJlDZ5t+msP", + "isStandardContent": false, + "name": "\u6a2a\u67462 <1>", + "partId": "RMBP", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/横杆2.stl b/model/gen2/assets/横杆2.stl new file mode 100644 index 00000000..3b7ff533 Binary files /dev/null and b/model/gen2/assets/横杆2.stl differ diff --git a/model/gen2/assets/横杆3.part b/model/gen2/assets/横杆3.part new file mode 100644 index 00000000..69325329 --- /dev/null +++ b/model/gen2/assets/横杆3.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MOT0xoI5qNjvxnH0z", + "isStandardContent": false, + "name": "\u6a2a\u67463 <1>", + "partId": "RMBf", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/横杆3.stl b/model/gen2/assets/横杆3.stl new file mode 100644 index 00000000..c2a3be68 Binary files /dev/null and b/model/gen2/assets/横杆3.stl differ diff --git a/model/gen2/assets/横杆4.part b/model/gen2/assets/横杆4.part new file mode 100644 index 00000000..43a7e0ca --- /dev/null +++ b/model/gen2/assets/横杆4.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MLqo4Lsvzz5zxWbrl", + "isStandardContent": false, + "name": "\u6a2a\u67464 <1>", + "partId": "RMBj", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/横杆4.stl b/model/gen2/assets/横杆4.stl new file mode 100644 index 00000000..bbe793e0 Binary files /dev/null and b/model/gen2/assets/横杆4.stl differ diff --git a/model/gen2/assets/电池仓.part b/model/gen2/assets/电池仓.part new file mode 100644 index 00000000..16a9373a --- /dev/null +++ b/model/gen2/assets/电池仓.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MDvrSJHryY8/2Hi0J", + "isStandardContent": false, + "name": "\u7535\u6c60\u4ed3 <1>", + "partId": "RBHz", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/电池仓.stl b/model/gen2/assets/电池仓.stl new file mode 100644 index 00000000..fd6f401a Binary files /dev/null and b/model/gen2/assets/电池仓.stl differ diff --git a/model/gen2/assets/电芯.part b/model/gen2/assets/电芯.part new file mode 100644 index 00000000..2b99efc0 --- /dev/null +++ b/model/gen2/assets/电芯.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "Mdy8jYdL+09IxJQ6R", + "isStandardContent": false, + "name": "\u7535\u82af <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/电芯.stl b/model/gen2/assets/电芯.stl new file mode 100644 index 00000000..cc91c8a2 Binary files /dev/null and b/model/gen2/assets/电芯.stl differ diff --git a/model/gen2/assets/盖子.part b/model/gen2/assets/盖子.part new file mode 100644 index 00000000..1229749e --- /dev/null +++ b/model/gen2/assets/盖子.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "c3418b956eedf6bf166586dd", + "fullConfiguration": "default", + "id": "MolC3SwHEf2Qq06MU", + "isStandardContent": false, + "name": "\u76d6\u5b50 <1>", + "partId": "RIBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/盖子.stl b/model/gen2/assets/盖子.stl new file mode 100644 index 00000000..bc29455a Binary files /dev/null and b/model/gen2/assets/盖子.stl differ diff --git a/model/gen2/assets/胸部支撑_右.part b/model/gen2/assets/胸部支撑_右.part new file mode 100644 index 00000000..84e9b1ac --- /dev/null +++ b/model/gen2/assets/胸部支撑_右.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MCqQDpJWvDl+yXTAF", + "isStandardContent": false, + "name": "\u80f8\u90e8\u652f\u6491-\u53f3 <1>", + "partId": "RdDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/胸部支撑_右.stl b/model/gen2/assets/胸部支撑_右.stl new file mode 100644 index 00000000..dbc3321b Binary files /dev/null and b/model/gen2/assets/胸部支撑_右.stl differ diff --git a/model/gen2/assets/胸部支撑_左.part b/model/gen2/assets/胸部支撑_左.part new file mode 100644 index 00000000..c11cb00b --- /dev/null +++ b/model/gen2/assets/胸部支撑_左.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MI7nrS71IHygPjEQu", + "isStandardContent": false, + "name": "\u80f8\u90e8\u652f\u6491-\u5de6 <1>", + "partId": "RVDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/胸部支撑_左.stl b/model/gen2/assets/胸部支撑_左.stl new file mode 100644 index 00000000..29337f3b Binary files /dev/null and b/model/gen2/assets/胸部支撑_左.stl differ diff --git a/model/gen2/assets/脚踝.part b/model/gen2/assets/脚踝.part new file mode 100644 index 00000000..f1e26cd6 --- /dev/null +++ b/model/gen2/assets/脚踝.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "MADl4hipSxmbYTIZt", + "isStandardContent": false, + "name": "\u811a\u8e1d <1>", + "partId": "RzBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝.stl b/model/gen2/assets/脚踝.stl new file mode 100644 index 00000000..4442a13e Binary files /dev/null and b/model/gen2/assets/脚踝.stl differ diff --git a/model/gen2/assets/脚踝__2.part b/model/gen2/assets/脚踝__2.part new file mode 100644 index 00000000..a9f9cfec --- /dev/null +++ b/model/gen2/assets/脚踝__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "MErAFGluT0GUcFkk0", + "isStandardContent": false, + "name": "\u811a\u8e1d <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝__2.stl b/model/gen2/assets/脚踝__2.stl new file mode 100644 index 00000000..28cf6678 Binary files /dev/null and b/model/gen2/assets/脚踝__2.stl differ diff --git a/model/gen2/assets/脚踝盖板.part b/model/gen2/assets/脚踝盖板.part new file mode 100644 index 00000000..2d13ea6a --- /dev/null +++ b/model/gen2/assets/脚踝盖板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "3ed0f82d52069da883c2af7a", + "fullConfiguration": "default", + "id": "MmXV24uIhptbe5WAb", + "isStandardContent": false, + "name": "\u811a\u8e1d\u76d6\u677f <1>", + "partId": "R2BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝盖板.stl b/model/gen2/assets/脚踝盖板.stl new file mode 100644 index 00000000..1401ca79 Binary files /dev/null and b/model/gen2/assets/脚踝盖板.stl differ diff --git a/model/gen2/assets/脚踝盖板__2.part b/model/gen2/assets/脚踝盖板__2.part new file mode 100644 index 00000000..e7f89d1c --- /dev/null +++ b/model/gen2/assets/脚踝盖板__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "69b5318f86ab1549d2294bb3", + "elementId": "08e441b1ea6915f4c8e5b947", + "fullConfiguration": "default", + "id": "MY1VzHUkp29mkYztr", + "isStandardContent": false, + "name": "\u811a\u8e1d\u76d6\u677f <2>", + "partId": "JrD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/脚踝盖板__2.stl b/model/gen2/assets/脚踝盖板__2.stl new file mode 100644 index 00000000..1f26d34d Binary files /dev/null and b/model/gen2/assets/脚踝盖板__2.stl differ diff --git a/model/gen2/assets/路由器挂杆_上.part b/model/gen2/assets/路由器挂杆_上.part new file mode 100644 index 00000000..256987fc --- /dev/null +++ b/model/gen2/assets/路由器挂杆_上.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MydLgTIuREEzGv2GG", + "isStandardContent": false, + "name": "\u8def\u7531\u5668\u6302\u6746-\u4e0a <1>", + "partId": "RoCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/路由器挂杆_上.stl b/model/gen2/assets/路由器挂杆_上.stl new file mode 100644 index 00000000..a9fa5415 Binary files /dev/null and b/model/gen2/assets/路由器挂杆_上.stl differ diff --git a/model/gen2/assets/路由器挂杆_下.part b/model/gen2/assets/路由器挂杆_下.part new file mode 100644 index 00000000..329efa8a --- /dev/null +++ b/model/gen2/assets/路由器挂杆_下.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MdJHUl2i3izPMt9Ct", + "isStandardContent": false, + "name": "\u8def\u7531\u5668\u6302\u6746-\u4e0b <1>", + "partId": "RrCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/路由器挂杆_下.stl b/model/gen2/assets/路由器挂杆_下.stl new file mode 100644 index 00000000..251f1e90 Binary files /dev/null and b/model/gen2/assets/路由器挂杆_下.stl differ diff --git a/model/gen2/assets/转接件.part b/model/gen2/assets/转接件.part new file mode 100644 index 00000000..0b138df7 --- /dev/null +++ b/model/gen2/assets/转接件.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "80ba63b613294cdd216840c7", + "documentMicroversion": "988d004c15b7387e8f7ce387", + "documentVersion": "de65ed1f664a5219ffb57b25", + "elementId": "46406d0b66f4940cc911ad8a", + "fullConfiguration": "default", + "id": "MN9ebc5PQdTrTHfSL", + "isStandardContent": false, + "name": "\u8f6c\u63a5\u4ef6 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/转接件.stl b/model/gen2/assets/转接件.stl new file mode 100644 index 00000000..3be11f9d Binary files /dev/null and b/model/gen2/assets/转接件.stl differ diff --git a/model/gen2/assets/转接法兰.part b/model/gen2/assets/转接法兰.part new file mode 100644 index 00000000..3fb97f87 --- /dev/null +++ b/model/gen2/assets/转接法兰.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "c56408749058b658d80e6d2e", + "documentMicroversion": "15b7b67a921cb84d8b985bd8", + "documentVersion": "e686f077317bdcd23693af0b", + "elementId": "d97365e7f25c01f7e677c7c4", + "fullConfiguration": "default", + "id": "MLD5bO1BfctD8vbcp", + "isStandardContent": false, + "name": "\u8f6c\u63a5\u6cd5\u5170 <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/转接法兰.stl b/model/gen2/assets/转接法兰.stl new file mode 100644 index 00000000..eb619ec5 Binary files /dev/null and b/model/gen2/assets/转接法兰.stl differ diff --git a/model/gen2/assets/轴承轴向定位支撑.part b/model/gen2/assets/轴承轴向定位支撑.part new file mode 100644 index 00000000..48210dba --- /dev/null +++ b/model/gen2/assets/轴承轴向定位支撑.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "be59b5b3fc36c192f76143e2", + "fullConfiguration": "default", + "id": "MhYFeXPznLasm6sLJ", + "isStandardContent": false, + "name": "\u8f74\u627f\u8f74\u5411\u5b9a\u4f4d\u652f\u6491 <1>", + "partId": "JMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/轴承轴向定位支撑.stl b/model/gen2/assets/轴承轴向定位支撑.stl new file mode 100644 index 00000000..c5d9d0fe Binary files /dev/null and b/model/gen2/assets/轴承轴向定位支撑.stl differ diff --git a/model/gen2/assets/轴承轴向定位支撑__2.part b/model/gen2/assets/轴承轴向定位支撑__2.part new file mode 100644 index 00000000..55c77c5f --- /dev/null +++ b/model/gen2/assets/轴承轴向定位支撑__2.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "be59b5b3fc36c192f76143e2", + "fullConfiguration": "default", + "id": "MsOmGauaviz3dyT5u", + "isStandardContent": false, + "name": "\u8f74\u627f\u8f74\u5411\u5b9a\u4f4d\u652f\u6491 <2>", + "partId": "JUD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/轴承轴向定位支撑__2.stl b/model/gen2/assets/轴承轴向定位支撑__2.stl new file mode 100644 index 00000000..a150efa8 Binary files /dev/null and b/model/gen2/assets/轴承轴向定位支撑__2.stl differ diff --git a/model/gen2/assets/音响.part b/model/gen2/assets/音响.part new file mode 100644 index 00000000..5b527a42 --- /dev/null +++ b/model/gen2/assets/音响.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "Mlqrn7VrYrssl1rlG", + "isStandardContent": false, + "name": "\u97f3\u54cd <1>", + "partId": "RIJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/音响.stl b/model/gen2/assets/音响.stl new file mode 100644 index 00000000..9e275a09 Binary files /dev/null and b/model/gen2/assets/音响.stl differ diff --git a/model/gen2/assets/颈部支撑板.part b/model/gen2/assets/颈部支撑板.part new file mode 100644 index 00000000..a6418a7f --- /dev/null +++ b/model/gen2/assets/颈部支撑板.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "3fb6fc43af8407628b8a436e", + "documentMicroversion": "b26d4d3d1e4a010d0d624309", + "elementId": "2b20557d3415aa6a7868cfec", + "fullConfiguration": "default", + "id": "MEqAIszUMOTcIvTSN", + "isStandardContent": false, + "name": "\u9888\u90e8\u652f\u6491\u677f <1>", + "partId": "RkDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/颈部支撑板.stl b/model/gen2/assets/颈部支撑板.stl new file mode 100644 index 00000000..cf9efb8b Binary files /dev/null and b/model/gen2/assets/颈部支撑板.stl differ diff --git a/model/gen2/assets/马达应变片螺母.part b/model/gen2/assets/马达应变片螺母.part new file mode 100644 index 00000000..ea0e2f7f --- /dev/null +++ b/model/gen2/assets/马达应变片螺母.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "66b96c1baefaecb2587c6a38", + "documentMicroversion": "5a6771aad20d0878ba724103", + "documentVersion": "7a87d2599ff530a501cb9297", + "elementId": "f2384e9af7a4e4dc42e82439", + "fullConfiguration": "default", + "id": "MgSXd7s3VkizXEC4M", + "isStandardContent": false, + "name": "\u9a6c\u8fbe\u5e94\u53d8\u7247\u87ba\u6bcd <2>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/model/gen2/assets/马达应变片螺母.stl b/model/gen2/assets/马达应变片螺母.stl new file mode 100644 index 00000000..72dbb9c4 Binary files /dev/null and b/model/gen2/assets/马达应变片螺母.stl differ diff --git a/model/gen2/collision/gen2_collision.toml b/model/gen2/collision/gen2_collision.toml new file mode 100644 index 00000000..a1e07f96 --- /dev/null +++ b/model/gen2/collision/gen2_collision.toml @@ -0,0 +1,9 @@ +# Simplified primitive colliders generated from each link's visual mesh. +[defaults] +primitive = "box" +padding = 0.0 +scale = 1.0 + +# Keep the torso collider aligned with the body frame for predictable arm clearance. +[links.body_link] +alignment = "link" diff --git a/model/gen2/collision/robot_collision.urdf b/model/gen2/collision/robot_collision.urdf new file mode 100644 index 00000000..3a3c11de --- /dev/null +++ b/model/gen2/collision/robot_collision.urdf @@ -0,0 +1,926 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/gen2/config.json b/model/gen2/config.json new file mode 100644 index 00000000..1a65ecd4 --- /dev/null +++ b/model/gen2/config.json @@ -0,0 +1,10 @@ +// config.json general options +// for urdf or mujoco specific options, see documentation +{ + // Onshape assembly URL + "url": "https://cad.onshape.com/documents/3fb6fc43af8407628b8a436e/w/97510b712561d1cd4f61c300/e/926f5c272b7ea2919e153a0d", + // Output format: urdf or mujoco (required) + "output_format": "urdf", + "merge_stls": true, + "simplify_stls": "visual" +} diff --git a/model/gen2/gen2_fixed.xml b/model/gen2/gen2_fixed.xml new file mode 100644 index 00000000..0cf56738 --- /dev/null +++ b/model/gen2/gen2_fixed.xml @@ -0,0 +1,262 @@ + + + + diff --git a/model/gen2/robot.urdf b/model/gen2/robot.urdf new file mode 100644 index 00000000..f3327b16 --- /dev/null +++ b/model/gen2/robot.urdf @@ -0,0 +1,928 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/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 index ded6d8cd..521fd5e1 100644 --- 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 @@ -22,12 +22,22 @@ message CollisionSafetyConfig { double stop_distance_m = 2; } +message ProtectiveRecoveryConfig { + double clear_distance_m = 1; + double stable_period_s = 2; + double max_joint_velocity_rad_s = 3; + double max_joint_acceleration_rad_s2 = 4; + double history_duration_s = 5; + double max_distance_regression_m = 6; +} + message SelfCollisionTaskConfig { string id = 1; string arm_id = 2; SelfCollisionCheckerConfig checker = 10; DistanceSamplingConfig sampling = 11; CollisionSafetyConfig safety = 12; + ProtectiveRecoveryConfig recovery = 13; } message SelfCollisionTaskRootConfig {