体与器
位点（Locus）
The place where something is situated or occurs. — Cambridge Dictionary
在多体动力学中，准确描述物体上各个物质点的位置和方位是建立连接、施加外力和处理接触碰撞的基础。此外，在处理接触和碰撞时，这些点表面的法线方向、摩擦系数和回弹系数等物理属性也至关重要。
为了统一且高效地管理这些信息，Rible 引入了位点（Locus）和位点状态（LocusState）类型。其中Locus 是一个immutable类型，用于在物体的局部坐标系中定义一个物质点的不变属性：
position：该点在局部坐标系中的位置向量。axes：该点处的局部参考轴系（Axes），通常用于定义接触面的法线方向或关节的旋转/平移轴。默认情况下，第一根轴（x轴）即为接触法向。friction_coefficient和restitution_coefficient：分别定义该点处的摩擦系数和碰撞回弹系数。
构造位点
在构建刚体时，我们需要提供质心的 mass_locus 以及一系列用于后续连接或接触的位点 loci 向量。
julia
# 定义局部坐标系下的位点 (Locus)
mass_locus = Locus(SVector(-0.2, 0.0, 0.0)) # 质心坐标，默认无摩擦/回弹
# 定义两个位点。由于构造函数的灵活分派，我们可以按需提供参数
loci = [
# 位点1：指定位置和参考轴法线
Locus(SVector(0.0, 0.0, 0.0), SVector(1.0, 0.0, 0.0)),
# 位点2：指定位置、参考轴法线、摩擦系数(0.5)和回弹系数(0.1)
Locus(SVector(0.5, 0.0, 0.0), SVector(1.0, 0.0, 0.0), 0.5, 0.1)
]2-element Vector{Locus{3, Float64, 9}}:
Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0)
Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)LocusState 则是mutable类型，记录了该点在仿真过程中的全局（世界）坐标系状态：
frame：该点在全局坐标系下的完整运动学框架（CartesianFrame），包括它当前的全局位置、全局姿态（旋转矩阵）、全局线速度和全局角速度。force和torque：当前作用在该点上的全局力和力矩。contact_state：维护当前点的接触状态，例如是否正处于激活的接触中、接触间隙（gap）以及接触力等。
体（Body）
体用一源，显微无间。 — 程颐《易传序》
体（Body）是多体系统的基础构建块。在 Rible 中，所有的体都是抽象类型 AbstractBody{N,T} 的子类，其中 N 代表空间维度（2或3），T 代表数值类型（通常为 Float64）。这种参数化设计确保了系统既能处理平面问题也能处理空间问题，同时保持类型安全和高性能。
刚体 （RigidBody）
刚体（RigidBody）是多体系统中最基本的单元之一，在Rible中它由RigidBodyProperty、RigidBodyState、坐标描述（Coordinates）和用于可视化的 Mesh 共同构成。
刚体属性
RigidBodyProperty 定义刚体的不变量。这包括质量属性（质量、惯量）和几何特征（质心位置、位点 Loci）。 一个刚体的物理特性由 RigidBodyProperty 定义，它不仅包含动力学参数，还包含仿真所需的元数据。动力学参数主要涉及质量（mass）和惯性张量（inertia），其中惯性张量通常存储为 SMatrix 以优化内存和计算速度。 此外，每个刚体都有唯一的 id 和可选的 type（符号标记）。 布尔字段 contactable 和 visible 分别控制刚体是否参与接触检测以及是否在可视化中渲染。
julia
id = 1
mass = 2.5
inertia = SMatrix{3,3,Float64}(I) # 单位惯性矩阵
prop = RigidBodyProperty(
id,
true, # contactable
mass,
inertia,
mass_locus,
loci;
visible=true
)RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 2.5, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)])刚体状态
RigidBodyState 维护刚体的瞬时状态。这包含了在全局坐标系中的位置矢量和表示方向的旋转矩阵，以及线速度和角速度。这些状态量是仿真器在每个时间步更新的对象。
julia
r0 = SVector(0.0, 0.0, 0.0) # 初始位置
R = SMatrix{3,3}(I) # 初始姿态 (无旋转)
r_dot = zero(r0) # 初始线速度
omega = zero(r0) # 初始角速度
state = RigidBodyState(prop, r0, R, r_dot, omega)RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])])坐标描述
系统使用节点坐标法 (NCF) 来描述运动。根据物体的类型（如自由浮动的块、连接两点的连杆），选择合适的坐标公式。例如，NC1P3V（1点3矢量）适用于标准的6自由度刚体，NC3D2P（3D 2点）适用于由两个端点定义的连杆，而 NC2D2P 则用于平面内的连杆。
julia
# 对于一般的空间刚体，使用一个基准点和三个方向矢量 (NC1P3V)
coords = NCF.NC1P3V(r0, r0, R)NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6])可视化网格
可以为刚体加载可视化用的网格。
julia
# 加载 STL 文件并进行缩放/平移 (示例)
# mesh = load(assetpath("link.STL")) |> make_patch(; scale=0.1)
mesh = nothing # 此处省略详细网格构造
最后，将上述所有实例组装为刚体。
julia
# 最终组装实体
body = RigidBody(prop, state, coords, mesh)RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 2.5, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [2.5, -0.5, 2.5, -0.5, 2.5, -0.5, -0.5, 0.6, -0.5, 0.6, -0.5, 0.6, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [0.48, 0.4, 0.48, 0.4, 0.48, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing)在内部，会自动构建一个 RigidBodyCache 用于存储中间计算缓存。
器 （Apparatus）
大器免成。 ――《老子》帛书本
器件（Apparatus）由 铰接（Joint） 和力（Force）组成，是系统中代表物理上的关节或力（如弹簧）的核心组件，也是多体动力学方程中约束项和控制力的载体。
力（Force）
力（Force）负责计算作用在体上的广义力。所有力模型均继承自抽象类 AbstractForce。
基础力元包括：
距离弹簧阻尼器 (
DistanceSpringDamper)，这是最基础的力元，连接两个点（Loci），维护着DistanceSpringDamperState以存储跟踪当前的长度、伸长率和张力。旋转/扭转弹簧 (
Torsional/RotationalSpringDamper) 则用于产生恢复力矩，通常作用于关节的转轴上，抵抗相对旋转。
铰接（Joint）
铰接（Joint）定义了体与体之间的相对连接关系。所有铰接模型均继承自 AbstractJoint。
铰接通常（但不总是）会对多体动力学方程引入约束力。标准铰接及其变体通过 ProtoJoint 类型实现：
| 关节类型 | 自由度 | 说明 |
|---|---|---|
:Fixed | 0 | 固定关节。将两个刚体完全锁定，或将刚体上的某点锁定在空间固定位置（FixedPointJoint）。常用于基座固定。 |
:Revolute | 1 | 转动关节。允许绕共用轴旋转。需要指定两个物体上的点重合以及轴线共线。 |
:Prismatic | 1 | 移动关节。允许沿轴线平移，限制旋转。 |
:Cylindrical | 2 | 圆柱关节。允许绕轴旋转和沿轴滑动。 |
:Universal | 2 | 万向节。由两个正交的转动轴组成，允许两个方向的旋转。 |
:Spherical | 3 | 球关节。仅约束平动，允许绕连接点任意旋转。 |
定义连接
构建一个关节的核心在于清楚描述"连接关系"。这通过以下两个类型实现：
锚点 (Anchor) — 精确定位连接点的几何信息（位置与约束轴）。
Anchor(body, pid)— 从同一个位点读取位置与轴。Anchor(body, position_pid, axes_pid)— 位置与轴分别来自不同位点（常用于转动关节等需要指定轴向的场景）。
连接对 (Hen2Egg) — 定义"母体"（Hen）与"子体"（Egg）的配对。
Hen2Egg(hen_anchor, egg_anchor)— 建立两个锚点之间的对应关系。
构建关节时，先用 Anchor 明确两端几何，再用 Hen2Egg 描述拓扑方向。
铰接构造示例
julia
prop1 = RigidBodyProperty(1, true, 1.0, inertia, mass_locus, loci)
state1 = RigidBodyState(prop1, r0, R, r_dot, omega)
nmcs1 = NCF.NC1P3V(r0, r0, R)
body1 = RigidBody(prop1, state1, nmcs1, nothing)
prop2 = RigidBodyProperty(2, true, 1.0, inertia, mass_locus, loci)
r0_2 = r0 + SVector(1.0, 0.0, 0.0)
state2 = RigidBodyState(prop2, r0_2, R, r_dot, omega)
nmcs2 = NCF.NC1P3V(r0_2, r0_2, R)
body2 = RigidBody(prop2, state2, nmcs2, nothing)RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 2, :generic, 1.0, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.8, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.0, -0.2, 1.0, -0.2, 1.0, -0.2, -0.2, 0.54, -0.2, 0.54, -0.2, 0.54, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.08, 0.4, 1.08, 0.4, 1.08, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing)julia
# 1. 定义连接锚点：连接 body1 的第2个位点和 body2 的第1个位点
hen = Anchor(body1, 2)
egg = Anchor(body2, 1)
# 2. 建立连接关系
conn = Hen2Egg(hen, egg)Hen2Egg{Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}, Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}}(Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}(RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 1.0, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.0, -0.2, 1.0, -0.2, 1.0, -0.2, -0.2, 0.54, -0.2, 0.54, -0.2, 0.54, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.08, 0.4, 1.08, 0.4, 1.08, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing), [0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0])), Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}(RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 2, :generic, 1.0, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.8, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.0, -0.2, 1.0, -0.2, 1.0, -0.2, -0.2, 0.54, -0.2, 0.54, -0.2, 0.54, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.08, 0.4, 1.08, 0.4, 1.08, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing), [0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0])))如果关节需要特定的旋转轴或平移轴（例如转动关节），应使用 Anchor(body, position_pid, axes_pid) 分别指定连接位置与约束轴：
julia
# 指定 body1 上第1个位点作为位置/轴，body2 上第2个位点作为位置/轴
hen_axis = Anchor(body1, 1, 1)
egg_axis = Anchor(body2, 2, 2)
conn_axis = Hen2Egg(hen_axis, egg_axis)Hen2Egg{Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}, Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}}(Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}(RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 1.0, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.0, -0.2, 1.0, -0.2, 1.0, -0.2, -0.2, 0.54, -0.2, 0.54, -0.2, 0.54, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.08, 0.4, 1.08, 0.4, 1.08, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing), [0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0])), Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}(RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 2, :generic, 1.0, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.8, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.0, -0.2, 1.0, -0.2, 1.0, -0.2, -0.2, 0.54, -0.2, 0.54, -0.2, 0.54, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.08, 0.4, 1.08, 0.4, 1.08, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing), [0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0])))创建关节对象时，使用 ProtoJoint 函数和关节类型符号。
julia
# 创建一个球关节
joint_spherical = ProtoJoint(conn, :Spherical)
# 创建一个转动关节
joint_revolute = ProtoJoint(conn_axis, :Revolute)Rible.PrototypeJoint{Hen2Egg{Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}, Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}}, Vector{Int64}, Vector{Float64}, @NamedTuple{relative_core::StaticArraysCore.SMatrix{3, 3, Float64, 9}, transformations::SparseArrays.SparseMatrixCSC{Float64, Int64}, halves::Vector{SparseArrays.SparseMatrixCSC{Float64, Int64}}, hessians::Vector{SparseArrays.SparseMatrixCSC{Float64, Int64}}, joint_q::Vector{Float64}, joint_work::Vector{Float64}, trf_work::Vector{Float64}, num_of_coords_hen::Int64, num_of_coords_egg::Int64}, StaticArraysCore.SVector{3, Float64}}(Hen2Egg{Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}, Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}}(Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}(RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 1, :generic, 1.0, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.0, -0.2, 1.0, -0.2, 1.0, -0.2, -0.2, 0.54, -0.2, 0.54, -0.2, 0.54, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.08, 0.4, 1.08, 0.4, 1.08, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing), [0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0])), Anchor{RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}, StaticArraysCore.SVector{3, Float64}, Rible.Axes{3, Float64, 9}, Rible.Axes{3, Float64, 9}}(RigidBody{3, 3, Float64, 9, NC{3, 3, Float64, 9, 12, 144}, RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}, Nothing}(RigidBodyProperty{3, Float64, 9}(true, true, 2, :generic, 1.0, [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], Locus{3, Float64, 9}([-0.2, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}[Locus{3, Float64, 9}([0.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.0, 0.0), Locus{3, Float64, 9}([0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), 0.5, 0.1)]), RigidBodyState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([0.8, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}[Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.0, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0]), Rible.LocusState{3, 3, Float64, 9}(Rible.CartesianFrame{3, 3, Float64, 9}([1.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([0.0 0.0 1.0; 1.0 0.0 0.0; 0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], Rible.ContactState{3, Float64, 9}(false, true, Inf, Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), [1.0, 0.0, 0.0])]), NC{3, 3, Float64, 9, 12, 144}(1, 3, LNCData{3, 3, Float64, 9}([0.0, 0.0, 0.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]), [1 0 … 0 0; 0 1 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 1 0; 0 0 … 0 1], LinearAlgebra.Symmetric{Int64, SparseArrays.SparseMatrixCSC{Int64, Int64}}[[0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 2 0; 0 0 … 0 2], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0], [0 0 … 0 0; 0 0 … 0 0; … ; 0 0 … 0 0; 0 0 … 0 0]], 6, [1, 2, 3, 4, 5, 6]), RigidBodyCache{StaticArraysCore.SMatrix{3, 12, Float64, 36}, InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}, StaticArraysCore.MMatrix{6, 12, Float64, 72}}([1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], StaticArraysCore.SMatrix{3, 12, Float64, 36}[[1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [1.0 0.0 … 0.0 0.0; 0.0 1.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.0, -0.2, 1.0, -0.2, 1.0, -0.2, -0.2, 0.54, -0.2, 0.54, -0.2, 0.54, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 4, 2, 5, 3, 6, 1, 4, 2, 5, 3, 6, 7, 8, 9, 10, 11, 12], [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9, 10, 11, 12], [1.08, 0.4, 1.08, 0.4, 1.08, 0.4, 0.4, 2.0, 0.4, 2.0, 0.4, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], false), [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]), nothing), [0.5, 0.0, 0.0], Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]), Rible.Axes{3, Float64, 9}([-0.0 -0.0 1.0; 1.0 -0.0 0.0; -0.0 1.0 0.0]))), 5, 1, [1, 2, 3], Int64[], Int64[], [1, 2], [1.5, 0.0, 0.0, 0.0, 0.0], (relative_core = [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], transformations = sparse([1, 2, 3, 1, 2, 3, 1, 2, 3], [1, 2, 3, 13, 14, 15, 16, 17, 18], [-1.0, -1.0, -1.0, 1.0, 1.0, 1.0, 0.5, 0.5, 0.5], 3, 24), halves = SparseArrays.SparseMatrixCSC{Float64, Int64}[sparse(Int64[], Int64[], Float64[], 24, 24), sparse(Int64[], Int64[], Float64[], 24, 24), sparse(Int64[], Int64[], Float64[], 24, 24), sparse([4, 5, 6], [19, 20, 21], [1.0, 1.0, 1.0], 24, 24), sparse([4, 5, 6], [22, 23, 24], [1.0, 1.0, 1.0], 24, 24)], hessians = SparseArrays.SparseMatrixCSC{Float64, Int64}[sparse(Int64[], Int64[], Float64[], 24, 24), sparse(Int64[], Int64[], Float64[], 24, 24), sparse(Int64[], Int64[], Float64[], 24, 24), sparse([19, 20, 21, 4, 5, 6], [4, 5, 6, 19, 20, 21], [1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 24, 24), sparse([22, 23, 24, 4, 5, 6], [4, 5, 6, 22, 23, 24], [1.0, 1.0, 1.0, 1.0, 1.0, 1.0], 24, 24)], joint_q = [6.81027370964735e-310, 6.8102737096505e-310, 6.81027370965367e-310, 6.81026785143114e-310, 6.8102678507782e-310, 6.81027817236367e-310, 6.81027371133903e-310, 6.8102736866879e-310, 6.81027370966474e-310, 6.8102737096663e-310 … 6.81028213069193e-310, 5.629e-320, 1.9403e-319, 6.8102736867069e-310, 6.8102736867148e-310, 1.5e-323, 6.81027368671796e-310, 6.81037711985537e-310, 1.9403e-319, 1.9403e-319], joint_work = [6.81027370964735e-310, 6.8102737096505e-310, 6.81027370965367e-310, 6.81026785143114e-310, 6.8102678507782e-310, 6.81027817236367e-310, 6.81027371133903e-310, 6.8102736866879e-310, 6.81027370966474e-310, 6.8102737096663e-310 … 6.81028213069193e-310, 5.629e-320, 1.9403e-319, 6.8102736867069e-310, 6.8102736867148e-310, 1.5e-323, 6.81027368671796e-310, 6.81037711985537e-310, 1.9403e-319, 1.9403e-319], trf_work = [6.81027619120233e-310, 0.0, 6.81027619120075e-310], num_of_coords_hen = 12, num_of_coords_egg = 12), [0.0, 0.0, 0.0], [0.5, 0.0, 0.0])