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()