2 from os.path import dirname, join, abspath
7 import pinocchio
as pin
11 dirname(dirname(abspath(__file__))),
"models",
"example-robot-data",
"python"
14 import example_robot_data
17 x_goal = [1, 0, 1.5, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
18 x0 = [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
23 d_cog, cf, cm = 0.1525, 6.6e-5, 1e-6
30 [0.0, d_cog, 0.0, -d_cog],
31 [-d_cog, 0.0, d_cog, 0.0],
32 [-cm / cf, cm / cf, -cm / cf, cm / cf],
37 x_nom = [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
41 u = casadi.SX.sym(
"u", 4)
44 return casadi.Function(
"act_model", [u], [tau], [
"u"], [
"tau"])
48 q = casadi.SX.sym(
"dq", model.nq)
49 dq = casadi.SX.sym(
"q", model.nv)
50 v = casadi.SX.sym(
"v", model.nv)
51 dv = casadi.SX.sym(
"dv", model.nv)
53 q_next = cpin.integrate(model, q, dq)
56 dx = casadi.vertcat(dq, dv)
57 x = casadi.vertcat(q, v)
58 x_next = casadi.vertcat(q_next, v_next)
60 return casadi.Function(
"integrate", [x, dx], [x_next], [
"x",
"dx"], [
"x_next"])
64 q0 = casadi.SX.sym(
"q0", model.nq)
65 q1 = casadi.SX.sym(
"q1", model.nq)
66 v0 = casadi.SX.sym(
"v0", model.nv)
67 v1 = casadi.SX.sym(
"v1", model.nv)
69 q_diff = cpin.difference(model, q0, q1)
72 x0 = casadi.vertcat(q0, v0)
73 x1 = casadi.vertcat(q1, v1)
74 x_diff = casadi.vertcat(q_diff, v_diff)
76 return casadi.Function(
"difference", [x0, x1], [x_diff], [
"x0",
"x1"], [
"x_diff"])
81 u = casadi.SX.sym(
"u", nu)
86 q = casadi.SX.sym(
"q", model.nq)
87 v = casadi.SX.sym(
"v", model.nv)
89 a = cpin.aba(model, data, q, v, tau)
91 dq = v * dt + a * dt**2
94 x = casadi.vertcat(q, v)
95 dx = casadi.vertcat(dq, dv)
98 return casadi.Function(
"int_dyn", [x, u], [x_next], [
"x",
"u"], [
"x_next"])
102 dx = casadi.SX.sym(
"dx", model.nv * 2)
107 cost = 0.5 * e_goal.T @ e_goal
109 return casadi.Function(
"quad_cost", [dx], [cost], [
"dx"], [
"cost"])
113 def __init__(self, model, terminal_soft_constraint=False):
132 for i
in range(nodes):
136 1e-5 * 0.5 * e_reg.T @ e_reg
137 + 1e-5 * 0.5 * self.
c_us[:, i].T @ self.
c_us[:, i]
139 if terminal_soft_constraint:
142 self.
opti.minimize(obj)
145 for i
in range(nodes):
153 self.
opti.subject_to(gap == [0] * 12)
156 self.
opti.subject_to(self.
opti.bounded(0, self.
c_us, 5))
159 if not terminal_soft_constraint:
162 self.
opti.subject_to(e_goal == [0] * 12)
169 self.
opti.set_initial(
170 self.
c_dxs, np.vstack([np.zeros(12)
for _
in range(nodes + 1)]).T
172 self.
opti.set_initial(
173 self.
c_us, np.vstack([np.zeros(4)
for _
in range(nodes)]).T
176 def solve(self, approx_hessian=True):
177 opts = {
"verbose":
False}
180 "linear_solver":
"mumps",
182 "mu_strategy":
"adaptive",
186 opts[
"ipopt"][
"hessian_approximation"] =
"limited-memory"
189 self.
opti.solver(
"ipopt", opts)
196 if self.
sol.stats()[
"return_status"] ==
"Solve_Succeeded":
208 for idx, (dx_sol, u_sol)
in enumerate(
211 q = pin.integrate(self.
model, np.array(x_nom)[:nq], dx_sol[:nv])
214 self.
xs.
append(np.concatenate([q, v]))
218 self.
model, np.array(x_nom)[:nq], self.
sol.value(self.
c_dxs).T[nodes, :nv]
220 v = self.
sol.value(self.
c_dxs).T[nodes, nv:]
221 self.
xs.
append(np.concatenate([q, v]))
224 self.
gaps = {
"vector": [np.zeros(self.
model.nv * 2)],
"norm": [0]}
229 for idx, (x, u)
in enumerate(zip(self.
xs, self.
us)):
232 gap_q = pin.difference(self.
model, x_pin[:nq], self.
xs[idx + 1][:nq])
233 gap_v = self.
xs[idx + 1][nq:] - x_pin[nq:]
235 gap = np.concatenate([gap_q, gap_v])
248 a = pin.aba(self.
model, self.
data, q, v, tau)
250 dq = v * dt + a * dt**2
253 q_next = pin.integrate(self.
model, q, dq)
256 x_next = np.concatenate([q_next, v_next])
262 robot = example_robot_data.load(
"hector")
267 oc_problem.solve(approx_hessian=
True)
270 import matplotlib.pyplot
as plt
272 fig0, axs0 = plt.subplots(nrows=2)
274 xs = np.vstack(oc_problem.xs)
275 axs0[0].plot(xs[:, :3])
276 axs0[0].set_title(
"Quadcopter position")
278 axs0[1].plot(oc_problem.gaps[
"norm"])
279 axs0[1].set_title(
"Multiple shooting node gaps")
281 fig1, axs1 = plt.subplots(nrows=4)
282 us = np.vstack(oc_problem.us)
284 for idx, ax
in enumerate(axs1):
287 plt.show(block=
False)
290 if __name__ ==
"__main__":