def find_feasible_latents(self, observed): # Build an optimization to estimate the hidden variables try: prog = MathematicalProgram() # Add in all the appropriate variables with their bounds all_vars = self.prices[0].GetVariables() for price in self.prices[1:]: all_vars += price.GetVariables() mp_vars = prog.NewContinuousVariables(len(all_vars)) subs_dict = {} for v, mv in zip(all_vars, mp_vars): subs_dict[v] = mv lb = [] ub = [] prog.AddBoundingBoxConstraint(0., 1., mp_vars) prices_mp = [ self.prices[k].Substitute(subs_dict) for k in range(12) ] # Add the observation constraint for k, val in enumerate(observed[1:]): if val != 0: prog.AddConstraint(prices_mp[k] >= val - 2.) prog.AddConstraint(prices_mp[k] <= val + 2) # Find lower bounds prog.AddCost(sum(prices_mp)) solver = SnoptSolver() result = solver.Solve(prog) if result.is_success(): lb = [result.GetSolution(x).Evaluate() for x in prices_mp] lb_vars = result.GetSolution(mp_vars) # Find upper bound too prog.AddCost(-2. * sum(prices_mp)) result = solver.Solve(prog) if result.is_success(): ub_vars = result.GetSolution(mp_vars) ub = [result.GetSolution(x).Evaluate() for x in prices_mp] self.price_ub = ub self.price_lb = lb subs_dict = {} for k, v in enumerate(all_vars): if lb_vars[k] == ub_vars[k]: subs_dict[v] = lb_vars[k] else: new_var = sym.Variable( "feasible_%d" % k, sym.Variable.Type.RANDOM_UNIFORM) subs_dict[v] = new_var * (ub_vars[k] - lb_vars[k]) + lb_vars[k] self.prices = [ self.prices[k].Substitute(subs_dict) for k in range(12) ] return except RuntimeError as e: print("Runtime error: ", e) self.rollout_probability = 0.
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 create_qp1(self, plant_context, V, q_des, v_des, vd_des): # Determine contact points contact_positions_per_frame = {} active_contacts_per_frame = {} # Note this should be in frame space for frame, contacts in self.contacts_per_frame.items(): contact_positions = self.plant.CalcPointsPositions( plant_context, self.plant.GetFrameByName(frame), contacts, self.plant.world_frame()) active_contacts_per_frame[ frame] = contacts[:, np.where(contact_positions[2, :] <= 0.0)[0]] N_c = sum([ active_contacts.shape[1] for active_contacts in active_contacts_per_frame.values() ]) # num contact points if N_c == 0: print("Not in contact!") return None ''' Eq(7) ''' H = self.plant.CalcMassMatrixViaInverseDynamics(plant_context) # Note that CalcGravityGeneralizedForces assumes the form Mv̇ + C(q, v)v = tau_g(q) + tau_app # while Eq(7) assumes gravity is accounted in C (on the left hand side) C_7 = self.plant.CalcBiasTerm( plant_context) - self.plant.CalcGravityGeneralizedForces( plant_context) B_7 = self.B_7 # TODO: Double check Phi_foots = [] for frame, active_contacts in active_contacts_per_frame.items(): if active_contacts.size: Phi_foots.append( self.plant.CalcJacobianTranslationalVelocity( plant_context, JacobianWrtVariable.kV, self.plant.GetFrameByName(frame), active_contacts, self.plant.world_frame(), self.plant.world_frame())) Phi = np.vstack(Phi_foots) ''' Eq(8) ''' v_idx_act = self.v_idx_act H_f = H[0:v_idx_act, :] H_a = H[v_idx_act:, :] C_f = C_7[0:v_idx_act] C_a = C_7[v_idx_act:] B_a = self.B_a Phi_f_T = Phi.T[0:v_idx_act:, :] Phi_a_T = Phi.T[v_idx_act:, :] ''' Eq(9) ''' # Assume flat ground for now n = np.array([[0], [0], [1.0]]) d = np.array([[1.0, -1.0, 0.0, 0.0], [0.0, 0.0, 1.0, -1.0], [0.0, 0.0, 0.0, 0.0]]) v = np.zeros((N_d, N_c, N_f)) for i in range(N_d): for j in range(N_c): v[i, j] = (n + mu * d)[:, i] ''' Quadratic Program I ''' prog = MathematicalProgram() qdd = prog.NewContinuousVariables( self.plant.num_velocities(), name="qdd") # To ignore 6 DOF floating base self.qdd = qdd beta = prog.NewContinuousVariables(N_d, N_c, name="beta") self.beta = beta lambd = prog.NewContinuousVariables(N_f * N_c, name="lambda") self.lambd = lambd # Jacobians ignoring the 6DOF floating base J_foots = [] for frame, active_contacts in active_contacts_per_frame.items(): if active_contacts.size: num_active_contacts = active_contacts.shape[1] J_foot = np.zeros((N_f * num_active_contacts, Atlas.TOTAL_DOF)) # TODO: Can this be simplified? for i in range(num_active_contacts): J_foot[N_f * i:N_f * (i + 1), :] = self.plant.CalcJacobianSpatialVelocity( plant_context, JacobianWrtVariable.kV, self.plant.GetFrameByName(frame), active_contacts[:, i], self.plant.world_frame(), self.plant.world_frame())[3:] J_foots.append(J_foot) J = np.vstack(J_foots) assert (J.shape == (N_c * N_f, Atlas.TOTAL_DOF)) eta = prog.NewContinuousVariables(J.shape[0], name="eta") self.eta = eta x = prog.NewContinuousVariables( self.x_size, name="x") # x_com, y_com, x_com_d, y_com_d self.x = x u = prog.NewContinuousVariables(self.u_size, name="u") # x_com_dd, y_com_dd self.u = u ''' Eq(10) ''' w = 0.01 epsilon = 1.0e-8 K_p = 10.0 K_d = 4.0 frame_weights = np.ones((Atlas.TOTAL_DOF)) q = self.plant.GetPositions(plant_context) qd = self.plant.GetVelocities(plant_context) # Convert q, q_nom to generalized velocities form q_err = self.plant.MapQDotToVelocity(plant_context, q_des - q) # print(f"Pelvis error: {q_err[0:3]}") ## FIXME: Not sure if it's a good idea to ignore the x, y, z position of pelvis # ignored_pose_indices = {3, 4, 5} # Ignore x position, y position ignored_pose_indices = {} # Ignore x position, y position relevant_pose_indices = list( set(range(Atlas.TOTAL_DOF)) - set(ignored_pose_indices)) self.relevant_pose_indices = relevant_pose_indices qdd_ref = K_p * q_err + K_d * (v_des - qd) + vd_des # Eq(27) of [1] qdd_err = qdd_ref - qdd qdd_err = qdd_err * frame_weights qdd_err = qdd_err[relevant_pose_indices] prog.AddCost( V(x, u) + w * ((qdd_err).dot(qdd_err)) + epsilon * np.sum(np.square(beta)) + eta.dot(eta)) ''' Eq(11) - 0.003s ''' eq11_lhs = H_f.dot(qdd) + C_f eq11_rhs = Phi_f_T.dot(lambd) prog.AddLinearConstraint(eq(eq11_lhs, eq11_rhs)) ''' Eq(12) - 0.005s ''' alpha = 0.1 # TODO: Double check Jd_qd_foots = [] for frame, active_contacts in active_contacts_per_frame.items(): if active_contacts.size: Jd_qd_foot = self.plant.CalcBiasTranslationalAcceleration( plant_context, JacobianWrtVariable.kV, self.plant.GetFrameByName(frame), active_contacts, self.plant.world_frame(), self.plant.world_frame()) Jd_qd_foots.append(Jd_qd_foot.flatten()) Jd_qd = np.concatenate(Jd_qd_foots) assert (Jd_qd.shape == (N_c * 3, )) eq12_lhs = J.dot(qdd) + Jd_qd eq12_rhs = -alpha * J.dot(qd) + eta prog.AddLinearConstraint(eq(eq12_lhs, eq12_rhs)) ''' Eq(13) - 0.015s ''' def tau(qdd, lambd): return self.B_a_inv.dot(H_a.dot(qdd) + C_a - Phi_a_T.dot(lambd)) self.tau = tau eq13_lhs = self.tau(qdd, lambd) prog.AddLinearConstraint(ge(eq13_lhs, -self.sorted_max_efforts)) prog.AddLinearConstraint(le(eq13_lhs, self.sorted_max_efforts)) ''' Eq(14) ''' for j in range(N_c): beta_v = beta[:, j].dot(v[:, j]) prog.AddLinearConstraint(eq(lambd[N_f * j:N_f * j + 3], beta_v)) ''' Eq(15) ''' for b in beta.flat: prog.AddLinearConstraint(b >= 0.0) ''' Eq(16) ''' prog.AddLinearConstraint(ge(eta, eta_min)) prog.AddLinearConstraint(le(eta, eta_max)) ''' Enforce x as com ''' com = self.plant.CalcCenterOfMassPosition(plant_context) com_d = self.plant.CalcJacobianCenterOfMassTranslationalVelocity( plant_context, JacobianWrtVariable.kV, self.plant.world_frame(), self.plant.world_frame()).dot(qd) prog.AddLinearConstraint(x[0] == com[0]) prog.AddLinearConstraint(x[1] == com[1]) prog.AddLinearConstraint(x[2] == com_d[0]) prog.AddLinearConstraint(x[3] == com_d[1]) ''' Enforce u as com_dd ''' com_dd = ( self.plant.CalcBiasCenterOfMassTranslationalAcceleration( plant_context, JacobianWrtVariable.kV, self.plant.world_frame(), self.plant.world_frame()) + self.plant.CalcJacobianCenterOfMassTranslationalVelocity( plant_context, JacobianWrtVariable.kV, self.plant.world_frame(), self.plant.world_frame()).dot(qdd)) prog.AddLinearConstraint(u[0] == com_dd[0]) prog.AddLinearConstraint(u[1] == com_dd[1]) ''' Respect joint limits ''' for name, limit in Atlas.JOINT_LIMITS.items(): # Get the corresponding joint value joint_pos = self.plant.GetJointByName(name).get_angle( plant_context) # Get the corresponding actuator index act_idx = getActuatorIndex(self.plant, name) # Use the actuator index to find the corresponding generalized coordinate index # q_idx = np.where(B_7[:,act_idx] == 1)[0][0] q_idx = getJointIndexInGeneralizedVelocities(self.plant, name) if joint_pos >= limit.upper: # print(f"Joint {name} max reached") prog.AddLinearConstraint(qdd[q_idx] <= 0.0) elif joint_pos <= limit.lower: # print(f"Joint {name} min reached") prog.AddLinearConstraint(qdd[q_idx] >= 0.0) return prog
# Print the newly added cost print(cost_1) # The newly added cost is stored in prog.linear_costs() print(prog.linear_costs()[0]) cost_2 = prog.AddLinearCost(2 * x[1] + 3) print(f"number of linear cost objects: {len(prog.linear_costs())}") # Can also add costs in a vector coefficient form cost_3 = prog.AddLinearCost([3., 4.], 5.0, x) print(cost_3) # Can also just call the generic "AddCost". # If drake thinks its linear, gets added to linear costs list cost_4 = prog.AddCost(x[0] + 3 * x[1] + 5) print(f"Number of linear cost objects after calling AddCost: {len(prog.linear_costs())}") # New program, now with constraints 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)
# WARNING: If you return a scalar for a constraint, or a vector for # a cost, you may get the following cryptic error: # "Unable to cast Python instance to c++ type" link_7_distance_to_target_vector = lambda q: [link_7_distance_to_target(q)] # New program: Using a custom evaluator 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()
Adapted from the MathematicalProgram tutorial on the Drake website: https://drake.mit.edu """ from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve import numpy as np import matplotlib.pyplot as plt # Create empty MathematicalProgram prog = MathematicalProgram() # Add 2 continuous decision variables # x is a numpy array - we can use an optional second argument to name the variables x = prog.NewContinuousVariables(2) #print(x) prog.AddConstraint(x[0] + x[1] == 1) prog.AddConstraint(x[0] <= x[1]) prog.AddCost(x[0]**2 + x[1]**2) # Make and add a visualization callback fig = plt.figure() curve_x = np.linspace(1, 10, 100) ax = plt.gca() ax.plot(curve_x, 9. / curve_x) ax.plot(-curve_x, -9. / curve_x) ax.plot(0, 0, 'o') x_init = [4., 5.] point_x, = ax.plot(x_init[0], x_init[1], 'x') ax.axis('equal') def update(x): global iter_count
def compute_input(self, x, xd, initial_guess=None): prog = MathematicalProgram() # Joint configuration states & Contact forces q = prog.NewContinuousVariables(rows=self.T + 1, cols=self.nq, name='q') v = prog.NewContinuousVariables(rows=self.T + 1, cols=self.nq, name='v') u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u') contact = prog.NewContinuousVariables(rows=self.T, cols=self.nf, name='lambda1') z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z') # Add Initial Condition Constraint prog.AddConstraint(eq(q[0], np.array(x[0:3]))) prog.AddConstraint(eq(v[0], np.array(x[3:6]))) # Add Final Condition Constraint prog.AddConstraint(eq(q[self.T], np.array(xd[0:3]))) prog.AddConstraint(eq(v[self.T], np.array(xd[3:6]))) prog.AddConstraint(z[0, 0] == 0) prog.AddConstraint(z[0, 1] == 0) # Add Dynamics Constraints for t in range(self.T): # Add Dynamics Constraints prog.AddConstraint( eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1]))) prog.AddConstraint(v[t + 1, 0] == ( v[t, 0] + self.sim.params['h'] * (-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0]))) prog.AddConstraint(v[t + 1, 1] == (v[t, 1] + self.sim.params['h'] * (-self.sim.params['c'] * v[t, 1] + contact[t, 0] - contact[t, 1]))) prog.AddConstraint(v[t + 1, 2] == ( v[t, 2] + self.sim.params['h'] * (-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1]))) # Add Contact Constraints with big M = self.contact prog.AddConstraint(ge(contact[t], 0)) prog.AddConstraint(contact[t, 0] + self.sim.params['k'] * (q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0) prog.AddConstraint(contact[t, 1] + self.sim.params['k'] * (q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0) # Mixed Integer Constraints M = self.contact_max prog.AddConstraint(contact[t, 0] <= M) prog.AddConstraint(contact[t, 1] <= M) prog.AddConstraint(contact[t, 0] <= M * z[t, 0]) prog.AddConstraint(contact[t, 1] <= M * z[t, 1]) prog.AddConstraint( contact[t, 0] + self.sim.params['k'] * (q[t, 1] - q[t, 0] - self.sim.params['d']) <= M * (1 - z[t, 0])) prog.AddConstraint( contact[t, 1] + self.sim.params['k'] * (q[t, 2] - q[t, 1] - self.sim.params['d']) <= M * (1 - z[t, 1])) prog.AddConstraint(z[t, 0] + z[t, 1] == 1) # Add Input Constraints. Contact Constraints already enforced in big-M # prog.AddConstraint(le(u[t], self.input_max)) # prog.AddConstraint(ge(u[t], -self.input_max)) # Add Costs prog.AddCost(u[t].dot(u[t])) # Set Initial Guess as empty. Otherwise, start from last solver iteration. if (type(initial_guess) == type(None)): initial_guess = np.empty(prog.num_vars()) # Populate initial guess by linearly interpolating between initial # and final states #qinit = np.linspace(x[0:3], xd[0:3], self.T + 1) qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1)) vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1)) uinit = np.tile(np.array([0, 0]), (self.T, 1)) finit = np.tile(np.array([0, 0]), (self.T, 1)) prog.SetDecisionVariableValueInVector(q, qinit, initial_guess) prog.SetDecisionVariableValueInVector(v, vinit, initial_guess) prog.SetDecisionVariableValueInVector(u, uinit, initial_guess) prog.SetDecisionVariableValueInVector(contact, finit, initial_guess) # Solve the program if (self.solver == "ipopt"): solver_id = IpoptSolver().solver_id() elif (self.solver == "snopt"): solver_id = SnoptSolver().solver_id() elif (self.solver == "osqp"): solver_id = OsqpSolver().solver_id() elif (self.solver == "mosek"): solver_id = MosekSolver().solver_id() elif (self.solver == "gurobi"): solver_id = GurobiSolver().solver_id() solver = MixedIntegerBranchAndBound(prog, solver_id) #result = solver.Solve(prog, initial_guess) result = solver.Solve() if result != result.kSolutionFound: raise ValueError('Infeasible optimization problem') sol = result.GetSolution() q_opt = result.GetSolution(q) v_opt = result.GetSolution(v) u_opt = result.GetSolution(u) f_opt = result.GetSolution(contact) return sol, q_opt, v_opt, u_opt, f_opt
current_state, current_tau234, is_symbolic=False) diff = desired_state - next_state ret = np.zeros_like(stacked) ret[0:STATE_SIZE] = diff return ret stacked = np.concatenate([next_state, state_over_time[i], tau234]) bounds = np.ones(stacked.shape) * DYNAMICS_EPSILON mp.AddConstraint(next_state_constraint, -bounds, bounds, stacked).evaluator().set_description( "Constrain next_state_constraint %d" % i) tau234_over_time[i] = tau234 state_over_time[i + 1] = next_state mp.AddCost(0.1 * tau234_over_time[:, 0].dot(tau234_over_time[:, 0])) mp.AddCost(0.1 * tau234_over_time[:, 1].dot(tau234_over_time[:, 1])) mp.AddCost(0.1 * tau234_over_time[:, 2].dot(tau234_over_time[:, 2])) # This cost is incorrect, different case for if front wheel is left / right of back wheel # mp.AddCost(-(state_over_time[:,0].dot(state_over_time[:,0]))) final_state = state_over_time[-1] # 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])
def compute_input(self, x, xd, initial_guess=None, tol=0.0): prog = MathematicalProgram() # Joint configuration states & Contact forces q = prog.NewContinuousVariables(rows=self.T + 1, cols=self.nq, name='q') v = prog.NewContinuousVariables(rows=self.T + 1, cols=self.nq, name='v') u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u') contact = prog.NewContinuousVariables(rows=self.T, cols=self.nf, name='lambda') # alpha = prog.NewContinuousVariables(rows=self.T, cols=2, name='alpha') beta = prog.NewContinuousVariables(rows=self.T, cols=2, name='beta') # Add Initial Condition Constraint prog.AddConstraint(eq(q[0], np.array(x[0:3]))) prog.AddConstraint(eq(v[0], np.array(x[3:6]))) # Add Final Condition Constraint prog.AddConstraint(eq(q[self.T], np.array(xd[0:3]))) prog.AddConstraint(eq(v[self.T], np.array(xd[3:6]))) # Add Dynamics Constraints for t in range(self.T): # Add Dynamics Constraints prog.AddConstraint( eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1]))) prog.AddConstraint(v[t + 1, 0] == ( v[t, 0] + self.sim.params['h'] * (-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0]))) prog.AddConstraint(v[t + 1, 1] == (v[t, 1] + self.sim.params['h'] * (-self.sim.params['c'] * v[t, 1] + contact[t, 0] - contact[t, 1]))) prog.AddConstraint(v[t + 1, 2] == ( v[t, 2] + self.sim.params['h'] * (-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1]))) # Add Contact Constraints prog.AddConstraint(ge(alpha[t], 0)) prog.AddConstraint(ge(beta[t], 0)) prog.AddConstraint(alpha[t, 0] == contact[t, 0]) prog.AddConstraint(alpha[t, 1] == contact[t, 1]) prog.AddConstraint( beta[t, 0] == (contact[t, 0] + self.sim.params['k'] * (q[t, 1] - q[t, 0] - self.sim.params['d']))) prog.AddConstraint( beta[t, 1] == (contact[t, 1] + self.sim.params['k'] * (q[t, 2] - q[t, 1] - self.sim.params['d']))) # Complementarity constraints. Start with relaxed version and start constraining. prog.AddConstraint(alpha[t, 0] * beta[t, 0] <= tol) prog.AddConstraint(alpha[t, 1] * beta[t, 1] <= tol) # Add Input Constraints and Contact Constraints prog.AddConstraint(le(contact[t], self.contact_max)) prog.AddConstraint(ge(contact[t], -self.contact_max)) prog.AddConstraint(le(u[t], self.input_max)) prog.AddConstraint(ge(u[t], -self.input_max)) # Add Costs prog.AddCost(u[t].dot(u[t])) # Set Initial Guess as empty. Otherwise, start from last solver iteration. if (type(initial_guess) == type(None)): initial_guess = np.empty(prog.num_vars()) # Populate initial guess by linearly interpolating between initial # and final states #qinit = np.linspace(x[0:3], xd[0:3], self.T + 1) qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1)) vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1)) uinit = np.tile(np.array([0, 0]), (self.T, 1)) finit = np.tile(np.array([0, 0]), (self.T, 1)) prog.SetDecisionVariableValueInVector(q, qinit, initial_guess) prog.SetDecisionVariableValueInVector(v, vinit, initial_guess) prog.SetDecisionVariableValueInVector(u, uinit, initial_guess) prog.SetDecisionVariableValueInVector(contact, finit, initial_guess) # Solve the program if (self.solver == "ipopt"): solver = IpoptSolver() elif (self.solver == "snopt"): solver = SnoptSolver() result = solver.Solve(prog, initial_guess) if (self.solver == "ipopt"): print("Ipopt Solver Status: ", result.get_solver_details().status, ", meaning ", result.get_solver_details().ConvertStatusToString()) elif (self.solver == "snopt"): val = result.get_solver_details().info status = self.snopt_status(val) print("Snopt Solver Status: ", result.get_solver_details().info, ", meaning ", status) sol = result.GetSolution() q_opt = result.GetSolution(q) v_opt = result.GetSolution(v) u_opt = result.GetSolution(u) f_opt = result.GetSolution(contact) return sol, q_opt, v_opt, u_opt, f_opt
# Do forward kinematics plant.SetPositions(context, iiwa, q) X_WL7 = plant.CalcRelativeTransform(context, resolve_frame(plant, W), resolve_frame(plant, L7)) p_TL7 = X_WL7.translation() - p_WT # Return scalar squared distance return p_TL7.dot(p_TL7) # NOTE: Returning a scalar for a constraint or a vector for a cost could result in cryptic errors # Create a mathematical program prog = MathematicalProgram() q = prog.NewContinuousVariables(plant_f.num_positions()) # define nominal configuration q0 = np.zeros(plant_f.num_positions()) # Add the custom cost prog.AddCost(link_7_distance_to_target, vars=q, description="IK_Cost") # Solve the problem CheckProgram(prog) print('Start optimization') result = Solve(prog, initial_guess=q0) # Number of autodiff and float calls during optimization print(f"Number float evaluations: {float_calls}") print(f"Number of autodiff evaluations: {autodiff_calls}") print(f"Success? {result.is_success()}") print(result.get_solution_result()) qsol = result.GetSolution(q) print(qsol) print(f"Initial distance to target: {link_7_distance_to_target(q0):.3f}") print(f"Final distance to target: {link_7_distance_to_target(qsol):.3f}")