Skip to content

Instantly share code, notes, and snippets.

@edxmorgan

edxmorgan/CRBI.py

Last active November 8, 2024 17:16
Show Gist options
  • Save edxmorgan/bccf3328253e4cb46c0a85bee1b9c1af to your computer and use it in GitHub Desktop.
Save edxmorgan/bccf3328253e4cb46c0a85bee1b9c1af to your computer and use it in GitHub Desktop.
Download ZIP
composite rigid body inertia (CRBI) algorithm used in articulated rigid body dynamics for robots with multiple joints.
Raw
CRBI.py
I, i_X_p = rig_dyn.model()
Ic_i = []
for i in range(0, ss.n_joints):
Ic_i.append(I[i])
for i in range(ss.n_joints-1, -1, -1):
if i != 0:
p_X_i_f = pluck.inverse_spatial_transform(i_X_p[i]).T
Ic_i[i-1] = Ic_i[i-1] + p_X_i_f@Ic_i[i]@i_X_p[i]
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment