def compute_input(self, x, xd, initial_guess=None):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda1')
z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
prog.AddConstraint(z[0, 0] == 0)
prog.AddConstraint(z[0, 1] == 0)
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints with big M = self.contact
prog.AddConstraint(ge(contact[t], 0))
prog.AddConstraint(contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0)
prog.AddConstraint(contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0)
# Mixed Integer Constraints
M = self.contact_max
prog.AddConstraint(contact[t, 0] <= M)
prog.AddConstraint(contact[t, 1] <= M)
prog.AddConstraint(contact[t, 0] <= M * z[t, 0])
prog.AddConstraint(contact[t, 1] <= M * z[t, 1])
prog.AddConstraint(
contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) <= M *
(1 - z[t, 0]))
prog.AddConstraint(
contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) <= M *
(1 - z[t, 1]))
prog.AddConstraint(z[t, 0] + z[t, 1] == 1)
# Add Input Constraints. Contact Constraints already enforced in big-M
# prog.AddConstraint(le(u[t], self.input_max))
# prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver_id = IpoptSolver().solver_id()
elif (self.solver == "snopt"):
solver_id = SnoptSolver().solver_id()
elif (self.solver == "osqp"):
solver_id = OsqpSolver().solver_id()
elif (self.solver == "mosek"):
solver_id = MosekSolver().solver_id()
elif (self.solver == "gurobi"):
solver_id = GurobiSolver().solver_id()
solver = MixedIntegerBranchAndBound(prog, solver_id)
#result = solver.Solve(prog, initial_guess)
result = solver.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem')
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
def compute_input(self, x, xd, initial_guess=None, tol=0.0):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda')
#
alpha = prog.NewContinuousVariables(rows=self.T, cols=2, name='alpha')
beta = prog.NewContinuousVariables(rows=self.T, cols=2, name='beta')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints
prog.AddConstraint(ge(alpha[t], 0))
prog.AddConstraint(ge(beta[t], 0))
prog.AddConstraint(alpha[t, 0] == contact[t, 0])
prog.AddConstraint(alpha[t, 1] == contact[t, 1])
prog.AddConstraint(
beta[t, 0] == (contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d'])))
prog.AddConstraint(
beta[t, 1] == (contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d'])))
# Complementarity constraints. Start with relaxed version and start constraining.
prog.AddConstraint(alpha[t, 0] * beta[t, 0] <= tol)
prog.AddConstraint(alpha[t, 1] * beta[t, 1] <= tol)
# Add Input Constraints and Contact Constraints
prog.AddConstraint(le(contact[t], self.contact_max))
prog.AddConstraint(ge(contact[t], -self.contact_max))
prog.AddConstraint(le(u[t], self.input_max))
prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver = IpoptSolver()
elif (self.solver == "snopt"):
solver = SnoptSolver()
result = solver.Solve(prog, initial_guess)
if (self.solver == "ipopt"):
print("Ipopt Solver Status: ",
result.get_solver_details().status, ", meaning ",
result.get_solver_details().ConvertStatusToString())
elif (self.solver == "snopt"):
val = result.get_solver_details().info
status = self.snopt_status(val)
print("Snopt Solver Status: ",
result.get_solver_details().info, ", meaning ", status)
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt