阅读信息
约 1445 个字 9 分钟 本页总访问量：加载中... 次
二维刚体动力学仿真代码说明文档
注意
Partly generated by chatgpt
1. apply_impulse()函数
这是冲量施加函数，用于对指定物体施加冲量并更新其运动状态。
函数中，根据冲量-动量定理计算冲量对物体速度和角速度的影响，同时考虑冲量作用点产生的力矩效应。
另外，还处理了碰撞时间修正（TOI），通过调整物体在碰撞时刻之前的位置和旋转角度来避免穿透问题。
所有的状态更新都通过原子操作累加到下一时间步的增量数组中，确保多线程环境下的数据一致性。
| Python |
|---|
| @ti.func
def apply_impulse(t, i, impulse, location, toi_input):
# *********每行代码添加注释************
"""
:param t: 当前时间
:param i: 物体的索引
:param impulse: 施加的冲量向量
:param location: 施加力的世界坐标
:param toi_input: 碰撞时间修正，time of impact
:return:
"""
# 1. 计算增量
"""速度的增量=冲量/质量"""
delta_v = impulse * inverse_mass[i]
# location - x[t, i]：冲量作用点到物体质心的向量（v）
"""角速度的增量=v x 冲量向量 / 转动惯量"""
delta_omega = (location - x[t, i]).cross(impulse) * inverse_inertia[i]
# 保证TOI不小于0、不大于dt，避免冲量过度施加导致物体不稳定
toi = ti.min(ti.max(0.0, toi_input), dt)
# 2. 更新参数
# 修正位置
# x_inc[t + 1, i]：位置增量；toi * (-delta_v)：碰撞时间前产生的位移*修正位置方向
ti.atomic_add(x_inc[t + 1, i], toi * (-delta_v))
# 修正旋转角度
ti.atomic_add(rotation_inc[t + 1, i], toi * (-delta_omega))
# 把冲量对速度的影响加到下一个时间步的速度增量上
ti.atomic_add(v_inc[t + 1, i], delta_v)
# 把冲量对角速度的影响加到下一个时间步的角速度增量上
ti.atomic_add(omega_inc[t + 1, i], delta_omega)
|
2. collide()函数
这是地面碰撞检测和响应函数，用于处理所有物体与地面的碰撞物理仿真。
函数遍历每个物体的四个角点，对每个角点执行完整的碰撞检测流程：
首先应用重力影响更新速度，然后检测角点是否会与地面发生碰撞，如果检测到碰撞则计算并施加反弹冲量和摩擦力。
函数还包含时间修正机制来处理穿透问题，以及罚函数法作为最后的安全网来修复任何残留的数值误差导致的穿透现象。
整个过程确保物体能够真实地在地面上反弹、滑动和停止。
| Python |
|---|
| @ti.kernel
def collide(t: ti.i32):
# *********每行代码添加注释************
"""
:param t: 当前时间步
:return:
"""
# 遍历所有物体i
for i in range(n_objects):
# hs表示物体半宽半高，用中心点计算物体角点坐标
hs = halfsize[i]
# 遍历矩形物体的4个角k
for k in ti.static(range(4)):
# k为0123时，offset_scale分别表示[-1,-1], [1,-1], [-1,1], [1,1], 即左下角、右下角、左上角、右上角
offset_scale = ti.Vector([k % 2 * 2 - 1, k // 2 % 2 * 2 - 1])
# 调用to_world函数，将角点从物体局部坐标转换到世界坐标
# corner_x, corner_v, rela_pos分别表示角点位置、速度、相对质心的向量
corner_x, corner_v, rela_pos = to_world(t, i, offset_scale * hs)
# 增加重力对速度的影响
"""速度增量 = 重力大小 * 重力方向向量 * 时间步长"""
corner_v = corner_v + dt * gravity * ti.Vector([0.0, 1.0])
# 定义地面向量
normal = ti.Vector([0.0, 1.0]) # 法向量：竖直向上
tao = ti.Vector([1.0, 0.0]) # 切向量：水平
# 计算力矩臂的长度
"""力矩臂大小 = 质心到角点向量长度 * sin夹角；方向为叉乘方向"""
rn = rela_pos.cross(normal) # 法线方向的力矩臂
rt = rela_pos.cross(tao) # 切线方向的力矩臂
# 计算冲量对速度的影响
"""
总速度变化 = 平移产生的速度变化 + 旋转产生的速度变化
= J/m + (J*r/I)*r
= J * (1/m + r^2/I)
"""
impulse_contribution = inverse_mass[i] + (rn) ** 2 * \
inverse_inertia[i] # 用于法线方向的冲量
timpulse_contribution = inverse_mass[i] + (rt) ** 2 * \
inverse_inertia[i] # 用于切线方向的冲量
# 点乘计算角点沿法线方向的速度
rela_v_ground = normal.dot(corner_v)
# 初始化
impulse = 0.0 # 沿法线的反弹冲量
timpulse = 0.0 # 沿切向的摩擦冲量
new_corner_x = corner_x + dt * corner_v # 预测下一时刻角点的位置
toi = 0.0 # 时间修正
# 检查角点是否正在撞向地面
# 如果速度朝向地面，且预测下一时刻穿过地面，则和地面碰撞
if rela_v_ground < 0 and new_corner_x[1] < ground_height:
# 计算冲量，防止物体继续下沉，使其以一定速度反弹
"""
elasticity：弹性系数
1 + elasticity：恢复因子
impulse_contribution：施加单位冲量时速度的变化量
冲量大小 = 恢复因子 * 下落速度 / impulse_contribution
负号保证冲量方向和下落方向相反
"""
impulse = -(1 +
elasticity) * rela_v_ground / impulse_contribution
# 如果需要施加向上冲量，执行下面代码
if impulse > 0:
# 根据角点切向速度计算阻力
"""
timpulse_contribution：施加单位冲量时水平速度的变化量
摩擦力 = 水平速度 / timpulse_contribution：施加单位冲量时水平速度的变化量
"""
timpulse = -corner_v.dot(tao) / timpulse_contribution
# 摩擦大小需要小于压力*摩擦系数
timpulse = ti.min(friction * impulse,
ti.max(-friction * impulse, timpulse))
# 时间修正：由于时间步长固定，物体可能在一帧内直接穿过地面
# 如果角点还在地面上方
if corner_x[1] > ground_height:
# 计算碰撞时间
"""
运动方程：h = h0 + v*t
碰撞时：ground_height = corner_x[1] + corner_v[1]*toi
"""
toi = -(corner_x[1] - ground_height) / ti.min(corner_v[1], -1e-3)
# 对物体施加碰撞冲量，让物体产生正确的反弹和摩擦效果
# 调用函数apply_impulse，impulse * normal + timpulse * tao表示切向和法向合成后的向量
apply_impulse(t, i, impulse * normal + timpulse * tao, new_corner_x, toi)
# 处理特殊情况，施加惩罚推动角点回到地面
penalty = 0.0
# 如果角点仍然穿透地面
if new_corner_x[1] < ground_height:
# 计算惩罚冲量，保证角点不会穿透地面
"""定义惩罚为： penalty参数 * 穿透的深度 / “等效质量” * 时间步长"""
penalty = -dt * penalty * (
new_corner_x[1] - ground_height) / impulse_contribution
# 调用函数apply_impulse，施加惩罚冲量
apply_impulse(t, i, penalty * normal, new_corner_x, 0)
|
3. apply_spring_force()函数
这是并行处理弹簧和关节的函数。
函数根据每个弹簧的当前长度、受冲量影响的目标长度，计算出相应的弹性力冲量，并调用 apply_impulse 函数施加冲量。
对于作为关节的连接，会额外计算并施加一个阻尼冲量来消除连接点间的相对速度，以增强稳定性。
| Python |
|---|
| @ti.kernel
def apply_spring_force(t: ti.i32):
# *********每行代码添加注释************
"""
:param t: 当前时间
:return:
"""
# 遍历所有弹簧关节i
for i in range(n_springs):
# a、b表示当前弹簧连接的两个物体索引
a = spring_anchor_a[i]
b = spring_anchor_b[i]
# 调用to_world将弹簧端点偏移量转换到世界坐标
# pos_a, vel_a, rela_a分别表示位置、速度、相对质心的向量
pos_a, vel_a, rela_a = to_world(t, a, spring_offset_a[i])
pos_b, vel_b, rela_b = to_world(t, b, spring_offset_b[i])
# 计算两个端点之间的向量，即弹簧的向量
dist = pos_a - pos_b
# 计算当前弹簧长度，加1e-4防止除以零
length = dist.norm() + 1e-4
# 计算当前时间步弹簧的激活量
# actuation[t, i]表示在时间步t，弹簧i的激活程度
act = actuation[t, i]
# 判断当前弹簧是否是关节约束
# spring_length[i] == -1表示是关节，没有长度变化，只连接物体
is_joint = spring_length[i] == -1
# 计算弹簧目标长度
"""
spring_length[i]：弹簧的基础长度
spring_actuation[i]：弹簧的激活能力系数（能变化多少比例）
act：当前的激活信号（-1到1之间）
target_length：计算出的目标长度
目标长度 = 基础长度 × (1 + 变化比例)
变化比例 = 激活能力 × 激活信号
"""
target_length = spring_length[i] * (1.0 + spring_actuation[i] * act)
# 如果是关节，两个点重合，长度为零
if is_joint:
target_length = 0.0
# 计算冲量
"""冲量 = 弹簧伸长量 * 弹簧刚度 / 方向向量"""
impulse = dt * (length -
target_length) * spring_stiffness[i] / length * dist
# 如果是关节，进一步计算
if is_joint:
# 计算两个端点的相对速度
rela_vel = vel_a - vel_b
# 计算相对速度的大小，加0.1防止除以零
rela_vel_norm = rela_vel.norm() + 1e-1
# 计算阻尼方向，和相对运动方向相反
impulse_dir = rela_vel / rela_vel_norm
# 计算冲量对速度的贡献
"""冲量的贡献 = 物体A/B的平移贡献 + 物体A/B的旋转贡献"""
impulse_contribution = inverse_mass[a] + impulse_dir.cross(rela_a)**2 * inverse_inertia[a] + \
inverse_mass[b] + impulse_dir.cross(rela_b)**2 * inverse_inertia[b]
# 将关节两端的相对速度投影到冲量方向
"""冲量增加量 = 相对速度大小 / “有效质量” * 方向向量"""
impulse += rela_vel_norm / impulse_contribution * impulse_dir
# 调用apply_impulse函数，对两个端点施加冲量，保证作用力等于反作用力
apply_impulse(t, a, -impulse, pos_a, 0.0)
apply_impulse(t, b, impulse, pos_b, 0.0)
|
4. advance_toi()函数
这是将各个参数推进到当前时间步的函数。
函数基于半隐式积分法，更新每个物体的速度、位置、角速度和旋转角度。
同时引入阻尼衰减与外部冲量的修正项，确保运动状态既符合物理规律，又能实时响应碰撞、约束或交互。
| Python |
|---|
| @ti.kernel
def advance_toi(t: ti.i32):
# *********每行代码添加注释************
"""
:param t: 当前时间
:return:
"""
# 遍历所有物体
for i in range(n_objects):
# s表示阻尼因子
"""阻尼因子 = e ^ {时间步长 * 阻尼系数}"""
s = ti.exp(-dt * damping)
# 半隐式积分法
# 更新物体i在当前时间步t的速度
"""
当前速度 = 上一时刻速度 × 阻尼衰减
+ 外部冲量引起的速度增量
+ 当前重力产生的速度增量
"""
v[t, i] = s * v[t - 1, i] + v_inc[t, i] + dt * gravity * ti.Vector([0.0, 1.0])
# 更新物体i在当前时间步t的位置
"""
当前位置 = 上一时刻位置
+ 当前由速度引起的位移
+ 外部的作用引起的位移突变
"""
x[t, i] = x[t - 1, i] + dt * v[t, i] + x_inc[t, i]
# 更新物体i的角速度
"""
当前角速度 = 上一时刻角速度 × 阻尼衰减因子
+ 外部作用引起的角速度突变
"""
omega[t, i] = s * omega[t - 1, i] + omega_inc[t, i]
# 更新物体i的旋转角度
"""
当前旋转角度 = 上一时刻角度
+ 当前由角速度引起的旋转增量
+ 外部作用引起的旋转修正
"""
rotation[t, i] = rotation[t - 1, i] + dt * omega[t, i] + rotation_inc[t, i]
|
其他部分
1. 初始化与配置部分
ti.init(default_fp=real)：初始化 Taichi 计算环境，设定浮点精度为 f32（32位浮点数）。max_steps, steps, vis_interval, output_vis_interval：控制仿真最大步数、仿真步数、可视化更新间隔和输出图像更新间隔。x, v, rotation, omega, halfsize, inverse_mass, inverse_inertia, etc.：定义机器人物体的物理属性，包括位置、速度、旋转角度、角速度、物体的尺寸、逆质量与逆转动惯量等。loss, goal：用于神经网络优化的目标函数和目标位置。n_objects, n_springs：分别表示物体的数量与弹簧连接件（关节）的数量。
2. 神经网络部分
3. 物理仿真部分
-
initialize_properties()：
-
初始化物体的逆质量和逆转动惯量。质量和转动惯量是物体物理运动的关键参数，用于计算碰撞、运动和反弹行为。
-
to_world(t, i, rela_x)：
-
将物体局部坐标系中的位置（rela_x）转换到世界坐标中，返回物体在世界坐标中的位置、速度以及相对质心的位置。
-
apply_impulse(t, i, impulse, location, toi_input)：
-
见第一部分
-
collide(t: ti.i32)：
-
见第二部分
-
apply_spring_force(t: ti.i32)：
-
见第三部分
-
advance_toi(t: ti.i32)：
-
见第四部分
4. 损失函数计算部分
5. 仿真与优化过程
-
forward(output=None, visualize=True)：
-
执行仿真，更新物体的状态（位置、速度等），并通过 nn1 和 nn2 计算控制信号，控制弹簧的激活。
- 每隔一定步数更新一次可视化，将物体和弹簧的状态绘制在屏幕上。
- 支持将每个时间步的可视化结果保存为图像。
-
optimize(visualize=True)：
-
使用反向传播（ti.ad.Tape）来优化神经网络的权重。通过梯度下降法调整神经网络的参数，使得损失函数最小化。
- 进行若干次迭代优化，并更新网络权重和偏置项。
- 在每次迭代中计算梯度，并进行梯度裁剪（
gradient_clip），避免梯度爆炸。
- 最终的优化目标是使机器人的行为符合预期。
6. 可视化部分
-
gui = ti.GUI('Rigid Body Simulation', (512, 512))：
-
初始化 Taichi 的 GUI 界面，用于展示仿真结果。
-
gui.line() 和 gui.circle()：
-
绘制物体、弹簧、关节等在屏幕上的可视化效果。gui.line 绘制物体的四条边（物体的轮廓），gui.circle 绘制物体的圆形脚。
-
gui.show()：
-
更新并展示当前仿真状态。每次调用 gui.show()，都刷新显示内容，将物体、弹簧等信息实时展示在界面上。
7. 主函数 (main)
-
setup_robot(*robots[robot_id]())：
-
初始化机器人的状态，加载不同的机器人工具配置。robot_id 选择不同的机器人配置。
-
optimize()：
-
优化神经网络，通过反向传播调整网络参数，训练机器人进行目标优化。
-
forward('final{}'.format(robot_id), visualize=True)：
-
执行仿真，并将仿真结果可视化。最后将仿真结果保存为图像（如 PNG 格式）。