def findMaxForce(theta1, theta2, theta3): theta12 = theta2 + theta1 theta123 = theta3 + theta12 s1 = sin(theta1) s2 = sin(theta2) s3 = sin(theta3) s12 = sin(theta12) s123 = sin(theta123) c1 = cos(theta1) c2 = cos(theta2) c3 = cos(theta3) c12 = cos(theta12) c123 = cos(theta123) J = np.array([[ -l_1 * s1 - l_2 * s12 - l_3 * s123, -l_2 * s12 - l_3 * s123, -l_3 * s123 ], [l_1 * c1 + l_2 * c12 + l_3 * c123, l_2 * c12 + l_3 * c123, l_3 * c123]]) tau1 = ( -l_1 * c1 * m_2 * g # torque due to l_1 - l_2 link motor + (-l_1 * c1 + l_2 / 2.0 * c12) * m_m * g # torque due to battery link motor + (-l_1 * c1 + l_2 / 2.0 * c12 + l_b * cos(theta12 + np.pi / 2.0)) * m_b * g # torque due to battery + (-l_1 * c1 + l_2) * m_3 * g # torque due to l_2 - l_3 link motor + (-l_1 * c1 + l_2 + l_3 * c3) * m_w * g # torque due to front wheel ) # print("tau1 = " + str(tau1)) mp = MathematicalProgram() tau23 = mp.NewContinuousVariables(2, "tau23") tau123 = np.append(tau1, tau23) mp.AddCost(J.dot(tau123)[0]) mp.AddConstraint(tau23[0]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2) mp.AddConstraint(tau23[1]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2) mp.AddLinearConstraint(J.dot(tau123)[1] >= -HUB_MOTOR_FORCE / 2.0) result = Solve(mp) is_success = result.is_success() torques = result.GetSolution(tau23) full_tau = np.append(tau1, torques) output_force = J.dot(full_tau) # print("is_success = " + str(is_success)) # print("tau = " + str(full_tau)) # print("F = " + str(J.dot(full_tau))) return is_success, output_force, torques
def minimize_height(diagram_f, plant_f, d_context_f, frame_list): """Fragile and slow :(""" context_f = plant_f.GetMyContextFromRoot(d_context_f) diagram_ad = diagram_f.ToAutoDiffXd() plant_ad = diagram_ad.GetSubsystemByName(plant_f.get_name()) d_context_ad = diagram_ad.CreateDefaultContext() d_context_ad.SetTimeStateAndParametersFrom(d_context_f) context_ad = plant_ad.GetMyContextFromRoot(d_context_ad) def prepare_plant_and_context(z): if z.dtype == float: plant, context = plant_f, context_f else: plant, context = plant_ad, context_ad set_frame_heights(plant, context, frame_list, z) return plant, context prog = MathematicalProgram() num_z = len(frame_list) z_vars = prog.NewContinuousVariables(num_z, "z") q0 = plant_f.GetPositions(context_f) z0 = get_frame_heights(plant_f, context_f, frame_list) cost = prog.AddCost(np.sum(z_vars)) prog.AddBoundingBoxConstraint([0.01] * num_z, [5.] * num_z, z_vars) # # N.B. Cannot use AutoDiffXd due to cylinders. distance = MinimumDistanceConstraint(plant=plant_f, plant_context=context_f, minimum_distance=0.05) def distance_with_z(z): plant, context = prepare_plant_and_context(z) q = plant.GetPositions(context) return distance.Eval(q) prog.AddConstraint(distance_with_z, lb=distance.lower_bound(), ub=distance.upper_bound(), vars=z_vars) result = Solve(prog, initial_guess=z0) assert result.is_success() z = result.GetSolution(z_vars) set_frame_heights(plant_f, context_f, frame_list, z) q = plant_f.GetPositions(context_f) return q
def main(): parser = ArgumentParser() parser.add_argument('--datapath', default='../out/data.npy') parser.add_argument('--normalize', dest='normalize', action='store_true') parser.add_argument('--no-normalize', dest='normalize', action='store_false') parser.set_defaults(normalize=True) opts = parser.parse_args() data = dynamics.unmarshal_data(np.load(opts.datapath)) n = data.poslambdas.shape[0] lambdas = np.vstack((data.poslambdas, data.neglambdas, data.gammas)) ds = np.vstack((data.xdots, data.us, np.ones(data.xdots.shape))) mp = MathematicalProgram() M = mp.NewContinuousVariables(3, 3, "M") W = mp.NewContinuousVariables(3, 3, "W") W[0, 2] = 0 W[1, 2] = 0 R = M.dot(lambdas) + W.dot(ds) elementwise_positive_constraint(mp, R) lambdas_vec = np.expand_dims(np.ndarray.flatten(lambdas), 0) R_vec = np.ndarray.flatten(R) mp.AddLinearCost(lambdas_vec.dot(R_vec)[0]) result = Solve(mp) print(result.is_success()) Msol = result.GetSolution(M) # When mix 0's in with W, need to evaluate expression evaluator = np.vectorize(lambda x: x.Evaluate()) Wsol = evaluator(result.GetSolution(W)) print(Msol) print(Wsol)
def calcTorqueOutput(self, context, output): q_v = self.EvalVectorInput(context, self.input_q_v_idx).get_value() if self.is_wbc: r = self.EvalVectorInput(context, self.input_r_des_idx).get_value() rd = self.EvalVectorInput(context, self.input_rd_des_idx).get_value() rdd = self.EvalVectorInput(context, self.input_rdd_des_idx).get_value() V = lambda x, u: self.V_full(x, u, r, rd, rdd) q_des = self.EvalVectorInput(context, self.input_q_des_idx).get_value() v_des = self.EvalVectorInput(context, self.input_v_des_idx).get_value() vd_des = self.EvalVectorInput(context, self.input_vd_des_idx).get_value() else: y_des = self.EvalVectorInput(context, self.input_y_des_idx).get_value() V = lambda x, u: self.V_full(x, u, y_des) q_des = self.q_nom v_des = [0.0] * self.plant.num_velocities() vd_des = [0.0] * self.plant.num_velocities() current_plant_context = self.plant.CreateDefaultContext() self.plant.SetPositionsAndVelocities(current_plant_context, q_v) start_formulate_time = time.time() prog = self.create_qp1(current_plant_context, V, q_des, v_des, vd_des) if not prog: print("Invalid program!") output.SetFromVector([0] * self.plant.num_actuated_dofs()) return # print(f"Formulate time: {time.time() - start_formulate_time}s") start_solve_time = time.time() result = Solve(prog) # print(f"Solve time: {time.time() - start_solve_time}s") if not result.is_success(): print(f"FAILED") output.SetFromVector([0] * self.plant.num_actuated_dofs()) # print(f"Cost: {result.get_optimal_cost()}") qdd_sol = result.GetSolution(self.qdd) lambd_sol = result.GetSolution(self.lambd) x_sol = result.GetSolution(self.x) u_sol = result.GetSolution(self.u) beta_sol = result.GetSolution(self.beta) eta_sol = result.GetSolution(self.eta) tau = self.tau(qdd_sol, lambd_sol) output.SetFromVector(tau)
def generate(self): self.result = Solve(self.prog) assert (self.result.is_success())
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())
prog = MathematicalProgram() x = prog.NewContinuousVariables(2, "x") y = prog.NewContinuousVariables(3, "y") bounding_box = prog.AddLinearConstraint(x[1] <= 2) linear_eq = prog.AddLinearConstraint(x[1] + 2 * y[2] == 1) linear_ineq = prog.AddLinearConstraint(x[1] + 4 * y[1] >= 2) # New program, all the way to solving... prog = MathematicalProgram() # Declare x as decision variables. x = prog.NewContinuousVariables(4) # Add linear costs. To show that calling AddLinearCosts results in the sum of each individual # cost, we add two costs -3x[0] - x[1] and -5x[2]-x[3]+2 prog.AddLinearCost(-3*x[0] -x[1]) prog.AddLinearCost(-5*x[2] - x[3] + 2) # Add linear equality constraint 3x[0] + x[1] + 2x[2] == 30 prog.AddLinearConstraint(3*x[0] + x[1] + 2*x[2] == 30) # Add Linear inequality constraints prog.AddLinearConstraint(2*x[0] + x[1] + 3*x[2] + x[3] >= 15) prog.AddLinearConstraint(2*x[1] + 3*x[3] <= 25) # Add linear inequality constraint -100 <= x[0] + 2x[2] <= 40 prog.AddLinearConstraint(A=[[1., 2.]], lb=[-100], ub=[40], vars=[x[0], x[2]]) prog.AddBoundingBoxConstraint(0, np.inf, x) prog.AddLinearConstraint(x[1] <= 10) # Now solve the program. result = Solve(prog) print(f"Is solved successfully: {result.is_success()}") print(f"x optimal value: {result.GetSolution(x)}") print(f"optimal cost: {result.get_optimal_cost()}")
def generate(self): self.result = Solve(self.prog) assert(self.result.is_success())
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())
size=3)).ToQuaternion())) # Project to nonpenetration to seed sim q0 = mbp.GetPositions(mbp_context).copy() ik = InverseKinematics(mbp, mbp_context) q_dec = ik.q() prog = ik.prog() constraint = ik.AddMinimumDistanceConstraint(0.01) prog.AddQuadraticErrorCost(np.eye(q0.shape[0]) * 1.0, q0, q_dec) mbp.SetPositions(mbp_context, q0) prog.SetInitialGuess(q_dec, q0) print("Solving") print("Initial guess: ", q0) res = Solve(prog) #print(prog.GetSolverId().name()) q0_proj = res.GetSolution(q_dec) print("Final: ", q0_proj) mbp.SetPositions(mbp_context, q0_proj) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(1.0) try: simulator.AdvanceTo(2.0) except Exception as e: print("Exception in sim: ", e) scene_k -= 1
def __init__(self, A, B, C, K, T, Sigma, x0, x_lb, x_ub, check_states=[], noise_trunc=[]): n = A.shape[0] # n states if (len(check_states) == 0): check_states = range(0, n) if (len(noise_trunc) == 0): # set default to 3 std's noise_trunc = np.diag(Sigma)**(0.5) * 3. invsig = np.linalg.inv(Sigma) #Q = block_diag(*([invsig]*T)) # [n1,n2,...,nL]*[Q]*[n1;n2;...;nL] b = np.zeros((n, 1)) #n*T costs = [] eval_n_is = [] for prob_len in range(1, T): for bound in ['lo', 'hi']: for state in check_states: self.prog = MathematicalProgram() # the noise decision variables, we search for the first time it falsifies. # to not overconstrain the problem, if we are checking to falsify at t=i, # we do not want more constraints in i:T because maybe a better probability cost will be # that i,i+1,i+2 will go out of bounds, and then i+3 comes back, etc ... self.n_i = self.prog.NewContinuousVariables( n * prob_len, 'n') # the states as a function of time self.x = self.prog.NewIndeterminates(n * prob_len, 'x') # the maximum value allowed for a noise measurement (truncated normal distribution) for i, noise in enumerate(self.n_i): self.prog.AddConstraint(noise, -noise_trunc[i % n], noise_trunc[i % n]) # simulate the dynamics forward to obtain the constraints on the states # Drake will convert them to constraints on the decision variables x_t = x0.copy() for t in range(1, prob_len + 1): # which noise variables affect this step ind = np.arange((t - 1) * n, t * n) #import pdb; pdb.set_trace() x_t = (A - B * K.dot(C)).dot(x_t) - B.dot(K).dot( self.n_i[ind].reshape(n, -1)) # the falsifying constraint: if (prob_len == t): for state_i in range(n): dv_exist = (len(x_t[state_i][0].GetVariables()) > 0) if (state_i == state and dv_exist): if (bound == 'lo'): #print('adding a lower violation constraint state=%d t=%d' %(state_i, t)) # hits the lower bound self.prog.AddConstraint( x_t[state_i][0] <= x_lb[state_i][0] ) else: #print('adding an upper violation constraint state=%d t=%d' %(state_i, t)) # hits the upper bound self.prog.AddConstraint( x_t[state_i][0] >= x_ub[state_i][0] ) elif (state_i != state and dv_exist): # keep it within the bounds (satisfying) self.prog.AddConstraint( x_t[state_i][0] >= x_lb[state_i][0]) self.prog.AddConstraint( x_t[state_i][0] <= x_ub[state_i][0]) else: # dv_exist == False => # drake cannot get a constraint in the form of const <=> const pass else: for state_i in range(n): dv_exist = (len(x_t[state_i][0].GetVariables()) > 0) if (dv_exist): # the "box"/corridor constraint self.prog.AddConstraint( x_t[state_i][0], x_lb[state_i][0], x_ub[state_i][0]) # quadratic cost of the log likelihood self.prog.AddQuadraticCost(invsig, b, self.n_i[ind]) # solve the mathematical program result = Solve(self.prog) if (result.is_success()): eval_n_i = result.GetSolution(self.n_i) eval_n_is.append(eval_n_i.reshape(prob_len, n).T) # minus because the real cost is -1/2*x'Sx and for the prog I multiply by -1 to get max # this is the true multivariate pdf Q = block_diag( *([invsig] * prob_len)) # [n1,n2,...,nL]*[Q]*[n1;n2;...;nL] cost = (2.*np.pi)**(-(n*prob_len)/2.) * np.linalg.det(Q)**(-0.5) * \ np.exp(-0.5*eval_n_i.T.dot(Q).dot(eval_n_i)) costs.append(cost) print( 'found solution (lh/step/state/cost %s/%d/%d/%.2e)' % (bound, prob_len, state, cost)) else: print('nope :( (lh/step/state %s/%d/%d)' % (bound, prob_len, state)) costs = np.array(costs) if (len(costs) > 0): ind = np.argmax(costs) # the index step, the actual cost and the whole series of noises self.ind = ind self.max_cost = costs[ind] self.noise_seq = eval_n_is[ind] else: self.ind = -1 self.max_cost = -1 self.noise_seq = np.array([])
prog = MathematicalProgram() q = prog.NewContinuousVariables(plant_f.num_positions()) # Define nominal configuration q0 = np.zeros(plant_f.num_positions()) # Add basic cost. (This will be parsed into a quadratic cost) prog.AddCost((q - q0).dot(q - q0)) # Add constraint based on custom evaluator. prog.AddConstraint(link_7_distance_to_target_vector, lb=[0.1], ub=[0.2], vars=q) result = Solve(prog, initial_guess=q0) print(f"Success? {result.is_success()}") print(result.get_solution_result()) q_sol = result.GetSolution(q) print(q_sol) prog = MathematicalProgram() q = prog.NewContinuousVariables(plant_f.num_positions()) # Define nominal configuration. q0 = np.zeros(plant_f.num_positions()) # Add custom cost. prog.AddCost(link_7_distance_to_target, vars=q)
dircol.AddQuadraticCost(Q=np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]), b=np.array([1., 1., 1.]), c=1., vars=P[:3]) #Initial Trajectory initial_x_trajectory = PiecewisePolynomial.FirstOrderHold( [0., 20.], np.column_stack((initial_state, final_state))) print(initial_x_trajectory) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) #Solve the DirCol result = Solve(dircol) print("\n\n") print("THE FUNCTION RETURN") print(result.is_success()) print("\n\n") assert (result.is_success()) #Final Trajectory print("\n\n") print("THE STATE TRAJ PRINT") u_trajectory = dircol.ReconstructInputTrajectory(result) x_trajectory = dircol.ReconstructStateTrajectory(result) #For the input u times 100 times_input = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
point_x, = ax.plot(x_init[0], x_init[1], 'x') ax.axis('equal') def update(x): global iter_count point_x.set_xdata(x[0]) point_x.set_ydata(x[1]) ax.set_title(f"iteration {iter_count}") fig.canvas.draw() fig.canvas.flush_events() # Update iter_counter iter_count += 1 plt.pause(1.0) # Add a visualization callback - it does more than just visualization iter_count = 0 prog.AddVisualizationCallback(update, x) # Solve the optimization problem - optional second argument is initial guess. Third argument is solver parameters result = Solve(prog, x_init, None) # Print the result print("Success? ", result.is_success()) # Print the solution print("x* = ", result.GetSolution(x)) # Print the optimal cost print('optimal cost = ', result.get_optimal_cost()) # Print the name of the solver print('solver is: ', result.get_solver_id().name())
def plot_sublevelset_expression(ax, e, vertices=51, **kwargs): """ Plots the 2D sub-level set e(x) <= 1, which must contain the origin. Args: ax: the matplotlib axis to receive the plot e: a symbolic expression in two variables vertices: number of sample points along the boundary kwargs: are passed to the matplotlib fill method Returns: the return values from matplotlib's fill command. """ x = list(e.GetVariables()) assert len(x) == 2, "e must be an expression in two variables" # Handle the special case where e is a degree 2 polynomial. if e.is_polynomial(): p = Polynomial(e) if p.TotalDegree() == 2: env = {a: 0 for a in x} c = e.Evaluate(env) e1 = e.Jacobian(x) b = Evaluate(e1, env) e2 = Jacobian(e1, x) A = 0.5 * Evaluate(e2, env) return plot_sublevelset_quadratic(ax, A, b, c, vertices, **kwargs) # Find the level-set in polar coordinates, by sampling theta and # root-finding (on the scalar expression) to find a rplus and rminus. Xplus = np.empty((2, vertices)) Xminus = np.empty((2, vertices)) i = 0 for theta in np.linspace(0, np.pi, vertices): prog = MathematicalProgram() r = prog.NewContinuousVariables(1, "r")[0] env = {x[0]: r * np.cos(theta), x[1]: r * np.sin(theta)} scalar = e.Substitute(env) b = prog.AddBoundingBoxConstraint(0, np.inf, r) prog.AddConstraint(scalar == 1) prog.AddQuadraticCost([1], [0], [r]) prog.SetInitialGuess(r, 0.1) # or anything non-zero. result = Solve(prog) assert result.is_success(), "Failed to find the level set" rplus = result.GetSolution(r) Xplus[0, i] = rplus * np.cos(theta) Xplus[1, i] = rplus * np.sin(theta) b.evaluator().UpdateLowerBound([-np.inf]) b.evaluator().UpdateUpperBound([0]) prog.SetInitialGuess(r, -0.1) # or anything non-zero. result = Solve(prog) assert result.is_success(), "Failed to find the level set" rminus = result.GetSolution(r) Xminus[0, i] = rminus * np.cos(theta) Xminus[1, i] = rminus * np.sin(theta) i = i + 1 return ax.fill(np.hstack((Xplus[0, :], Xminus[0, :])), np.hstack((Xplus[1, :], Xminus[1, :])), **kwargs)
# Constrain final velocity to be 0 # for j in range(4, 8): # mp.AddConstraint(final_state[j] <= 0.0) # mp.AddConstraint(final_state[j] >= 0.0) # Constrain final front wheel position final_front_wheel_pos = eom.findFrontWheelPosition(final_state[0], final_state[1], final_state[2]) # mp.AddConstraint(final_front_wheel_pos[1] <= STEP_HEIGHT - w_r) mp.AddConstraint(final_front_wheel_pos[1] >= STEP_HEIGHT - w_r).evaluator( ).set_description("Constrain final_front_wheel_pos[1] >= 0.01") print("Begin solving...") t = time.time() result = Solve(mp) solve_traj_opt_duration = time.time() - t print("Done solving in " + str(solve_traj_opt_duration) + "s (" + str(solve_traj_opt_duration / 60.0) + "m)") is_success = result.is_success() print("is_success = " + str(is_success)) torque_over_time_star = result.GetSolution(tau234_over_time) state_over_time_star = result.GetSolution(state_over_time) time_step = 0 last_input = 'c' max_time_step = len(state_over_time_star) while time_step < max_time_step: state = state_over_time_star[time_step] torque = np.array( [i.Evaluate() for i in torque_over_time_star[time_step]])