-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathInverse_Dynamic.py
289 lines (256 loc) · 13.7 KB
/
Inverse_Dynamic.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
import numpy as np
from scipy import sparse
import time
from Inverse_Kinematics import InverseKinematic
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
import osqp
import mujoco
import matplotlib.pyplot as plt
from tqdm import tqdm
class InverseDynamic(InverseKinematic):
def __init__(self):
super().__init__()
self.mu = 0.6 # Friction coefficient
self.F_z_max = 250.0 # Max contact force
self.tau_min = -50.0 # Torque limits
self.tau_max = 50.0
self.F_dim = 3 * len(self.contact_legs) # Number of contact forces
self.ddxc_dim = 3 * len(self.contact_legs) # Contact accelerations
self.ddq_dim = self.model.nv # Joint accelerations (DOFs)
# Weight matrices for cost function
# self.WF = sparse.csc_matrix(np.eye(self.F_dim) *1000 ) # Weight for contact forces
# self.Wc = sparse.csc_matrix(np.eye(self.ddxc_dim)* 1000 ) # Weight for contact accelerations
# self.Wddq = sparse.csc_matrix(np.eye(self.ddq_dim)* 100) # Weight for joint accelerations
# as phase = swing
self.WF = sparse.csc_matrix(np.diag([5, 5, 0.5, 1, 1, 0.01])) # Weight for contact forces
self.Wc = sparse.csc_matrix(np.diag([10^-3, 10^-3, 10^-3, 10^3, 10^3, 10^3])) # Weight for contact accelerations
self.Wddq = sparse.csc_matrix(np.eye(self.ddq_dim)* 100) # Weight for joint accelerations
# Solver initialization
self.prob = osqp.OSQP()
# Placeholder for problem matrices and bounds
self.A = None # Combined constraint matrix
self.l = None # Lower bounds
self.u = None # Upper bounds
self.P = None # Hessian matrix
self.q = None # Gradient vector
def whole_body_dynamics_constraint(self):
"""
Formulate the whole-body dynamics constraints.
"""
# print("Starting whole-body dynamics constraint...")
# Compute mass matrix and bias forces
M = np.zeros((self.model.nv, self.model.nv))
mujoco.mj_fullM(self.model, M, self.data.qM) # Mass matrix
B = self.data.qfrc_bias.reshape(-1, 1) # Nonlinear terms
S = np.hstack((np.zeros((self.model.nv - 6, 6)), np.eye(self.model.nv - 6)))
# self.tau = np.linalg.pinv(S.T) @ (M @ np.vstack((np.zeros((6, 1)), self.ddq_cmd)) + B - self.data.qfrc_inverse.reshape(-1, 1))
tau_cmd = np.vstack((np.zeros((6, 1)), self.tau.reshape(-1, 1)))
# J_contact
A1 = sparse.csc_matrix(-self.J1.T) # Transposed contact Jacobian
A2 = sparse.csc_matrix((self.J1.shape[1], self.J1.shape[0])) # Placeholder
A3 = sparse.csc_matrix(M) # Mass matrix in sparse format
A_matrix = sparse.hstack([A1, A2, A3]) # Constraint matrix
dynamics_u = tau_cmd - B - M @ np.vstack((np.zeros((6, 1)), self.ddq_cmd)) # Equality constraint RHS
dynamics_l = dynamics_u # Equality constraint bounds
# print("Whole-body dynamics constraint formulated.")
return A_matrix, dynamics_l, dynamics_u
def kinematic_constraints(self):
"""
Formulate the kinematic constraints.
"""
# print("Formulating kinematic constraints...")
A1 = sparse.csc_matrix((self.F_dim, self.F_dim)) # Placeholder
A2 = sparse.csc_matrix(np.eye(self.ddxc_dim)) # Identity matrix
A3 = -self.J1 # Negative Jacobian
A_matrix = sparse.hstack([A1, A2, A3],format='csc') # Constraint matrix
# print(f"J_contact: {self.J_contact.shape}, ddq_dik: {self.ddq_dik.shape}, dJ_contact: {self.dJ_contact.shape}, qvel: {self.qvel.shape}")
l = self.J1 @ np.vstack((np.zeros((6, 1)), self.ddq_cmd)) + self.J1_dot @ self.data.qvel.copy().reshape(-1,1) # Lower bound
u = l
return A_matrix, l, u
def reaction_force_constraints(self):
"""
Formulate the reaction force constraints.
"""
# Friction cone
S_single = np.array([
[1, 0, -self.mu],
[-1, 0, -self.mu],
[0, 1, -self.mu],
[0, -1, -self.mu]
])
S = sparse.block_diag([S_single] * len(self.contact_legs))
F_r_max = (np.ones(S.shape[0]) * self.F_z_max).reshape(-1, 1)
A1 = sparse.csc_matrix(S)
A2 = sparse.csc_matrix((S.shape[0], self.ddxc_dim))
A3 = sparse.csc_matrix((S.shape[0], self.ddq_dim))
A_react = sparse.hstack([A1, A2, A3],format='csc')
l_react = -np.inf * np.ones(S.shape[0]).reshape(-1, 1)
u_react = F_r_max
return A_react, l_react, u_react
def compute_contact_force_direction_constraints(self):
Fz_single = np.array([
[0, 0, 1], # -Fz ≤ 0 → Fz ≥ 0
])
Fz = sparse.block_diag([Fz_single] * len(self.contact_legs))
A1 = sparse.csc_matrix(Fz)
A2 = sparse.csc_matrix((Fz.shape[0], self.ddxc_dim))
A3 = sparse.csc_matrix((Fz.shape[0], self.ddq_dim))
A_matrix = sparse.hstack([A1, A2, A3],format='csc')
l = np.zeros(Fz.shape[0]).reshape(-1, 1)
u = np.ones(Fz.shape[0]).reshape(-1, 1) * np.inf
return A_matrix, l, u
def setup_cost_function(self):
"""
Define the cost function.
"""
self.P = sparse.block_diag([self.WF, self.Wc, self.Wddq], format='csc')
self.q = np.zeros(self.P.shape[0])
def combine_constraints(self, ):
"""
Combine all constraints into a single matrix.
"""
A_dyn, l_dyn, u_dyn = self.whole_body_dynamics_constraint()
A_react, l_react, u_react = self.reaction_force_constraints()
A_contact, l_contact, u_contact = self.kinematic_constraints()
A_contact_force_dir, l_contact_force_dir, u_contact_force_dir = self.compute_contact_force_direction_constraints()
self.A = sparse.vstack([A_dyn, A_react, A_contact, A_contact_force_dir])
self.l = np.vstack([l_dyn, l_react, l_contact, l_contact_force_dir])
self.u = np.vstack([u_dyn, u_react, u_contact, u_contact_force_dir])
#
def solve(self):
"""
Solve the optimization problem usinga OSQP.
"""
self.setup_cost_function()
self.combine_constraints()
self.prob = osqp.OSQP()
# print(f"P shape: {self.P.shape}, q shape: {self.q.shape}, A shape: {self.A.shape}, l shape: {self.l.shape}, u shape: {self.u.shape}")
self.prob.setup(P=self.P, q=self.q, A=self.A, l=self.l, u=self.u, verbose=False)
result = self.prob.solve()
# Extract accelerations and forces
Fc_sol = result.x[:self.F_dim]
ddxc_sol = result.x[self.F_dim:self.F_dim + self.ddxc_dim]
ddq_sol = result.x[-self.ddq_dim:]
# print(f"ddq_sol: {ddq_sol.shape}, Fc_sol: {Fc_sol.shape}, ddxc_sol: {ddxc_sol.shape}")
self.ErrorPlotting.Fc_data.append(Fc_sol)
self.ErrorPlotting.ddxc_data.append(ddxc_sol)
self.ErrorPlotting.ddq_diff_data.append(ddq_sol)
return Fc_sol.reshape(-1, 1), ddxc_sol.reshape(-1, 1), ddq_sol.reshape(-1, 1)
def compute_torque(self):
"""
Compute joint torques using the inverse dynamics solution.
"""
# print("Computing torques...")
Fc_sol, ddxc_sol, ddq_sol = self.solve()
M = np.zeros((self.model.nv, self.model.nv))
mujoco.mj_fullM(self.model, M, self.data.qM) # Mass matrix
B = self.data.qfrc_bias.reshape(-1, 1) # Nonlinear terms
S = np.hstack((np.zeros((self.model.nv - 6, 6)), np.eye(self.model.nv - 6)))
self.ErrorPlotting.tau_data.append(self.tau)
self.tau = np.linalg.pinv(S.T) @ (M @ (np.vstack((np.zeros((6, 1)), self.ddq_cmd)) + ddq_sol) + B - self.J1.T @ Fc_sol)
self.tau = np.clip(self.tau, self.tau_min, self.tau_max)
def send_command_api(self):
"""
Send the computed torques to the robot.
"""
# print("Sending command API...")
ks = 10000
dq_m = self.qd.reshape(-1,1) + self.tau.reshape(-1,1) / ks
# print(dq_m.shape)
self.cmd.send_motor_commands(self.kp, self.kd, self.change_q_order(dq_m), self.change_q_order(self.dqd), self.change_q_order(self.tau))
# self.cmd.send_motor_commands(self.kp, self.kd, self.change_q_order(self.qd), self.change_q_order(self.dqd), self.change_q_order(self.tau))
def send_command_ik(self):
"""
Send the computed torques to the robot.
"""
# print("Sending command API...")
M = np.zeros((self.model.nv, self.model.nv))
mujoco.mj_fullM(self.model, M, self.data.qM) # Mass matrix
B = self.data.qfrc_bias.reshape(-1, 1) # Nonlinear terms
S = np.hstack((np.zeros((self.model.nv - 6, 6)), np.eye(self.model.nv - 6)))
self.tau = np.linalg.pinv(S.T) @ (M @ np.vstack((np.zeros((6, 1)), self.ddq_cmd)) + B - self.data.qfrc_inverse.reshape(-1, 1))
self.cmd.send_motor_commands(self.kp, self.kd, self.change_q_order(self.qd), self.change_q_order(self.dqd), self.change_q_order(self.tau))
def ID_main(self):
"""
Main function to run the inverse dynamics calculations.
"""
controller = "ID"
running_time = 5000
self.start_joint_updates()
self.cmd.move_to_initial_position()
self.initialize()
x_sw = self.compute_desired_swing_leg_trajectory()
x_b = self.compute_desired_body_state() # update the body state for the next cycle
x_sw_dot = self.compute_desired_swing_leg_velocity_trajectory()
x_b_dot = self.compute_desired_body_state_velocity_trajectory()
if controller == "IK":
for i in tqdm(range(running_time)):
index = i % self.K -1
if index == 0:
self.transition_legs()
# self.initialize()
x_sw = self.compute_desired_swing_leg_trajectory()
x_b = self.compute_desired_body_state() # update the body state for the next cycle
x_sw_dot = self.compute_desired_swing_leg_velocity_trajectory()
x_b_dot = self.compute_desired_body_state_velocity_trajectory()
self.calculate(x_sw, x_b, x_sw_dot, x_b_dot, index)
self.send_command_ik()
self.plot_error_ik()
plt.show()
else:
for i in tqdm(range(running_time)):
self.ErrorPlotting.torque_sensor_data.append(self.joint_toque)
index = i % self.K -1
if index == 0:
self.transition_legs()
# self.initialize()
x_sw = self.compute_desired_swing_leg_trajectory()
x_b = self.compute_desired_body_state() # update the body state for the next cycle
x_sw_dot = self.compute_desired_swing_leg_velocity_trajectory()
x_b_dot = self.compute_desired_body_state_velocity_trajectory()
self.calculate(x_sw, x_b, x_sw_dot, x_b_dot, index)
self.compute_torque()
self.send_command_api()
self.plot_error_ik()
self.plot_error_id()
plt.show()
def plot_error_ik(self):
self.ErrorPlotting.plot_q_error(self.ErrorPlotting.q_desired_data,
self.ErrorPlotting.q_current_data,
self.ErrorPlotting.q_error_data,
"qd_send")
self.ErrorPlotting.plot_q_error(self.ErrorPlotting.dq_desired_data,
self.ErrorPlotting.dq_current_data,
self.ErrorPlotting.dq_error_data,
"dqd_send")
self.ErrorPlotting.plot_q_error(self.ErrorPlotting.ddq_desired_data,
self.ErrorPlotting.ddq_current_data,
self.ErrorPlotting.ddq_error_data,
"ddqd_send")
self.ErrorPlotting.plot_state_error_trajectories(self.ErrorPlotting.xb_data,
self.ErrorPlotting.x2_data,
self.ErrorPlotting.dx_b_data,
"Body")
self.ErrorPlotting.plot_state_error_trajectories(self.ErrorPlotting.xw_data,
self.ErrorPlotting.x3_data,
self.ErrorPlotting.dx_sw_data,
"Swing")
# self.ErrorPlotting.plot_foot_location(
# self.ErrorPlotting.FL_position,
# self.ErrorPlotting.FR_position,
# self.ErrorPlotting.RL_position,
# self.ErrorPlotting.RR_position,
# "foot location") # FL, FR, RL, RR,
# self.ErrorPlotting.plot_torque(self.ErrorPlotting.tau_data,"joint toque IK")
def plot_error_id(self):
self.ErrorPlotting.plot_torque(self.ErrorPlotting.tau_data,"joint toque")
self.ErrorPlotting.plot_contact_acceleration(self.ErrorPlotting.ddxc_data, "contact foot acceleration")
self.ErrorPlotting.plot_contact_force(self.ErrorPlotting.Fc_data, "Fc")
self.ErrorPlotting.plot_full_body_state(self.ErrorPlotting.ddq_diff_data, "ddq error")
# self.ErrorPlotting.plot_torque(self.ErrorPlotting.output_data,"ctrl output")
self.ErrorPlotting.plot_torque(self.ErrorPlotting.torque_sensor_data,"joint toque sensor")
plt.show()
if __name__ == "__main__":
ChannelFactoryInitialize(1, "lo")
inv_dyn = InverseDynamic()
inv_dyn.ID_main()