def __init__(self, sample_times, num_vars, degree, continuity_degree):
self.sample_times = sample_times
self.n = num_vars
self.degree = degree
self.prog = MathematicalProgram()
self.coeffs = []
for i in range(len(sample_times)):
self.coeffs.append(
self.prog.NewContinuousVariables(num_vars, degree + 1, "C"))
self.result = None
# Add continuity constraints
for s in range(len(sample_times) - 1):
trel = sample_times[s + 1] - sample_times[s]
coeffs = self.coeffs[s]
for var in range(self.n):
for deg in range(continuity_degree + 1):
# Don't use eval here, because I want left and right
# values of the same time
left_val = 0
for d in range(deg, self.degree + 1):
left_val += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
right_val = self.coeffs[s + 1][var,
deg] * math.factorial(deg)
self.prog.AddLinearConstraint(left_val == right_val)
# Add cost to minimize highest order terms
for s in range(len(sample_times) - 1):
self.prog.AddQuadraticCost(np.eye(num_vars), np.zeros(
(num_vars, 1)), self.coeffs[s][:, -1])
def is_verified(rho):
# initialize optimization problem
# (with Drake there is no need to specify that
# this is going to be a SOS program!)
prog = MathematicalProgram()
# SOS indeterminates
x = prog.NewIndeterminates(2, 'x')
# Lyapunov function
V = x.dot(P).dot(x)
V_dot = 2*x.dot(P).dot(f(x))
# degree of the polynomial lambda(x)
# no need to change it, but if you really want to,
# keep l_deg even (why?) and do not set l_deg greater than 10
# (otherwise optimizations will take forever)
l_deg = 4
assert l_deg % 2 == 0
# SOS Lagrange multipliers
l = prog.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression()
# main condition above
eps = 1e-3 # do not change
prog.AddSosConstraint(- V_dot - l * (rho - V) - eps*x.dot(x))
# solve SOS program
# no objective function in this formulation
result = Solve(prog)
# return True if feasible, False if infeasible
return result.is_success()
def LQR(self, ztraj, utraj, Q, R, Qf):
tspan = utraj.get_segment_times()
dztrajdt = ztraj.derivative(1)
context = self.CreateDefaultContext()
nZ = context.num_continuous_states()
nU = self.GetInputPort('u').size()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
z = prog.NewIndeterminates(nZ,'z')
ucon = prog.NewIndeterminates(nU,'u')
#nY = self.GetOutputPort('z').size()
#N = np.zeros([nX, nU])
times = ztraj.get_segment_times()
K = []
S = []
for t in times:
# option 1
z0 = ztraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
sym_context.SetContinuousState(z0+z)
sym_context.FixInputPort(0, u0+ucon )
# zdot=f(z,u)==>zhdot=f(zh+z0,uh+u0)-z0dot
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() # - dztrajdt.value(t).transpose()
mapping = dict(zip(z, z0))
mapping.update(dict(zip(ucon, u0)))
A = Evaluate(Jacobian(f, z), mapping)
B = Evaluate(Jacobian(f, ucon), mapping)
k, s = LinearQuadraticRegulator(A, B, Q, R)
import pdb; pdb.set_trace()
if(len(K) == 0):
K = np.ravel(k).reshape(nU*nZ,1)
S = np.ravel(s).reshape(nZ*nZ,1)
else:
K = np.hstack( (K, np.ravel(k).reshape(nU*nZ,1)) )
S = np.hstack( (S, np.ravel(s).reshape(nZ*nZ,1)) )
#
# option 2
#context.SetContinuousState(xtraj.value(t) )
#context.FixInputPort(0, utraj.value(t) )
#linearized_plant = Linearize(self, context)
#K.append(LinearQuadraticRegulator(linearized_plant.A(),
# linearized_plant.B(),
# Q, R)) #self, context, Q, R)
Kpp = PiecewisePolynomial.FirstOrderHold(times, K)
return Kpp
def solve_lcp(P, q):
prog = MathematicalProgram()
x = prog.NewContinuousVariables(q.size)
prog.AddLinearComplementarityConstraint(P, q, x)
result = Solve(prog)
status = result.is_success()
z = result.GetSolution(x)
return (z, status)
def SOS_compute_1(self, S, rho_prev):
# fix V and rho, search for L and u
prog = MathematicalProgram()
x = prog.NewIndeterminates(2, "x")
# Define u
K = prog.NewContinuousVariables(2, "K")
# Fixed Lyapunov
V = x.dot(np.dot(S, x))
Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0]
# Define the Lagrange multipliers.
(lambda_, constraint) = prog.NewSosPolynomial(Variables(x), 2)
prog.AddLinearConstraint(K[0] * x[0] <= 2.5)
prog.AddSosConstraint(-Vdot - lambda_.ToExpression() * (rho_prev - V))
result = prog.Solve()
# print(lambda_.ToExpression())
# print(lambda_.decision_variables())
lc = [prog.GetSolution(var) for var in lambda_.decision_variables()]
lbda_coeff = np.ones([3, 3])
lbda_coeff[0, 0] = lc[0]
lbda_coeff[0, 1] = lbda_coeff[1, 0] = lc[1]
lbda_coeff[2, 0] = lbda_coeff[0, 2] = lc[2]
lbda_coeff[1, 1] = lc[3]
lbda_coeff[2, 1] = lbda_coeff[1, 2] = lc[4]
lbda_coeff[2, 2] = lc[5]
return lbda_coeff
def build_mpmiqp(self):
# express the constrained dynamics as a list of polytopes in the (x,u,x+)-space
P = graph_representation(self.S)
m = big_m(P)
# initialize program
self.prog = MathematicalProgram()
self.x = []
self.u = []
self.d = []
obj = 0.
self.binaries_lower_bound = []
# initial conditions (set arbitrarily to zero in the building phase)
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
self.initial_condition = []
for k in range(self.S.nx):
self.initial_condition.append(self.prog.AddLinearConstraint(self.x[0][k] == 0.).evaluator())
# loop over time
for t in range(self.N):
# create input, mode and next state variables
self.u.append(self.prog.NewContinuousVariables(self.S.nu))
self.d.append(self.prog.NewBinaryVariables(self.S.nm))
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
# enforce constrained dynamics (big-m methods)
xux = np.concatenate((self.x[t], self.u[t], self.x[t+1]))
for i in range(self.S.nm):
mi_sum = np.sum([m[i][j] * self.d[t][j] for j in range(self.S.nm) if j != i], axis=0)
for k in range(P[i].A.shape[0]):
self.prog.AddLinearConstraint(P[i].A[k].dot(xux) <= P[i].b[k] + mi_sum[k])
# SOS1 on the binaries
self.prog.AddLinearConstraint(sum(self.d[t]) == 1.)
# stage cost to the objective
obj += .5 * self.u[t].dot(self.R).dot(self.u[t])
obj += .5 * self.x[t].dot(self.Q).dot(self.x[t])
# terminal constraint
for k in range(self.X_N.A.shape[0]):
self.prog.AddLinearConstraint(self.X_N.A[k].dot(self.x[self.N]) <= self.X_N.b[k])
# terminal cost
obj += .5 * self.x[self.N].dot(self.P).dot(self.x[self.N])
self.objective = self.prog.AddQuadraticCost(obj)
# set solver
self.solver = GurobiSolver()
self.prog.SetSolverOption(self.solver.solver_type(), 'OutputFlag', 1)
def RegionOfAttraction(system, context, V=None):
# TODO(russt): Add python binding for has_only_continuous_state
# assert(context.has_only_continuous_state())
# TODO(russt): Handle more general cases.
x0 = context.get_continuous_state_vector().CopyToVector()
# Check that x0 is a fixed point.
xdot0 = system.EvalTimeDerivatives(context).get_vector().CopyToVector()
#assert np.allclose(xdot0, 0*xdot0), "context does not describe a fixed point."
sym_system = system.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
x = prog.NewIndeterminates(sym_context.num_continuous_states(),'x')
# Evaluate the dynamics (in relative coordinates)
sym_context.SetContinuousState(x0+x)
f = sym_system.EvalTimeDerivatives(sym_context).get_vector().CopyToVector()
if V is None:
# Solve a Lyapunov equation to find the Lyapunov candidate.
#A = Evaluate(Jacobian(f, x), dict(zip(x, x0)))
#A = np.array([[0., 1.], [-10.*np.cos(x[0]), -0.1]])
#A = A.Evaluate(dict(zip(x, x0)))
A = np.array([[0., 1.], [-10.*np.cos(x0[0]), -0.1]])
Q = np.eye(sym_context.num_continuous_states())
P = RealContinuousLyapunovEquation(A, Q)
V = x.dot(P.dot(x))
#for i in range(len(f)):
# f[i] = f[i].Substitute(dict(zip(x,x-x0)))
Vdot = V.Jacobian(x).dot(f)
# Check Hessian of Vdot at origin
import pdb; pdb.set_trace()
H = Evaluate(0.5*Jacobian(Vdot.Jacobian(x),x), dict(zip(x, 0*x0)))
print('H=')
print(H)
print('P(Lyapunov)=')
print(P)
assert isPositiveDefinite(-H), "Vdot is not negative definite at the fixed point."
#V = FixedLyapunovMaximizeLevelSet(prog, x, V, Vdot)
V = FixedLyapunovSearchRho(prog, x, V, Vdot)
# Put V back into global coordinates
V = V.Substitute(dict(zip(x,x-x0)))
return V
def RegionOfAttraction(self, context, zdotraj, V=None):
z0 = context.get_continuous_state_vector().CopyToVector()
if(zdotraj is None):
zdotraj = 0.0*z0
# Check that x0 is a "fixed point" (on the trajectory).
zdot0 = self.EvalTimeDerivatives(context).CopyToVector()
assert np.allclose(zdot0, zdotraj), "context does not describe a fixed point." #0*xdot0),
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
z = prog.NewIndeterminates(sym_context.num_continuous_states(),'z')
# Evaluate the dynamics (in relative coordinates)
sym_context.SetContinuousState(z0+z)
#import pdb; pdb.set_trace()
uinput = self.GetInputPort('u')
u0 = uinput.EvalBasicVector(context).CopyToVector()
nU = uinput.size()
ucon = prog.NewIndeterminates(nU,'u')
sym_context.FixInputPort(0, u0+ucon )
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector()
mapping = dict(zip(z, z0))
mapping.update(dict(zip(ucon, u0)))
if V is None:
# Solve a Lyapunov equation to find the Lyapunov candidate.
A = Evaluate(Jacobian(f, z), mapping)
Q = np.eye(sym_context.num_continuous_states())
import pdb; pdb.set_trace()
P = RealContinuousLyapunovEquation(A, Q)
V = x.dot(P.dot(z))
Vdot = V.Jacobian(z).dot(f)
# Check Hessian of Vdot at origin
H = Evaluate(0.5*Jacobian(Vdot.Jacobian(z),z), mapping)
assert isPositiveDefinite(-H), "Vdot is not negative definite at the fixed point."
#V = FixedLyapunovMaximizeLevelSet(prog, z, V, Vdot)
V = self.FixedLyapunovSearchRho(prog, z, V, Vdot)
# Put V back into global coordinates
mapping = dict(zip(z,z-z0))
mapping.update(dict(zip(ucon, u0)))
V = V.Substitute(mapping)
return V
def initialize_problem(n_x, n_u, N, x0):
''' returns base MathematicalProgram object and empty decision vars '''
prog = MathematicalProgram()
# Decision Variables
x = prog.NewContinuousVariables(n_x, N, 'x')
u = prog.NewContinuousVariables(n_u, N-1, 'u')
z = [] # placeholder for obstacle binary variables
q = [] # placeholder for obstacle slack buffer variables
# initial conditions constraints
prog.AddBoundingBoxConstraint(x0, x0, x[:, 0])
decision_vars = [x, u, z, q]
return prog, decision_vars
def footstep_planner(terrain, n_steps, step_span):
# initialize optimization problem
prog = MathematicalProgram()
# optimization variables
decision_variables = add_decision_variables(prog, terrain, n_steps)
# constraints
set_initial_and_goal_position(prog, terrain, decision_variables)
relative_position_limits(prog, n_steps, step_span, decision_variables)
step_sequence(prog, n_steps, step_span, decision_variables)
one_stone_per_foot(prog, n_steps, decision_variables)
foot_in_stepping_stone(prog, terrain, n_steps, decision_variables)
# objective function
minimize_step_length(prog, n_steps, decision_variables)
# solve
bb = branch_and_bound.MixedIntegerBranchAndBound(prog, OsqpSolver().solver_id())
result = bb.Solve()
# ensure that the problem is feasible
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem.')
# retrieve result of the optimization
decision_variables_opt = [bb.GetSolution(v) for v in decision_variables]
objective_opt = bb.GetOptimalCost()
return decision_variables_opt, objective_opt
def CheckLevelSet(self, prev_x, x0, Vs, Vsdot, rho, multiplier_degree):
prog = MathematicalProgram()
x = prog.NewIndeterminates(len(prev_x),'x')
V = Vs.Substitute(dict(zip(prev_x, x)))
Vdot = Vsdot.Substitute(dict(zip(prev_x, x)))
slack = prog.NewContinuousVariables(1,'a')[0]
#mapping = dict(zip(x, np.ones(len(x))))
#V_norm = 0.0*V
#for i in range(len(x)):
# basis = np.ones(len(x))
# V_norm = V_norm + V.Substitute(dict(zip(x, basis)))
#V = V/V_norm
#Vdot = Vdot/V_norm
#prog.AddConstraint(V_norm == 0)
# in relative state (lambda(xbar)
Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression()
Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar)
prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V)
prog.AddCost(-slack)
#import pdb; pdb.set_trace()
result = Solve(prog)
if(not result.is_success()):
print('%s, %s' %(result.get_solver_id().name(),result.get_solution_result()) )
print('slack = %f' %(result.GetSolution(slack)) )
print('Rho = %f' %(rho))
#assert result.is_success()
return -1.0
return result.GetSolution(slack)
def LQR(self, xtraj, utraj, Q, R, Qf):
tspan = utraj.get_segment_times()
context = self.CreateDefaultContext()
nX = context.num_continuous_states()
nU = self.GetInputPort('u').size()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
x = prog.NewIndeterminates(nX, 'x')
ucon = prog.NewIndeterminates(nU, 'u')
#nY = self.GetOutputPort('x').size()
#N = np.zeros([nX, nU])
times = xtraj.get_segment_times()
K = []
import pdb
pdb.set_trace()
for t in times:
# option 1
x0 = xtraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
sym_context.SetContinuousState(x0 + x)
sym_context.FixInputPort(0, u0 + ucon)
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector()
A = Evaluate(Jacobian(f, x), dict(zip(x, x0)))
B = Evaluate(Jacobian(f, ucon), dict(zip(ucon, u0)))
K.append(LinearQuadraticRegulator(A, B, Q, R))
# option 2
#context.SetContinuousState(xtraj.value(t) )
#context.FixInputPort(0, utraj.value(t) )
#linearized_plant = Linearize(self, context)
#K.append(LinearQuadraticRegulator(linearized_plant.A(),
# linearized_plant.B(),
# Q, R)) #self, context, Q, R)
Kpp = PiecewisePolynomial.FirstOrderHold(times, K)
return Kpp
def RegionOfAttraction(self, context, V=None):
x0 = context.get_continuous_state_vector().CopyToVector()
# Check that x0 is a fixed point.
xdot0 = self.EvalTimeDerivatives(context).CopyToVector()
assert np.allclose(
xdot0, 0 * xdot0), "context does not describe a fixed point."
import pdb
pdb.set_trace()
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
prog = MathematicalProgram()
x = prog.NewIndeterminates(sym_context.num_continuous_states(),
'x')
# Evaluate the dynamics (in relative coordinates)
sym_context.SetContinuousState(x0 + x)
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector()
if V is None:
# Solve a Lyapunov equation to find the Lyapunov candidate.
A = Evaluate(Jacobian(f, x), dict(zip(x, x0)))
Q = np.eye(sym_context.num_continuous_states())
import pdb
pdb.set_trace()
P = RealContinuousLyapunovEquation(A, Q)
V = x.dot(P.dot(x))
Vdot = V.Jacobian(x).dot(f)
# Check Hessian of Vdot at origin
H = Evaluate(0.5 * Jacobian(Vdot.Jacobian(x), x), dict(zip(x, x0)))
assert isPositiveDefinite(
-H), "Vdot is not negative definite at the fixed point."
#V = FixedLyapunovMaximizeLevelSet(prog, x, V, Vdot)
V = self.FixedLyapunovSearchRho(prog, x, V, Vdot)
# Put V back into global coordinates
V = V.Substitute(dict(zip(x, x - x0)))
return V
def run_complex_mathematical_program():
print "\n\ncomplex mathematical program"
mp = MathematicalProgram()
x = mp.NewContinuousVariables(1, 'x')
mp.AddCost(cost, x)
mp.AddConstraint(quad_constraint, [1.0], [2.0], x)
mp.SetInitialGuess(x, [1.1])
print mp.Solve()
res = mp.GetSolution(x)
print res
print "(finished) complex mathematical program"
def run_simple_mathematical_program():
print "\n\nsimple mathematical program"
mp = MathematicalProgram()
x = mp.NewContinuousVariables(1, "x")
mp.AddLinearCost(x[0] * 1.0)
mp.AddLinearConstraint(x[0] >= 1)
print mp.Solve()
print mp.GetSolution(x)
print "(finished) simple mathematical program"
def intercepting_with_obs_avoidance(self,
p0,
v0,
pf,
vf,
T,
obstacles=None,
p_puck=None):
"""Intercepting trajectory that avoids obs"""
x0 = np.array(np.concatenate((p0, v0), axis=0))
xf = np.concatenate((pf, vf), axis=0)
prog = MathematicalProgram()
# state and control inputs
N = int(T / self.params.dt) # number of time steps
state = prog.NewContinuousVariables(N + 1, 4, 'state')
cmd = prog.NewContinuousVariables(N, 2, 'input')
# Initial and final state
prog.AddLinearConstraint(eq(state[0], x0))
#prog.AddLinearConstraint(eq(state[-1], xf))
prog.AddQuadraticErrorCost(Q=10.0 * np.eye(4),
x_desired=xf,
vars=state[-1])
self.add_dynamics(prog, N, state, cmd)
## Input saturation
self.add_input_limits(prog, cmd, N)
# Arena constraints
self.add_arena_limits(prog, state, N)
for k in range(N):
prog.AddQuadraticCost(cmd[k].flatten().dot(cmd[k]))
# Add non-linear constraints - will solve with SNOPT
# Avoid other players
if obstacles != None:
for obs in obstacles:
self.avoid_other_player_nl(prog, state, obs, N)
# avoid hitting the puck while generating a kicking trajectory
if not p_puck.any(None):
self.avoid_puck_nl(prog, state, N, p_puck)
solver = SnoptSolver()
result = solver.Solve(prog)
solution_found = result.is_success()
if not solution_found:
print("Solution not found for intercepting_with_obs_avoidance")
u_values = result.GetSolution(cmd)
return solution_found, u_values.T
def test_create_constraint_input_array(self):
prog = MathematicalProgram()
q = prog.NewContinuousVariables(1, 'q')
r = prog.NewContinuousVariables(rows=2, cols=3, name='r')
q_val = 0.5
r_val = np.arange(6).reshape((2,3))
'''
array([[0, 1, 2],
[3, 4, 5]])
'''
constraint = prog.AddConstraint(le(q, r[0,0] + 2*r[1,1]))
input_array = create_constraint_input_array(constraint, {
"q": q_val,
"r": r_val
})
expected_input_array = [0.5, 0, 4]
np.testing.assert_array_almost_equal(input_array, expected_input_array)
z = prog.NewContinuousVariables(2, 'z')
z_val = np.array([0.0, 0.5])
# https://stackoverflow.com/questions/64736910/using-le-or-ge-with-scalar-left-hand-side-creates-unsized-formula-array
# constraint = prog.AddConstraint(le(z[1], r[0,2] + 2*r[1,0]))
constraint = prog.AddConstraint(le([z[1]], r[0,2] + 2*r[1,0]))
input_array = create_constraint_input_array(constraint, {
"z": z_val,
"r": r_val
})
expected_input_array = [3, 2, 0.5]
np.testing.assert_array_almost_equal(input_array, expected_input_array)
a = prog.NewContinuousVariables(rows=2, cols=1, name='a')
a_val = np.array([[1.0], [2.0]])
constraint = prog.AddConstraint(eq(a[0], a[1]))
input_array = create_constraint_input_array(constraint, {
"a": a_val
})
expected_input_array = [1.0, 2.0]
np.testing.assert_array_almost_equal(input_array, expected_input_array)
def SOS_traj_optim(S, rho_guess):
# S provides the initial V guess
# STEP 1: search for L and u with fixed V and p
mp1 = MathematicalProgram()
x = mp1.NewIndeterminates(3, "x")
V = x.dot(np.dot(S, x))
print(S)
# Define the Lagrange multipliers.
(lambda_, constraint) = mp1.NewSosPolynomial(Variables(x), 4)
xd = mp1.NewFreePolynomial(Variables(x), 2)
yd = mp1.NewFreePolynomial(Variables(x), 2)
thetd = mp1.NewFreePolynomial(Variables(x), 2)
u = np.vstack((xd, yd))
u = np.vstack((u, thetd))
Vdot = Jacobian([V], x).dot(plant(x, u))[0]
mp1.AddSosConstraint(-Vdot + lambda_.ToExpression() * (V - rho_guess))
result = mp1.Solve()
# print(type(lambda_).__dict__.keys())
print(type(lambda_.decision_variables()).__dict__.keys())
L = [mp1.GetSolution(var) for var in lambda_.decision_variables()]
# print(lambda_.monomial_to_coefficient_map())
return L, u
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1)[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) assert result.is_success() return result.GetSolution(slack)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree):
#import pdb; pdb.set_trace()
prog = MathematicalProgram()
x = prog.NewIndeterminates(len(prev_x),'x')
V = prev_V.Substitute(dict(zip(prev_x, x)))
Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x)))
slack = prog.NewContinuousVariables(1,'a')[0]
Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression()
#print('V degree: %d' %(Polynomial(V).TotalDegree()))
#print('Vdot degree: %d' %(Polynomial(Vdot).TotalDegree()))
#print('SOS degree: %d' %(Polynomial(-Vdot + Lambda*(V - rho) - slack*V).TotalDegree()))
prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V)
prog.AddCost(-slack)
result = Solve(prog)
#assert result.is_success()
return result.GetSolution(slack)
def SOS_compute_4(self, l_coeff, S, rho):
prog = MathematicalProgram()
# fix V and lbda, searcu for u and rho
x = prog.NewIndeterminates(2, "x")
# get lbda from before
l = np.array([x[1], x[0], 1])
lbda = l.dot(np.dot(l_coeff, l))
# Define u
K = prog.NewContinuousVariables(2, "K")
# Fixed Lyapunov
V = x.dot(np.dot(S, x))
Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0]
prog.AddSosConstraint(-Vdot - lbda * (rho - V))
prog.Solve()
K = prog.GetSolution(K)
return rho, K
def compute_trajectory_to_other_world(self, state_initial, minimum_time,
maximum_time):
'''
Your mission is to implement this function.
A successful implementation of this function will compute a dynamically feasible trajectory
which satisfies these criteria:
- Efficiently conserve fuel
- Reach "orbit" of the far right world
- Approximately obey the dynamic constraints
- Begin at the state_initial provided
- Take no more than maximum_time, no less than minimum_time
The above are defined more precisely in the provided notebook.
Please note there are two return args.
:param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation
:param: minimum_time: float, minimum time allowed for trajectory
:param: maximum_time: float, maximum time allowed for trajectory
:return: three return args separated by commas:
trajectory, input_trajectory, time_array
trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation.
input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation.
time_array: an array with N rows.
'''
# length of horizon (excluding init state)
N = 50
trajectory = np.zeros((N + 1, 4))
input_trajectory = np.ones((N, 2)) * 10.0
### My implementation of Direct Transcription
# (states and control are all decision vars using Euler integration)
mp = MathematicalProgram()
# let trajectory duration be a decision var
total_time = mp.NewContinuousVariables(1, "total_time")
dt = total_time[0] / N
# create the control decision var (m*N) and state decision var (n*[N+1])
idx = 0
u_list = mp.NewContinuousVariables(2, "u_{}".format(idx))
state_list = mp.NewContinuousVariables(4, "state_{}".format(idx))
state_list = np.vstack(
(state_list,
mp.NewContinuousVariables(4, "state_{}".format(idx + 1))))
for idx in range(1, N):
u_list = np.vstack(
(u_list, mp.NewContinuousVariables(2, "u_{}".format(idx))))
state_list = np.vstack(
(state_list,
mp.NewContinuousVariables(4, "state_{}".format(idx + 1))))
### Constraints
# set init state as constraint on stage 0 decision vars
for state_idx in range(4):
mp.AddLinearConstraint(
state_list[0, state_idx] == state_initial[state_idx])
# interstage equality constraint on state vars via Euler integration
# note: Direct Collocation could improve accuracy for same computation
for idx in range(1, N + 1):
state_new = state_list[idx - 1, :] + dt * self.rocket_dynamics(
state_list[idx - 1, :], u_list[idx - 1, :])
for state_idx in range(4):
mp.AddConstraint(state_list[idx,
state_idx] == state_new[state_idx])
# constraint on time
mp.AddLinearConstraint(total_time[0] <= maximum_time)
mp.AddLinearConstraint(total_time[0] >= minimum_time)
# constraint on final state distance (squared)to second planet
final_dist_to_world_2_sq = (
self.world_2_position[0] - state_list[-1, 0])**2 + (
self.world_2_position[1] - state_list[-1, 1])**2
mp.AddConstraint(final_dist_to_world_2_sq <= 0.25)
# constraint on final state speed (squared
final_speed_sq = state_list[-1, 2]**2 + state_list[-1, 3]**2
mp.AddConstraint(final_speed_sq <= 1)
### Cost
# equal cost on vertical/horizontal accels, weight shouldn't matter since this is the only cost
mp.AddQuadraticCost(1 * u_list[:, 0].dot(u_list[:, 0]))
mp.AddQuadraticCost(1 * u_list[:, 1].dot(u_list[:, 1]))
### Solve and parse
result = Solve(mp)
trajectory = result.GetSolution(state_list)
input_trajectory = result.GetSolution(u_list)
tf = result.GetSolution(total_time)
time_array = np.linspace(0, tf[0], N + 1)
print "optimization successful: ", result.is_success()
print "total num decision vars (x: (N+1)*4, u: 2N, total_time: 1): {}".format(
mp.num_vars())
print "solver used: ", result.get_solver_id().name()
print "optimal trajectory time: {:.2f} sec".format(tf[0])
return trajectory, input_trajectory, time_array
def trajopt_simulation(x0, xf, u_max=0.5): # numeric parameters time_interval = .1 time_steps = 400 # initialize optimization prog = MathematicalProgram() # optimization variables state = prog.NewContinuousVariables(time_steps + 1, 3, 'state') u = prog.NewContinuousVariables(time_steps, 1, 'u') # final position constraint # for residual in constraint_state_to_orbit(state[-2], state[-1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual == 0) # initial position constraint prog.AddConstraint(eq(state[0],x0)) # discretized dynamics for t in range(time_steps): residuals = dubins_discrete_dynamics(state[t], state[t+1], u[t], time_interval) for residual in residuals: prog.AddConstraint(residual == 0) # control limits prog.AddConstraint(u[t][0] <= u_max) prog.AddConstraint(-u_max <= u[t][0]) # cost - increases cost if off the orbit p = state[t][:2] - xf[:2] v = (state[t+1][:2] - state[t][:2]) / time_interval # prog.AddCost(time_interval) # prog.AddCost(u[t][0]*u[t][0]*time_interval) # prog.AddCost(p.dot(v)*time_interval) for residual in constraint_state_to_orbit(state[t], state[t+1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual >= 0) prog.AddQuadraticCost(residual) # prog.AddCost((p.dot(p))) # # initial guess state_guess = interpolate_dubins_state( np.array([0, 0, np.pi]), xf, time_steps, time_interval, 1/u_max ) prog.SetInitialGuess(state, state_guess) solver = SnoptSolver() result = solver.Solve(prog) assert result.is_success() # retrieve optimal solution u_opt = result.GetSolution(u).T state_opt = result.GetSolution(state).T return state_opt, u_opt
def static_controller(self, qref, verbose=False):
"""
Generates a controller to maintain a static pose
Arguments:
qref: (N,) numpy array, the static pose to be maintained
Return Values:
u: (M,) numpy array, actuations to best achieve static pose
f: (C,) numpy array, associated normal reaction forces
static_controller generates the actuations and reaction forces assuming the velocity and accelerations are zero. Thus, the equation to solve is:
N(q) = B*u + J*f
where N is a vector of gravitational and conservative generalized forces, B is the actuation selection matrix, and J is the contact-Jacobian transpose.
Currently, static_controller only considers the effects of the normal forces. Frictional forces are not yet supported
"""
#TODO: Solve for friction forces as well
# Check inputs
if qref.ndim > 1 or qref.shape[0] != self.multibody.num_positions():
raise ValueError(
f"Reference position mut be ({self.multibody.num_positions(),},) array"
)
# Set the context
context = self.multibody.CreateDefaultContext()
self.multibody.SetPositions(context, qref)
# Get the necessary properties
G = self.multibody.CalcGravityGeneralizedForces(context)
Jn, _ = self.GetContactJacobians(context)
phi = self.GetNormalDistances(context)
B = self.multibody.MakeActuationMatrix()
#Collect terms
A = np.concatenate([B, Jn.transpose()], axis=1)
# Create the mathematical program
prog = MathematicalProgram()
l_var = prog.NewContinuousVariables(self.num_contacts(), name="forces")
u_var = prog.NewContinuousVariables(self.multibody.num_actuators(),
name="controls")
# Ensure dynamics approximately satisfied
prog.AddL2NormCost(A=A,
b=-G,
vars=np.concatenate([u_var, l_var], axis=0))
# Enforce normal complementarity
prog.AddBoundingBoxConstraint(np.zeros(l_var.shape),
np.full(l_var.shape, np.inf), l_var)
prog.AddConstraint(phi.dot(l_var) == 0)
# Solve
result = Solve(prog)
# Check for a solution
if result.is_success():
u = result.GetSolution(u_var)
f = result.GetSolution(l_var)
if verbose:
printProgramReport(result, prog)
return (u, f)
else:
print(f"Optimization failed. Returning zeros")
if verbose:
printProgramReport(result, prog)
return (np.zeros(u_var.shape), np.zeros(l_var.shape))
def test_Feedback_Linearization_controller_BS(x, t):
# Output feedback linearization 2
#
# y1= ||r-rf||^2/2
# y2= wx
# y3= wy
# y4= wz
#
# Analysis: The zero dynamics is unstable.
global g, xf, Aout, Bout, Mout, T_period, w_freq, radius
print("%%%%%%%%%%%%%%%%%%%%%")
print("%%CLF-QP %%%%%%%%%%%")
print("%%%%%%%%%%%%%%%%%%%%%")
print("t:", t)
prog = MathematicalProgram()
u_var = prog.NewContinuousVariables(4, "u_var")
c_var = prog.NewContinuousVariables(1, "c_var")
# # # Example 1 : Circle
x_f = radius * math.cos(w_freq * t)
y_f = radius * math.sin(w_freq * t)
# print("x_f:",x_f)
# print("y_f:",y_f)
dx_f = -radius * math.pow(w_freq, 1) * math.sin(w_freq * t)
dy_f = radius * math.pow(w_freq, 1) * math.cos(w_freq * t)
ddx_f = -radius * math.pow(w_freq, 2) * math.cos(w_freq * t)
ddy_f = -radius * math.pow(w_freq, 2) * math.sin(w_freq * t)
dddx_f = radius * math.pow(w_freq, 3) * math.sin(w_freq * t)
dddy_f = -radius * math.pow(w_freq, 3) * math.cos(w_freq * t)
ddddx_f = radius * math.pow(w_freq, 4) * math.cos(w_freq * t)
ddddy_f = radius * math.pow(w_freq, 4) * math.sin(w_freq * t)
# # Example 2 : Lissajous curve a=1 b=2
# ratio_ab=2;
# a=1;
# b=ratio_ab*a;
# delta_lissajous = 0 #math.pi/2;
# x_f = radius*math.sin(a*w_freq*t+delta_lissajous)
# y_f = radius*math.sin(b*w_freq*t)
# # print("x_f:",x_f)
# # print("y_f:",y_f)
# dx_f = radius*math.pow(a*w_freq,1)*math.cos(a*w_freq*t+delta_lissajous)
# dy_f = radius*math.pow(b*w_freq,1)*math.cos(b*w_freq*t)
# ddx_f = -radius*math.pow(a*w_freq,2)*math.sin(a*w_freq*t+delta_lissajous)
# ddy_f = -radius*math.pow(b*w_freq,2)*math.sin(b*w_freq*t)
# dddx_f = -radius*math.pow(a*w_freq,3)*math.cos(a*w_freq*t+delta_lissajous)
# dddy_f = -radius*math.pow(b*w_freq,3)*math.cos(b*w_freq*t)
# ddddx_f = radius*math.pow(a*w_freq,4)*math.sin(a*w_freq*t+delta_lissajous)
# ddddy_f = radius*math.pow(b*w_freq,4)*math.sin(b*w_freq*t)
e1 = np.array([1, 0, 0]) # e3 elementary vector
e2 = np.array([0, 1, 0]) # e3 elementary vector
e3 = np.array([0, 0, 1]) # e3 elementary vector
# epsilonn= 1e-0
# alpha = 100;
# kp1234=alpha*1/math.pow(epsilonn,4) # gain for y
# kd1=4/math.pow(epsilonn,3) # gain for y^(1)
# kd2=12/math.pow(epsilonn,2) # gain for y^(2)
# kd3=4/math.pow(epsilonn,1) # gain for y^(3)
# kp5= 10; # gain for y5
q = np.zeros(7)
qd = np.zeros(6)
q = x[0:8]
qd = x[8:15]
print("qnorm:", np.dot(q[3:7], q[3:7]))
q0 = q[3]
q1 = q[4]
q2 = q[5]
q3 = q[6]
xi1 = q[7]
v = qd[0:3]
w = qd[3:6]
xi2 = qd[6]
xd = xf[0]
yd = xf[1]
zd = xf[2]
wd = xf[11:14]
# Useful vectors and matrices
(Rq, Eq, wIw, I_inv) = robobee_plantBS.GetManipulatorDynamics(q, qd)
F1q = np.zeros((3, 4))
F1q[0, :] = np.array([q2, q3, q0, q1])
F1q[1, :] = np.array([-1 * q1, -1 * q0, q3, q2])
F1q[2, :] = np.array([q0, -1 * q1, -1 * q2, q3])
Rqe3 = np.dot(Rq, e3)
Rqe3_hat = np.zeros((3, 3))
Rqe3_hat[0, :] = np.array([0, -Rqe3[2], Rqe3[1]])
Rqe3_hat[1, :] = np.array([Rqe3[2], 0, -Rqe3[0]])
Rqe3_hat[2, :] = np.array([-Rqe3[1], Rqe3[0], 0])
Rqe1 = np.dot(Rq, e1)
Rqe1_hat = np.zeros((3, 3))
Rqe1_hat[0, :] = np.array([0, -Rqe1[2], Rqe1[1]])
Rqe1_hat[1, :] = np.array([Rqe1[2], 0, -Rqe1[0]])
Rqe1_hat[2, :] = np.array([-Rqe1[1], Rqe1[0], 0])
Rqe1_x = np.dot(e2.T, Rqe1)
Rqe1_y = np.dot(e1.T, Rqe1)
w_hat = np.zeros((3, 3))
w_hat[0, :] = np.array([0, -w[2], w[1]])
w_hat[1, :] = np.array([w[2], 0, -w[0]])
w_hat[2, :] = np.array([-w[1], w[0], 0])
Rw = np.dot(Rq, w)
Rw_hat = np.zeros((3, 3))
Rw_hat[0, :] = np.array([0, -Rw[2], Rw[1]])
Rw_hat[1, :] = np.array([Rw[2], 0, -Rw[0]])
Rw_hat[2, :] = np.array([-Rw[1], Rw[0], 0])
#- Checking the derivation
# print("F1qEqT-(-Rqe3_hat)",np.dot(F1q,Eq.T)-(-Rqe3_hat))
# Rqe3_cal = np.zeros(3)
# Rqe3_cal[0] = 2*(q3*q1+q0*q2)
# Rqe3_cal[1] = 2*(q3*q2-q0*q1)
# Rqe3_cal[2] = (q0*q0+q3*q3-q1*q1-q2*q2)
# print("Rqe3 - Rqe3_cal", Rqe3-Rqe3_cal)
# Four output
y1 = x[0] - x_f
y2 = x[1] - y_f
y3 = x[2] - zd - 0
y4 = math.atan2(Rqe1_x, Rqe1_y) - math.pi / 8
# print("Rqe1_x:", Rqe1_x)
eta1 = np.zeros(3)
eta1 = np.array([y1, y2, y3])
eta5 = y4
# print("y4", y4)
# First derivative of first three output and yaw output
eta2 = np.zeros(3)
eta2 = v - np.array([dx_f, dy_f, 0])
dy1 = eta2[0]
dy2 = eta2[1]
dy3 = eta2[2]
x2y2 = (math.pow(Rqe1_x, 2) + math.pow(Rqe1_y, 2)) # x^2+y^2
eta6_temp = np.zeros(3) #eta6_temp = (ye2T-xe1T)/(x^2+y^2)
eta6_temp = (Rqe1_y * e2.T - Rqe1_x * e1.T) / x2y2
# print("eta6_temp:", eta6_temp)
# Body frame w ( multiply R)
eta6 = np.dot(eta6_temp, np.dot(-Rqe1_hat, np.dot(Rq, w)))
# World frame w
# eta6 = np.dot(eta6_temp,np.dot(-Rqe1_hat,w))
# print("Rqe1_hat:", Rqe1_hat)
# Second derivative of first three output
eta3 = np.zeros(3)
eta3 = -g * e3 + Rqe3 * xi1 - np.array([ddx_f, ddy_f, 0])
ddy1 = eta3[0]
ddy2 = eta3[1]
ddy3 = eta3[2]
# Third derivative of first three output
eta4 = np.zeros(3)
# Body frame w ( multiply R)
eta4 = Rqe3 * xi2 + np.dot(-Rqe3_hat, np.dot(Rq, w)) * xi1 - np.array(
[dddx_f, dddy_f, 0])
# World frame w
# eta4 = Rqe3*xi2+np.dot(np.dot(F1q,Eq.T),w)*xi1 - np.array([dddx_f,dddy_f,0])
dddy1 = eta4[0]
dddy2 = eta4[1]
dddy3 = eta4[2]
# Fourth derivative of first three output
B_qw_temp = np.zeros(3)
# Body frame w
B_qw_temp = xi1 * (-np.dot(Rw_hat, np.dot(Rqe3_hat, Rw)) +
np.dot(Rqe3_hat, np.dot(Rq, np.dot(I_inv, wIw)))
) # np.dot(I_inv,wIw)*xi1-2*w*xi2
B_qw = B_qw_temp + xi2 * (-2 * np.dot(Rqe3_hat, Rw)) - np.array(
[ddddx_f, ddddy_f, 0]) #np.dot(Rqe3_hat,B_qw_temp)
# World frame w
# B_qw_temp = xi1*(-np.dot(w_hat,np.dot(Rqe3_hat,w))+np.dot(Rqe3_hat,np.dot(I_inv,wIw))) # np.dot(I_inv,wIw)*xi1-2*w*xi2
# B_qw = B_qw_temp+xi2*(-2*np.dot(Rqe3_hat,w)) - np.array([ddddx_f,ddddy_f,0]) #np.dot(Rqe3_hat,B_qw_temp)
# B_qw = B_qw_temp - np.dot(w_hat,np.dot(Rqe3_hat,w))*xi1
# Second derivative of yaw output
# Body frame w
dRqe1_x = np.dot(e2, np.dot(-Rqe1_hat, Rw)) # \dot{x}
dRqe1_y = np.dot(e1, np.dot(-Rqe1_hat, Rw)) # \dot{y}
alpha1 = 2 * (Rqe1_x * dRqe1_x +
Rqe1_y * dRqe1_y) / x2y2 # (2xdx +2ydy)/(x^2+y^2)
# World frame w
# dRqe1_x = np.dot(e2,np.dot(-Rqe1_hat,w)) # \dot{x}
# dRqe1_y = np.dot(e1,np.dot(-Rqe1_hat,w)) # \dot{y}
# alpha1 = 2*(Rqe1_x*dRqe1_x+Rqe1_y*dRqe1_y)/x2y2 # (2xdx +2ydy)/(x^2+y^2)
# # alpha2 = math.pow(dRqe1_y,2)-math.pow(dRqe1_x,2)
# Body frame w
B_yaw_temp3 = np.zeros(3)
B_yaw_temp3 = alpha1 * np.dot(Rqe1_hat, Rw) + np.dot(
Rqe1_hat, np.dot(Rq, np.dot(I_inv, wIw))) - np.dot(
Rw_hat, np.dot(Rqe1_hat, Rw))
B_yaw = np.dot(eta6_temp,
B_yaw_temp3) # +alpha2 :Could be an error in math.
g_yaw = np.zeros(3)
g_yaw = -np.dot(eta6_temp, np.dot(Rqe1_hat, np.dot(Rq, I_inv)))
# World frame w
# B_yaw_temp3 =np.zeros(3)
# B_yaw_temp3 = alpha1*np.dot(Rqe1_hat,w)+np.dot(Rqe1_hat,np.dot(I_inv,wIw))-np.dot(w_hat,np.dot(Rqe1_hat,w))
# B_yaw = np.dot(eta6_temp,B_yaw_temp3) # +alpha2 :Could be an error in math.
# g_yaw = np.zeros(3)
# g_yaw = -np.dot(eta6_temp,np.dot(Rqe1_hat,I_inv))
# print("g_yaw:", g_yaw)
# Decoupling matrix A(x)\in\mathbb{R}^4
A_fl = np.zeros((4, 4))
A_fl[0:3, 0] = Rqe3
# Body frame w
A_fl[0:3, 1:4] = -np.dot(Rqe3_hat, np.dot(Rq, I_inv)) * xi1
# World frame w
# A_fl[0:3,1:4] = -np.dot(Rqe3_hat,I_inv)*xi1
A_fl[3, 1:4] = g_yaw
A_fl_inv = np.linalg.inv(A_fl)
A_fl_det = np.linalg.det(A_fl)
# print("I_inv:", I_inv)
print("A_fl:", A_fl)
print("A_fl_det:", A_fl_det)
# Drift in the output dynamics
U_temp = np.zeros(4)
U_temp[0:3] = B_qw
U_temp[3] = B_yaw
# Output dyamics
eta = np.hstack([eta1, eta2, eta3, eta5, eta4, eta6])
eta_norm = np.dot(eta, eta)
# v-CLF QP controller
FP_PF = np.dot(Aout.T, Mout) + np.dot(Mout, Aout)
PG = np.dot(Mout, Bout)
L_FVx = np.dot(eta, np.dot(FP_PF, eta))
L_GVx = 2 * np.dot(eta.T, PG) # row vector
L_fhx_star = U_temp
Vx = np.dot(eta, np.dot(Mout, eta))
# phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+(min_e_Q/max_e_P)*Vx*1 # exponentially stabilizing
# phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+min_e_Q*eta_norm # more exact bound - exponentially stabilizing
phi0_exp = L_FVx + np.dot(
L_GVx, L_fhx_star) # more exact bound - exponentially stabilizing
phi1_decouple = np.dot(L_GVx, A_fl)
# # Solve QP
v_var = np.dot(A_fl, u_var) + L_fhx_star
Quadratic_Positive_def = np.matmul(A_fl.T, A_fl)
QP_det = np.linalg.det(Quadratic_Positive_def)
c_QP = 2 * np.dot(L_fhx_star.T, A_fl)
c_QP_extned = np.hstack([c_QP, -1])
u_var_extended = np.hstack([u_var, c_var])
# CLF_QP_cost_v = np.dot(v_var,v_var) // Exact quadratic cost
CLF_QP_cost_v_effective = np.dot(
u_var, np.dot(Quadratic_Positive_def, u_var)) + np.dot(
c_QP, u_var) - c_var[0] # Quadratic cost without constant term
# CLF_QP_cost_u = np.dot(u_var,u_var)
phi1 = np.dot(phi1_decouple, u_var) + c_var[0] * eta_norm
#----Printing intermediate states
# print("L_fhx_star: ",L_fhx_star)
# print("c_QP:", c_QP)
# print("Qp : ",Quadratic_Positive_def)
# print("Qp det: ", QP_det)
# print("c_QP", c_QP)
# print("phi0_exp: ", phi0_exp)
# print("PG:", PG)
# print("L_GVx:", L_GVx)
# print("eta6", eta6)
# print("d : ", phi1_decouple)
# print("Cost expression:", CLF_QP_cost_v)
#print("Const expression:", phi0_exp+phi1)
#----Different solver option // Gurobi did not work with python at this point (some binding issue 8/8/2018)
solver = IpoptSolver()
# solver = GurobiSolver()
# print solver.available()
# assert(solver.available()==True)
# assertEqual(solver.solver_type(), mp.SolverType.kGurobi)
# solver.Solve(prog)
# assertEqual(result, mp.SolutionResult.kSolutionFound)
# mp.AddLinearConstraint()
# print("x:", x)
# print("phi_0_exp:", phi0_exp)
# print("phi1_decouple:", phi1_decouple)
# print("eta1:", eta1)
# print("eta2:", eta2)
# print("eta3:", eta3)
# print("eta4:", eta4)
# print("eta5:", eta5)
# print("eta6:", eta6)
# Output Feedback Linearization controller
mu = np.zeros(4)
k = np.zeros((4, 14))
k = np.matmul(Bout.T, Mout)
k = np.matmul(np.linalg.inv(R), k)
mu = -1 / 1 * np.matmul(k, eta)
v = -U_temp + mu
U_fl = np.matmul(
A_fl_inv, v
) # Output Feedback controller to comare the result with CLF-QP solver
# Set up the QP problem
prog.AddQuadraticCost(CLF_QP_cost_v_effective)
prog.AddConstraint(phi0_exp + phi1 <= 0)
prog.AddConstraint(c_var[0] >= 0)
prog.AddConstraint(c_var[0] <= 100)
prog.AddConstraint(u_var[0] <= 30)
prog.SetSolverOption(mp.SolverType.kIpopt, "print_level",
5) # CAUTION: Assuming that solver used Ipopt
print("CLF value:", Vx) # Current CLF value
prog.SetInitialGuess(u_var, U_fl)
prog.Solve() # Solve with default osqp
# solver.Solve(prog)
print("Optimal u : ", prog.GetSolution(u_var))
U_CLF_QP = prog.GetSolution(u_var)
C_CLF_QP = prog.GetSolution(c_var)
#---- Printing for debugging
# dx = robobee_plantBS.evaluate_f(U_fl,x)
# print("dx", dx)
# print("\n######################")
# # print("qe3:", A_fl[0,0])
# print("u:", u)
# print("\n####################33")
# deta4 = B_qw+Rqe3*U_fl_zero[0]+np.matmul(-np.matmul(Rqe3_hat,I_inv),U_fl_zero[1:4])*xi1
# deta6 = B_yaw+np.dot(g_yaw,U_fl_zero[1:4])
# print("deta4:",deta4)
# print("deta6:",deta6)
print(C_CLF_QP)
phi1_opt = np.dot(phi1_decouple, U_CLF_QP) + C_CLF_QP * eta_norm
phi1_opt_FL = np.dot(phi1_decouple, U_fl)
print("FL u: ", U_fl)
print("CLF u:", U_CLF_QP)
print("Cost FL: ", np.dot(mu, mu))
v_CLF = np.dot(A_fl, U_CLF_QP) + L_fhx_star
print("Cost CLF: ", np.dot(v_CLF, v_CLF))
print("Constraint FL : ", phi0_exp + phi1_opt_FL)
print("Constraint CLF : ", phi0_exp + phi1_opt)
u = U_CLF_QP
print("eigenvalues minQ maxP:", [min_e_Q, max_e_P])
return u
print("A:", Aout, "B:", Bout)
Mout = solve_continuous_are(Aout, Bout, Q, R)
# print("Mout",Mout)
# print("M_out:", Mout)
# Minimum and maximum eigenvalues for Q and Mout used for CLF-QP constraint
eval_Q = np.linalg.eigvals(Q)
eval_P = np.linalg.eigvals(Mout)
min_e_Q = np.min(eval_Q)
max_e_P = np.max(eval_P)
print("Evalues: ", [min_e_Q, max_e_P])
# Set up for QP problem
prog = MathematicalProgram()
u_var = prog.NewContinuousVariables(4, "u_var")
c_var = prog.NewContinuousVariables(1, "c_var")
solverid = prog.GetSolverId()
tol = 1e-10
prog.SetSolverOption(mp.SolverType.kIpopt, "tol", tol)
prog.SetSolverOption(mp.SolverType.kIpopt, "constr_viol_tol", tol)
prog.SetSolverOption(mp.SolverType.kIpopt, "acceptable_tol", tol)
prog.SetSolverOption(mp.SolverType.kIpopt, "acceptable_constr_viol_tol", tol)
prog.SetSolverOption(mp.SolverType.kIpopt, "print_level",
5) # CAUTION: Assuming that solver used Ipopt
# #-[2] Linearization and get (K,S) for LQR
In the next cell you need to code the SOS program we just described. We have already set up the problem for you. You only have to write two lines of code: - A line where you add the SOS constraint described in the "Not quite there yet..." subsection above. To do this, use the method `AddSosConstraint` of `MathematicalProgram` (same method we've used in the previous case). - A line where you set the objective function of the SOS program. Remember that we'd like to maximize `rho`. To this end, use the method `AddLinearCost` of `MathematicalProgram`, but notice that writing `prog.AddLinearCost(rho)` the variable `rho` will be *minimized*. Any idea for a quick workaround? Hint: it shouldn't take more than one character! """ # initialize optimization problem prog2 = MathematicalProgram() # SOS indeterminates x = prog2.NewIndeterminates(2, 'x') # Lyapunov function V = x.dot(P).dot(x) V_dot = 2*x.dot(P).dot(f(x)) # degree of the polynomial lambda(x) # no need to change it, but if you really want to, # keep l_deg even and do not set l_deg greater than 10 l_deg = 4 assert l_deg % 2 == 0 # SOS Lagrange multipliers
def findLyapunovFunctionSOS(self, xtraj, utraj, deg_V, deg_L):
times = xtraj.get_segment_times()
prog = MathematicalProgram()
# Declare the "indeterminates", x. These are the variables which define the polynomials
x = prog.NewIndeterminates(3, 'x')
ucon = prog.NewIndeterminates(2, 'u')
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
sym_context.SetContinuousState(x)
sym_context.FixInputPort(0, ucon)
f = sym_system.EvalTimeDerivativesTaylor(
sym_context).CopyToVector() # - dztrajdt.value(t).transpose()
for t in times:
x0 = xtraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
f_fb = self.EvalClosedLoopDynamics(x, ucon, f, x0, u0)
# Construct a polynomial V that contains all monomials with s,c,thetadot up to degree n.
V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression()
eps = 1e-4
constraint1 = prog.AddSosConstraint(
V - eps * (x - x0).dot(x - x0)
) # constraint to enforce that V is strictly positive away from x0.
Vdot = V.Jacobian(x).dot(
f_fb
) # Construct the polynomial which is the time derivative of V
#L = prog.NewFreePolynomial(Variables(x), deg_L).ToExpression() # Construct a polynomial L representing the
# "Lagrange multiplier".
# Add a constraint that Vdot is strictly negative away from x0
constraint2 = prog.AddSosConstraint(-Vdot[0] - eps *
(x - x0).dot(x - x0))
#import pdb; pdb.set_trace()
# Add V(0) = 0 constraint
constraint3 = prog.AddLinearConstraint(
V.Substitute({
x[0]: 0.0,
x[1]: 1.0,
x[2]: 0.0
}) == 1.0)
# Add V(theta=xxx) = 1, just to set the scale.
constraint4 = prog.AddLinearConstraint(
V.Substitute({
x[0]: 1.0,
x[1]: 0.0,
x[2]: 0.0
}) == 1.0)
# Call the solver (default).
#result = Solve(prog)
# Call the solver (specific).
#solver = CsdpSolver()
#solver = ScsSolver()
#solver = IpoptSolver()
#result = solver.Solve(prog, None, None)
result = Solve(prog)
# print out the result.
print("Success? ", result.is_success())
print(result.get_solver_id().name())
Vsol = Polynomial(result.GetSolution(V))
print('Time t=%f:\nV=' % (t))
print(Vsol.RemoveTermsWithSmallCoefficients(1e-6))
return Vsol, Vsol.Jacobian(x).dot(f_fb)
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
mp = MathematicalProgram()
#control variables = u = self.nu x 1
#ddot{q} as variable w/ q's shape
controls = mp.NewContinuousVariables(self.nu, "controls")
qdd = mp.NewContinuousVariables(q.shape[0], "qdd")
#make variables for lambda's and beta's
k = 0
b = mp.NewContinuousVariables(2, "betas_%d" % k)
betas = b
for i in range(len(self.grasp_points)):
b = mp.NewContinuousVariables(
2, "betas_%d" % k) #pair of betas for each contact point
betas = np.vstack((betas, b))
mp.AddLinearConstraint(
betas[i, 0] >= 0).evaluator().set_description(
"beta_0 greater than 0 for %d" % i)
mp.AddLinearConstraint(
betas[i, 1] >= 0).evaluator().set_description(
"beta_1 greater than 0 for %d" % i)
#c_0=normals+mu*tangent
#c1 = normals-mu*tangent
n = grasp_normals_world_now.T[:, 0:2] #row=contact point?
t = get_perpendicular2d(n[0])
c_i1 = n[0] + np.dot(self.mu, t)
c_i2 = n[0] - np.dot(self.mu, t)
l = c_i1.dot(betas[0, 0]) + c_i2.dot(betas[0, 1])
lambdas = l
for i in range(1, len(self.grasp_points)):
n = grasp_normals_world_now.T[:, 0:
2] #row=contact point? transpose then index
t = get_perpendicular2d(n[i])
c_i1 = n[i] - np.dot(self.mu, t)
c_i2 = n[i] + np.dot(self.mu, t)
l = c_i1.dot(betas[i, 0]) + c_i2.dot(betas[i, 1])
lambdas = np.vstack((lambdas, l))
control_period = 0.0333
#Constrain the dyanmics
dyn = M.dot(qdd) + C - B.dot(controls)
for i in range(q.shape[0]):
j_c = 0
for l in range(len(self.grasp_points)):
j_sub = J_contact[:, l, :].T
j_c += j_sub.dot(lambdas[l])
# print(j_c.shape)
# print(dyn.shape)
mp.AddConstraint(dyn[i] - j_c[i] == 0)
#PD controller using kinematics
Kp = 100.0
Kd = 1.0
control_period = 0.0333
next_q = q + v.dot(control_period) + np.dot(qdd.dot(control_period**2),
.5)
next_q_dot = v + qdd.dot(control_period)
mp.AddQuadraticCost(Kp * (qdes - next_q).dot((qdes - next_q).T) + Kd *
(next_q_dot).dot(next_q_dot.T))
Kp_ext = 100.
mp.AddQuadraticCost(Kp_ext * (qdes - next_q)[-3:].dot(
(qdes - next_q)[-3:].T))
res = Solve(mp)
c = res.GetSolution(controls)
return c
def PlanGraspPoints(self):
# First, extract the bounding geometry of the object.
# Generally, our geometry is coming from 3d models, so
# we have to do some legwork to extract 2D geometry. For
# the examples we'll use in this set, we'll assume
# that extracting the convex hull of the first visual element
# is a good representation of the object geometry. (This is
# not great! But it'll do the job for us, since we're going
# to experiment with only simple objects.)
kinsol = self.hand.doKinematics(
self.x_nom[0:self.hand.get_num_positions()])
self.manipuland_link_index = \
self.hand.FindBody(self.manipuland_link_name).get_body_index()
body = self.hand.get_body(self.manipuland_link_index)
# For projecting into XY plane
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0.,
1.]])
all_points = ExtractPlanarObjectGeometryConvexHull(body, Tview)
# For later use: precompute the fingertip positions of the
# robot in the nominal posture.
nominal_fingertip_points = np.empty((2, self.num_fingers))
for i in range(self.num_fingers):
nominal_fingertip_points[:, i] = self.hand.transformPoints(
kinsol, self.fingertip_position, self.finger_link_indices[i],
0)[0:2, 0]
# Search for an optimal grasp with N points
random.seed(42)
np.random.seed(42)
def get_random_point_and_normal_on_surface(all_points):
num_points = all_points.shape[1]
i = random.choice(range(num_points - 1))
first_point = np.asarray([all_points[0][i], all_points[1][i]])
second_point = np.asarray(
[all_points[0][i + 1], all_points[1][i + 1]])
first_to_second = second_point - first_point
# Clip to avoid interpolating close to a corner
interpolate_param = np.clip(np.random.rand(), 0.2, 0.8)
rand_point = first_point + interpolate_param * first_to_second
normal = np.array([-first_to_second[1], first_to_second[0]]) \
/ np.linalg.norm(first_to_second)
return rand_point, normal
best_conv_volume = 0
best_points = []
best_normals = []
best_finger_assignments = []
for i in range(self.n_grasp_search_iters):
grasp_points = []
normals = []
for j in range(self.num_fingers):
point, normal = \
get_random_point_and_normal_on_surface(all_points)
# check for duplicates or close points -- fingertip
# radius is 0.2, so require twice that margin to avoid
# intersection fingertips.
num_rejected = 0
while min([1.0] + [
np.linalg.norm(grasp_point - point, 2)
for grasp_point in grasp_points
]) <= 0.4:
point, normal = \
get_random_point_and_normal_on_surface(all_points)
num_rejected += 1
if num_rejected > 10000:
print "Rejected 10000 points in a row due to crowding." \
" Your object is a bit too small for your number of" \
" fingers."
break
grasp_points.append(point)
normals.append(normal)
if achieves_force_closure(grasp_points, normals, self.mu):
# Test #1: Rank in terms of convex hull volume of grasp points
volume = compute_convex_hull_volume(grasp_points)
if volume < best_conv_volume:
continue
# Test #2: Does IK work for this point?
self.grasp_points = grasp_points
self.grasp_normals = normals
# Pick optimal finger assignment that
# minimizes distance between fingertips in the
# nominal posture, and the chosen grasp points
grasp_points_world = self.transform_grasp_points_manipuland(
self.x_nom)[0:2, :]
prog = MathematicalProgram()
# We'd *really* like these to be binary variables,
# but unfortunately don't have a MIP solver available in the
# course docker container. Instead, we'll solve an LP,
# and round the result to the nearest feasible integer
# solutions. Intuitively, this LP should probably reach its
# optimal value at an extreme point (where the variables
# all take integer value). I've not yet observed this
# not occuring in practice!
assignment_vars = prog.NewContinuousVariables(
self.num_fingers, self.num_fingers, "assignment")
for grasp_i in range(self.num_fingers):
# Every row and column of assignment vars sum to one --
# each finger matches to one grasp position
prog.AddLinearConstraint(
np.sum(assignment_vars[:, grasp_i]) == 1.)
prog.AddLinearConstraint(
np.sum(assignment_vars[grasp_i, :]) == 1.)
for finger_i in range(self.num_fingers):
# If this grasp assignment is active,
# add a cost equal to the distance between
# nominal pose and grasp position
prog.AddLinearCost(
assignment_vars[grasp_i, finger_i] *
np.linalg.norm(grasp_points_world[:, grasp_i] -
nominal_fingertip_points[:,
finger_i]))
prog.AddBoundingBoxConstraint(
0., 1., assignment_vars[grasp_i, finger_i])
result = Solve(prog)
assignments = result.GetSolution(assignment_vars)
# Round assignments to nearest feasible set
claimed = [False] * self.num_fingers
for grasp_i in range(self.num_fingers):
order = np.argsort(assignments[grasp_i, :])
fill_i = self.num_fingers - 1
while claimed[order[fill_i]] is not False:
fill_i -= 1
if fill_i < 0:
raise Exception("Finger association failed. "
"Horrible bug. Tell Greg")
assignments[grasp_i, :] *= 0.
assignments[grasp_i, order[fill_i]] = 1.
claimed[order[fill_i]] = True
# Populate actual assignments
self.grasp_finger_assignments = []
for grasp_i in range(self.num_fingers):
for finger_i in range(self.num_fingers):
if assignments[grasp_i, finger_i] == 1.:
self.grasp_finger_assignments.append(
(finger_i, np.array(self.fingertip_position)))
continue
qinit, info = self.ComputeTargetPosture(
self.x_nom,
self.x_nom[(self.nq - 3):self.nq],
backoff_distance=0.2)
if info != 1:
continue
best_conv_volume = volume
best_points = grasp_points
best_normals = normals
best_finger_assignments = self.grasp_finger_assignments
if len(best_points) == 0:
print "After %d attempts, couldn't find a good grasp "\
"for this object." % self.n_grasp_search_iters
print "Proceeding with a horrible random guess."
best_points = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_normals = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_finger_assignments = [(i, self.fingertip_position)
for i in range(self.num_fingers)]
self.grasp_points = best_points
self.grasp_normals = best_normals
self.grasp_finger_assignments = best_finger_assignments
