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