def __init__(self, nb_steps, duration, omega2, state_estimator, com_target, stance, tube_radius=0.02, tube_margin=0.01): start_com = state_estimator.com start_comd = state_estimator.comd tube = COMTube(start_com, com_target.p, stance, tube_radius, tube_margin) dt = duration / nb_steps I = eye(3) A = asarray(bmat([[I, dt * I], [zeros((3, 3)), I]])) B = asarray(bmat([[.5 * dt**2 * I], [dt * I]])) x_init = hstack([start_com, start_comd]) x_goal = hstack([com_target.p, com_target.pd]) C1, e1 = tube.primal_hrep D2, e2 = tube.dual_hrep C1 = hstack([C1, zeros(C1.shape)]) C2 = zeros((D2.shape[0], A.shape[1])) D1 = zeros((C1.shape[0], B.shape[1])) C = vstack([C1, C2]) D = vstack([D1, D2]) e = hstack([e1, e2]) lmpc = LinearPredictiveControl(A, B, C, D, e, x_init, x_goal, nb_steps, wxt=1., wxc=1e-1, wu=1e-1) lmpc.build() try: lmpc.solve() U = lmpc.U P = lmpc.X[:-1, 0:3] V = lmpc.X[:-1, 3:6] Z = P + (gravity - U) / omega2 preview = ZMPPreviewBuffer([stance]) preview.update(P, V, Z, [dt] * nb_steps, omega2) except ValueError as e: print "%s error:" % type(self).__name__, e preview = None self.lmpc = lmpc self.preview = preview self.tube = tube
class WrenchPredictiveController(NonlinearPredictiveController): weights = { 'p': 1e-0, 'v': 1e-2, 't': 1e-2, 'u': 1e-3, 'l': 1e-6, 'Ld': 1e-5, # this one has a huge impact } T_max = 6. # maximum duration of a step in [s] min_com_height = 0.5 # relative to contacts max_com_height = 1.5 # relative to contacts dT_max = 0.35 # as low as possible dT_min = 0.05 # as high as possible p_max = [+100, +100, +100] # [m] p_min = [-100, -100, -100] # [m] u_max = [+100, +100, +100] # [m] / [s]^2 u_min = [-100, -100, -100] # [m] / [s]^2 v_max = [+10, +10, +10] # [m] / [s] v_min = [-10, -10, -10] # [m] / [s] l_max = [+30] * 16 dT_init = .5 * (dT_min + dT_max) omega2 = 9.81 / 0.8 omega = sqrt(omega2) def __init__(self, nb_steps, state_estimator, com_target, contact_sequence, omega2, swing_duration=None): super(WrenchPredictiveController, self).__init__() t_build_start = time() end_com = list(com_target.p) end_comd = list(com_target.pd) nb_contacts = len(contact_sequence) start_com = list(state_estimator.com) start_comd = list(state_estimator.comd) p_0 = self.nlp.new_constant('p_0', 3, start_com) v_0 = self.nlp.new_constant('v_0', 3, start_comd) p_k = p_0 v_k = v_0 T_swing = 0 T_total = 0 for k in range(nb_steps): contact = contact_sequence[nb_contacts * k / nb_steps] u_k = self.nlp.new_variable('u_%d' % k, 3, init=[0., 0., 0.], lb=self.u_min, ub=self.u_max) dT_k = self.nlp.new_variable('dT_%d' % k, 1, init=[self.dT_init], lb=[self.dT_min], ub=[self.dT_max]) l_k = self.nlp.new_variable('l_%d' % k, 16, init=[0.] * 16, lb=[0.] * 16, ub=self.l_max) c = contact.p w_k = casadi.mtimes(contact.wrench_span, l_k) f_k, tau_k = w_k[:3, :], w_k[3:, :] Ld_k = casadi.cross(c - p_k, f_k) + tau_k m = state_estimator.pendulum.com_state.mass self.nlp.add_equality_constraint(u_k, f_k / m + gravity) # hard angular-momentum constraint just don't work # nlp.add_constraint(Ld_k, lb=[0., 0., 0.], ub=[0., 0., 0.]) self.nlp.extend_cost(self.weights['Ld'] * casadi.dot(Ld_k, Ld_k) * dT_k) self.nlp.extend_cost(self.weights['l'] * casadi.dot(l_k, l_k) * dT_k) self.nlp.extend_cost(self.weights['u'] * casadi.dot(u_k, u_k) * dT_k) p_next = p_k + v_k * dT_k + u_k * dT_k**2 / 2 v_next = v_k + u_k * dT_k T_total = T_total + dT_k if 2 * k < nb_steps: T_swing = T_swing + dT_k p_k = self.nlp.new_variable('p_%d' % (k + 1), 3, init=start_com, lb=self.p_min, ub=self.p_max) v_k = self.nlp.new_variable('v_%d' % (k + 1), 3, init=start_comd, lb=self.v_min, ub=self.v_max) self.nlp.add_equality_constraint(p_next, p_k) self.nlp.add_equality_constraint(v_next, v_k) self.nlp.add_constraint(T_swing, lb=[swing_duration], ub=[100], name='T_swing') p_last, v_last = p_k, v_k p_diff = p_last - end_com v_diff = v_last - end_comd self.nlp.extend_cost(self.weights['p'] * casadi.dot(p_diff, p_diff)) self.nlp.extend_cost(self.weights['v'] * casadi.dot(v_diff, v_diff)) self.nlp.extend_cost(self.weights['t'] * T_total) self.nlp.create_solver() # self.build_time = time() - t_build_start self.com_target = com_target self.contact_sequence = contact_sequence self.end_com = array(end_com) self.end_comd = array(end_comd) self.nb_contacts = nb_contacts self.nb_steps = nb_steps self.omega2 = omega2 self.preview = ZMPPreviewBuffer(contact_sequence) self.start_com = array(start_com) self.start_comd = array(start_comd) self.state_estimator = state_estimator self.swing_dT_min = self.dT_min def solve_nlp(self): t_solve_start = time() X = self.nlp.solve() self.solve_time = time() - t_solve_start # print "Symbols:", self.nlp.var_symbols N = self.nb_steps Y = X[:-6].reshape((N, 3 + 3 + 3 + 1 + 16)) P = Y[:, 0:3] V = Y[:, 3:6] # U = Y[:, 6:9] # not used dT = Y[:, 9] L = Y[:, 10:26] p_last = X[-6:-3] v_last = X[-3:] Z, Ld = self.compute_Z_and_Ld(P, L) cp_nog_last = p_last + v_last / self.omega cp_nog_last_des = self.end_com + self.end_comd / self.omega cp_error = norm(cp_nog_last - cp_nog_last_des) T_swing = sum(dT[:self.nb_steps / 2]) if self.swing_duration is not None and \ T_swing > 1.25 * self.swing_duration: self.update_swing_dT_min(self.swing_dT_min / 2) self.cp_error = cp_error if self.cp_error > 0.1: # and not self.preview.is_empty: print("Warning: preview not updated as CP error was", cp_error) self.nlp.warm_start(X) self.preview.update(P, V, Z, dT, self.omega2) self.p_last = p_last self.v_last = v_last self.print_results() def warm_start(self, preview_step_controller): pass def compute_Z_and_Ld(self, P, L): Ld, Z = [], [] contact_sequence = self.contact_sequence for k in range(self.nb_steps): contact = contact_sequence[2 * k / self.nb_steps] w_k = numpy.dot(contact.wrench_span, L[k]) f_k, tau_k = w_k[:3], w_k[3:] # comdd_k = f_k + gravity # gravity - comdd = -f z_k = P[k] - f_k / self.omega2 Ld_k = numpy.cross(contact.p - P[k], f_k) + tau_k Ld.append(Ld_k) Z.append(z_k) return array(Z), array(Ld) def on_tick(self, sim): self.preview.forward(sim.dt) start_com = list(self.state_estimator.com) start_comd = list(self.state_estimator.comd) self.nlp.update_constant('p_0', start_com) self.nlp.update_constant('v_0', start_comd) self.solve_nlp()
def __init__(self, nb_steps, state_estimator, com_target, contact_sequence, omega2, swing_duration=None): super(WrenchPredictiveController, self).__init__() t_build_start = time() end_com = list(com_target.p) end_comd = list(com_target.pd) nb_contacts = len(contact_sequence) start_com = list(state_estimator.com) start_comd = list(state_estimator.comd) p_0 = self.nlp.new_constant('p_0', 3, start_com) v_0 = self.nlp.new_constant('v_0', 3, start_comd) p_k = p_0 v_k = v_0 T_swing = 0 T_total = 0 for k in range(nb_steps): contact = contact_sequence[nb_contacts * k / nb_steps] u_k = self.nlp.new_variable('u_%d' % k, 3, init=[0., 0., 0.], lb=self.u_min, ub=self.u_max) dT_k = self.nlp.new_variable('dT_%d' % k, 1, init=[self.dT_init], lb=[self.dT_min], ub=[self.dT_max]) l_k = self.nlp.new_variable('l_%d' % k, 16, init=[0.] * 16, lb=[0.] * 16, ub=self.l_max) c = contact.p w_k = casadi.mtimes(contact.wrench_span, l_k) f_k, tau_k = w_k[:3, :], w_k[3:, :] Ld_k = casadi.cross(c - p_k, f_k) + tau_k m = state_estimator.pendulum.com_state.mass self.nlp.add_equality_constraint(u_k, f_k / m + gravity) # hard angular-momentum constraint just don't work # nlp.add_constraint(Ld_k, lb=[0., 0., 0.], ub=[0., 0., 0.]) self.nlp.extend_cost(self.weights['Ld'] * casadi.dot(Ld_k, Ld_k) * dT_k) self.nlp.extend_cost(self.weights['l'] * casadi.dot(l_k, l_k) * dT_k) self.nlp.extend_cost(self.weights['u'] * casadi.dot(u_k, u_k) * dT_k) p_next = p_k + v_k * dT_k + u_k * dT_k**2 / 2 v_next = v_k + u_k * dT_k T_total = T_total + dT_k if 2 * k < nb_steps: T_swing = T_swing + dT_k p_k = self.nlp.new_variable('p_%d' % (k + 1), 3, init=start_com, lb=self.p_min, ub=self.p_max) v_k = self.nlp.new_variable('v_%d' % (k + 1), 3, init=start_comd, lb=self.v_min, ub=self.v_max) self.nlp.add_equality_constraint(p_next, p_k) self.nlp.add_equality_constraint(v_next, v_k) self.nlp.add_constraint(T_swing, lb=[swing_duration], ub=[100], name='T_swing') p_last, v_last = p_k, v_k p_diff = p_last - end_com v_diff = v_last - end_comd self.nlp.extend_cost(self.weights['p'] * casadi.dot(p_diff, p_diff)) self.nlp.extend_cost(self.weights['v'] * casadi.dot(v_diff, v_diff)) self.nlp.extend_cost(self.weights['t'] * T_total) self.nlp.create_solver() # self.build_time = time() - t_build_start self.com_target = com_target self.contact_sequence = contact_sequence self.end_com = array(end_com) self.end_comd = array(end_comd) self.nb_contacts = nb_contacts self.nb_steps = nb_steps self.omega2 = omega2 self.preview = ZMPPreviewBuffer(contact_sequence) self.start_com = array(start_com) self.start_comd = array(start_comd) self.state_estimator = state_estimator self.swing_dT_min = self.dT_min
def __init__(self, nb_steps, com_state, com_target, contact_sequence, omega2=None, swing_duration=None): super(COPPredictiveController, self).__init__() end_com = list(com_target.p) end_comd = list(com_target.pd) nb_contacts = len(contact_sequence) start_com = list(com_state.p) start_comd = list(com_state.pd) p_0 = self.nlp.new_constant('p_0', 3, start_com) v_0 = self.nlp.new_constant('v_0', 3, start_comd) p_k = p_0 v_k = v_0 T_swing = 0 T_total = 0 for k in xrange(nb_steps): contact = contact_sequence[nb_contacts * k / nb_steps] u_k = self.nlp.new_variable('u_%d' % k, 3, init=[0, 0, 0], lb=self.u_min, ub=self.u_max) dT_k = self.nlp.new_variable('dT_%d' % k, 1, init=[self.dT_init], lb=[self.dT_min], ub=[self.dT_max]) alpha_k = self.nlp.new_variable('alpha_%d' % k, 1, init=[0.], lb=[-self.alpha_lim], ub=[+self.alpha_lim]) beta_k = self.nlp.new_variable('beta_%d' % k, 1, init=[0.], lb=[-self.beta_lim], ub=[+self.beta_lim]) lambda_k = self.nlp.new_variable('lambda_%d' % k, 1, init=[self.lambda_init], lb=[self.lambda_min], ub=[self.lambda_max]) z_k = contact.p + alpha_k * contact.X * contact.t + \ beta_k * contact.Y * contact.b self.nlp.add_equality_constraint(u_k, lambda_k * (p_k - z_k) + gravity) self.nlp.extend_cost(self.weights['alpha'] * alpha_k * alpha_k * dT_k) self.nlp.extend_cost(self.weights['beta'] * beta_k * beta_k * dT_k) # self.nlp.extend_cost( # self.weights['lambda'] * lambda_k ** 2 * dT_k) self.nlp.extend_cost(self.weights['u'] * casadi.dot(u_k, u_k) * dT_k) # Full precision (no Taylor expansion) omega_k = casadi.sqrt(lambda_k) p_next = p_k \ + v_k / omega_k * casadi.sinh(omega_k * dT_k) \ + u_k / lambda_k * (cosh(omega_k * dT_k) - 1.) v_next = v_k * cosh(omega_k * dT_k) \ + u_k / omega_k * sinh(omega_k * dT_k) T_total = T_total + dT_k if 2 * k < nb_steps: T_swing = T_swing + dT_k self.add_friction_constraint(contact, p_k, z_k) self.add_friction_constraint(contact, p_next, z_k) # slower: # add_linear_friction_constraints(contact, p_k, z_k) # add_linear_friction_constraints(contact, p_next, z_k) p_k = self.nlp.new_variable('p_%d' % (k + 1), 3, init=start_com, lb=self.p_min, ub=self.p_max) v_k = self.nlp.new_variable('v_%d' % (k + 1), 3, init=start_comd, lb=self.v_min, ub=self.v_max) self.nlp.add_equality_constraint(p_next, p_k) self.nlp.add_equality_constraint(v_next, v_k) self.nlp.add_constraint(T_swing, lb=[swing_duration], ub=[100], name='T_swing') p_last, v_last = p_k, v_k p_diff = p_last - end_com v_diff = v_last - end_comd self.nlp.extend_cost(self.weights['p'] * casadi.dot(p_diff, p_diff)) self.nlp.extend_cost(self.weights['v'] * casadi.dot(v_diff, v_diff)) self.nlp.extend_cost(self.weights['t'] * T_total) self.nlp.create_solver() # self.com_target = com_target self.end_com = array(end_com) self.end_comd = array(end_comd) self.nb_steps = nb_steps self.preview = ZMPPreviewBuffer(contact_sequence)
class COPPredictiveController(NonlinearPredictiveController): weights = { # EFFECT ON CONVERGENCE SPEED 'p': 1e-0, # base task, sweet range ~ [1, 10] 'v': 1e-2, # sweet spot ~ 1/100 't': 1e-2, # 'u': 1e-3, # sweet spot ~ 1/1000 'alpha': 1e-5, # best range ~ [1e-5, 1e-4] 'beta': 1e-5, # idem 'lambda': 0, # dreadful effect: leave as 0! } alpha_lim = 0.5 beta_lim = 0.5 dT_max = 0.35 # as low as possible dT_min = 0.05 # as high as possible lambda_max = 10000 lambda_min = 9.81 / 10 p_max = [+100, +100, +100] # [m] p_min = [-100, -100, -100] # [m] u_max = [+100, +100, +100] # [m] / [s]^2 u_min = [-100, -100, -100] # [m] / [s]^2 v_max = [+10, +10, +10] # [m] / [s] v_min = [-10, -10, -10] # [m] / [s] dT_init = .5 * (dT_min + dT_max) lambda_init = 9.81 / 1. # significant effect def __init__(self, nb_steps, com_state, com_target, contact_sequence, omega2=None, swing_duration=None): super(COPPredictiveController, self).__init__() end_com = list(com_target.p) end_comd = list(com_target.pd) nb_contacts = len(contact_sequence) start_com = list(com_state.p) start_comd = list(com_state.pd) p_0 = self.nlp.new_constant('p_0', 3, start_com) v_0 = self.nlp.new_constant('v_0', 3, start_comd) p_k = p_0 v_k = v_0 T_swing = 0 T_total = 0 for k in xrange(nb_steps): contact = contact_sequence[nb_contacts * k / nb_steps] u_k = self.nlp.new_variable('u_%d' % k, 3, init=[0, 0, 0], lb=self.u_min, ub=self.u_max) dT_k = self.nlp.new_variable('dT_%d' % k, 1, init=[self.dT_init], lb=[self.dT_min], ub=[self.dT_max]) alpha_k = self.nlp.new_variable('alpha_%d' % k, 1, init=[0.], lb=[-self.alpha_lim], ub=[+self.alpha_lim]) beta_k = self.nlp.new_variable('beta_%d' % k, 1, init=[0.], lb=[-self.beta_lim], ub=[+self.beta_lim]) lambda_k = self.nlp.new_variable('lambda_%d' % k, 1, init=[self.lambda_init], lb=[self.lambda_min], ub=[self.lambda_max]) z_k = contact.p + alpha_k * contact.X * contact.t + \ beta_k * contact.Y * contact.b self.nlp.add_equality_constraint(u_k, lambda_k * (p_k - z_k) + gravity) self.nlp.extend_cost(self.weights['alpha'] * alpha_k * alpha_k * dT_k) self.nlp.extend_cost(self.weights['beta'] * beta_k * beta_k * dT_k) # self.nlp.extend_cost( # self.weights['lambda'] * lambda_k ** 2 * dT_k) self.nlp.extend_cost(self.weights['u'] * casadi.dot(u_k, u_k) * dT_k) # Full precision (no Taylor expansion) omega_k = casadi.sqrt(lambda_k) p_next = p_k \ + v_k / omega_k * casadi.sinh(omega_k * dT_k) \ + u_k / lambda_k * (cosh(omega_k * dT_k) - 1.) v_next = v_k * cosh(omega_k * dT_k) \ + u_k / omega_k * sinh(omega_k * dT_k) T_total = T_total + dT_k if 2 * k < nb_steps: T_swing = T_swing + dT_k self.add_friction_constraint(contact, p_k, z_k) self.add_friction_constraint(contact, p_next, z_k) # slower: # add_linear_friction_constraints(contact, p_k, z_k) # add_linear_friction_constraints(contact, p_next, z_k) p_k = self.nlp.new_variable('p_%d' % (k + 1), 3, init=start_com, lb=self.p_min, ub=self.p_max) v_k = self.nlp.new_variable('v_%d' % (k + 1), 3, init=start_comd, lb=self.v_min, ub=self.v_max) self.nlp.add_equality_constraint(p_next, p_k) self.nlp.add_equality_constraint(v_next, v_k) self.nlp.add_constraint(T_swing, lb=[swing_duration], ub=[100], name='T_swing') p_last, v_last = p_k, v_k p_diff = p_last - end_com v_diff = v_last - end_comd self.nlp.extend_cost(self.weights['p'] * casadi.dot(p_diff, p_diff)) self.nlp.extend_cost(self.weights['v'] * casadi.dot(v_diff, v_diff)) self.nlp.extend_cost(self.weights['t'] * T_total) self.nlp.create_solver() # self.com_target = com_target self.end_com = array(end_com) self.end_comd = array(end_comd) self.nb_steps = nb_steps self.preview = ZMPPreviewBuffer(contact_sequence) def solve_nlp(self): X = self.nlp.solve() self.nlp.warm_start(X) # print "Symbols:", self.nlp.var_symbols N = self.nb_steps Y = X[:-6].reshape((N, 3 + 3 + 3 + 1 + 1 + 1 + 1)) P = Y[:, 0:3] V = Y[:, 3:6] # U = Y[:, 6:9] # not used dT = Y[:, 9] Alpha = Y[:, 10] Beta = Y[:, 11] Lambda = Y[:, 12] p_last = X[-6:-3] v_last = X[-3:] Z = self.compute_Z(Alpha, Beta) self.preview.update(P, V, Z, dT, Lambda) self.p_last = p_last self.v_last = v_last self.print_results() def compute_Z(self, Alpha, Beta): Z = [] contact_sequence = self.contact_sequence for k in xrange(self.nb_steps): contact = contact_sequence[2 * k / self.nb_steps] z_k = contact.p + Alpha[k] * contact.X * contact.t + \ Beta[k] * contact.Y * contact.b Z.append(z_k) return array(Z) def on_tick(self, sim): self.preview.forward(sim.dt) start_com = list(self.robot.com) start_comd = list(self.robot.comd) self.nlp.update_constant('p_0', start_com) self.nlp.update_constant('v_0', start_comd) self.solve_nlp()
def __init__(self, nb_steps, state_estimator, preview_ref): PVZ_ref, contacts = preview_ref.discretize(nb_steps) P_ref = PVZ_ref[:, 0:3] X_ref = PVZ_ref[:, 0:6] Z_ref = PVZ_ref[:, 6:9] GZ_ref = Z_ref - P_ref dt = preview_ref.duration / nb_steps I, fzeros = eye(3), zeros((4, 3)) omega2 = preview_ref.omega2 omega = sqrt(omega2) a = omega * dt A = asarray( bmat([[cosh(a) * I, sinh(a) / omega * I], [omega * sinh(a) * I, cosh(a) * I]])) B = vstack([(1. - cosh(a)) * I, -omega * sinh(a) * I]) x_init = hstack([state_estimator.com, state_estimator.comd]) Delta_x_init = x_init - X_ref[0] Delta_x_goal = zeros((6, )) C = [None] * nb_steps D = [None] * nb_steps e = [None] * nb_steps for k in xrange(nb_steps): contact = contacts[k] force_inequalities = contact.force_inequalities C_fric = hstack([force_inequalities, fzeros]) D_fric = -force_inequalities e_fric = dot(force_inequalities, GZ_ref[k]) if not all(e_fric > -1e-10): print "Warning: reference violates friction cone constraints" C_cop = zeros((4, 6)) D_cop = zeros((4, 3)) e_cop = zeros(4) for (i, vertex) in enumerate(contact.vertices): successor = (i + 1) % len(contact.vertices) edge = contact.vertices[successor] - vertex normal = cross(P_ref[k] - vertex, edge) C_cop[i, 0:3] = cross(edge, Z_ref[k] - vertex) # * Delta_p D_cop[i, :] = normal # * Delta_z e_cop[i] = -dot(normal, GZ_ref[k]) if not all(e_cop > -1e-10): print "Warning: reference violates friction cone constraints" C[k] = vstack([C_fric, C_cop]) D[k] = vstack([D_fric, D_cop]) e[k] = hstack([e_fric, e_cop]) lmpc = LinearPredictiveControl(A, B, C, D, e, Delta_x_init, Delta_x_goal, nb_steps, wxt=1., wxc=1e-3, wu=1e-3) lmpc.build() try: lmpc.solve() X = X_ref + lmpc.X Z = Z_ref[:-1] + lmpc.U P, V = X[:, 0:3], X[:, 3:6] assert len(X) == nb_steps + 1 assert len(Z) == nb_steps preview = ZMPPreviewBuffer(preview_ref.contact_sequence) preview.update(P, V, Z, [dt] * nb_steps, omega2) if __debug__: self.Delta_X = lmpc.X self.Delta_Z = lmpc.U self.X_ref = X_ref self.P_ref = P_ref self.Z_ref = Z_ref self.contacts = contacts except ValueError as e: print "%s error:" % type(self).__name__, e preview = None self.lmpc = lmpc self.nb_steps = nb_steps self.preview = preview
def __init__(self, nb_steps, state_estimator, com_target, contact_sequence, omega2, swing_duration=None): super(FIPPredictiveController, self).__init__() t_build_start = time() omega = sqrt(omega2) end_com = list(com_target.p) end_comd = list(com_target.pd) nb_contacts = len(contact_sequence) start_com = list(state_estimator.com) start_comd = list(state_estimator.comd) p_0 = self.nlp.new_constant('p_0', 3, start_com) v_0 = self.nlp.new_constant('v_0', 3, start_comd) p_k = p_0 v_k = v_0 T_swing = 0 T_total = 0 for k in xrange(nb_steps): contact = contact_sequence[nb_contacts * k / nb_steps] z_min = list(contact.p - [1, 1, 1]) # TODO: smarter z_max = list(contact.p + [1, 1, 1]) u_k = self.nlp.new_variable('u_%d' % k, 3, init=[0, 0, 0], lb=self.u_min, ub=self.u_max) z_k = self.nlp.new_variable('z_%d' % k, 3, init=list(contact.p), lb=z_min, ub=z_max) dT_k = self.nlp.new_variable('dT_%d' % k, 1, init=[self.dT_init], lb=[self.dT_min], ub=[self.dT_max]) CZ_k = z_k - contact.p self.nlp.add_equality_constraint(u_k, omega2 * (p_k - z_k) + gravity) self.nlp.extend_cost(self.weights['zmp'] * casadi.dot(CZ_k, CZ_k) * dT_k) self.nlp.extend_cost(self.weights['acc'] * casadi.dot(u_k, u_k) * dT_k) # Full precision (no Taylor expansion) p_next = p_k \ + v_k / omega * casadi.sinh(omega * dT_k) \ + u_k / omega2 * (cosh(omega * dT_k) - 1.) v_next = v_k * cosh(omega * dT_k) \ + u_k / omega * sinh(omega * dT_k) T_total = T_total + dT_k if 2 * k < nb_steps: T_swing = T_swing + dT_k self.add_com_boundary_constraint(contact, p_k) self.add_friction_constraint(contact, p_k, z_k) # self.add_friction_constraint(contact, p_next, z_k) # self.add_linear_friction_constraints(contact, p_k, z_k) # self.add_linear_friction_constraints(contact, p_next, z_k) # self.add_cop_constraint(contact, p_k, z_k) # self.add_cop_constraint(contact, p_next, z_k) self.add_linear_cop_constraints(contact, p_k, z_k) # self.add_linear_cop_constraints(contact, p_next, z_k) p_k = self.nlp.new_variable('p_%d' % (k + 1), 3, init=start_com, lb=self.p_min, ub=self.p_max) v_k = self.nlp.new_variable('v_%d' % (k + 1), 3, init=start_comd, lb=self.v_min, ub=self.v_max) self.nlp.add_equality_constraint(p_next, p_k) self.nlp.add_equality_constraint(v_next, v_k) if swing_duration is not None: self.nlp.add_constraint(T_swing, lb=[swing_duration], ub=[self.T_max], name='T_swing') p_last, v_last, z_last = p_k, v_k, z_k cp_last = p_last + v_last / omega cp_last = end_com + end_comd / omega + gravity / omega2 z_last = z_k # last_contact = contact_sequence[-1] # self.add_friction_constraint(last_contact, p_last, cp_last) # self.add_linear_cop_constraints(last_contact, p_last, cp_last) self.nlp.add_equality_constraint(z_last, cp_last) p_diff = p_last - end_com v_diff = v_last - end_comd cp_diff = p_diff + v_diff / omega # self.nlp.extend_cost(self.weights['pos'] * casadi.dot(p_diff, p_diff)) # self.nlp.extend_cost(self.weights['vel'] * casadi.dot(v_diff, v_diff)) self.nlp.extend_cost(self.weights['cp'] * casadi.dot(cp_diff, cp_diff)) self.nlp.extend_cost(self.weights['time'] * T_total) self.nlp.create_solver() # self.build_time = time() - t_build_start self.contact_sequence = contact_sequence self.cp_error = 1e-6 # => self.is_ready_to_switch is initially True self.end_com = array(end_com) self.end_comd = array(end_comd) self.nb_contacts = nb_contacts self.nb_steps = nb_steps self.omega = omega self.omega2 = omega2 self.p_last = None self.preview = ZMPPreviewBuffer(contact_sequence) self.state_estimator = state_estimator self.swing_dT_min = self.dT_min self.swing_duration = swing_duration self.v_last = None
class FIPPredictiveController(NonlinearPredictiveController): """ Non-linear H-representation Predictive Controller. """ weights = { # EFFECT ON CONVERGENCE SPEED 'cp': 1e-0, # main task 'time': 1e-2, # sweet spot ~ 0.01 (> is faster but less stable) 'acc': 1e-3, # sweet spot ~ 1e-3 'zmp': 1e-5, # best range ~ [1e-5, 1e-4] } T_max = 6. # maximum duration of a step in [s] dT_max = 0.35 # as low as possible dT_min = 0.03 # as high as possible, but caution when > sim.dt p_max = [+100, +100, +100] # [m] p_min = [-100, -100, -100] # [m] u_max = [+100, +100, +100] # [m] / [s]^2 u_min = [-100, -100, -100] # [m] / [s]^2 v_max = [+10, +10, +10] # [m] / [s] v_min = [-10, -10, -10] # [m] / [s] dT_init = .5 * (dT_min + dT_max) def __init__(self, nb_steps, state_estimator, com_target, contact_sequence, omega2, swing_duration=None): super(FIPPredictiveController, self).__init__() t_build_start = time() omega = sqrt(omega2) end_com = list(com_target.p) end_comd = list(com_target.pd) nb_contacts = len(contact_sequence) start_com = list(state_estimator.com) start_comd = list(state_estimator.comd) p_0 = self.nlp.new_constant('p_0', 3, start_com) v_0 = self.nlp.new_constant('v_0', 3, start_comd) p_k = p_0 v_k = v_0 T_swing = 0 T_total = 0 for k in xrange(nb_steps): contact = contact_sequence[nb_contacts * k / nb_steps] z_min = list(contact.p - [1, 1, 1]) # TODO: smarter z_max = list(contact.p + [1, 1, 1]) u_k = self.nlp.new_variable('u_%d' % k, 3, init=[0, 0, 0], lb=self.u_min, ub=self.u_max) z_k = self.nlp.new_variable('z_%d' % k, 3, init=list(contact.p), lb=z_min, ub=z_max) dT_k = self.nlp.new_variable('dT_%d' % k, 1, init=[self.dT_init], lb=[self.dT_min], ub=[self.dT_max]) CZ_k = z_k - contact.p self.nlp.add_equality_constraint(u_k, omega2 * (p_k - z_k) + gravity) self.nlp.extend_cost(self.weights['zmp'] * casadi.dot(CZ_k, CZ_k) * dT_k) self.nlp.extend_cost(self.weights['acc'] * casadi.dot(u_k, u_k) * dT_k) # Full precision (no Taylor expansion) p_next = p_k \ + v_k / omega * casadi.sinh(omega * dT_k) \ + u_k / omega2 * (cosh(omega * dT_k) - 1.) v_next = v_k * cosh(omega * dT_k) \ + u_k / omega * sinh(omega * dT_k) T_total = T_total + dT_k if 2 * k < nb_steps: T_swing = T_swing + dT_k self.add_com_boundary_constraint(contact, p_k) self.add_friction_constraint(contact, p_k, z_k) # self.add_friction_constraint(contact, p_next, z_k) # self.add_linear_friction_constraints(contact, p_k, z_k) # self.add_linear_friction_constraints(contact, p_next, z_k) # self.add_cop_constraint(contact, p_k, z_k) # self.add_cop_constraint(contact, p_next, z_k) self.add_linear_cop_constraints(contact, p_k, z_k) # self.add_linear_cop_constraints(contact, p_next, z_k) p_k = self.nlp.new_variable('p_%d' % (k + 1), 3, init=start_com, lb=self.p_min, ub=self.p_max) v_k = self.nlp.new_variable('v_%d' % (k + 1), 3, init=start_comd, lb=self.v_min, ub=self.v_max) self.nlp.add_equality_constraint(p_next, p_k) self.nlp.add_equality_constraint(v_next, v_k) if swing_duration is not None: self.nlp.add_constraint(T_swing, lb=[swing_duration], ub=[self.T_max], name='T_swing') p_last, v_last, z_last = p_k, v_k, z_k cp_last = p_last + v_last / omega cp_last = end_com + end_comd / omega + gravity / omega2 z_last = z_k # last_contact = contact_sequence[-1] # self.add_friction_constraint(last_contact, p_last, cp_last) # self.add_linear_cop_constraints(last_contact, p_last, cp_last) self.nlp.add_equality_constraint(z_last, cp_last) p_diff = p_last - end_com v_diff = v_last - end_comd cp_diff = p_diff + v_diff / omega # self.nlp.extend_cost(self.weights['pos'] * casadi.dot(p_diff, p_diff)) # self.nlp.extend_cost(self.weights['vel'] * casadi.dot(v_diff, v_diff)) self.nlp.extend_cost(self.weights['cp'] * casadi.dot(cp_diff, cp_diff)) self.nlp.extend_cost(self.weights['time'] * T_total) self.nlp.create_solver() # self.build_time = time() - t_build_start self.contact_sequence = contact_sequence self.cp_error = 1e-6 # => self.is_ready_to_switch is initially True self.end_com = array(end_com) self.end_comd = array(end_comd) self.nb_contacts = nb_contacts self.nb_steps = nb_steps self.omega = omega self.omega2 = omega2 self.p_last = None self.preview = ZMPPreviewBuffer(contact_sequence) self.state_estimator = state_estimator self.swing_dT_min = self.dT_min self.swing_duration = swing_duration self.v_last = None def solve_nlp(self): t_solve_start = time() X = self.nlp.solve() self.solve_time = time() - t_solve_start # print "Symbols:", self.nlp.var_symbols N = self.nb_steps Y = X[:-6].reshape((N, 3 + 3 + 3 + 3 + 1)) P = Y[:, 0:3] V = Y[:, 3:6] # U = Y[:, 6:9] # not used Z = Y[:, 9:12] dT = Y[:, 12] p_last = X[-6:-3] v_last = X[-3:] cp_nog_last = p_last + v_last / self.omega cp_nog_last_des = self.end_com + self.end_comd / self.omega cp_error = norm(cp_nog_last - cp_nog_last_des) T_swing = sum(dT[:self.nb_steps / 2]) if self.swing_duration is not None and \ T_swing > 1.25 * self.swing_duration: self.update_swing_dT_min(self.swing_dT_min / 2) self.cp_error = cp_error if self.cp_error > 0.1: # and not self.preview.is_empty: print "Warning: preview not updated as CP error was", cp_error return self.nlp.warm_start(X) # save as initial guess for next iteration self.preview.update(P, V, Z, dT, self.omega2) self.p_last = p_last self.v_last = v_last self.print_results() def warm_start(self, previous_step_controller): X = previous_step_controller.nlp.initvals N = previous_step_controller.nb_steps warm_start = X[(3 + 3 + 3 + 3 + 1) * (N / 2):] if len(warm_start) != len(self.nlp.initvals): warn("previous controller has no warm-start info for next phase") return self.update_dT_min(previous_step_controller.dT_min) self.nlp.warm_start(warm_start) def on_tick(self, sim): self.preview.forward(sim.dt) start_com = list(self.state_estimator.com) start_comd = list(self.state_estimator.comd) self.nlp.update_constant('p_0', start_com) self.nlp.update_constant('v_0', start_comd) self.solve_nlp()