2 from os.path
import abspath, dirname, join
6 import pinocchio
as pin
12 dirname(dirname(dirname(abspath(__file__)))),
18 import example_robot_data
21 x_goal = [1, 0, 1.5, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
22 x0 = [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
27 d_cog, cf, cm = 0.1525, 6.6e-5, 1e-6
34 [0.0, d_cog, 0.0, -d_cog],
35 [-d_cog, 0.0, d_cog, 0.0],
36 [-cm / cf, cm / cf, -cm / cf, cm / cf],
41 x_nom = [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0]
45 u = casadi.SX.sym(
"u", 4)
48 return casadi.Function(
"act_model", [u], [tau], [
"u"], [
"tau"])
52 q = casadi.SX.sym(
"dq", model.nq)
53 dq = casadi.SX.sym(
"q", model.nv)
54 v = casadi.SX.sym(
"v", model.nv)
55 dv = casadi.SX.sym(
"dv", model.nv)
57 q_next = cpin.integrate(model, q, dq)
60 dx = casadi.vertcat(dq, dv)
61 x = casadi.vertcat(q, v)
62 x_next = casadi.vertcat(q_next, v_next)
64 return casadi.Function(
"integrate", [x, dx], [x_next], [
"x",
"dx"], [
"x_next"])
68 q0 = casadi.SX.sym(
"q0", model.nq)
69 q1 = casadi.SX.sym(
"q1", model.nq)
70 v0 = casadi.SX.sym(
"v0", model.nv)
71 v1 = casadi.SX.sym(
"v1", model.nv)
73 q_diff = cpin.difference(model, q0, q1)
76 x0 = casadi.vertcat(q0, v0)
77 x1 = casadi.vertcat(q1, v1)
78 x_diff = casadi.vertcat(q_diff, v_diff)
80 return casadi.Function(
"difference", [x0, x1], [x_diff], [
"x0",
"x1"], [
"x_diff"])
85 u = casadi.SX.sym(
"u", nu)
90 q = casadi.SX.sym(
"q", model.nq)
91 v = casadi.SX.sym(
"v", model.nv)
93 a = cpin.aba(model, data, q, v, tau)
95 dq = v * dt + a * dt**2
98 x = casadi.vertcat(q, v)
99 dx = casadi.vertcat(dq, dv)
102 return casadi.Function(
"int_dyn", [x, u], [x_next], [
"x",
"u"], [
"x_next"])
106 dx = casadi.SX.sym(
"dx", model.nv * 2)
111 cost = 0.5 * e_goal.T @ e_goal
113 return casadi.Function(
"quad_cost", [dx], [cost], [
"dx"], [
"cost"])
117 def __init__(self, model, terminal_soft_constraint=False):
136 for i
in range(nodes):
140 1e-5 * 0.5 * e_reg.T @ e_reg
141 + 1e-5 * 0.5 * self.
c_us[:, i].T @ self.
c_us[:, i]
143 if terminal_soft_constraint:
146 self.
opti.minimize(obj)
149 for i
in range(nodes):
157 self.
opti.subject_to(gap == [0] * 12)
160 self.
opti.subject_to(self.
opti.bounded(0, self.
c_us, 5))
163 if not terminal_soft_constraint:
166 self.
opti.subject_to(e_goal == [0] * 12)
173 self.
opti.set_initial(
174 self.
c_dxs, np.vstack([np.zeros(12)
for _
in range(nodes + 1)]).T
176 self.
opti.set_initial(
177 self.
c_us, np.vstack([np.zeros(4)
for _
in range(nodes)]).T
180 def solve(self, approx_hessian=True):
181 opts = {
"verbose":
False}
184 "linear_solver":
"mumps",
186 "mu_strategy":
"adaptive",
190 opts[
"ipopt"][
"hessian_approximation"] =
"limited-memory"
193 self.
opti.solver(
"ipopt", opts)
200 if self.
sol.stats()[
"return_status"] ==
"Solve_Succeeded":
212 for idx, (dx_sol, u_sol)
in enumerate(
215 q = pin.integrate(self.
model, np.array(x_nom)[:nq], dx_sol[:nv])
218 self.
xs.
append(np.concatenate([q, v]))
222 self.
model, np.array(x_nom)[:nq], self.
sol.value(self.
c_dxs).T[nodes, :nv]
224 v = self.
sol.value(self.
c_dxs).T[nodes, nv:]
225 self.
xs.
append(np.concatenate([q, v]))
228 self.
gaps = {
"vector": [np.zeros(self.
model.nv * 2)],
"norm": [0]}
233 for idx, (x, u)
in enumerate(zip(self.
xs, self.
us)):
236 gap_q = pin.difference(self.
model, x_pin[:nq], self.
xs[idx + 1][:nq])
237 gap_v = self.
xs[idx + 1][nq:] - x_pin[nq:]
239 gap = np.concatenate([gap_q, gap_v])
252 a = pin.aba(self.
model, self.
data, q, v, tau)
254 dq = v * dt + a * dt**2
257 q_next = pin.integrate(self.
model, q, dq)
260 x_next = np.concatenate([q_next, v_next])
266 robot = example_robot_data.load(
"hector")
271 oc_problem.solve(approx_hessian=
True)
275 import matplotlib.pyplot
as plt
277 _, axs0 = plt.subplots(nrows=2)
279 xs = np.vstack(oc_problem.xs)
280 axs0[0].plot(xs[:, :3])
281 axs0[0].set_title(
"Quadcopter position")
283 axs0[1].plot(oc_problem.gaps[
"norm"])
284 axs0[1].set_title(
"Multiple shooting node gaps")
286 _, axs1 = plt.subplots(nrows=4)
287 us = np.vstack(oc_problem.us)
289 for idx, ax
in enumerate(axs1):
292 plt.show(block=
False)
293 except ImportError
as err:
295 "Error while initializing the viewer. "
296 "It seems you should install Python meshcat"
302 if __name__ ==
"__main__":