2 import pinocchio
as pin
4 from os.path import dirname, join, abspath
6 np.set_printoptions(linewidth=np.inf)
51 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))),
"models")
53 model_path = join(pinocchio_model_dir,
"example-robot-data/robots")
54 mesh_dir = pinocchio_model_dir
55 urdf_filename =
"solo12.urdf"
56 urdf_model_path = join(join(model_path,
"solo_description/robots"), urdf_filename)
58 model, collision_model, visual_model = pin.buildModelsFromUrdf(
59 urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
61 data = model.createData()
86 v0 = np.zeros(model.nv)
87 a0 = np.zeros(model.nv)
92 g_grav = pin.rnea(model, data, q0, v0, a0)
100 feet_names = [
"FL_FOOT",
"FR_FOOT",
"HL_FOOT",
"HR_FOOT"]
101 feet_ids = [model.getFrameId(n)
for n
in feet_names]
102 bl_id = model.getFrameId(
"base_link")
103 ncontact = len(feet_names)
108 np.copy(pin.computeFrameJacobian(model, data, q0, id, pin.LOCAL))
for id
in feet_ids
111 Js__feet_bl = [np.copy(J[:3, :6])
for J
in Js__feet_q]
114 Jc__feet_bl_T = np.zeros([6, 3 * ncontact])
115 Jc__feet_bl_T[:, :] = np.vstack(Js__feet_bl).T
119 ls = np.linalg.pinv(Jc__feet_bl_T) @ g_bl
122 ls__f = np.split(ls, ncontact)
124 pin.framesForwardKinematics(model, data, q0)
128 for l__f, foot_id
in zip(ls__f, feet_ids):
129 l_sp__f = pin.Force(l__f, np.zeros(3))
130 l_sp__bl = data.oMf[bl_id].actInv(data.oMf[foot_id].
act(l_sp__f))
131 ls__bl.append(np.copy(l_sp__bl.vector))
133 print(
"\n--- CONTACT FORCES ---")
134 for l__f, foot_id, name
in zip(ls__bl, feet_ids, feet_names):
135 print(
"Contact force at foot {} expressed at the BL is: {}".format(name, l__f))
139 "Error between contact forces and gravity at base link: {}".format(
140 np.linalg.norm(g_bl - sum(ls__bl))
146 Js_feet_j = [np.copy(J[:3, 6:])
for J
in Js__feet_q]
148 Jc__feet_j_T = np.zeros([12, 3 * ncontact])
149 Jc__feet_j_T[:, :] = np.vstack(Js_feet_j).T
152 tau = g_j - Jc__feet_j_T @ ls
158 pin.framesForwardKinematics(model, data, q0)
160 joint_names = [
"FL_KFE",
"FR_KFE",
"HL_KFE",
"HR_KFE"]
161 joint_ids = [model.getJointId(n)
for n
in joint_names]
163 fs_ext = [pin.Force(np.zeros(6))
for _
in range(len(model.joints))]
164 for idx, joint
in enumerate(model.joints):
165 if joint.id
in joint_ids:
166 fext__bl = pin.Force(ls__bl[joint_ids.index(joint.id)])
167 fs_ext[idx] = data.oMi[joint.id].actInv(data.oMf[bl_id].
act(fext__bl))
169 tau_rnea = pin.rnea(model, data, q0, v0, a0, fs_ext)
171 print(
"\n--- ID: JOINT TORQUES ---")
172 print(
"Tau from RNEA: {}".format(tau_rnea))
173 print(
"Tau computed manually: {}".format(np.append(np.zeros(6), tau)))
174 print(
"Tau error: {}".format(np.linalg.norm(np.append(np.zeros(6), tau) - tau_rnea)))
178 Js_feet3d_q = [np.copy(J[:3, :])
for J
in Js__feet_q]
179 acc = pin.forwardDynamics(
184 np.append(np.zeros(6), tau),
185 np.vstack(Js_feet3d_q),
189 print(
"\n--- FD: ACC. & CONTACT FORCES ---")
190 print(
"Norm of the FD acceleration: {}".format(np.linalg.norm(acc)))
191 print(
"Contact forces manually: {}".format(ls))
192 print(
"Contact forces FD: {}".format(data.lambda_c))
193 print(
"Contact forces error: {}".format(np.linalg.norm(data.lambda_c - ls)))