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 mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, **kwargs): """ Solves the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. The first nc variables in x are continuous, the remaining are binaries. Arguments ---------- nc : int Number of continuous variables in the problem. H : numpy.ndarray Positive definite Hessian of the cost function. f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. Returns ---------- sol : dict Dictionary with the solution of the MIQP. Fields ---------- min : float Minimum of the MIQP (None if the problem is unfeasible). argmin : numpy.ndarray Argument that minimizes the MIQP (None if the problem is unfeasible). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # build program prog = MathematicalProgram() x = np.hstack(( prog.NewContinuousVariables(nc), prog.NewBinaryVariables(n_x - nc) )) [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x) sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin']) return sol
def make_multiple_dircol_trajectories(num_trajectories, num_samples, initial_conditions=None): from pydrake.all import ( AutoDiffXd, Expression, Variable, MathematicalProgram, SolverType, SolutionResult, DirectCollocationConstraint, AddDirectCollocationConstraint, PiecewisePolynomial, ) import pydrake.symbolic as sym from pydrake.examples.pendulum import (PendulumPlant) # initial_conditions maps (ti) -> [1xnum_states] initial state if initial_conditions is not None: initial_conditions = intial_cond_dict[initial_conditions] assert callable(initial_conditions) plant = PendulumPlant() context = plant.CreateDefaultContext() dircol_constraint = DirectCollocationConstraint(plant, context) # num_trajectories = 15; # num_samples = 15; prog = MathematicalProgram() # K = prog.NewContinuousVariables(1,7,'K') def cos(x): if isinstance(x, AutoDiffXd): return x.cos() elif isinstance(x, Variable): return sym.cos(x) return math.cos(x) def sin(x): if isinstance(x, AutoDiffXd): return x.sin() elif isinstance(x, Variable): return sym.sin(x) return math.sin(x) def final_cost(x): return 100. * (cos(.5 * x[0])**2 + x[1]**2) h = [] u = [] x = [] xf = (math.pi, 0.) for ti in range(num_trajectories): h.append(prog.NewContinuousVariables(1)) prog.AddBoundingBoxConstraint(.01, .2, h[ti]) # prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps u.append(prog.NewContinuousVariables(1, num_samples, 'u' + str(ti))) x.append(prog.NewContinuousVariables(2, num_samples, 'x' + str(ti))) # Use Russ's initial conditions, unless I pass in a function myself. if initial_conditions is None: x0 = (.8 + math.pi - .4 * ti, 0.0) else: x0 = initial_conditions(ti) assert len(x0) == 2 #TODO: undo this hardcoding. prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0]) nudge = np.array([.2, .2]) prog.AddBoundingBoxConstraint(xf - nudge, xf + nudge, x[ti][:, -1]) # prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1]) for i in range(num_samples - 1): AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:, i], x[ti][:, i + 1], u[ti][:, i], u[ti][:, i + 1], prog) for i in range(num_samples): prog.AddQuadraticCost([1.], [0.], u[ti][:, i]) # prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()])) # prog.AddBoundingBoxConstraint([-3.], [3.], u[ti][:,i]) # prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x)) # prog.AddCost(final_cost, x[ti][:,-1]) # prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs? #prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation) prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out") # result = prog.Solve() # print(result) # print(prog.GetSolution(K)) # print(prog.GetSolution(K).dot(control_basis(x[0][:,0]))) #if (result != SolutionResult.kSolutionFound): # f = open('/tmp/snopt.out', 'r') # print(f.read()) # f.close() return prog, h, u, x
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 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 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) number_of_grasp_points = 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) # print('shape' + str(np.shape(qdes))) # print('self.nq' + str(self.nq)) # print('self.nu' + str(self.nu)) 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. kd = 1 kp = 25 from pydrake.all import MathematicalProgram, Solve mp = MathematicalProgram() u = mp.NewContinuousVariables(self.nu, "u") qdd = mp.NewContinuousVariables(self.nq, "qdd") x_y_dimensions = 2 lambda_variable = mp.NewContinuousVariables(x_y_dimensions, number_of_grasp_points, "lambda_variable") forces = np.zeros((self.nq)) for i in range(number_of_grasp_points): current_forces = np.transpose(J_contact)[:,i,:].dot(lambda_variable[:,i]) forces = forces + current_forces leftHandSide = M.dot(qdd) + C rightHandSide = B.dot(u) + forces for i in range(len(leftHandSide)): mp.AddConstraint(leftHandSide[i] == rightHandSide[i]) # Calculate Normals normals = np.array([]) for i in range(number_of_grasp_points): current_grasp_normal = grasp_normals_world_now[0:2, i] normals = np.vstack((normals, current_grasp_normal)) if normals.size else current_grasp_normal # Calculate Tangeants tangeants = np.array([]) for i in range(number_of_grasp_points): current_grasp_tangeant = np.array([normals[i, 1], -normals[i, 0]]) tangeants = np.vstack((tangeants, current_grasp_tangeant)) if tangeants.size else current_grasp_tangeant # Create beta variables beta = mp.NewContinuousVariables(2, number_of_grasp_points, "b0") # Create Grasps for i in range(number_of_grasp_points): c0 = normals[i] - self.mu * tangeants[i] c1 = normals[i] + self.mu * tangeants[i] right_hand_lambda1 = c0 * beta[0, i] + c1 * beta[1, i] mp.AddConstraint(lambda_variable[0, i] == right_hand_lambda1[0]) mp.AddConstraint(lambda_variable[1, i] == right_hand_lambda1[1]) mp.AddConstraint(beta[0, i] >= 0) mp.AddConstraint(beta[1, i] >= 0) mp.AddConstraint(normals[i].dot(lambda_variable[:, i]) >= 0.25) # Copying the control period of the constructor. Probably not supposed to do this... next_tick_qd = v + qdd * self.cont¨rol_period next_tick_q = q + next_tick_qd * self.control_period q_error = qdes - next_tick_q proportionalCost = q_error.dot(np.transpose(q_error)) qd_error = 0 - next_tick_qd diffCost = qd_error.dot(np.transpose(qd_error)) mp.AddQuadraticCost(kp * proportionalCost + kd * diffCost) result = Solve(mp) u_solution = result.GetSolution(u) u = np.zeros(self.nu) return u_solution
class Optimization: def __init__(self, config): self.physical_params = config["physical_params"] self.T = config["T"] self.dt = config["dt"] self.initial_state = config["xinit"] self.goal_state = config["xgoal"] self.ellipse_arc = config["ellipse_arc"] self.deviation_cost = config["deviation_cost"] self.Qf = config["Qf"] self.limits = config["limits"] self.n_state = 6 self.n_nominal_forces = 4 self.tire_model = config["tire_model"] self.initial_guess_config = config["initial_guess_config"] self.puddle_model = config["puddle_model"] self.force_constraint = config["force_constraint"] self.visualize_initial_guess = config["visualize_initial_guess"] self.dynamics = Dynamics(self.physical_params.lf, self.physical_params.lr, self.physical_params.m, self.physical_params.Iz, self.dt) def build_program(self): self.prog = MathematicalProgram() # Declare variables. state = self.prog.NewContinuousVariables(rows=self.T + 1, cols=self.n_state, name='state') nominal_forces = self.prog.NewContinuousVariables( rows=self.T, cols=self.n_nominal_forces, name='nominal_forces') steers = self.prog.NewContinuousVariables(rows=self.T, name="steers") slip_ratios = self.prog.NewContinuousVariables(rows=self.T, cols=2, name="slip_ratios") self.state = state self.nominal_forces = nominal_forces self.steers = steers self.slip_ratios = slip_ratios # Set the initial state. xinit = pack_state_vector(self.initial_state) for i, s in enumerate(xinit): self.prog.AddConstraint(state[0, i] == s) # Constrain xdot to always be at least some value to prevent numerical issues with optimizer. for i in range(self.T + 1): s = unpack_state_vector(state[i]) self.prog.AddConstraint(s["xdot"] >= self.limits.min_xdot) # Constrain slip ratio to be at least a certain magnitude. if i != self.T: self.prog.AddConstraint( slip_ratios[i, 0]**2.0 >= self.limits.min_slip_ratio_mag**2.0) self.prog.AddConstraint( slip_ratios[i, 1]**2.0 >= self.limits.min_slip_ratio_mag**2.0) # Control rate limits. for i in range(self.T - 1): ddelta = self.dt * (steers[i + 1] - steers[i]) f_dkappa = self.dt * (slip_ratios[i + 1, 0] - slip_ratios[i, 0]) r_dkappa = self.dt * (slip_ratios[i + 1, 1] - slip_ratios[i, 1]) self.prog.AddConstraint(ddelta <= self.limits.max_ddelta) self.prog.AddConstraint(ddelta >= -self.limits.max_ddelta) self.prog.AddConstraint(f_dkappa <= self.limits.max_dkappa) self.prog.AddConstraint(f_dkappa >= -self.limits.max_dkappa) self.prog.AddConstraint(r_dkappa <= self.limits.max_dkappa) self.prog.AddConstraint(r_dkappa >= -self.limits.max_dkappa) # Control value limits. for i in range(self.T): self.prog.AddConstraint(steers[i] <= self.limits.max_delta) self.prog.AddConstraint(steers[i] >= -self.limits.max_delta) # Add dynamics constraints by constraining residuals to be zero. for i in range(self.T): residuals = self.dynamics.nominal_dynamics(state[i], state[i + 1], nominal_forces[i], steers[i]) for r in residuals: self.prog.AddConstraint(r == 0.0) # Add the cost function. self.add_cost(state) # Add a different force constraint depending on the configuration. if self.force_constraint == ForceConstraint.NO_PUDDLE: self.add_no_puddle_force_constraints( state, nominal_forces, steers, self.tire_model.get_deterministic_model(), slip_ratios) elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED: self.add_mean_constrained() elif self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED: self.add_chance_constraints() else: raise NotImplementedError("ForceConstraint type invalid.") return def constant_input_initial_guess(self, state, steers, slip_ratios, nominal_forces): # Guess the slip ratio as the minimum allowed value. gslip_ratios = np.tile( np.array([ self.limits.min_slip_ratio_mag, self.limits.min_slip_ratio_mag ]), (self.T, 1)) # Guess the slip angle as a linearly ramping steer that then becomes constant. # This is done by creating an array of values corresponding to end_delta then # filling in the initial ramp up phase. gsteers = self.initial_guess_config["end_delta"] * np.ones(self.T) igc = self.initial_guess_config for i in range(igc["ramp_steps"]): gsteers[i] = (i / igc["ramp_steps"]) * (igc["end_delta"] - igc["start_delta"]) # Create empty arrays for state and forces. gstate = np.empty((self.T + 1, self.n_state)) gstate[0] = pack_state_vector(self.initial_state) gforces = np.empty((self.T, 4)) all_slips = self.T * [None] # Simulate the dynamics for the initial guess differently depending on the force # constraint being used. if self.force_constraint == ForceConstraint.NO_PUDDLE: tire_model = self.tire_model.get_deterministic_model() for i in range(self.T): s = unpack_state_vector(gstate[i]) # Simulate the dynamics for one time step. gstate[i + 1], forces, slips = self.dynamics.simulate( gstate[i], gsteers[i], gslip_ratios[i, 0], gslip_ratios[i, 1], tire_model, self.dt) # Store the results. gforces[i] = pack_force_vector(forces) all_slips[i] = slips elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED or self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED: # mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy]) mean_model = self.tire_model.get_mean_model( self.puddle_model.get_mean_fun()) for i in range(self.T): # Update the tire model based off the conditions of the world # at (x, y) s = unpack_state_vector(gstate[i]) tire_model = lambda slip_ratio, slip_angle: mean_model( slip_ratio, slip_angle, s["X"], s["Y"]) # Simulate the dynamics for one time step. gstate[i + 1], forces, slips = self.dynamics.simulate( gstate[i], gsteers[i], gslip_ratios[i, 0], gslip_ratios[i, 1], tire_model, self.dt) # Store the results. gforces[i] = pack_force_vector(forces) all_slips[i] = slips else: raise NotImplementedError( "Invalid value for self.force_constraint") # Declare array for the initial guess and set the values. initial_guess = np.empty(self.prog.num_vars()) self.prog.SetDecisionVariableValueInVector(state, gstate, initial_guess) self.prog.SetDecisionVariableValueInVector(steers, gsteers, initial_guess) self.prog.SetDecisionVariableValueInVector(slip_ratios, gslip_ratios, initial_guess) self.prog.SetDecisionVariableValueInVector(nominal_forces, gforces, initial_guess) if self.visualize_initial_guess: # TODO: reorganize visualizations psis = gstate[:, 2] xs = gstate[:, 4] ys = gstate[:, 5] fig, ax = plt.subplots() if self.force_constraint != ForceConstraint.NO_PUDDLE: plot_puddles(ax, self.puddle_model) plot_ellipse_arc(ax, self.ellipse_arc) plot_planned_trajectory(ax, xs, ys, psis, gsteers, self.physical_params) # Plot the slip ratios/angles plot_slips(all_slips) plot_forces(gforces) if self.force_constraint == ForceConstraint.NO_PUDDLE: generate_animation(xs, ys, psis, gsteers, self.physical_params, self.dt, puddle_model=None) else: generate_animation(xs, ys, psis, gsteers, self.physical_params, self.dt, puddle_model=self.puddle_model) return initial_guess def solve_program(self): # Generate initial guess initial_guess = self.constant_input_initial_guess( self.state, self.steers, self.slip_ratios, self.nominal_forces) # Solve the problem. solver = SnoptSolver() result = solver.Solve(self.prog, initial_guess) solver_details = result.get_solver_details() print("Exit flag: " + str(solver_details.info)) self.visualize_results(result) def visualize_results(self, result): state_res = result.GetSolution(self.state) psis = state_res[:, 2] xs = state_res[:, 4] ys = state_res[:, 5] steers = result.GetSolution(self.steers) fig, ax = plt.subplots() plot_ellipse_arc(ax, self.ellipse_arc) plot_puddles(ax, self.puddle_model) plot_planned_trajectory(ax, xs, ys, psis, steers, self.physical_params) generate_animation(xs, ys, psis, steers, self.physical_params, self.dt, puddle_model=self.puddle_model) plt.show() def add_cost(self, state): # Add the final state cost function. diff_state = pack_state_vector(self.goal_state) - state[-1] self.prog.AddQuadraticCost(diff_state.T @ self.Qf @ diff_state) # Get the approx distance function for the ellipse. fun = self.ellipse_arc.approx_dist_fun() for i in range(self.T): s = unpack_state_vector(state[i]) self.prog.AddCost(self.deviation_cost * fun(s["X"], s["Y"])) def add_mean_constrained(self): # mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy]) mean_model = self.tire_model.get_mean_model( self.puddle_model.get_mean_fun()) for i in range(self.T): # Get slip angles and slip ratios. s = unpack_state_vector(self.state[i]) F = unpack_force_vector(self.nominal_forces[i]) # get the tire model at this position in space. tire_model = lambda slip_ratio, slip_angle: mean_model( slip_ratio, slip_angle, s["X"], s["Y"]) # Unpack values delta = self.steers[i] alpha_f, alpha_r = self.dynamics.slip_angles( s["xdot"], s["ydot"], s["psidot"], delta) kappa_f = self.slip_ratios[i, 0] kappa_r = self.slip_ratios[i, 1] # Compute expected forces. E_Ffx, E_Ffy = tire_model(kappa_f, alpha_f) E_Frx, E_Fry = tire_model(kappa_r, alpha_r) # Constrain these expected force values to be equal to the nominal # forces in the optimization. self.prog.AddConstraint(E_Ffx - F["f_long"] == 0.0) self.prog.AddConstraint(E_Ffy - F["f_lat"] == 0.0) self.prog.AddConstraint(E_Frx - F["r_long"] == 0.0) self.prog.AddConstraint(E_Fry - F["r_lat"] == 0.0) def add_chance_constrained(self): pass def add_no_puddle_force_constraints(self, state, forces, steers, pacejka_model, slip_ratios): """ Args: prog: state: forces: steers: pacejka_model: function with signature (slip_ratio, slip_angle) using pydrake.math """ for i in range(self.T): # Get slip angles and slip ratios. s = unpack_state_vector(state[i]) F = unpack_force_vector(forces[i]) delta = steers[i] alpha_f, alpha_r = self.dynamics.slip_angles( s["xdot"], s["ydot"], s["psidot"], delta) kappa_f = slip_ratios[i, 0] kappa_r = slip_ratios[i, 1] Ffx, Ffy = pacejka_model(kappa_f, alpha_f) Frx, Fry = pacejka_model(kappa_r, alpha_r) # Constrain the values from the pacejka model to be equal # to the desired values in the optimization. self.prog.AddConstraint(Ffx - F["f_long"] == 0.0) self.prog.AddConstraint(Ffy - F["f_lat"] == 0.0) self.prog.AddConstraint(Frx - F["r_long"] == 0.0) self.prog.AddConstraint(Fry - F["r_lat"] == 0.0)
def compute_opt_trajectory(self, state_initial, goal_func, verbose=True): ''' nonlinear trajectory optimization to land drone starting at state_initial, on a vehicle target trajectory given by the goal_func :return: three return args separated by commas: trajectory, input_trajectory, time_array trajectory: a 2d array with N rows, and 6 columns. See simulate_states_over_time for more documentation. input_trajectory: a 2d array with N-1 row, and 4 columns. See simulate_states_over_time for more documentation. time_array: an array with N rows. ''' # initialize math program import time start_time = time.time() mp = MathematicalProgram() num_time_steps = 40 booster = mp.NewContinuousVariables(3, "booster_0") booster_over_time = booster[np.newaxis, :] state = mp.NewContinuousVariables(6, "state_0") state_over_time = state[np.newaxis, :] dt = mp.NewContinuousVariables(1, "dt") for k in range(1, num_time_steps - 1): booster = mp.NewContinuousVariables(3, "booster_%d" % k) booster_over_time = np.vstack((booster_over_time, booster)) for k in range(1, num_time_steps): state = mp.NewContinuousVariables(6, "state_%d" % k) state_over_time = np.vstack((state_over_time, state)) goal_state = goal_func(dt[0] * 39) # calculate states over time for i in range(1, num_time_steps): sim_next_state = state_over_time[ i - 1, :] + dt[0] * self.drone_dynamics( state_over_time[i - 1, :], booster_over_time[i - 1, :]) # add constraints to restrict the next state to the decision variables for j in range(6): mp.AddConstraint(sim_next_state[j] == state_over_time[i, j]) # don't hit ground mp.AddLinearConstraint(state_over_time[i, 2] >= 0.05) # enforce that we must be thrusting within some constraints mp.AddLinearConstraint(booster_over_time[i - 1, 0] <= 5.0) mp.AddLinearConstraint(booster_over_time[i - 1, 0] >= -5.0) mp.AddLinearConstraint(booster_over_time[i - 1, 1] <= 5.0) mp.AddLinearConstraint(booster_over_time[i - 1, 1] >= -5.0) # keep forces in a reasonable position mp.AddLinearConstraint(booster_over_time[i - 1, 2] >= 1.0) mp.AddLinearConstraint( booster_over_time[i - 1, 0] <= booster_over_time[i - 1, 2]) mp.AddLinearConstraint( booster_over_time[i - 1, 0] >= -booster_over_time[i - 1, 2]) mp.AddLinearConstraint( booster_over_time[i - 1, 1] <= booster_over_time[i - 1, 2]) mp.AddLinearConstraint( booster_over_time[i - 1, 1] >= -booster_over_time[i - 1, 2]) # add constraints on initial state for i in range(6): mp.AddLinearConstraint(state_over_time[0, i] == state_initial[i]) # add constraints on dt mp.AddLinearConstraint(dt[0] >= 0.001) # add constraints on end state # end goal velocity mp.AddConstraint(state_over_time[-1, 0] <= goal_state[0] + 0.01) mp.AddConstraint(state_over_time[-1, 0] >= goal_state[0] - 0.01) mp.AddConstraint(state_over_time[-1, 1] <= goal_state[1] + 0.01) mp.AddConstraint(state_over_time[-1, 1] >= goal_state[1] - 0.01) mp.AddConstraint(state_over_time[-1, 2] <= goal_state[2] + 0.01) mp.AddConstraint(state_over_time[-1, 2] >= goal_state[2] - 0.01) mp.AddConstraint(state_over_time[-1, 3] <= goal_state[3] + 0.01) mp.AddConstraint(state_over_time[-1, 3] >= goal_state[3] - 0.01) mp.AddConstraint(state_over_time[-1, 4] <= goal_state[4] + 0.01) mp.AddConstraint(state_over_time[-1, 4] >= goal_state[4] - 0.01) mp.AddConstraint(state_over_time[-1, 5] <= goal_state[5] + 0.01) mp.AddConstraint(state_over_time[-1, 5] >= goal_state[5] - 0.01) # add the cost function mp.AddQuadraticCost( 0.01 * booster_over_time[:, 0].dot(booster_over_time[:, 0])) mp.AddQuadraticCost( 0.01 * booster_over_time[:, 1].dot(booster_over_time[:, 1])) mp.AddQuadraticCost( 0.01 * booster_over_time[:, 2].dot(booster_over_time[:, 2])) # add more penalty on dt because minimizing time turns out to be more important mp.AddQuadraticCost(10000 * dt[0] * dt[0]) solved = mp.Solve() if verbose: print solved # extract booster_over_time = mp.GetSolution(booster_over_time) output_states = mp.GetSolution(state_over_time) dt = mp.GetSolution(dt) time_array = np.zeros(40) for i in range(40): time_array[i] = i * dt trajectory = self.simulate_states_over_time(state_initial, time_array, booster_over_time) durations = time_array[1:len(time_array )] - time_array[0:len(time_array) - 1] fuel_consumption = ( np.sum(booster_over_time[:len(time_array)]**2, axis=1) * durations).sum() if verbose: print 'expected remaining fuel consumption', fuel_consumption print("took %s seconds" % (time.time() - start_time)) print '' return trajectory, booster_over_time, time_array, fuel_consumption
def compute_trajectory(self, state_initial, goal_state, flight_time, exact=False, verbose=True): ''' nonlinear trajectory optimization to land drone starting at state_initial, to a goal_state, in a specific flight_time :return: three return args separated by commas: trajectory, input_trajectory, time_array trajectory: a 2d array with N rows, and 6 columns. See simulate_states_over_time for more documentation. input_trajectory: a 2d array with N-1 row, and 4 columns. See simulate_states_over_time for more documentation. time_array: an array with N rows. ''' # initialize math program import time start_time = time.time() mp = MathematicalProgram() num_time_steps = int(min(40, flight_time / 0.05)) dt = flight_time / num_time_steps time_array = np.arange(0.0, flight_time - 0.00001, dt) # hacky way to ensure it goes down num_time_steps = len(time_array) # to ensure these are equal lenghts flight_time = dt * num_time_steps if verbose: print '' print 'solving problem with no guess' print 'initial state', state_initial print 'goal state', goal_state print 'flight time', flight_time print 'num time steps', num_time_steps print 'exact traj', exact print 'dt', dt booster = mp.NewContinuousVariables(3, "booster_0") booster_over_time = booster[np.newaxis, :] state = mp.NewContinuousVariables(6, "state_0") state_over_time = state[np.newaxis, :] for k in range(1, num_time_steps - 1): booster = mp.NewContinuousVariables(3, "booster_%d" % k) booster_over_time = np.vstack((booster_over_time, booster)) for k in range(1, num_time_steps): state = mp.NewContinuousVariables(6, "state_%d" % k) state_over_time = np.vstack((state_over_time, state)) # calculate states over time for i in range(1, len(time_array)): time_step = time_array[i] - time_array[i - 1] sim_next_state = state_over_time[ i - 1, :] + time_step * self.drone_dynamics( state_over_time[i - 1, :], booster_over_time[i - 1, :]) # add constraints to restrict the next state to the decision variables for j in range(6): mp.AddLinearConstraint(sim_next_state[j] == state_over_time[i, j]) # don't hit ground mp.AddLinearConstraint(state_over_time[i, 2] >= 0.05) # enforce that we must be thrusting within some constraints mp.AddLinearConstraint(booster_over_time[i - 1, 0] <= 5.0) mp.AddLinearConstraint(booster_over_time[i - 1, 0] >= -5.0) mp.AddLinearConstraint(booster_over_time[i - 1, 1] <= 5.0) mp.AddLinearConstraint(booster_over_time[i - 1, 1] >= -5.0) # keep forces in a reasonable position mp.AddLinearConstraint(booster_over_time[i - 1, 2] >= 1.0) mp.AddLinearConstraint(booster_over_time[i - 1, 2] <= 15.0) mp.AddLinearConstraint( booster_over_time[i - 1, 0] <= booster_over_time[i - 1, 2]) mp.AddLinearConstraint( booster_over_time[i - 1, 0] >= -booster_over_time[i - 1, 2]) mp.AddLinearConstraint( booster_over_time[i - 1, 1] <= booster_over_time[i - 1, 2]) mp.AddLinearConstraint( booster_over_time[i - 1, 1] >= -booster_over_time[i - 1, 2]) # add constraints on initial state for i in range(6): mp.AddLinearConstraint(state_over_time[0, i] == state_initial[i]) # add constraints on end state # 100 should be a lower constant... mp.AddLinearConstraint( state_over_time[-1, 0] <= goal_state[0] + flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 0] >= goal_state[0] - flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 1] <= goal_state[1] + flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 1] >= goal_state[1] - flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 2] <= goal_state[2] + flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 2] >= goal_state[2] - flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 3] <= goal_state[3] + flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 3] >= goal_state[3] - flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 4] <= goal_state[4] + flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 4] >= goal_state[4] - flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 5] <= goal_state[5] + flight_time / 100.) mp.AddLinearConstraint( state_over_time[-1, 5] >= goal_state[5] - flight_time / 100.) # add the cost function mp.AddQuadraticCost( 10. * booster_over_time[:, 0].dot(booster_over_time[:, 0])) mp.AddQuadraticCost( 10. * booster_over_time[:, 1].dot(booster_over_time[:, 1])) mp.AddQuadraticCost( 10. * booster_over_time[:, 2].dot(booster_over_time[:, 2])) for i in range(1, len(time_array) - 1): cost_multiplier = np.exp( 4.5 * i / (len(time_array) - 1)) # exp starting at 1 and going to around 90 # penalize difference_in_state dist_to_goal_pos = goal_state[:3] - state_over_time[i - 1, :3] mp.AddQuadraticCost(cost_multiplier * dist_to_goal_pos.dot(dist_to_goal_pos)) # penalize difference in velocity if exact: dist_to_goal_vel = goal_state[-3:] - state_over_time[i - 1, -3:] mp.AddQuadraticCost(cost_multiplier / 2. * dist_to_goal_vel.dot(dist_to_goal_vel)) else: dist_to_goal_vel = goal_state[-3:] - state_over_time[i - 1, -3:] mp.AddQuadraticCost(cost_multiplier / 6. * dist_to_goal_vel.dot(dist_to_goal_vel)) solved = mp.Solve() if verbose: print solved # extract booster_over_time = mp.GetSolution(booster_over_time) output_states = mp.GetSolution(state_over_time) durations = time_array[1:len(time_array )] - time_array[0:len(time_array) - 1] fuel_consumption = ( np.sum(booster_over_time[:len(time_array)]**2, axis=1) * durations).sum() if verbose: print 'expected remaining fuel consumption', fuel_consumption print("took %s seconds" % (time.time() - start_time)) print 'goal state', goal_state print 'end state', output_states[-1] print '' return output_states, booster_over_time, time_array
xf = (math.pi, 0.) if num_states == 4: x0 += (0., 0.) xf += (0., 0.) prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0]) prog.AddBoundingBoxConstraint(xf, xf, x[ti][:, -1]) for i in range(num_samples - 1): AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:, i], x[ti][:, i + 1], u[ti][:, i], u[ti][:, i + 1], prog) for i in range(num_samples): prog.AddQuadraticCost([1.], [0.], u[ti][:, i]) # prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()])) # prog.AddBoundingBoxConstraint([0.], [0.], u[ti][:,i]) # prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x)) # prog.AddCost(final_cost, x[ti][:,-1]) #prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation) prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out") result = prog.Solve() print(result) print(prog.GetSolution(K)) print(prog.GetSolution(K).dot(control_basis(x[0][:, 0]))) #if (result != SolutionResult.kSolutionFound): # f = open('/tmp/snopt.out', 'r')
def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5): """ Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. Arguments ---------- H : numpy.ndarray Positive definite Hessian of the cost function. f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. tol : float Maximum value of a residual of an inequality to consider the related constraint active. Returns ---------- sol : dict Dictionary with the solution of the QP. Fields ---------- min : float Minimum of the QP (None if the problem is unfeasible). argmin : numpy.ndarray Argument that minimizes the QP (None if the problem is unfeasible). active_set : list of int Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible). multiplier_inequality : numpy.ndarray Lagrange multipliers for the inequality constraints (None if the problem is unfeasible). multiplier_equality : numpy.ndarray Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # reshape inputs if len(f.shape) == 2: f = np.reshape(f, f.shape[0]) # build program prog = MathematicalProgram() x = prog.NewContinuousVariables(n_x) inequalities = [] for i in range(n_ineq): lhs = A[i, :] + 1.e-15 * np.random.rand( (n_x) ) # drake raises a RuntimeError if the in the expression x does not appear (e.g.: 0 x <= 1) rhs = b[i] + 1.e-15 * np.random.rand( 1 ) # in case the constraint is 0 x <= 0 the previous trick ends up adding the constraint x <= 0 to the program... inequalities.append(prog.AddLinearConstraint(lhs.dot(x) <= rhs)) for i in range(n_eq): prog.AddLinearConstraint(C[i, :].dot(x) == d[i]) prog.AddQuadraticCost(.5 * x.dot(H).dot(x) + f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), "OutputFlag", 0) result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None, 'active_set': None, 'multiplier_inequality': None, 'multiplier_equality': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x).reshape(n_x, 1) sol['min'] = .5 * sol['argmin'].T.dot(H).dot( sol['argmin'])[0, 0] + f.dot(sol['argmin'])[0] sol['active_set'] = np.where( A.dot(sol['argmin']) - b > -tol)[0].tolist() # retrieve multipliers through KKT conditions lhs = A[sol['active_set'], :] rhs = b[sol['active_set'], :] if n_eq > 0: lhs = np.vstack((lhs, C)) rhs = np.vstack((rhs, d)) H_inv = np.linalg.inv(H) M = lhs.dot(H_inv).dot(lhs.T) m = -np.linalg.inv(M).dot(lhs.dot(H_inv).dot(f.reshape(n_x, 1)) + rhs) sol['multiplier_inequality'] = np.zeros((n_ineq, 1)) for i, j in enumerate(sol['active_set']): sol['multiplier_inequality'][j, 0] = m[i] if n_eq > 0: sol['multiplier_equality'] = m[len(sol['active_set']):, :] return sol
class PPTrajectory(): 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 eval(self, t, derivative_order=0): if derivative_order > self.degree: return 0 s = 0 while s < len(self.sample_times) - 1 and t >= self.sample_times[s + 1]: s += 1 trel = t - self.sample_times[s] if self.result is None: coeffs = self.coeffs[s] else: coeffs = self.result.GetSolution(self.coeffs[s]) deg = derivative_order val = 0 * coeffs[:, 0] for var in range(self.n): for d in range(deg, self.degree + 1): val[var] += coeffs[var, d]*np.power(trel, d-deg) * \ math.factorial(d)/math.factorial(d-deg) return val def add_constraint(self, t, derivative_order, lb, ub=None): ''' Adds a constraint of the form d^deg lb <= x(t) / dt^deg <= ub ''' if ub is None: ub = lb assert (derivative_order <= self.degree) val = self.eval(t, derivative_order) self.prog.AddLinearConstraint(val, lb, ub) def generate(self): self.result = Solve(self.prog) assert (self.result.is_success())
class HybridModelPredictiveController(object): def __init__(self, S, N, Q, R, P, X_N): # store inputs self.S = S self.N = N self.Q = Q self.R = R self.P = P self.X_N = X_N # mpMIQP self.build_mpmiqp() 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 set_initial_condition(self, x0): for k, c in enumerate(self.initial_condition): c.UpdateLowerBound(x0[k:k+1]) c.UpdateUpperBound(x0[k:k+1]) def feedforward(self, x0): # overwrite initial condition self.set_initial_condition(x0) # solve MIQP result = self.solver.Solve(self.prog) # check feasibility if result != SolutionResult.kSolutionFound: return None, None, None, None # get cost obj = self.prog.EvalBindingAtSolution(self.objective)[0] # store argmin in list of vectors u = [self.prog.GetSolution(ut) for ut in self.u] x = [self.prog.GetSolution(xt) for xt in self.x] d = [self.prog.GetSolution(dt) for dt in self.d] # retrieve mode sequence and check integer feasibility ms = [np.argmax(dt) for dt in d] return u, x, ms, obj def feedback(self, x0): # get feedforward and extract first input u_feedforward = self.feedforward(x0)[0] if u_feedforward is None: return None return u_feedforward[0]
def compute_trajectory(self, state_initial, minimum_time, maximum_time): ''' :param: state_initial: :param state_initial: numpy array of length 6, see satellite_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 6 columns. See simulate_states_over_time for more documentation. input_trajectory: a 2d array with N-1 row, and 4 columns. See simulate_states_over_time for more documentation. time_array: an array with N rows. ''' mp = MathematicalProgram() # use direct transcription N = 50 #N = 200 # Unpack state bounds pmax = self.pmax vmax = self.vmax hmax = self.hmax wmax = self.wmax # Calculate number of states and control input nq = len(state_initial) nu = 4 #### BEGIN: Decision Variables #### # Inputs k = 0 u = mp.NewContinuousVariables(nu,"u_%d" % k) u_over_time = u for k in range(1,N-1): u = mp.NewContinuousVariables(nu, "u_%d" % k) u_over_time = np.vstack((u_over_time, u)) # States k = 0 states = mp.NewContinuousVariables(nq,"states_over_time_%d" % k) states_over_time = states for k in range(1,N): states = mp.NewContinuousVariables(nq, "states_over_time_%d" % k) states_over_time = np.vstack((states_over_time, states)) # Final Time tf = mp.NewContinuousVariables(1,"tf") dt = tf / (N-1) #### END: Decision Variables #### #### BEGIN: Input constraints #### for i in range(N-1): for j in range(nu): mp.AddConstraint(u_over_time[i,j] >= 0.0) mp.AddConstraint(u_over_time[i,j] <= 0.10) #### END: Input constraints #### #### BEGIN: Time constraints #### # Final time must be between minimum_time and maximum_time mp.AddConstraint(tf[0] <= maximum_time) mp.AddConstraint(tf[0] >= minimum_time) #### END: Time constraints #### #### BEGIN: State constraints #### # initial state constraint for j in range(nq): mp.AddLinearConstraint(states_over_time[0,j] >= state_initial[j]) mp.AddLinearConstraint(states_over_time[0,j] <= state_initial[j]) # state constraints for all time for i in range(N): mp.AddLinearConstraint(states_over_time[i,0] <= pmax) mp.AddLinearConstraint(states_over_time[i,0] >= -pmax) mp.AddLinearConstraint(states_over_time[i,1] <= pmax) mp.AddLinearConstraint(states_over_time[i,1] >= -pmax) mp.AddLinearConstraint(states_over_time[i,2] <= vmax) mp.AddLinearConstraint(states_over_time[i,2] >= -vmax) mp.AddLinearConstraint(states_over_time[i,3] <= vmax) mp.AddLinearConstraint(states_over_time[i,3] >= -vmax) mp.AddLinearConstraint(states_over_time[i,4] <= hmax) mp.AddLinearConstraint(states_over_time[i,4] >= -hmax) mp.AddLinearConstraint(states_over_time[i,5] <= wmax) mp.AddLinearConstraint(states_over_time[i,5] >= -wmax) # dynamic constraint for i in range(N-1): dx = self.satellite_dynamics(states_over_time[i,:],u_over_time[i,:]) for j in range(nq): mp.AddConstraint(states_over_time[i+1,j]<=states_over_time[i,j]+dx[j]*dt[0]) mp.AddConstraint(states_over_time[i+1,j]>=states_over_time[i,j]+dx[j]*dt[0]) # Final state constraint final_state_error = states_over_time[-1,:] - self.goalState mp.AddConstraint((final_state_error).dot(final_state_error) <= 0.001**2) #### END: State constraints #### #### BEGIN: Costs #### # Quadratic cost on fuel (aka control) for j in range(nu): mp.AddQuadraticCost(u_over_time[:,j].dot(u_over_time[:,j])) #### END: Costs #### #### Solve the Optimization! #### result = Solve(mp) #print result.is_success() #### Name outputs appropriately #### # Time - knot points optimal_tf = result.GetSolution(tf) time_step = optimal_tf / (N-1) time_array = np.arange(0.0,optimal_tf+time_step,time_step) # Allocate to input trajectory input_trajectory = result.GetSolution(u_over_time) # Allocate to trajectory output trajectory = result.GetSolution(states_over_time) # save to csv if os.path.exists("traj.csv"): os.remove("traj.csv") if os.path.exists("input_traj.csv"): os.remove("input_traj.csv") with open('traj.csv', 'a') as csvFile: writer = csv.writer(csvFile) for i in range(N): row = [time_array[i], trajectory[i,0], trajectory[i,1], trajectory[i,2], trajectory[i,3], trajectory[i,4], trajectory[i,5]] writer.writerow(row) with open('input_traj.csv', 'a') as csvFile: writer = csv.writer(csvFile) for i in range(N-1): row = [input_trajectory[i,0], input_trajectory[i,1], input_trajectory[i,2], input_trajectory[i,3]] writer.writerow(row) csvFile.close() return trajectory, input_trajectory, time_array
def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5, **kwargs): """ Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d. Arguments ---------- H : numpy.ndarray Positive definite Hessian of the cost function. f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. tol : float Maximum value of a residual of an inequality to consider the related constraint active. Returns ---------- sol : dict Dictionary with the solution of the QP. Fields ---------- min : float Minimum of the QP (None if the problem is unfeasible). argmin : numpy.ndarray Argument that minimizes the QP (None if the problem is unfeasible). active_set : list of int Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible). multiplier_inequality : numpy.ndarray Lagrange multipliers for the inequality constraints (None if the problem is unfeasible). multiplier_equality : numpy.ndarray Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # build program prog = MathematicalProgram() x = prog.NewContinuousVariables(n_x) [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] inequalities = [] prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None, 'active_set': None, 'multiplier_inequality': None, 'multiplier_equality': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x) sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin']) sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist() # retrieve multipliers through KKT conditions lhs = A[sol['active_set']] rhs = b[sol['active_set']] if n_eq > 0: lhs = np.vstack((lhs, C)) rhs = np.concatenate((rhs, d)) H_inv = np.linalg.inv(H) M = lhs.dot(H_inv).dot(lhs.T) m = - np.linalg.inv(M).dot(lhs.dot(H_inv).dot(f) + rhs) sol['multiplier_inequality'] = np.zeros(n_ineq) for i, j in enumerate(sol['active_set']): sol['multiplier_inequality'][j] = m[i] if n_eq > 0: sol['multiplier_equality'] = m[len(sol['active_set']):] return sol
def diff_drive_pd_mp( self, x, x_des): # x_des = [x, theta, yaw, x_dot, theta_dot, yaw_dot] m_s = 0.2 #kg d = 0.085 #m m_c = 0.056 I_3 = 0.000228373 #.0000989844 #kg*m^2 I_2 = .0000989844 R = 0.0333375 g = -9.81 #may need to be set as -9.81; test to see L = 0.03 A_val_theta = (m_s * d * g * (3 * m_c + m_s)) / (3 * m_c * I_3 + 3 * m_c * m_s * d**2 + m_s * I_3) B_val_theta = (-m_s * d / R - 3 * m_c * m_s) / ( 3 * m_c * I_3 + 3 * m_c * m_s * d**2 + m_s * I_3) * math.pi / 180 #multiplicand for u to change in theta_dot B_val_phi = -(2 * L / R) / (6 * m_c * L**2 + m_c * R**2 + 2 * I_2) mp = MathematicalProgram() k = mp.NewContinuousVariables(3, 'k_val') # [kp, kd, ky] u = mp.NewContinuousVariables(2, 'u_val') theta_dot_post = mp.NewContinuousVariables( 1, 'theta_dot_post_val') #estimated value of theta dot after control phi_dot_post = mp.NewContinuousVariables( 1, 'phi_dot_post_val') #estimated value of theta dot after control ''' kp = 1. #regulate theta (pitch) position kd = kp / 20. #regulate theta (pitch) velocity ky = 0.5 #regulate phi (yaw) position ''' actuator_limit = .1 #estimate; 0.1 is probably high #mp.AddConstraint(theta[0] == np.arcsin(2*(x[0]*x[2] - x[1]*x[3]))) #pitch theta = np.arcsin(2 * (x[0] * x[2] - x[1] * x[3])) phi = np.arctan2(2 * (x[0] * x[1] + x[2] * x[3]), 1 - 2 * (x[1]**2 + x[2]**2)) #yaw theta_dot = x[ 10] #Shown to be ~1.5% below true theta_dot on average in experiments mp.AddConstraint(k[0] >= 0.) mp.AddConstraint(k[1] >= 0.) mp.AddConstraint(k[2] >= 0.) mp.AddConstraint(u[0] == k[0] * (x_des[1] - theta + k[1] * (x_des[4] - theta_dot)) + k[2] * (x_des[2] - phi)) mp.AddConstraint(u[0] <= -actuator_limit) mp.AddConstraint(u[0] >= -actuator_limit) mp.AddConstraint(u[1] == -u[0]) mp.AddConstraint(theta_dot_post[0] == theta_dot + B_val_theta * u[0] * 0.0005 + A_val_theta * theta * .0005) mp.AddConstraint(phi_dot_post[0] == x[11] + B_val_phi * u[0] * .0005) theta_dot_des = -(theta - x_des[1]) / (2 * .0005) mp.AddQuadraticCost((theta_dot_post[0] - theta_dot_des)**2) phi_dot_des = -(phi - x_des[2]) / 2 print('theta_dot_des', theta_dot_des) print('theta', theta) mp.AddQuadraticCost(0.001 * (phi_dot_post[0] - phi_dot_des)**2) result = Solve(mp) print('Success: ', result.is_success()) print('Solver id: ', result.get_solver_id().name()) print('k: ', result.GetSolution(k)) print('u: ', result.GetSolution(u)) return result.GetSolution(u)
d[1] = -1 d[2] = 1 d[3] = -1 d = np.array([5.0752e+01, 4.7343e+05, 8.4125e+05, 6.2668e+05]) phi0 = -36332.36234347365 print("Qp : ", Quadratic_Positive_def) print("Qp det: ", QP_det) # Quadratic cost : u^TQu + c^Tu CLF_QP_cost_v_effective = np.dot(u_var, np.dot(Quadratic_Positive_def, u_var)) + np.dot(c, u_var) prog.AddQuadraticCost(CLF_QP_cost_v_effective) prog.AddConstraint(np.dot(d, u_var) + phi0 <= 0) solver = IpoptSolver() prog.Solve() # solver.Solve(prog) print("Optimal u : ", prog.GetSolution(u_var)) u_CLF_QP = prog.GetSolution(u_var) # ('A_fl:', ) # ('A_fl_det:', 137180180557.17741) # ('Qp : ', array([[ 1.0000e+00, -1.5475e-13, 4.0035e-14, 3.7932e-15], # [-1.5475e-13, 5.7298e+07, 2.1803e+05, -3.3461e+06], # [ 4.0035e-14, 2.1803e+05, 6.2061e+07, 3.5442e+07], # [ 3.7932e-15, -3.3461e+06, 3.5442e+07, 2.5742e+07]]))
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state): # Call base method to ensure we do not get recursion. # (This makes sure relevant event handlers get called.) LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events, discrete_state) new_control_input = discrete_state. \ get_mutable_vector().get_mutable_value() x = self.EvalVectorInput( context, self.robot_state_input_port.get_index()).get_value() setpoint = self.EvalAbstractInput( context, self.setpoint_input_port.get_index()).get_value() q_full = x[:self.nq] v_full = x[self.nq:] q = x[self.controlled_inds] v = x[self.nq:][self.controlled_inds] kinsol = self.rbt.doKinematics(q_full, v_full) M_full = self.rbt.massMatrix(kinsol) C = self.rbt.dynamicsBiasTerm(kinsol, {}, None)[self.controlled_inds] # Python slicing doesn't work in 2D, so do it row-by-row M = np.zeros((self.nq_reduced, self.nq_reduced)) for k in range(self.nq_reduced): M[:, k] = M_full[self.controlled_inds, k] # Pick a qdd that results in minimum deviation from the desired # end effector pose (according to the end effector frame's jacobian # at the current state) # v_next = v + control_period * qdd # q_next = q + control_period * (v + v_next) / 2. # ee_v = J*v # ee_p = from forward kinematics # ee_v_next = J*v_next # ee_p_next = ee_p + control_period * (ee_v + ee_v_next) / 2. # min u and qdd # || q_next - q_des ||_Kq # + || v_next - v_des ||_Kv # + || qdd ||_Ka # + || Kee_v - ee_v_next ||_Kee_pt # + || Kee_pt - ee_p_next ||_Kee_v # + the messily-implemented angular ones? # s.t. M qdd + C = B u # (no contact modeling for now) prog = MathematicalProgram() qdd = prog.NewContinuousVariables(self.nq_reduced, "qdd") u = prog.NewContinuousVariables(self.nu, "u") prog.AddQuadraticCost(qdd.T.dot(setpoint.Ka).dot(qdd)) v_next = v + self.control_period * qdd q_next = q + self.control_period * (v + v_next) / 2. if setpoint.v_des is not None: v_err = setpoint.v_des - v_next prog.AddQuadraticCost(v_err.T.dot(setpoint.Kv).dot(v_err)) if setpoint.q_des is not None: q_err = setpoint.q_des - q_next prog.AddQuadraticCost(q_err.T.dot(setpoint.Kq).dot(q_err)) if setpoint.ee_frame is not None and setpoint.ee_pt is not None: # Convert x to autodiff for Jacobians computation q_full_ad = np.empty(self.nq, dtype=AutoDiffXd) for i in range(self.nq): der = np.zeros(self.nq) der[i] = 1 q_full_ad.flat[i] = AutoDiffXd(q_full.flat[i], der) kinsol_ad = self.rbt.doKinematics(q_full_ad) tf_ad = self.rbt.relativeTransform(kinsol_ad, 0, setpoint.ee_frame) # Compute errors in EE pt position and velocity (in world frame) ee_p_ad = tf_ad[0:3, 0:3].dot(setpoint.ee_pt) + tf_ad[0:3, 3] ee_p = np.hstack([y.value() for y in ee_p_ad]) J_ee = np.vstack([y.derivatives() for y in ee_p_ad]).reshape(3, self.nq) J_ee = J_ee[:, self.controlled_inds] ee_v = J_ee.dot(v) ee_v_next = J_ee.dot(v_next) ee_p_next = ee_p + self.control_period * (ee_v + ee_v_next) / 2. if setpoint.ee_pt_des is not None: ee_p_err = setpoint.ee_pt_des.reshape( (3, 1)) - ee_p_next.reshape((3, 1)) prog.AddQuadraticCost( (ee_p_err.T.dot(setpoint.Kee_pt).dot(ee_p_err))[0, 0]) if setpoint.ee_v_des is not None: ee_v_err = setpoint.ee_v_des.reshape( (3, 1)) - ee_v_next.reshape((3, 1)) prog.AddQuadraticCost( (ee_v_err.T.dot(setpoint.Kee_v).dot(ee_v_err))[0, 0]) # Also compute errors in EE cardinal vector directions vs targets in world frame for i, vec in enumerate( (setpoint.ee_x_des, setpoint.ee_y_des, setpoint.ee_z_des)): if vec is not None: direction = np.zeros(3) direction[i] = 1. ee_dir_ad = tf_ad[0:3, 0:3].dot(direction) ee_dir_p = np.hstack([y.value() for y in ee_dir_ad]) J_ee_dir = np.vstack([y.derivatives() for y in ee_dir_ad ]).reshape(3, self.nq) J_ee_dir = J_ee_dir[:, self.controlled_inds] ee_dir_v = J_ee_dir.dot(v) ee_dir_v_next = J_ee_dir.dot(v_next) ee_dir_p_next = ee_dir_p + self.control_period * ( ee_dir_v + ee_dir_v_next) / 2. ee_dir_p_err = vec.reshape((3, 1)) - ee_dir_p_next.reshape( (3, 1)) prog.AddQuadraticCost((ee_dir_p_err.T.dot( setpoint.Kee_xyz).dot(ee_dir_p_err))[0, 0]) prog.AddQuadraticCost((ee_dir_v_next.T.dot( setpoint.Kee_xyzd).dot(ee_dir_v_next))) LHS = np.dot(M, qdd) + C RHS = np.dot(self.B, u) for i in range(self.nq_reduced): prog.AddLinearConstraint(LHS[i] == RHS[i]) result = prog.Solve() u = prog.GetSolution(u) new_control_input[:] = u
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) 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, :, :] normals = grasp_normals_world_now[0:2, :].T # 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. ''' YOUR CODE HERE ''' # This is not proper orthogonal vector...but it somehow makes it work... # whereas the correct does not def ortho2(x): return np.array([x[0],-x[1]]) #return np.array([x[1],-x[0]]) prog = MathematicalProgram() dt = self.control_period u = prog.NewContinuousVariables(self.nu, "u") qdd = prog.NewContinuousVariables(q.shape[0], "qdd") for i in range(qdd.shape[0]): prog.AddLinearConstraint(qdd[i] <= 40) prog.AddLinearConstraint(qdd[i] >= -40) #prog.AddLinearConstraint(v[i] + qdd[i] * dt <= 80) #prog.AddLinearConstraint(v[i] - qdd[i] * dt >= -80) beta = prog.NewContinuousVariables(n_cf, 2, "beta") #prog.AddQuadraticCost(0.1*np.sum(beta**2)) for i in range(n_cf): prog.AddLinearConstraint(beta[i,0] >= 0) prog.AddLinearConstraint(beta[i,1] >= 0) prog.AddLinearConstraint(beta[i,0] <= 10) prog.AddLinearConstraint(beta[i,1] <= 10) c = np.zeros((n_cf,2,2)) for i in range(n_cf): c[i,0] = normals[i] - self.mu * ortho2(normals[i]) c[i,1] = normals[i] + self.mu * ortho2(normals[i]) const = M.dot(qdd) + C - B.dot(u) ## eq 0 for i in range(n_cf): lambda_i = c[i,0]*beta[i,0] + c[i,1]*beta[i,1] tmp = J_contact[:, i, :].T.dot(lambda_i) const -= tmp for i in range(const.shape[0]): prog.AddLinearConstraint(const[i] == 0.0) Kp = 1000 Kd = 10 #v2 = v + dt * qdd #q2 = q + v * dt + 0.5 * qdd * dt*dt v2 = v + dt * qdd q2 = q + (v+v2)/2 * dt #+ 0.5 * qdd * dt*dt #v2 = v + dt * qdd #q2 = q + v2 * dt prog.AddQuadraticCost(Kp * np.sum((qdes - q2) ** 2) + Kd * np.sum(v2**2)) result = prog.Solve() u = prog.GetSolution(u) return u
def constraint_evaluator1(z): return np.array([(-(z[6] + z[7])*sin(z[2])/m)*dt + z[3] - z[8], ((z[6] + z[7])*cos(z[2]) - m*g/m)*dt + z[4] - z[9], ((z[6] - z[7])*r/I)*dt + z[5] - z[10] ]) def constraint_evaluator2(z): return np.array([z[3]*dt + z[0] - z[6], z[4]*dt + z[1] - z[7], z[5]*dt + z[2] - z[8]]) for n in range(N-1): # Will eventually be prog.AddConstraint(x[:,n+1] == A@x[:,n] + B@u[:,n]) # See drake issues 12841 and 8315 # prog.AddConstraint(eq(dx[0,n+1], dx[0,n] + dt*(-(u[0,n] + u[1,n])*sin(x[2,n])/m))) # prog.AddConstraint(eq(dx[1,n+1], dx[1,n] + dt*((u[0,n] + u[1,n])*cos(x[2,n]) - m*g/m))) # prog.AddConstraint(eq(dx[2,n+1], dx[2,n] + dt*((u[0,n] - u[1,n])*r/I))) prog.AddQuadraticCost(sum(x[:,n]**2) + sum(dx[:,n]**2) + sum(u[:,n]**2)) prog.AddConstraint(constraint_evaluator1, lb=np.array([0]*3), ub=np.array([0]*3), vars=[x[0, n], x[1, n], x[2, n], dx[0, n], dx[1, n], dx[2, n], u[0, n], u[1, n], dx[0, n+1], dx[1, n+1], dx[2, n+1],]) prog.AddConstraint(constraint_evaluator2, lb=np.array([0]*3), ub=np.array([0]*3), vars=[x[0, n], x[1, n], x[2, n], dx[0, n], dx[1, n], dx[2, n], x[0, n+1], x[1, n+1], x[2, n+1],])
def intercepting_with_obs_avoidance_bb(self, p0, v0, pf, vf, T, obstacles=None, p_puck=None): """kick while avoiding obstacles using big M formulation and branch and bound""" 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 command 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)) 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) # Add Mixed-integer constraints - will solve with BB # avoid hitting the puck while generating a kicking trajectory # MILP formulation with B&B solver x_obs_puck = prog.NewBinaryVariables(rows=N + 1, cols=2) # obs x_min, obs x_max y_obs_puck = prog.NewBinaryVariables(rows=N + 1, cols=2) # obs y_min, obs y_max self.avoid_puck_bigm(prog, x_obs_puck, y_obs_puck, state, N, p_puck) # Avoid other players if obstacles != None: x_obs_player = list() y_obs_player = list() for i, obs in enumerate(obstacles): x_obs_player.append(prog.NewBinaryVariables( rows=N + 1, cols=2)) # obs x_min, obs x_max y_obs_player.append(prog.NewBinaryVariables( rows=N + 1, cols=2)) # obs y_min, obs y_max self.avoid_other_player_bigm(prog, x_obs_player[i], y_obs_player[i], state, obs, N) # Solve with simple b&b solver for k in range(N): prog.AddQuadraticCost(cmd[k].flatten().dot(cmd[k])) bb = branch_and_bound.MixedIntegerBranchAndBound( prog, OsqpSolver().solver_id()) result = bb.Solve() if result != result.kSolutionFound: raise ValueError('Infeasible optimization problem.') u_values = np.array([bb.GetSolution(u) for u in cmd]).T solution_found = result.kSolutionFound return solution_found, u_values
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. ''' from pydrake.all import MathematicalProgram import numpy as np import math N = 100 t = maximum_time #input_trajectory = np.ones((N,2))*10.0 #time_used = 100.0 #time_array = np.arange(0.0, time_used, time_used/(N+1)) #trajectory = self.simulate_states_over_time(state_initial, time_array, input_trajectory) #return trajectory, input_trajectory, time_array mp = MathematicalProgram() def addLinearEqual(x, y, length): for i in range(length): mp.AddLinearConstraint(x[i] == y[i]) def addEpsilonEq(a, b, e): mp.AddConstraint(a <= b + e) mp.AddConstraint(a >= b - e) def addEqual(x, y, length, e): for i in range(length): addEpsilonEq(x[i], y[i], e) def worldTwoDistSquared(x): return np.sum((self.world_2_position - x) ** 2) inp_traj = mp.NewContinuousVariables(N-1, 2, "inp") traj = mp.NewContinuousVariables(N, 4, "traj") #mp.NewContinuousVariables(1, "t") #mp.AddLinearConstraint(t[0] == 10.0) time_step = t / N addLinearEqual(traj[0], state_initial, 4) for i in range(N-1): #todo add constraint forcing orbit to not occur until time t predicted = traj[i] + time_step * self.rocket_dynamics(traj[i], inp_traj[i]) addEqual(traj[i+1], predicted, 4, 1e-8) mp.AddQuadraticCost(np.sum(inp_traj[i]**2)) #mp.AddQuadraticCost(time_step*np.sum(inp_traj[i]**2)) addEpsilonEq(worldTwoDistSquared(traj[-1][:2]), 0.5**2, 1e-8) velocity = traj[-1][2:] / time_step addEpsilonEq(velocity.dot(velocity), self.G*self.M2/0.5, 1e-8) addEpsilonEq(velocity.dot(traj[-1][:2] - self.world_2_position), 0, 1e-8) #mp.AddLinearConstraint(t == 100.0) #mp.AddLinearConstraint(t >= minimum_time) #mp.AddLinearConstraint(t <= maximum_time) print(mp) print(mp.Solve()) time_used = t # mp.GetSolution(t) time_array = np.arange(0.0, time_used, time_used/N) trajectory = mp.GetSolution(traj) input_trajectory = mp.GetSolution(inp_traj) #print('time=', time_array) #print('input=', input_trajectory) #print('traj=', trajectory) true_trajectory = self.simulate_states_over_time(state_initial, time_array, input_trajectory) #print(trajectory - true_trajectory) #print(trajectory) #print(true_trajectory) return trajectory, input_trajectory, time_array #todo add constraint ensuring orbit to not occurs at time t #mp.AddLinearCost(x[0]*1.0) #input_trajectory = np.ones((N,2))*10.0 #time_used = 100.0 #time_array = np.arange(0.0, time_used, time_used/(N+1)) #trajectory = self.simulate_states_over_time(state_initial, time_array, input_trajectory) #return trajectory, input_trajectory, time_array
def MILP_compute_traj(self, obst_idx, x_out, y_out, dx, dy, pose_initial=[0., 0.]): ''' Find trajectory with MILP Outputs trajectory (waypoints) and new K for control ''' mp = MathematicalProgram() N = 8 k = 0 # define state traj st = mp.NewContinuousVariables(2, "state_%d" % k) # # binary variables for obstalces constraint c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = c states = st for k in range(1, N): st = mp.NewContinuousVariables(2, "state_%d" % k) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) st = mp.NewContinuousVariables(2, "state_%d" % (N + 1)) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) # variables encoding max x y dist from obstacle x_margin = mp.NewContinuousVariables(1, "x_margin") y_margin = mp.NewContinuousVariables(1, "y_margin") ### define cost for i in range(N): mp.AddLinearCost(-states[i, 0]) # go as far forward as possible mp.AddLinearCost(-states[-1, 0]) mp.AddLinearCost(-x_margin[0]) mp.AddLinearCost(-y_margin[0]) # bound x y margin so it doesn't explode mp.AddLinearConstraint(x_margin[0] <= 3.) mp.AddLinearConstraint(y_margin[0] <= 3.) # x y is non ngative adn at least above robot radius mp.AddLinearConstraint(x_margin[0] >= 0.05) mp.AddLinearConstraint(y_margin[0] >= 0.05) M = 1000 # slack var for integer things # state constraint for i in range(2): # initial state constraint x y mp.AddLinearConstraint(states[0, i] <= pose_initial[i]) mp.AddLinearConstraint(states[0, i] >= pose_initial[i]) for i in range(N): mp.AddQuadraticCost((states[i + 1, 1] - states[i, 1])**2) mp.AddLinearConstraint(states[i + 1, 0] <= states[i, 0] + dx) mp.AddLinearConstraint(states[i + 1, 0] >= states[i, 0] - dx) mp.AddLinearConstraint(states[i + 1, 1] <= states[i, 1] + dy) mp.AddLinearConstraint(states[i + 1, 1] >= states[i, 1] - dy) # obstacle constraint for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[i, 0] <= rng_min - x_margin[0] + M * obs[i, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[i, 0] >= rng_max + x_margin[0] - M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[i, 1] <= states[i, 0] * np.tan(ang_min) - y_margin[0] + M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[i, 1] >= states[i, 0] * np.tan(ang_max) + y_margin[0] - M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c # obstacle constraint for last state for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[N, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[N, 0] <= rng_min - x_margin[0] + M * obs[N, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[N, 0] >= rng_max + x_margin[0] - M * obs[N, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[N, 1] <= states[N, 0] * np.tan(ang_min) - y_margin[0] + M * obs[N, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[N, 1] >= states[N, 0] * np.tan(ang_max) + y_margin[0] - M * obs[N, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c mp.Solve() trajectory = mp.GetSolution(states) xm = mp.GetSolution(x_margin) ym = mp.GetSolution(y_margin) x_out[:] = trajectory[:, 0] y_out[:] = trajectory[:, 1] return trajectory, xm[0], ym[0]
def solve(self, quad_start_q, quad_final_q, time_used): mp = MathematicalProgram() # We want to solve this for a certain number of knot points N = 40 # num knot points time_increment = time_used / (N + 1) dt = time_increment time_array = np.arange(0.0, time_used, time_increment) quad_u = mp.NewContinuousVariables(2, "u_0") quad_q = mp.NewContinuousVariables(6, "quad_q_0") for i in range(1, N): u = mp.NewContinuousVariables(2, "u_%d" % i) quad = mp.NewContinuousVariables(6, "quad_q_%d" % i) quad_u = np.vstack((quad_u, u)) quad_q = np.vstack((quad_q, quad)) for i in range(N): mp.AddLinearConstraint(quad_u[i][0] <= 3.0) # force mp.AddLinearConstraint(quad_u[i][0] >= 0.0) # force mp.AddLinearConstraint(quad_u[i][1] <= 10.0) # torque mp.AddLinearConstraint(quad_u[i][1] >= -10.0) # torque mp.AddLinearConstraint(quad_q[i][0] <= 1000.0) # pos x mp.AddLinearConstraint(quad_q[i][0] >= -1000.0) mp.AddLinearConstraint(quad_q[i][1] <= 1000.0) # pos y mp.AddLinearConstraint(quad_q[i][1] >= -1000.0) mp.AddLinearConstraint( quad_q[i][2] <= 60.0 * np.pi / 180.0) # pos theta mp.AddLinearConstraint(quad_q[i][2] >= -60.0 * np.pi / 180.0) mp.AddLinearConstraint(quad_q[i][3] <= 1000.0) # vel x mp.AddLinearConstraint(quad_q[i][3] >= -1000.0) mp.AddLinearConstraint(quad_q[i][4] <= 1000.0) # vel y mp.AddLinearConstraint(quad_q[i][4] >= -1000.0) mp.AddLinearConstraint(quad_q[i][5] <= 10.0) # vel theta mp.AddLinearConstraint(quad_q[i][5] >= -10.0) for i in range(1, N): quad_q_dyn_feasible = self.dynamics(quad_q[i - 1, :], quad_u[i - 1, :], dt) # Direct transcription constraints on states to dynamics for j in range(6): quad_state_err = (quad_q[i][j] - quad_q_dyn_feasible[j]) eps = 0.001 mp.AddConstraint(quad_state_err <= eps) mp.AddConstraint(quad_state_err >= -eps) # Initial and final quad and ball states for j in range(6): mp.AddLinearConstraint(quad_q[0][j] == quad_start_q[j]) mp.AddLinearConstraint(quad_q[-1][j] == quad_final_q[j]) # Quadratic cost on the control input R_force = 10.0 R_torque = 100.0 Q_quad_x = 100.0 Q_quad_y = 100.0 mp.AddQuadraticCost(R_force * quad_u[:, 0].dot(quad_u[:, 0])) mp.AddQuadraticCost(R_torque * quad_u[:, 1].dot(quad_u[:, 1])) mp.AddQuadraticCost(Q_quad_x * quad_q[:, 0].dot(quad_q[:, 1])) mp.AddQuadraticCost(Q_quad_y * quad_q[:, 1].dot(quad_q[:, 1])) # Solve the optimization print "Number of decision vars: ", mp.num_vars() # mp.SetSolverOption(SolverType.kSnopt, "Major iterations limit", 100000) print "Solve: ", mp.Solve() quad_traj = mp.GetSolution(quad_q) input_traj = mp.GetSolution(quad_u) return (quad_traj, input_traj, time_array)
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
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. ''' mp = MathematicalProgram() max_speed = 0.99 desired_distance = 0.5 u_cost_factor = 1000. N = 50 # trajectory = np.zeros((N+1,4)) # input_trajectory = np.ones((N,2))*10.0 time_used = (maximum_time + minimum_time) / 2. time_step = time_used / (N + 1) time_array = np.arange(0.0, time_used, time_step) k = 0 # Create continuous variables for u & x u = mp.NewContinuousVariables(2, "u_%d" % k) x = mp.NewContinuousVariables(4, "x_%d" % k) u_over_time = u x_over_time = x for k in range(1, N): u = mp.NewContinuousVariables(2, "u_%d" % k) x = mp.NewContinuousVariables(4, "x_%d" % k) u_over_time = np.vstack((u_over_time, u)) x_over_time = np.vstack((x_over_time, x)) # trajectory is N+1 in size x = mp.NewContinuousVariables(4, "x_%d" % (N + 1)) x_over_time = np.vstack((x_over_time, x)) # Add cost # for k in range(0, N): mp.AddQuadraticCost(u_cost_factor * u_over_time[:, 0].dot(u_over_time[:, 0])) mp.AddQuadraticCost(u_cost_factor * u_over_time[:, 1].dot(u_over_time[:, 1])) # Add constraint for initial state mp.AddLinearConstraint(x_over_time[0, 0] >= state_initial[0]) mp.AddLinearConstraint(x_over_time[0, 0] <= state_initial[0]) mp.AddLinearConstraint(x_over_time[0, 1] >= state_initial[1]) mp.AddLinearConstraint(x_over_time[0, 1] <= state_initial[1]) mp.AddLinearConstraint(x_over_time[0, 2] >= state_initial[2]) mp.AddLinearConstraint(x_over_time[0, 2] <= state_initial[2]) mp.AddLinearConstraint(x_over_time[0, 3] >= state_initial[3]) mp.AddLinearConstraint(x_over_time[0, 3] <= state_initial[3]) # Add constraint between x & u for k in range(1, N + 1): next_step = self.rocket_dynamics(x_over_time[k - 1, :], u_over_time[k - 1, :]) mp.AddConstraint(x_over_time[k, 0] == x_over_time[k - 1, 0] + time_step * next_step[0]) mp.AddConstraint(x_over_time[k, 1] == x_over_time[k - 1, 1] + time_step * next_step[1]) mp.AddConstraint(x_over_time[k, 2] == x_over_time[k - 1, 2] + time_step * next_step[2]) mp.AddConstraint(x_over_time[k, 3] == x_over_time[k - 1, 3] + time_step * next_step[3]) # Make sure it never goes too far from the planets # for k in range(1, N): # mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_2_position[:]) <= 10) # mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_1_position[:]) <= 10) # Make sure u never goes above a threshold max_u = 6. for k in range(0, N): mp.AddLinearConstraint(u_over_time[k, 0] <= max_u) mp.AddLinearConstraint(-u_over_time[k, 0] <= max_u) mp.AddLinearConstraint(u_over_time[k, 1] <= max_u) mp.AddLinearConstraint(-u_over_time[k, 1] <= max_u) # Make sure it reaches world 2 mp.AddConstraint( self.two_norm(x_over_time[-1, 0:2] - self.world_2_position) <= desired_distance) mp.AddConstraint( self.two_norm(x_over_time[-1, 0:2] - self.world_2_position) >= desired_distance) # Make sure it's speed isn't too high mp.AddConstraint(self.two_norm(x_over_time[-1, 2:4]) <= max_speed**2.) # Get result result = Solve(mp) x_over_time_result = result.GetSolution(x_over_time) u_over_time_result = result.GetSolution(u_over_time) print("Success", result.is_success()) print("Final position", x_over_time_result[-1, :]) print( "Final distance to world2", self.two_norm(x_over_time_result[-1, 0:2] - self.world_2_position)) print("Final speed", self.two_norm(x_over_time_result[-1, 2:4])) print("Fuel consumption", (u_over_time_result**2.).sum()) trajectory = x_over_time_result input_trajectory = u_over_time_result return trajectory, input_trajectory, time_array
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 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. ''' #Determine a time within the middle of the range for (min time, max time) N = 50 time_diff = ((maximum_time - minimum_time) / 2.) if (maximum_time - minimum_time) % 2 == 1: time_used = (time_diff - .5) + minimum_time + 3 else: time_used = (time_diff) + minimum_time + 3 print(time_used) time_array = np.arange(0.0, time_used, time_used / (1.0 * N)) dt = time_used / (1.0 * N) mp = MathematicalProgram() #Base case for state/control variables k = 0 state = mp.NewContinuousVariables(4, "state_%d" % k) #x,y,xdot,ydot state_over_time = state u = mp.NewContinuousVariables(2, "u_%d" % k) u_over_time = u num_time_steps = N #int(time_used) #Set Nx4 vector to represent the state variables; (N-1)x2 vector to represent the control inputs for k in range(1, num_time_steps): state = mp.NewContinuousVariables(4, "state_%d" % k) #x,y,xdot,ydot state_over_time = np.vstack((state_over_time, state)) for k in range(1, num_time_steps - 1): u = mp.NewContinuousVariables(2, "u_%d" % k) u_over_time = np.vstack((u_over_time, u)) #constrain initial states print(state_initial) mp.AddLinearConstraint(state_over_time[ 0, 0] == state_initial[0]).evaluator().set_description('start [0]') mp.AddLinearConstraint(state_over_time[ 0, 1] == state_initial[1]).evaluator().set_description('start [1]') mp.AddLinearConstraint(state_over_time[ 0, 2] == state_initial[2]).evaluator().set_description('start [2]') mp.AddLinearConstraint(state_over_time[0, 3] <= state_initial[3] ).evaluator().set_description('start [3], less') mp.AddLinearConstraint( state_over_time[0, 3] >= state_initial[3]).evaluator( ).set_description('start [3], greater') #find dynamics constraints for each state to make standard direct transcription constraints for i in range(0, num_time_steps - 1): next_state_change = self.rocket_dynamics(state_over_time[i, :], u_over_time[i, :]) #constrain every state via dynamics mp.AddLinearConstraint( state_over_time[i + 1, 0] == state_over_time[i, 0] + next_state_change[0] * dt).evaluator().set_description('dynamics[0]' + str(i)) mp.AddLinearConstraint( state_over_time[i + 1, 1] == state_over_time[i, 1] + next_state_change[1] * dt + np.cos(u_over_time[i, 0] / 100.)).evaluator().set_description('dynamics[1]' + str(i)) mp.AddConstraint( state_over_time[i + 1, 2] == state_over_time[i, 2] + next_state_change[2] * dt + np.sin(state_over_time[i, 0] / 100.)).evaluator().set_description('dynamics[2]' + str(i)) mp.AddConstraint(state_over_time[i + 1, 3] == state_over_time[i, 3] + next_state_change[3] * dt).evaluator().set_description('dynamics[3]' + str(i)) #Add a quadratic cost for the amount of fuel used over the course of the trajectory fuel_cost_gain = 100.0 mp.AddQuadraticCost(fuel_cost_gain * u_over_time[:, 0].dot(u_over_time[:, 0])) mp.AddQuadraticCost(fuel_cost_gain * u_over_time[:, 1].dot(u_over_time[:, 1])) #constrain final states world_2 = self.world_2_position mp.AddConstraint( (state_over_time[-1, 2]**2 + state_over_time[-1, 3]**2) <= 1.0).evaluator().set_description( 'speed constraint') #final vel < 1m/s mp.AddConstraint(((state_over_time[-1, 0] - world_2[0])**2 + (state_over_time[-1, 1] - world_2[1])**2) <= .5**2 ).evaluator().set_description('distance constraint') #constrain system to not stray to far from planets world_1 = self.world_1_position dist_between_planets = (world_1[0] - world_2[0])**2 + (world_1[1] - world_2[1])**2 kp = 5.0 for i in range(num_time_steps): mp.AddConstraint(((state_over_time[i, 0] - world_2[0])**2 + (state_over_time[i, 1] - world_2[1])**2) <= dist_between_planets * kp) result = Solve(mp) trajectory = result.GetSolution(state_over_time) input_trajectory = result.GetSolution(u_over_time) print(result.is_success()) if not result.is_success(): infeasible = GetInfeasibleConstraints(mp, result) print "Infeasible constraints:" for i in range(len(infeasible)): print infeasible[i] return trajectory, input_trajectory, time_array