Rible.jl

简体中文

English

简体中文

English

Appearance

误差、代价与灵敏度分析 ​

灵敏度分析相关接口仍在开发中。本页描述的相关API仍可能于后续或针对不同求解器而调整。

在轨迹优化（如 iLQR）和强化学习中，我们需要一个标量指标来评价系统表现。Rible 通过 AbstractObjective 体系将物理观测（Gauges）与执行消耗（Actions）聚合为统一的数学目标。灵敏度分析则计算该目标对参数的梯度，支撑梯度类优化算法。

从误差到代价 ​

在 Rible 中，误差是在量器层面定义的局部标量，而代价是在系统层级定义的加权全局指标。

误差量器 ​

如 控制枢纽 所述，ErrorGauge 计算单一维度的偏差——例如末端执行器到目标的欧氏距离，或极杆偏离垂直线的角度。每个 ErrorGauge 产生一个可微分的标量误差。

目标函数 ​

Objective 对所有激活的量器和致动器执行加权求和。总代价 由两部分组成：

轨迹代价终端代价

一个典型的 Objective 对象包含：

  • 轨迹误差权重 — 每个时间步施加。

  • 终端误差权重 — 仅在最终时间步施加，强调目标达成。

  • 致动器权重 — 对控制输入 大小的惩罚（对应能量消耗）。

  • 时间缩放因子 () — 处理变步长或特定积分方案下的权重调整。

核心评价接口：

  • cost!(bot, objt, inst_state, u; mode=:trajectory) — 计算单步瞬时代价。

  • cost!(bot, objt, solver, dt) — 计算整条轨迹的总代价。

微分接口 ​

为梯度类算法提供无缓存微分：

  • cost_gradient! — 代价关于 、、 和参数 的梯度。

  • cost_hessian! — 二阶导数，用于局部二次近似（如 iLQR）。

这些接口底层自动调用 `measure_jacobian!`。

灵敏度分析概述 ​

灵敏度分析度量系统输出对输入与参数变化的响应程度。Rible 提供两类方法，均兼容 Zhong06 离散积分框架：

特性伴随法直接法
计算方向反向模式前向模式
适用场景参数多、输出少参数少、状态维度高
计算复杂度，与参数数目弱相关，与参数数目线性
内存消耗较高（存储前向轨迹）较低（在线推进）
典型应用大规模优化、策略训练实时控制、在线监控

  • 参数维度高（如神经网络）且目标为标量时，优先用伴随法。

  • 参数少且需要在线灵敏度轨迹时，优先用直接法。

伴随灵敏度分析 ​

伴随法通过反向递推变量，一次反向扫描即可得到目标函数对大规模参数的梯度。

理论基础 ​

考虑离散系统：

目标函数：

引入伴随变量 ，反向递推：

参数梯度为：

直接灵敏度分析 ​

直接法将状态方程与灵敏度方程联立，前向推进状态与敏感度。

理论基础 ​

定义灵敏度矩阵  ，灵敏度方程为：

可与原方程同时前向积分，得到 与 的全时域轨迹。

灵敏度分析接口 ​

本页优先介绍公共工作流：先计算目标函数，再构造灵敏度求解器，随后调用 solve!，最后检查 sim.solver_cache。具体缓存类型名属于求解器族内部实现，只有在扩展底层算法时才需要关心。

灵敏度求解器本质上是在已有动力学求解器和目标函数之外再包一层。

  • 伴随法 — adj_solver = DiscreteAdjointDynamicsSolver(dyn_solver, objt)，随后 adj_sen_solver = AdjointDynamicsSensitivitySolver(dyn_solver, adj_solver)。

  • 直接法 — drc_sen_solver = DirectDynamicsSensitivitySolver(dyn_solver, objt)。

  • 执行入口 — solve!(prob, sensitivity_solver; ...) 会返回一个仿真结果，灵敏度输出存放在 sim.solver_cache。

这样做的好处是它延续了动力学模块的公共接口：DynamicsProblem 负责物理模型，DynamicsSolver 负责前向积分，而灵敏度分析只是额外叠加的一层求解策略。

求解完成后，优先从 sim.solver_cache 读取结果，而不是直接依赖某个求解器族的缓存类型名。

  • 伴随梯度 — ∂J∂x₀ᵀ、∂J∂θᵀ、∂J∂cᵀ

  • 直接法雅可比 — Jac_state、Jac_action、Jac_control_params

伴随法字段描述标量目标函数对初始状态和参数组的变化率。直接法字段描述传播后状态对初始状态、驱动量和控制参数的变化率。

示例：代价与灵敏度工作流 ​

步骤 1：构建最小系统 ​

创建一个含误差量器和控制枢纽的单体结构。

julia
using Rible
using StaticArrays
using LinearAlgebra
import TypeSortedCollections as TSC

mass_locus = Locus(SVector(0.0, 0.0, 0.0))
loci = [Locus(SVector(0.0, 0.0, 0.0), SVector(1.0, 0.0, 0.0))]
inertia = SMatrix{3,3,Float64}(I)

prop = RigidBodyProperty(1, true, 1.0, inertia, mass_locus, loci; visible = false)
r0 = SVector(0.0, 0.0, 0.0)
R0 = SMatrix{3,3,Float64}(I)
state = RigidBodyState(prop, r0, R0, zero(r0), zero(r0))
coords = NCF.NC1P3V(r0, r0, R0)
body = RigidBody(prop, state, coords, nothing)

bodies = TSC.TypeSortedCollection([body])
apparatuses = TSC.TypeSortedCollection(Int[])
st = Structure(bodies, apparatuses, Connectivity(bodies, apparatuses))
update!(st)

步骤 2：定义量器、枢纽和目标函数 ​

julia
error_gauge = ErrorGauge(
    1,
    Signifier(body, 1),
    PositionCaptum(),
    [0.1, 0.0, 0.0],
)

capta_gauges = TSC.TypeSortedCollection(Int[])
error_gauges = TSC.TypeSortedCollection([error_gauge])
force_actuator = ExternalForceActuator(
    1,
    Signifier(body, 1),
    NaiveOperator(2),
    [1.0 0.0; 0.0 1.0; 0.0 0.0],
    [0.0, 0.0],
)
actuators = TSC.TypeSortedCollection([force_actuator])

coalition = Coalition(st, capta_gauges, error_gauges, actuators)
hub = ControlHub(st, capta_gauges, error_gauges, actuators, coalition)
bot = Robot(st, hub)
prob = DynamicsProblem(bot; env=GravityEnv())

objt = Objective(
    [10.0],      # 轨迹误差权重
    [1.0],       # 轨迹控制权重
    [100.0],     # 终端误差权重
    [5.0],       # 终端控制权重
    t -> 1 / (1 + t),
)
Objective{Vector{Float64}, Main.var"#2#3"}([10.0], [1.0], [100.0], [5.0], Main.var"#2#3"())

步骤 3：计算代价 ​

julia
inst = bot.structure.state.system
u = [0.2, -0.1]

ϕ_traj = cost!(bot, objt, inst, u; mode = :trajectory)
ϕ_term = cost!(bot, objt, inst, u; mode = :terminal)
ϕ_traj, ϕ_term
(0.07500000000000001, 0.6250000000000001)

步骤 4：伴随灵敏度求解 ​

julia
dyn_solver = DynamicsSolver(Zhong06())
adj_solver = DiscreteAdjointDynamicsSolver(dyn_solver, objt)
adj_sen_solver = AdjointDynamicsSensitivitySolver(dyn_solver, adj_solver)

adj_sim = solve!(prob, adj_sen_solver; tspan = (0.0, 0.01), dt = 1e-3)
Rible.SimulationResult{Simulator{DynamicsProblem{Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}, Rible.Contactless, Rible.Naive, @NamedTuple{}}, Rible.var"#solve!##6#solve!##7", Float64, StructArrays.StructVector{@NamedTuple{residual::Float64, iteration::Int64, walltime::Float64, num_of_contacts::Int64}, @NamedTuple{residual::Vector{Float64}, iteration::Vector{Int64}, walltime::Vector{Float64}, num_of_contacts::Vector{Int64}}, Int64}}, Rible.Adjoint_Sensitivity_Zhong06_Constant_Mass_Cache{AdjointDynamicsSensitivitySolver{DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}, DiscreteAdjointDynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}, Objective{Vector{Float64}, Main.var"#2#3"}}}, Rible.Zhong06JacobianWorkspace{Float64}, Vector{ComponentArrays.ComponentVector{Float64, Vector{Float64}, Tuple{ComponentArrays.Axis{(a = ViewAxis(1:12, Shaped1DAxis((12,))), b = ViewAxis(13:24, Shaped1DAxis((12,))), μ = ViewAxis(25:30, Shaped1DAxis((6,))), τ = ViewAxis(31:30, Shaped1DAxis((0,))))}}}}, Vector{Float64}, Rible.NewtonWorkspace{Vector{Float64}, Matrix{Float64}}, Rible.Zhong06JacobianBlocks{Matrix{Float64}, Matrix{Float64}, Matrix{Float64}}, CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Rible.CostGradient{Vector{Float64}}, Rible.CostGradient{Vector{Float64}}, Vector{Float64}, Vector{Vector{Float64}}, Vector{Vector{Float64}}, Vector{Vector{Float64}}, Vector{Vector{Float64}}, Vector{Vector{Float64}}}}(Simulator{DynamicsProblem{Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}, Rible.Contactless, Rible.Naive, @NamedTuple{}}, Rible.var"#solve!##6#solve!##7", Float64, StructArrays.StructVector{@NamedTuple{residual::Float64, iteration::Int64, walltime::Float64, num_of_contacts::Int64}, @NamedTuple{residual::Vector{Float64}, iteration::Vector{Int64}, walltime::Vector{Float64}, num_of_contacts::Vector{Int64}}, Int64}}(DynamicsProblem{Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}, Rible.Contactless, Rible.Naive, @NamedTuple{}}(Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}(Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}(3, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}((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}[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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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)],), ([1],)), TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}((), ()), Connectivity{Vector{Vector{Int64}}}(1, 0, 0, 0, 12, 6, 0, 6, 6, 6, 0, [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]], [[1, 2, 3, 4, 5, 6]], [[1, 2, 3, 4, 5, 6]], Vector{Int64}[], Vector{Int64}[], [[1]], Signifier{Int64}[Signifier{Int64}(1, 1)], [[1, 2, 3]], [[1, 2, 3]], 2, 3, 3, 0, 3, Vector{Int64}[], Vector{Int64}[]), Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}(CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}[CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0])]), (activated_bits = Bool[0], persistent_bits = Bool[0], friction_coefficients = [0.0], restitution_coefficients = [0.0], gaps = [Inf]), Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}(InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), sparsevec(Int64[], Float64[], 12), sparsevec(Int64[], Float64[], 12), true))), ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}(TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}((), ()), TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}((ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}[ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}(1, Signifier{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}}(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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), 1), PositionCaptum(), [0.1, 0.0, 0.0])],), ([1],)), TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}((ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}[ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}(1, Signifier{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}}(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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), 1), NaiveOperator(2), [1.0 0.0; 0.0 1.0; 0.0 0.0], [0.0, 0.0])],), ([1],)), Coalition(1, 0, 1, 0, 1, Vector{Int64}[], [[1]], 2, [[1, 2]]), (c = Float64[], e = [Inf], u = [0.0, 0.0])), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}[CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.001, [0.0, 0.0, -4.905e-6, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.00981, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.00981, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.002, [0.0, 0.0, -1.962e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.01962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.01962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.003, [0.0, 0.0, -4.4145e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.029429999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.029429999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.004, [0.0, 0.0, -7.848e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.03924, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.03924, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.005, [0.0, 0.0, -0.00012262499999999999, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.04904999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.04904999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.006, [0.0, 0.0, -0.00017657999999999997, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.05885999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.05885999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.007, [0.0, 0.0, -0.00024034499999999994, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.06866999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.06866999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.008, [0.0, 0.0, -0.00031391999999999987, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.0784799999999999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.0784799999999999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.009, [0.0, 0.0, -0.00039730499999999976, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.08828999999999988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.08828999999999988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.01, [0.0, 0.0, -0.0004904999999999996, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.09809999999999984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.09809999999999984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0])], Vector{Rible.Contact{Float64}}[[Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))]], @NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}[(na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = [])], [(c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0])]), Rible.NoPolicy(), Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}(Rible.NoGeometry(), Rible.Gravity{Float64}(9.81)), Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}([0], [0], [0], [0], Rible.var"#default_objective##0#default_objective##1"()), Rible.Contactless(), Rible.Naive(), NamedTuple()), Rible.var"#solve!##6#solve!##7"(), (0.0, 0.01), true, 10, Rible.SolverHistory{StructArrays.StructVector{@NamedTuple{residual::Float64, iteration::Int64, walltime::Float64, num_of_contacts::Int64}, @NamedTuple{residual::Vector{Float64}, iteration::Vector{Int64}, walltime::Vector{Float64}, num_of_contacts::Vector{Int64}}, Int64}}([(residual = 6.8102662851134e-310, iteration = 137843650211600, walltime = 5.0e-324, num_of_contacts = 137841324080912), (residual = 6.8102662851142e-310, iteration = 137843650211600, walltime = 1.0e-323, num_of_contacts = 137841324081008), (residual = 6.81037628259394e-310, iteration = 137841324075440, walltime = 1.5e-323, num_of_contacts = 137843567493128), (residual = 6.8103768120932e-310, iteration = 137841632515808, walltime = 2.0e-323, num_of_contacts = 137843567493128), (residual = 6.81026628513633e-310, iteration = 137843650211600, walltime = 2.5e-323, num_of_contacts = 137841324048496), (residual = 6.81026628515135e-310, iteration = 137841324078512, walltime = 3.0e-323, num_of_contacts = 137841324174592), (residual = 6.8102662851569e-310, iteration = 137843650211600, walltime = 3.5e-323, num_of_contacts = 0), (residual = 6.8103768120932e-310, iteration = 137843650211600, walltime = 4.0e-323, num_of_contacts = 0), (residual = 6.81038523323896e-310, iteration = 137841324174304, walltime = 4.4e-323, num_of_contacts = 0), (residual = 6.81038523323896e-310, iteration = 137843650211600, walltime = 5.0e-323, num_of_contacts = 0)]), true), Rible.Adjoint_Sensitivity_Zhong06_Constant_Mass_Cache{AdjointDynamicsSensitivitySolver{DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}, DiscreteAdjointDynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}, Objective{Vector{Float64}, Main.var"#2#3"}}}, Rible.Zhong06JacobianWorkspace{Float64}, Vector{ComponentArrays.ComponentVector{Float64, Vector{Float64}, Tuple{ComponentArrays.Axis{(a = ViewAxis(1:12, Shaped1DAxis((12,))), b = ViewAxis(13:24, Shaped1DAxis((12,))), μ = ViewAxis(25:30, Shaped1DAxis((6,))), τ = ViewAxis(31:30, Shaped1DAxis((0,))))}}}}, Vector{Float64}, Rible.NewtonWorkspace{Vector{Float64}, Matrix{Float64}}, Rible.Zhong06JacobianBlocks{Matrix{Float64}, Matrix{Float64}, Matrix{Float64}}, CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Rible.CostGradient{Vector{Float64}}, Rible.CostGradient{Vector{Float64}}, Vector{Float64}, Vector{Vector{Float64}}, Vector{Vector{Float64}}, Vector{Vector{Float64}}, Vector{Vector{Float64}}, Vector{Vector{Float64}}}(AdjointDynamicsSensitivitySolver{DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}, DiscreteAdjointDynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}, Objective{Vector{Float64}, Main.var"#2#3"}}}(DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}(Zhong06(), Rible.NoBodySolver(), Rible.NoApparatusSolver(), Rible.NoContactSolver(), NamedTuple()), DiscreteAdjointDynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}, Objective{Vector{Float64}, Main.var"#2#3"}}(DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}(Zhong06(), Rible.NoBodySolver(), Rible.NoApparatusSolver(), Rible.NoContactSolver(), NamedTuple()), Objective{Vector{Float64}, Main.var"#2#3"}([10.0], [1.0], [100.0], [5.0], Main.var"#2#3"()))), Rible.Zhong06JacobianWorkspace{Float64}([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0 0.0; 0.0 0.0; … ; 0.0 0.0; 0.0 0.0], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0], Matrix{Float64}(undef, 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.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], Matrix{Float64}(undef, 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.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 0), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 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; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Matrix{Float64}(undef, 0, 12), Matrix{Float64}(undef, 0, 0), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0], Float64[], [-0.1 0.0 … 0.0 0.0], [0.0 0.0 … 0.0 0.0], Matrix{Float64}(undef, 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], Rible.GaugeWorkspace{Float64}[Rible.GaugeWorkspace{Float64}([-0.1, 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], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], Matrix{Float64}(undef, 3, 0), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[])]), ComponentArrays.ComponentVector{Float64, Vector{Float64}, Tuple{ComponentArrays.Axis{(a = ViewAxis(1:12, Shaped1DAxis((12,))), b = ViewAxis(13:24, Shaped1DAxis((12,))), μ = ViewAxis(25:30, Shaped1DAxis((6,))), τ = ViewAxis(31:30, Shaped1DAxis((0,))))}}}[(a = [100.04471700978681, 0.0, 0.4905985800452181, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [90.03576172679661, 0.0, 0.4415346993752634, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [90.03576172679661, 0.0, 0.4415346993752634, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [80.02780544480541, 0.0, 0.3924708677063077, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [80.02780544480541, 0.0, 0.3924708677063077, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [70.02084716680622, 0.0, 0.3434072318457352, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [70.02084716680622, 0.0, 0.3434072318457352, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [60.014885897780104, 0.0, 0.29434403611477383, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [60.014885897780104, 0.0, 0.29434403611477383, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [50.00992064469025, 0.0, 0.24528162205711926, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [50.00992064469025, 0.0, 0.2452816220571193, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [40.00595041647601, 0.0, 0.19622042814871843, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [40.00595041647601, 0.0, 0.19622042814871843, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [30.002974224047065, 0.0, 0.14716098950870724, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [30.002974224047065, 0.0, 0.14716098950870724, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [20.0009910802775, 0.0, 0.09810393761149645, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [20.0009910802775, 0.0, 0.09810393761149645, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [10.0, 0.0, 0.04904999999999996, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[]), (a = [10.0, 0.0, 0.04904999999999996, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], b = [0.0, 0.0, 0.0, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0, 0.0, -0.0], μ = [-0.0, -0.0, -0.0, -0.0, -0.0, -0.0], τ = Float64[])], [0.001, 0.0009990009990009992, 0.000998003992015968, 0.0009970089730807579, 0.00099601593625498, 0.0009950248756218907, 0.0009940357852882705, 0.00099304865938431, 0.000992063492063492, 0.000991080277502478], Rible.NewtonWorkspace{Vector{Float64}, Matrix{Float64}}([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-10.008955282990199, 0.0, -0.04906388066995473, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.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; -0.0 -0.0 … 0.0 0.0], [100.04471700978681, 0.0, 0.4905985800452181, -0.0, -0.0, -0.0, 0.0, -0.0, -0.0, 0.0  …  -0.0, 0.0, 0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0, -0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], Matrix{Float64}(undef, 0, 0), [1.0 0.0 … -0.0 -0.0; 0.0 1.0 … -0.0 -0.0; … ; -0.0 0.0 … -0.002 0.0; -0.0 0.0 … -0.0 -0.002], [1, 2, 3, 25, 30, 29, 7, 26, 28, 10  …  28, 22, 23, 27, 25, 26, 27, 28, 29, 30]), Rible.Zhong06JacobianBlocks{Matrix{Float64}, Matrix{Float64}, Matrix{Float64}}([1.0 0.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], [-1.0 0.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], [-1.0 0.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; 0.0 0.0], [-0.0 -0.0 -0.0; -0.0 -0.0 -0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0005, [0.0, 0.0, -2.4525e-6, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.004905, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.004905, 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.CostGradient{Vector{Float64}}([-10.0, 0.0, -0.04904999999999996, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0], Float64[], Float64[], Float64[]), Rible.CostGradient{Vector{Float64}}([-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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0], Float64[], Float64[], Float64[]), [-10.0099552829902, 0.0, -0.04906388066995471, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]], [Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[]], [[-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, 0.0, 0.0, 0.0], [-1.0, 0.0, -4.905e-5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.0001962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.00044144999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.0007848, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.00122625, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.0017657999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.0024034499999999993, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.0031391999999999987, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-1.0, 0.0, -0.003973049999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]))

步骤 5：直接灵敏度求解 ​

直接法复用同一个 dyn_solver 和 objt，只需更换外层求解器类型：

julia
drc_sen_solver = DirectDynamicsSensitivitySolver(dyn_solver, objt)
drc_sim = solve!(prob, drc_sen_solver; tspan = (0.0, 0.01), dt = 1e-3)
Rible.SimulationResult{Simulator{DynamicsProblem{Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}, Rible.Contactless, Rible.Naive, @NamedTuple{}}, Rible.var"#solve!##6#solve!##7", Float64, StructArrays.StructVector{@NamedTuple{residual::Float64, iteration::Int64, walltime::Float64, num_of_contacts::Int64}, @NamedTuple{residual::Vector{Float64}, iteration::Vector{Int64}, walltime::Vector{Float64}, num_of_contacts::Vector{Int64}}, Int64}}, Rible.Direct_Sensitivity_Zhong06_Constant_Mass_Cache{DirectDynamicsSensitivitySolver{DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}, Objective{Vector{Float64}, Main.var"#2#3"}}, Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Rible.Zhong06Constants{Float64}, Rible.Zhong06JacobianWorkspace{Float64}, Vector{Float64}, Vector{Matrix{Float64}}, Vector{Matrix{Float64}}, Vector{Matrix{Float64}}, Vector{Vector{Float64}}, Vector{Matrix{Float64}}, Vector{Vector{Float64}}, Vector{Matrix{Float64}}, Vector{Float64}, Matrix{Float64}, Vector{Float64}, Matrix{Float64}}}(Simulator{DynamicsProblem{Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}, Rible.Contactless, Rible.Naive, @NamedTuple{}}, Rible.var"#solve!##6#solve!##7", Float64, StructArrays.StructVector{@NamedTuple{residual::Float64, iteration::Int64, walltime::Float64, num_of_contacts::Int64}, @NamedTuple{residual::Vector{Float64}, iteration::Vector{Int64}, walltime::Vector{Float64}, num_of_contacts::Vector{Int64}}, Int64}}(DynamicsProblem{Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}, Rible.Contactless, Rible.Naive, @NamedTuple{}}(Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}(Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}(3, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}((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}[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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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)],), ([1],)), TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}((), ()), Connectivity{Vector{Vector{Int64}}}(1, 0, 0, 0, 12, 6, 0, 6, 6, 6, 0, [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]], [[1, 2, 3, 4, 5, 6]], [[1, 2, 3, 4, 5, 6]], Vector{Int64}[], Vector{Int64}[], [[1]], Signifier{Int64}[Signifier{Int64}(1, 1)], [[1, 2, 3]], [[1, 2, 3]], 2, 3, 3, 0, 3, Vector{Int64}[], Vector{Int64}[]), Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}(CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}[CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0])]), (activated_bits = Bool[0], persistent_bits = Bool[0], friction_coefficients = [0.0], restitution_coefficients = [0.0], gaps = [Inf]), Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}(InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), sparsevec(Int64[], Float64[], 12), sparsevec(Int64[], Float64[], 12), true))), ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}(TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}((), ()), TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}((ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}[ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}(1, Signifier{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}}(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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), 1), PositionCaptum(), [0.1, 0.0, 0.0])],), ([1],)), TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}((ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}[ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}(1, Signifier{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}}(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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), 1), NaiveOperator(2), [1.0 0.0; 0.0 1.0; 0.0 0.0], [0.0, 0.0])],), ([1],)), Coalition(1, 0, 1, 0, 1, Vector{Int64}[], [[1]], 2, [[1, 2]]), (c = Float64[], e = [Inf], u = [0.0, 0.0])), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}[CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.001, [0.0, 0.0, -4.905e-6, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.00981, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.00981, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.002, [0.0, 0.0, -1.962e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.01962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.01962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.003, [0.0, 0.0, -4.4145e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.029429999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.029429999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.004, [0.0, 0.0, -7.848e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.03924, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.03924, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.005, [0.0, 0.0, -0.00012262499999999999, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.04904999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.04904999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.006, [0.0, 0.0, -0.00017657999999999997, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.05885999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.05885999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.007, [0.0, 0.0, -0.00024034499999999994, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.06866999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.06866999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.008, [0.0, 0.0, -0.00031391999999999987, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.0784799999999999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.0784799999999999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.009, [0.0, 0.0, -0.00039730499999999976, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.08828999999999988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.08828999999999988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.01, [0.0, 0.0, -0.0004904999999999996, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.09809999999999984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.09809999999999984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0])], Vector{Rible.Contact{Float64}}[[Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))]], @NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}[(na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = [])], [(c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0])]), Rible.NoPolicy(), Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}(Rible.NoGeometry(), Rible.Gravity{Float64}(9.81)), Objective{Vector{Int64}, Rible.var"#default_objective##0#default_objective##1"}([0], [0], [0], [0], Rible.var"#default_objective##0#default_objective##1"()), Rible.Contactless(), Rible.Naive(), NamedTuple()), Rible.var"#solve!##6#solve!##7"(), (0.0, 0.01), true, 10, Rible.SolverHistory{StructArrays.StructVector{@NamedTuple{residual::Float64, iteration::Int64, walltime::Float64, num_of_contacts::Int64}, @NamedTuple{residual::Vector{Float64}, iteration::Vector{Int64}, walltime::Vector{Float64}, num_of_contacts::Vector{Int64}}, Int64}}([(residual = 6.81028570722603e-310, iteration = 137843567493128, walltime = 6.81028570722603e-310, num_of_contacts = 137843650211600), (residual = 6.8102857072268e-310, iteration = 137843700934336, walltime = 6.8102857072268e-310, num_of_contacts = 137843650211600), (residual = 6.81028570724184e-310, iteration = 137841408275600, walltime = 6.81028570724184e-310, num_of_contacts = 137841408263184), (residual = 6.81028570726e-310, iteration = 137841408275664, walltime = 6.81028570726e-310, num_of_contacts = 137841408266000), (residual = 6.8102857072624e-310, iteration = 137843701682608, walltime = 6.8102857072624e-310, num_of_contacts = 137843650211600), (residual = 6.81027195231513e-310, iteration = 137841408275728, walltime = 6.81027195231513e-310, num_of_contacts = 137841408268592), (residual = 6.8102704400142e-310, iteration = 137841408275792, walltime = 6.8102704400142e-310, num_of_contacts = 137841382724944), (residual = 6.81038357413965e-310, iteration = 137841408275856, walltime = 6.81038357413965e-310, num_of_contacts = 137841408271824), (residual = 6.81027044029876e-310, iteration = 137841408275920, walltime = 6.81027044029876e-310, num_of_contacts = 137841408274992), (residual = 6.81038523323896e-310, iteration = 137841717282992, walltime = 6.81038523323896e-310, num_of_contacts = 137843650211600)]), true), Rible.Direct_Sensitivity_Zhong06_Constant_Mass_Cache{DirectDynamicsSensitivitySolver{DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}, Objective{Vector{Float64}, Main.var"#2#3"}}, Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}, Rible.NoPolicy, Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}, Rible.Zhong06Constants{Float64}, Rible.Zhong06JacobianWorkspace{Float64}, Vector{Float64}, Vector{Matrix{Float64}}, Vector{Matrix{Float64}}, Vector{Matrix{Float64}}, Vector{Vector{Float64}}, Vector{Matrix{Float64}}, Vector{Vector{Float64}}, Vector{Matrix{Float64}}, Vector{Float64}, Matrix{Float64}, Vector{Float64}, Matrix{Float64}}(DirectDynamicsSensitivitySolver{DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}, Objective{Vector{Float64}, Main.var"#2#3"}}(DynamicsSolver{Zhong06, Rible.NoBodySolver, Rible.NoApparatusSolver, Rible.NoContactSolver, @NamedTuple{}}(Zhong06(), Rible.NoBodySolver(), Rible.NoApparatusSolver(), Rible.NoContactSolver(), NamedTuple()), Objective{Vector{Float64}, Main.var"#2#3"}([10.0], [1.0], [100.0], [5.0], Main.var"#2#3"())), Robot{Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}, ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}, StructArrays.StructVector{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, @NamedTuple{t::Vector{Float64}, q::Vector{Vector{Float64}}, q̇::Vector{Vector{Float64}}, q̈::Vector{Vector{Float64}}, F::Vector{Vector{Float64}}, λ::Vector{Vector{Float64}}, s::Vector{Vector{Float64}}, p::Vector{Vector{Float64}}, c::Vector{Vector{Float64}}}, Int64}, Vector{Vector{Rible.Contact{Float64}}}, Vector{@NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}}, StructArrays.StructVector{@NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}, @NamedTuple{c::Vector{Vector{Float64}}, e::Vector{Vector{Float64}}, u::Vector{Vector{Float64}}}, Int64}}(Structure{TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, Connectivity{Vector{Vector{Int64}}}, Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}, @NamedTuple{activated_bits::BitVector, persistent_bits::BitVector, friction_coefficients::Vector{Float64}, restitution_coefficients::Vector{Float64}, gaps::Vector{Float64}}, Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}}(3, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{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}}}, 1}((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}[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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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)],), ([1],)), TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}((), ()), Connectivity{Vector{Vector{Int64}}}(1, 0, 0, 0, 12, 6, 0, 6, 6, 6, 0, [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [[1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12]], [[1, 2, 3, 4, 5, 6]], [[1, 2, 3, 4, 5, 6]], Vector{Int64}[], Vector{Int64}[], [[1]], Signifier{Int64}[Signifier{Int64}(1, 1)], [[1, 2, 3]], [[1, 2, 3]], 2, 3, 3, 0, 3, Vector{Int64}[], Vector{Int64}[]), Rible.StructureState{CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}, Vector{CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}}}(CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}[CoordinatesState{Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}, Vector{Float64}, SubArray{Float64, 1, Vector{Float64}, Tuple{Vector{Int64}}, false}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0])]), (activated_bits = Bool[0], persistent_bits = Bool[0], friction_coefficients = [0.0], restitution_coefficients = [0.0], gaps = [Inf]), Rible.StructureCache{InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}}(InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseVector{Float64, Int64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), sparsevec(Int64[], Float64[], 12), sparsevec(Int64[], Float64[], 12), true))), ControlHub{TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}, Coalition, @NamedTuple{c::Vector{Float64}, e::Vector{Float64}, u::Vector{Float64}}}(TypeSortedCollections.TypeSortedCollection{Tuple{}, 0}((), ()), TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}}}, 1}((ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}[ErrorGauge{Signifier{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}}, PositionCaptum, Vector{Float64}}(1, Signifier{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}}(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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), 1), PositionCaptum(), [0.1, 0.0, 0.0])],), ([1],)), TypeSortedCollections.TypeSortedCollection{Tuple{Vector{ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}}}, 1}((ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}[ExternalForceActuator{Signifier{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}}, NaiveOperator, Matrix{Float64}, Float64}(1, Signifier{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}}(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, false, 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.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}[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)]), 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.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.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])]), 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]], InertiaCache{SparseArrays.SparseMatrixCSC{Float64, Int64}, SparseArrays.SparseMatrixCSC{Float64, Int64}, StaticArraysCore.MVector{12, Float64}}(sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 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), 1), NaiveOperator(2), [1.0 0.0; 0.0 1.0; 0.0 0.0], [0.0, 0.0])],), ([1],)), Coalition(1, 0, 1, 0, 1, Vector{Int64}[], [[1]], 2, [[1, 2]]), (c = Float64[], e = [Inf], u = [0.0, 0.0])), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}[CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.0, [0.0, 0.0, 0.0, 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, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.001, [0.0, 0.0, -4.905e-6, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.00981, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.00981, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.002, [0.0, 0.0, -1.962e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.01962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.01962, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.003, [0.0, 0.0, -4.4145e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.029429999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.029429999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.004, [0.0, 0.0, -7.848e-5, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.03924, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.03924, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.005, [0.0, 0.0, -0.00012262499999999999, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.04904999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.04904999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.006, [0.0, 0.0, -0.00017657999999999997, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.05885999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.05885999999999998, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.007, [0.0, 0.0, -0.00024034499999999994, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.06866999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.06866999999999997, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.008, [0.0, 0.0, -0.00031391999999999987, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.0784799999999999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.0784799999999999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.009, [0.0, 0.0, -0.00039730499999999976, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.08828999999999988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.08828999999999988, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), CoordinatesState{Float64, Vector{Float64}, Vector{Float64}, Vector{Float64}, Vector{Float64}}(0.01, [0.0, 0.0, -0.0004904999999999996, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], [0.0, 0.0, -0.09809999999999984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, -9.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[], [0.0, 0.0, -0.09809999999999984, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0])], Vector{Rible.Contact{Float64}}[[Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))], [Rible.Contact{Float64}(1, 0.0, 0.0, Rible.ContactState{3, Float64, 9}(false, true, 1.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]))]], @NamedTuple{na::Int64, bodyid2act_idx::Vector{Vector{Int64}}, persistent_idx::Vector{Int64}, activated_bits::BitVector, H::LinearAlgebra.Diagonal{Float64, Vector{Float64}}, activated_restitution_coefficients::Vector{Float64}, D::Matrix{Float64}, Dper::Matrix{Float64}, Dimp::Matrix{Float64}, ∂Dq̇∂q::Matrix{Float64}, ∂DᵀΛ∂q::Matrix{Float64}, ŕ::Vector{Float64}, L::Matrix{Float64}, Lv::Matrix{Float64}, Λ::Vector{Float64}, Γ::Vector{Float64}}[(na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = []), (na = 0, bodyid2act_idx = [[0]], persistent_idx = [], activated_bits = [0], H = Diagonal(Float64[]), activated_restitution_coefficients = [], D = Matrix{Float64}(undef, 0, 12), Dper = Matrix{Float64}(undef, 0, 12), Dimp = Matrix{Float64}(undef, 0, 12), ∂Dq̇∂q = Matrix{Float64}(undef, 0, 12), ∂DᵀΛ∂q = [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], ŕ = [], L = Matrix{Float64}(undef, 0, 0), Lv = Matrix{Float64}(undef, 0, 0), Λ = [], Γ = [])], [(c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0]), (c = Float64[], e = [0.005000000000000001], u = [0.0, 0.0])]), Rible.NoPolicy(), Rible.StaticEnvironment{Rible.NoGeometry, Rible.Gravity{Float64}}(Rible.NoGeometry(), Rible.Gravity{Float64}(9.81)), Rible.Zhong06Constants{Float64}(0.001, 1.0, 12, 12, 6, 2, 3, 0, 0, 12, 24, 30), Rible.Zhong06JacobianWorkspace{Float64}([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0 0.0; 0.0 0.0; … ; 0.0 0.0; 0.0 0.0], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0], Matrix{Float64}(undef, 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.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], Matrix{Float64}(undef, 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.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5], 12, 12), sparse(Int64[], Int64[], Float64[], 12, 0), sparse([1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12], [1.0, 1.0, 1.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], 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; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Matrix{Float64}(undef, 0, 12), Matrix{Float64}(undef, 0, 0), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0], Float64[], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0], Float64[], [-0.1 0.0 … 0.0 0.0], [0.0 0.0 … 0.0 0.0], Matrix{Float64}(undef, 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], Rible.GaugeWorkspace{Float64}[Rible.GaugeWorkspace{Float64}([-0.1, 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], [0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], Matrix{Float64}(undef, 3, 0), [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], Float64[])]), [0.001, 0.0009990009990009992, 0.000998003992015968, 0.0009970089730807579, 0.00099601593625498, 0.0009950248756218907, 0.0009940357852882705, 0.00099304865938431, 0.000992063492063492, 0.000991080277502478], [[1.0 -0.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], [1.0 -0.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], [1.0 -0.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], [1.0 -0.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], [1.0 -0.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], [1.0 -0.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], [1.0 -0.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], [1.0 -0.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], [1.0 -0.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], [1.0 -0.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; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0], [-0.0 -0.0; -0.0 -0.0; … ; -0.0 -0.0; 0.0 0.0]], [Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0), Matrix{Float64}(undef, 30, 0)], [[-0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.0009990009990009992, 0.0, -4.900099900099901e-8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.000998003992015968, 0.0, -1.9580838323353293e-7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.0009970089730807579, 0.0, -4.4012961116650055e-7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.00099601593625498, 0.0, -7.816733067729084e-7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.0009950248756218907, 0.0, -1.2201492537313433e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.0009940357852882705, 0.0, -1.7552683896620278e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.00099304865938431, 0.0, -2.3867428003972193e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.000992063492063492, 0.0, -3.1142857142857127e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [-0.000991080277502478, 0.0, -3.937611496531218e-6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], [[0.01 0.0 … 0.0 0.0; 0.0 0.01 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.009990009990009992 0.0 … 0.0 0.0; 0.0 0.009990009990009992 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.00998003992015968 0.0 … 0.0 0.0; 0.0 0.00998003992015968 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.009970089730807579 0.0 … 0.0 0.0; 0.0 0.009970089730807579 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0099601593625498 0.0 … 0.0 0.0; 0.0 0.0099601593625498 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.009950248756218907 0.0 … 0.0 0.0; 0.0 0.009950248756218907 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.009940357852882706 0.0 … 0.0 0.0; 0.0 0.009940357852882706 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0099304865938431 0.0 … 0.0 0.0; 0.0 0.0099304865938431 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.00992063492063492 0.0 … 0.0 0.0; 0.0 0.00992063492063492 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.009910802775024779 0.0 … 0.0 0.0; 0.0 0.009910802775024779 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0]], [[0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0], [0.0, 0.0]], [[0.001 0.0; 0.0 0.001], [0.0009990009990009992 0.0; 0.0 0.0009990009990009992], [0.000998003992015968 0.0; 0.0 0.000998003992015968], [0.0009970089730807579 0.0; 0.0 0.0009970089730807579], [0.00099601593625498 0.0; 0.0 0.00099601593625498], [0.0009950248756218907 0.0; 0.0 0.0009950248756218907], [0.0009940357852882705 0.0; 0.0 0.0009940357852882705], [0.00099304865938431 0.0; 0.0 0.00099304865938431], [0.000992063492063492 0.0; 0.0 0.000992063492063492], [0.000991080277502478 0.0; 0.0 0.000991080277502478]], [-10.0, 0.0, -0.04904999999999996, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [100.0 0.0 … 0.0 0.0; 0.0 100.0 … 0.0 0.0; … ; 0.0 0.0 … 0.0 0.0; 0.0 0.0 … 0.0 0.0], [0.0, 0.0], [5.0 0.0; 0.0 5.0]))

提取梯度与雅可比 ​

下面的示例直接从 sim.solver_cache 读取结果。

伴随法结果 ​

julia
∂J∂x0 = adj_sim.solver_cache.∂J∂x₀ᵀ
∂J∂θ = adj_sim.solver_cache.∂J∂θᵀ
∂J∂c = adj_sim.solver_cache.∂J∂cᵀ

∂J∂x0, ∂J∂θ, ∂J∂c
([-10.0099552829902, 0.0, -0.04906388066995471, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0  …  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[], Float64[]], [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]])

∂J∂x0 表示目标函数对初始状态的梯度，∂J∂θ 与 ∂J∂c 则分别汇集了对控制参数和结构参数的灵敏度轨迹。

直接法结果 ​

直接法会把状态雅可比直接存放在 drc_sim.solver_cache 中：

julia
Jac_state = drc_sim.solver_cache.Jac_state
Jac_action = drc_sim.solver_cache.Jac_action
Jac_control_params = drc_sim.solver_cache.Jac_control_params

state_sample = Jac_state[end][1:3, 1:3]
action_sample = Jac_action[end][1:3, :]
control_param_total = sum(sum(abs, block) for block in Jac_control_params)

(; state_sample, action_sample, control_param_total)
(state_sample = [1.0 -0.0 -0.0; -0.0 1.0 -0.0; -0.0 -0.0 1.0], action_sample = [-0.0 -0.0; -0.0 -0.0; -0.0 -0.0], control_param_total = 0.0)

这些雅可比分别描述状态对初始状态、驱动量和控制参数的灵敏度。这里 control_param_total 仍为零，是因为这里未引入参数化控制策略。