浮基机器人使用CalcInverseDynamics实现逆动力学的方法咨询

浮基机器人逆动力学返回值维度说明 #

CalcInverseDynamics返回的tau维度和机器人广义速度总维度nv完全一致，对于你使用的浮基LittleDog机器人，nv = 6（浮基基座的空间速度自由度） + 12（关节驱动自由度），所以返回18维是符合设计的正常结果。

• 前6维对应浮基基座需要的外部广义力，用来平衡当前加速度、偏置力、重力和你输入的接触力的作用
• 后12维就是你需要的关节执行器力矩，直接取tau[6:]即可得到12维的输出结果

如果你的输入期望加速度vd_d、接触力取值都是符合动力学约束的，前6维的计算结果会非常接近0，此时直接截取后12维的误差可以忽略。

你当前代码的问题 #

你目前给四个脚施加的接触力F_expected设为了全零，这种情况下重力、科氏离心力等项没有外部力平衡，浮基本身又没有驱动器，这部分的力需求都会体现在tau的前6维上，导致前6维结果不为0。你需要将实际规划或实测的脚端接触力填入F_expected，才能保证浮基部分的动力学平衡。

手动拆分动力学项的实现示例 #

如果你不想直接截取结果，想用CalcMassMatrix、CalcBiasTerm等API自行拆分计算关节力矩，可以参考以下代码：

import numpy as np

# 获取维度信息
n_float = 6 # 浮基自由度
n_act = 12 # 关节驱动自由度
nv = n_float + n_act

# 计算基础动力学项
M = np.zeros((nv, nv))
plant.CalcMassMatrix(plant_context, M)
Cv = plant.CalcBiasTerm(plant_context)
g = plant.CalcGravityGeneralizedForces(plant_context)

# 拆分浮基和关节对应的动力学项
M_jf = M[n_float:, :n_float] # 关节-浮基耦合质量块
M_jj = M[n_float:, n_float:] # 关节自身质量块
Cv_j = Cv[n_float:] # 关节对应的偏置项
g_j = g[n_float:] # 关节对应的重力项

# 拆分期望加速度
vd_f = vd_d[:n_float]
vd_j = vd_d[n_float:]

# 计算关节力矩
tau_joint = M_jf @ vd_f + M_jj @ vd_j + Cv_j + g_j

计算得到的tau_joint和直接截取tau[6:]的结果完全一致。

注意事项 #

如果你的期望加速度vd_d是纯运动学规划得到的、不满足接触约束，即使输入了正确的接触力，tau的前6维也不会归零，此时得到的关节力矩会存在偏差。如果要保证力矩的准确性，建议先验证期望加速度和接触力的动力学一致性。

内容的提问来源于stack exchange，提问作者zisangsang