茂之钳
de86f6004c
- MotionJoint with type, anchor, axis, limits - MotionSimulation: step/run with driver functions - Constant speed and custom driver support - Collision detection integration during simulation - 13 tests: all joint types, limits, reset, multi-joint chains
154 lines
5.6 KiB
C++
154 lines
5.6 KiB
C++
#pragma once
|
||
/**
|
||
* @file motion_simulation.h
|
||
* @brief 装配体运动仿真
|
||
*
|
||
* 定义运动副（铰链、滑动、球面等），支持步进仿真和碰撞响应。
|
||
*
|
||
* @ingroup brep
|
||
*/
|
||
|
||
#include "vde/brep/assembly.h"
|
||
#include "vde/core/point.h"
|
||
#include <vector>
|
||
#include <string>
|
||
#include <functional>
|
||
|
||
namespace vde::brep {
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Joint Types
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/// 运动副类型
|
||
enum class JointType {
|
||
Fixed, ///< 固定连接（无相对运动）
|
||
Revolute, ///< 旋转副：绕轴旋转
|
||
Prismatic, ///< 滑动副：沿轴平移
|
||
Cylindrical, ///< 圆柱副：旋转 + 平移（同轴）
|
||
Spherical, ///< 球面副：绕点旋转（3 自由度）
|
||
Planar, ///< 平面副：平面内 2D 运动
|
||
};
|
||
|
||
/// 运动副定义：连接装配体树中两个节点
|
||
struct MotionJoint {
|
||
std::string name; ///< 运动副名称
|
||
JointType type = JointType::Fixed;
|
||
|
||
/// 连接的节点路径（装配体树中名称，用于查找）
|
||
std::string parent_link; ///< 父连杆名称
|
||
std::string child_link; ///< 子连杆名称
|
||
|
||
/// 运动副几何定义
|
||
core::Point3D anchor; ///< 锚点（世界坐标）
|
||
core::Vector3D axis; ///< 轴方向（单位向量，对 Revolute/Prismatic/Cylindrical）
|
||
core::Vector3D plane_normal; ///< 平面法线（对 Planar）
|
||
|
||
/// 运动范围限制
|
||
double limit_min = -std::numeric_limits<double>::max(); ///< 下限（弧度或长度）
|
||
double limit_max = std::numeric_limits<double>::max(); ///< 上限
|
||
};
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Simulation State
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/// 仿真状态（单帧）
|
||
struct SimulationState {
|
||
double time = 0.0; ///< 仿真时间
|
||
std::vector<double> joint_values; ///< 各关节当前值（角度/位移）
|
||
std::vector<std::string> collisions; ///< 碰撞信息
|
||
};
|
||
|
||
/// 仿真配置
|
||
struct SimulationConfig {
|
||
double time_step = 0.016; ///< 时间步长（默认 60 FPS）
|
||
double duration = 1.0; ///< 总仿真时长
|
||
bool detect_collision = true; ///< 是否检测碰撞
|
||
bool stop_on_collision = false; ///< 碰撞时是否停止
|
||
};
|
||
|
||
// ═══════════════════════════════════════════════════════════
|
||
// Motion Simulation
|
||
// ═══════════════════════════════════════════════════════════
|
||
|
||
/**
|
||
* @brief 运动仿真引擎
|
||
*
|
||
* 管理运动副，执行步进仿真，更新装配体节点变换。
|
||
*
|
||
* @code{.cpp}
|
||
* MotionSimulation sim;
|
||
* sim.add_joint({"hinge", JointType::Revolute,
|
||
* "base", "arm", Point3D(0,0,0), Vector3D(0,1,0)});
|
||
*
|
||
* sim.set_driver(0, [](double t) { return sin(t); }); // 正弦驱动
|
||
*
|
||
* auto states = sim.run(assembly, {.time_step=0.016, .duration=2.0});
|
||
* for (auto& s : states) {
|
||
* std::cout << "t=" << s.time << " joint0=" << s.joint_values[0] << "\n";
|
||
* }
|
||
* @endcode
|
||
*
|
||
* @ingroup brep
|
||
*/
|
||
class MotionSimulation {
|
||
public:
|
||
/// 添加运动副
|
||
void add_joint(const MotionJoint& joint);
|
||
|
||
/// 获取运动副数量
|
||
[[nodiscard]] size_t joint_count() const { return joints_.size(); }
|
||
|
||
/// 设置关节驱动函数（t → value）
|
||
/// @param joint_index 关节索引
|
||
/// @param driver 驱动函数 f(t) → joint value
|
||
void set_driver(int joint_index, std::function<double(double)> driver);
|
||
|
||
/// 设置恒定速度驱动
|
||
/// @param joint_index 关节索引
|
||
/// @param speed 恒定速度（rad/s 或 units/s）
|
||
void set_constant_speed(int joint_index, double speed);
|
||
|
||
/**
|
||
* @brief 运行仿真
|
||
*
|
||
* 按时间步长推进，每步更新装配体节点变换。
|
||
* 若 detect_collision 为 true，每步检测干涉。
|
||
*
|
||
* @param assembly 装配体（会被原地修改）
|
||
* @param config 仿真配置
|
||
* @return 各时间步的状态列表
|
||
*/
|
||
[[nodiscard]] std::vector<SimulationState> run(
|
||
Assembly& assembly, const SimulationConfig& config);
|
||
|
||
/**
|
||
* @brief 单步仿真
|
||
*
|
||
* 推进一个时间步长，更新装配体和关节值。
|
||
*
|
||
* @param assembly 装配体
|
||
* @param dt 时间步长
|
||
* @return 当前仿真状态
|
||
*/
|
||
[[nodiscard]] SimulationState step(Assembly& assembly, double dt);
|
||
|
||
/// 重置仿真时间
|
||
void reset() { time_ = 0.0; joint_values_.assign(joints_.size(), 0.0); }
|
||
|
||
private:
|
||
std::vector<MotionJoint> joints_;
|
||
std::vector<std::function<double(double)>> drivers_;
|
||
std::vector<double> joint_values_;
|
||
double time_ = 0.0;
|
||
|
||
/// Apply joint transform to the assembly
|
||
void apply_joint_transform(Assembly& assembly, int joint_idx, double value);
|
||
|
||
/// Find assembly node by name
|
||
static AssemblyNode* find_node(AssemblyNode& root, const std::string& name);
|
||
};
|
||
|
||
} // namespace vde::brep
|