static-contact-dynamics.py
Go to the documentation of this file.
1 import numpy as np
2 import pinocchio as pin
3 
4 from os.path import dirname, join, abspath
5 
6 np.set_printoptions(linewidth=np.inf)
7 
8 # ----- PROBLEM STATEMENT ------
9 #
10 # We want to find the contact forces and torques required to stand still at a configuration 'q0'.
11 # We assume 3D contacts at each of the feet
12 #
13 # The dynamic equation would look like:
14 #
15 # M*q_ddot + g(q) + C(q, q_dot) = tau + J^T*lambda --> (for the static case) --> g(q) = tau + Jc^T*lambda (1)
16 
17 # ----- SOLVING STRATEGY ------
18 
19 # Split the equation between the base link (_bl) joint and the rest of the joints (_j). That is,
20 #
21 # | g_bl | | 0 | | Jc__feet_bl.T | | l1 |
22 # | g_j | = | tau | + | Jc__feet_j.T | * | l2 | (2)
23 # | l3 |
24 # | l4 |
25 
26 # First, find the contact forces l1, l2, l3, l4 (these are 3 dimensional) by solving for the first 6 rows of (2).
27 # That is,
28 #
29 # g_bl = Jc__feet_bl.T * | l1 |
30 # | l2 | (3)
31 # | l3 |
32 # | l4 |
33 #
34 # Thus we find the contact froces by computing the jacobian pseudoinverse,
35 #
36 # | l1 | = pinv(Jc__feet_bl.T) * g_bl (4)
37 # | l2 |
38 # | l3 |
39 # | l4 |
40 #
41 # Now, we can find the necessary torques using the bottom rows in (2). That is,
42 #
43 # | l1 |
44 # tau = g_j - Jc__feet_j.T * | l2 | (5)
45 # | l3 |
46 # | l4 |
47 
48 # ----- SOLUTION ------
49 
50 # 0. DATA
51 pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
52 
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)
57 
58 model, collision_model, visual_model = pin.buildModelsFromUrdf(
59  urdf_model_path, mesh_dir, pin.JointModelFreeFlyer()
60 )
61 data = model.createData()
62 
63 q0 = np.array(
64  [
65  0.0,
66  0.0,
67  0.235,
68  0.0,
69  0.0,
70  0.0,
71  1.0,
72  0.0,
73  0.8,
74  -1.6,
75  0.0,
76  -0.8,
77  1.6,
78  0.0,
79  0.8,
80  -1.6,
81  0.0,
82  -0.8,
83  1.6,
84  ]
85 )
86 v0 = np.zeros(model.nv)
87 a0 = np.zeros(model.nv)
88 
89 # 1. GRAVITY TERM
90 
91 # We compute the gravity terms by using the ID at desired configuration q0, with velocity and acceleration being 0. I.e., ID with a = v = 0
92 g_grav = pin.rnea(model, data, q0, v0, a0)
93 
94 g_bl = g_grav[:6]
95 g_j = g_grav[6:]
96 
97 # 2. FIND CONTACTS
98 
99 # First, we set the frame for our contacts. We assume the contacts are placed at the following 4 frames and they are 3D
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)
104 
105 # Now, we need to find the contact Jacobians appearing in (1).
106 # These are the Jacobians that relate the joint velocity to the velocity of each feet
107 Js__feet_q = [
108  np.copy(pin.computeFrameJacobian(model, data, q0, id, pin.LOCAL)) for id in feet_ids
109 ]
110 
111 Js__feet_bl = [np.copy(J[:3, :6]) for J in Js__feet_q]
112 
113 # Notice that we can write the equation above as an horizontal stack of Jacobians transposed and vertical stack of contact forces
114 Jc__feet_bl_T = np.zeros([6, 3 * ncontact])
115 Jc__feet_bl_T[:, :] = np.vstack(Js__feet_bl).T
116 
117 # Now I only need to do the pinv to compute the contact forces
118 
119 ls = np.linalg.pinv(Jc__feet_bl_T) @ g_bl # This is (3)
120 
121 # Contact forces at local coordinates (at each foot coordinate)
122 ls__f = np.split(ls, ncontact)
123 
124 pin.framesForwardKinematics(model, data, q0)
125 
126 # Contact forces at base link frame
127 ls__bl = []
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))
132 
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))
136 
137 # Notice that if we add all the contact forces are equal to the g_grav
138 print(
139  "Error between contact forces and gravity at base link: {}".format(
140  np.linalg.norm(g_bl - sum(ls__bl))
141  )
142 )
143 
144 # 3. FIND TAU
145 # Find Jc__feet_j
146 Js_feet_j = [np.copy(J[:3, 6:]) for J in Js__feet_q]
147 
148 Jc__feet_j_T = np.zeros([12, 3 * ncontact])
149 Jc__feet_j_T[:, :] = np.vstack(Js_feet_j).T
150 
151 # Apply (5)
152 tau = g_j - Jc__feet_j_T @ ls
153 
154 # 4. CROSS CHECKS
155 
156 # INVERSE DYNAMICS
157 # We can compare this torques with the ones one would obtain when computing the ID considering the external forces in ls
158 pin.framesForwardKinematics(model, data, q0)
159 
160 joint_names = ["FL_KFE", "FR_KFE", "HL_KFE", "HR_KFE"]
161 joint_ids = [model.getJointId(n) for n in joint_names]
162 
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))
168 
169 tau_rnea = pin.rnea(model, data, q0, v0, a0, fs_ext)
170 
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)))
175 
176 # FORWARD DYNAMICS
177 # We can also check the results using FD. FD with the tau we got, q0 and v0, should give 0 acceleration and the contact forces
178 Js_feet3d_q = [np.copy(J[:3, :]) for J in Js__feet_q]
179 acc = pin.forwardDynamics(
180  model,
181  data,
182  q0,
183  v0,
184  np.append(np.zeros(6), tau),
185  np.vstack(Js_feet3d_q),
186  np.zeros(12),
187 )
188 
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)))
pinocchio::motionSet::act
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
path


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:13