def optimization(self, quad_plants, ball_plants, contexts): prog = MathematicalProgram() #Time steps likely requires a line search. #Find the number of time steps until a ball reaches a threshold so that a quadrotor can catch it. N = 200 #Number of time steps n_u = 2*len(plants) n_x = 6*len(quad_plants) + 4*len(ball_plants) n_quads = len(quad_plants) n_balls = len(ball_plants) u = np.empty((self.n_u, N-1), dtype=Variable) x = np.empty((self.n_x, N), dtype=Variable) I = 0.00383 r = 0.25 mass = 0.486 g = 9.81 #Add all the decision variables for n in range(N-1): u[:,n] = prog.NewContinuousVariables(self.n_u, 'u' + str(n)) x[:,n] = prog.NewContinuousVariables(self.n_x, 'x' + str(n)) x[:,N-1] = prog.NewContinuousVariables(self.n_x, 'x' + str(N)) #Start at the correct initial conditions #Connect quadrotor and ball state variables to the initial condition of mathprog x0 = np.empty((self.n_x,1)) for k in range(n_quads + n_balls): if k < n_quads: x0[6*k:6*k+5] = self.quad_plants[k] #??? prog.AddBoundingBoxConstraint(x0, x0, x[:,0]) for n in range(N-1): for k in range(n_quads + n_balls): #Handles no collisions if k < n_quads: #Quad dynamic constraints dynamics_constraint_vel = #? dynamics_constraint_acc = #? prog.AddConstraint(dynamics_constraint_vel[0, 0]) prog.AddConstraint(dynamics_constraint_vel[1, 0]) prog.AddConstraint(dynamics_constraint_vel[2, 0]) prog.AddConstraint(dynamics_constraint_acc[0, 0]) prog.AddConstraint(dynamics_constraint_acc[1, 0]) prog.AddConstraint(dynamics_constraint_acc[2, 0]) else: #Ball dynamic constraints dynamics_constraint_vel = # dynamics_constraint_acc = # prog.AddConstraint(dynamics_constraint_vel[0, 0]) prog.AddConstraint(dynamics_constraint_vel[1, 0]) prog.AddConstraint(dynamics_constraint_acc[0, 0]) prog.AddConstraint(dynamics_constraint_acc[1, 0])
def initialize_problem(n_x, n_u, N, x0): ''' returns base MathematicalProgram object and empty decision vars ''' prog = MathematicalProgram() # Decision Variables x = prog.NewContinuousVariables(n_x, N, 'x') u = prog.NewContinuousVariables(n_u, N-1, 'u') z = [] # placeholder for obstacle binary variables q = [] # placeholder for obstacle slack buffer variables # initial conditions constraints prog.AddBoundingBoxConstraint(x0, x0, x[:, 0]) decision_vars = [x, u, z, q] return prog, decision_vars
def __init__(self, rtree, q_nom, control_period=0.001): self.rtree = rtree self.nq = rtree.get_num_positions() self.nv = rtree.get_num_velocities() self.nu = rtree.get_num_actuators() dim = 3 # 3D nd = 4 # for friction cone approx, hard coded for now self.nc = 4 # number of contacts; TODO don't hard code self.nf = self.nc * nd # number of contact force variables self.ncf = 2 # number of loop constraint forces; TODO don't hard code self.neps = self.nc * dim # number of slack variables for contact self.q_nom = q_nom self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom)) self.u_des = CassieFixedPointTorque() self.lfoot = rtree.FindBody("toe_left") self.rfoot = rtree.FindBody("toe_right") self.springs = 2 + np.array([ rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"), rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right") ]) umin = np.zeros(self.nu) umax = np.zeros(self.nu) ii = 0 for motor in rtree.actuators: umin[ii] = motor.effort_limit_min umax[ii] = motor.effort_limit_max ii += 1 slack_limit = 10.0 self.initialized = False #------------------------------------------------------------ # Add Decision Variables ------------------------------------ prog = MathematicalProgram() qddot = prog.NewContinuousVariables(self.nq, "joint acceleration") u = prog.NewContinuousVariables(self.nu, "input") bar = prog.NewContinuousVariables(self.ncf, "four bar forces") beta = prog.NewContinuousVariables(self.nf, "friction forces") eps = prog.NewContinuousVariables(self.neps, "slack variable") nparams = prog.num_vars() #------------------------------------------------------------ # Problem Constraints --------------------------------------- self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax, u).evaluator() self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000, beta).evaluator() self.con_slack_lim = prog.AddBoundingBoxConstraint( -slack_limit, slack_limit, eps).evaluator() bar_con = np.zeros((self.ncf, self.nq)) self.con_4bar = prog.AddLinearEqualityConstraint( bar_con, np.zeros(self.ncf), qddot).evaluator() if self.nc > 0: dyn_con = np.zeros( (self.nq, self.nq + self.nu + self.ncf + self.nf)) dyn_vars = np.concatenate((qddot, u, bar, beta)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() foot_con = np.zeros((self.neps, self.nq + self.neps)) foot_vars = np.concatenate((qddot, eps)) self.con_foot = prog.AddLinearEqualityConstraint( foot_con, np.zeros(self.neps), foot_vars).evaluator() else: dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf)) dyn_vars = np.concatenate((qddot, u, bar)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() #------------------------------------------------------------ # Problem Costs --------------------------------------------- self.Kp_com = 50 self.Kd_com = 2.0 * sqrt(self.Kp_com) # self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H))) self.Kp_qdd = np.diag( np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6)))) self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd) self.Kp_qdd[self.springs, self.springs] = 0 self.Kd_qdd[self.springs, self.springs] = 0 com_ddot_des = np.zeros(dim) qddot_des = np.zeros(self.nq) self.w_com = 0 self.w_qdd = np.zeros(self.nq) self.w_qdd[:6] = 10 self.w_qdd[8:10] = 1 self.w_qdd[3:6] = 1000 # self.w_qdd[self.springs] = 0 self.w_u = 0.001 * np.ones(self.nu) self.w_u[2:4] = 0.01 self.w_slack = 0.1 self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd), qddot_des, qddot).evaluator() self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des, u).evaluator() self.cost_slack = prog.AddQuadraticErrorCost( self.w_slack * np.eye(self.neps), np.zeros(self.neps), eps).evaluator() # self.cost_com = prog.AddQuadraticCost().evaluator() # self.cost_qdd = prog.AddQuadraticCost( # 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator() # self.cost_u = prog.AddQuadraticCost( # 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator() # self.cost_slack = prog.AddQuadraticCost( # 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator() REG = 1e-8 self.cost_reg = prog.AddQuadraticErrorCost( REG * np.eye(nparams), np.zeros(nparams), prog.decision_variables()).evaluator() # self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams), # np.zeros(nparams), prog.decision_variables()).evaluator() #------------------------------------------------------------ # Solver settings ------------------------------------------- self.solver = GurobiSolver() # self.solver = OsqpSolver() prog.SetSolverOption(SolverType.kGurobi, "Method", 2) #------------------------------------------------------------ # Save MathProg Variables ----------------------------------- self.qddot = qddot self.u = u self.bar = bar self.beta = beta self.eps = eps self.prog = prog
def __init__(self, rtree, q_nom, control_period=0.001, print_period=1.0, sim=True, cost_pub=False): LeafSystem.__init__(self) self.rtree = rtree self.nq = rtree.get_num_positions() self.nv = rtree.get_num_velocities() self.nu = rtree.get_num_actuators() self.cost_pub = cost_pub dim = 3 # 3D nd = 4 # for friction cone approx, hard coded for now self.nc = 4 # number of contacts; TODO don't hard code self.nf = self.nc * nd # number of contact force variables self.ncf = 2 # number of loop constraint forces; TODO don't hard code self.neps = self.nc * dim # number of slack variables for contact if sim: self.sim = True self._DeclareInputPort(PortDataType.kVectorValued, self.nq + self.nv) self._DeclareDiscreteState(self.nu) self._DeclareVectorOutputPort(BasicVector(self.nu), self.CopyStateOutSim) self.print_period = print_period self.last_print_time = -print_period self.last_v = np.zeros(3) self.lcmgl = lcmgl("Balancing-plan", true_lcm.LCM()) # self.lcmgl = None else: self.sim = False self._DeclareInputPort(PortDataType.kVectorValued, 1) self._DeclareInputPort(PortDataType.kVectorValued, kCassiePositions) self._DeclareInputPort(PortDataType.kVectorValued, kCassieVelocities) self._DeclareInputPort(PortDataType.kVectorValued, 2) self._DeclareDiscreteState(kCassieActuators * 8 + 1) self._DeclareVectorOutputPort( BasicVector(1), lambda c, o: self.CopyStateOut(c, o, 8 * kCassieActuators, 1)) self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 0, kCassieActuators)) #torque self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, kCassieActuators, kCassieActuators)) #motor_pos self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 2 * kCassieActuators, kCassieActuators)) #motor_vel self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 3 * kCassieActuators, kCassieActuators)) #kp self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 4 * kCassieActuators, kCassieActuators)) #kd self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 5 * kCassieActuators, kCassieActuators)) #ki self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 6 * kCassieActuators, kCassieActuators)) #leak self._DeclareVectorOutputPort( BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut( c, o, 7 * kCassieActuators, kCassieActuators)) #clamp self._DeclarePeriodicDiscreteUpdate(0.0005) self.q_nom = q_nom self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom)) self.u_des = CassieFixedPointTorque() self.lfoot = rtree.FindBody("toe_left") self.rfoot = rtree.FindBody("toe_right") self.springs = 2 + np.array([ rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"), rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"), rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right") ]) umin = np.zeros(self.nu) umax = np.zeros(self.nu) ii = 0 for motor in rtree.actuators: umin[ii] = motor.effort_limit_min umax[ii] = motor.effort_limit_max ii += 1 slack_limit = 10.0 self.initialized = False #------------------------------------------------------------ # Add Decision Variables ------------------------------------ prog = MathematicalProgram() qddot = prog.NewContinuousVariables(self.nq, "joint acceleration") u = prog.NewContinuousVariables(self.nu, "input") bar = prog.NewContinuousVariables(self.ncf, "four bar forces") beta = prog.NewContinuousVariables(self.nf, "friction forces") eps = prog.NewContinuousVariables(self.neps, "slack variable") nparams = prog.num_vars() #------------------------------------------------------------ # Problem Constraints --------------------------------------- self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax, u).evaluator() self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000, beta).evaluator() self.con_slack_lim = prog.AddBoundingBoxConstraint( -slack_limit, slack_limit, eps).evaluator() bar_con = np.zeros((self.ncf, self.nq)) self.con_4bar = prog.AddLinearEqualityConstraint( bar_con, np.zeros(self.ncf), qddot).evaluator() if self.nc > 0: dyn_con = np.zeros( (self.nq, self.nq + self.nu + self.ncf + self.nf)) dyn_vars = np.concatenate((qddot, u, bar, beta)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() foot_con = np.zeros((self.neps, self.nq + self.neps)) foot_vars = np.concatenate((qddot, eps)) self.con_foot = prog.AddLinearEqualityConstraint( foot_con, np.zeros(self.neps), foot_vars).evaluator() else: dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf)) dyn_vars = np.concatenate((qddot, u, bar)) self.con_dyn = prog.AddLinearEqualityConstraint( dyn_con, np.zeros(self.nq), dyn_vars).evaluator() #------------------------------------------------------------ # Problem Costs --------------------------------------------- self.Kp_com = 50 self.Kd_com = 2.0 * sqrt(self.Kp_com) # self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H))) self.Kp_qdd = np.diag( np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6)))) self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd) self.Kp_qdd[self.springs, self.springs] = 0 self.Kd_qdd[self.springs, self.springs] = 0 com_ddot_des = np.zeros(dim) qddot_des = np.zeros(self.nq) self.w_com = 0 self.w_qdd = np.zeros(self.nq) self.w_qdd[:6] = 10 self.w_qdd[8:10] = 1 self.w_qdd[3:6] = 1000 # self.w_qdd[self.springs] = 0 self.w_u = 0.001 * np.ones(self.nu) self.w_u[2:4] = 0.01 self.w_slack = 0.1 self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd), qddot_des, qddot).evaluator() self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des, u).evaluator() self.cost_slack = prog.AddQuadraticErrorCost( self.w_slack * np.eye(self.neps), np.zeros(self.neps), eps).evaluator() # self.cost_com = prog.AddQuadraticCost().evaluator() # self.cost_qdd = prog.AddQuadraticCost( # 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator() # self.cost_u = prog.AddQuadraticCost( # 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator() # self.cost_slack = prog.AddQuadraticCost( # 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator() REG = 1e-8 self.cost_reg = prog.AddQuadraticErrorCost( REG * np.eye(nparams), np.zeros(nparams), prog.decision_variables()).evaluator() # self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams), # np.zeros(nparams), prog.decision_variables()).evaluator() #------------------------------------------------------------ # Solver settings ------------------------------------------- self.solver = GurobiSolver() # self.solver = OsqpSolver() prog.SetSolverOption(SolverType.kGurobi, "Method", 2) #------------------------------------------------------------ # Save MathProg Variables ----------------------------------- self.qddot = qddot self.u = u self.bar = bar self.beta = beta self.eps = eps self.prog = prog
# Create a mathematical program prog = MathematicalProgram() # Number of knot points N = 284 # Create decision variables u = np.empty((1, N - 1), dtype=Variable) x = np.empty((2, N), dtype=Variable) for n in range(0, N - 1): u[:, n] = prog.NewContinuousVariables(1, 'u' + str(n)) x[:, n] = prog.NewContinuousVariables(2, 'x' + str(n)) x[:, N - 1] = prog.NewContinuousVariables(2, 'x' + str(N)) # Add constraints at every knot point x0 = [-2, 0] prog.AddBoundingBoxConstraint(x0, x0, x[:, 0]) for n in range(0, N - 1): # Add the dynamics as an equality constraint # prog.AddConstraint(eq(x[:,n+1], A.dot(x[:,n]) + B.dot(u[:,n]))) # Add the dynamics as a function handle constraint prog.AddConstraint(dynamicsCstr, lb=np.array([0., 0.]), ub=np.array([0., 0.]), vars=np.concatenate((x[:, n], u[:, n], x[:, n + 1]), axis=0), description="dynamics") prog.AddBoundingBoxConstraint(-1, 1, u[:, n]) xf = [0, 0] prog.AddBoundingBoxConstraint(xf, xf, x[:, N - 1]) # Solve the problem
import manipulation.meshcat_cpp_utils as dut import numpy as np from pydrake.all import MathematicalProgram, Solve, Meshcat prog = MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddCost(x.dot(x)) prog.AddBoundingBoxConstraint(-2, 2, x) result = Solve(prog) meshcat = Meshcat() X, Y = np.meshgrid(np.linspace(-3, 3, 35), np.linspace(-3, 3, 31)) dut.plot_mathematical_program(meshcat, "test", prog, X, Y, result)
# stance-foot force f = prog.NewContinuousVariables(rows=T, cols=nf, name='f') # heel strike impulse for the swing leg imp = prog.NewContinuousVariables(nf, name='imp') # generalized velocity after the heel strike # (if "mirrored", this velocity must coincide with the # initial velocity qd[0] to ensure periodicity) qd_post = prog.NewContinuousVariables(nq, name='qd_post') """Here are part of the constraints of the optimization problem.""" # lower and upper bound on the time steps for all t prog.AddBoundingBoxConstraint([h_min] * T, [h_max] * T, h) # link the configurations, velocities, and accelerations # uses implicit Euler method, https://en.wikipedia.org/wiki/Backward_Euler_method for t in range(T): prog.AddConstraint(eq(q[t+1], q[t] + h[t] * qd[t+1])) prog.AddConstraint(eq(qd[t+1], qd[t] + h[t] * qdd[t])) # manipulator equations for all t (implicit Euler) for t in range(T): vars = np.concatenate((q[t+1], qd[t+1], qdd[t], f[t])) prog.AddConstraint(manipulator_equations, lb=[0]*nq, ub=[0]*nq, vars=vars) # velocity reset across heel strike # see http://underactuated.mit.edu/multibody.html#impulsive_collision vars = np.concatenate((q[-1], qd[-1], qd_post, imp))
def optimal_trajectory(joints, start_position, end_position, signed_dist_fn, nc, time=10, knot_points=100): assert len(joints) == len(start_position) assert len(joints) == len(end_position) h = time / (knot_points - 1) nq = len(joints) prog = MathematicalProgram() q_var = [] v_var = [] for ii in range(knot_points): q_var.append(prog.NewContinuousVariables(nq, "q[" + str(ii) + "]")) v_var.append(prog.NewContinuousVariables(nq, "v[" + str(ii) + "]")) # --------------------------------------------------------------- # Initial & Final Pose Constraints ------------------------------ x_i = np.append(start_position, np.zeros(nq)) x_i_vars = np.append(q_var[0], v_var[0]) prog.AddLinearEqualityConstraint(np.eye(2 * nq), x_i, x_i_vars) tol = 0.01 * np.ones(2 * nq) x_f = np.append(end_position, np.zeros(nq)) x_f_vars = np.append(q_var[-1], v_var[-1]) prog.AddBoundingBoxConstraint(x_f - tol, x_f + tol, x_f_vars) # --------------------------------------------------------------- # Dynamics Constraints ------------------------------------------ for ii in range(knot_points - 1): dyn_con1 = np.hstack((np.eye(nq), np.eye(nq), -np.eye(nq))) dyn_var1 = np.concatenate((q_var[ii], v_var[ii], q_var[ii + 1])) prog.AddLinearEqualityConstraint(dyn_con1, np.zeros(nq), dyn_var1) # --------------------------------------------------------------- # Joint Limit Constraints --------------------------------------- q_min = np.array([j.lower_limits() for j in joints]) q_max = np.array([j.upper_limits() for j in joints]) for ii in range(knot_points): prog.AddBoundingBoxConstraint(q_min, q_max, q_var[ii]) # --------------------------------------------------------------- # Collision Constraints ----------------------------------------- for ii in range(knot_points): prog.AddConstraint(signed_dist_fn, np.zeros(nc), 1e8 * np.ones(nc), q_var[ii]) # --------------------------------------------------------------- # Dynamics Constraints ------------------------------------------ for ii in range(knot_points): prog.AddQuadraticErrorCost(np.eye(nq), np.zeros(nq), v_var[ii]) xi = np.array(start_position) xf = np.array(end_position) for ii in range(knot_points): prog.SetInitialGuess(q_var[ii], ii * (xf - xi) / (knot_points - 1) + xi) prog.SetInitialGuess(v_var[ii], np.zeros(nq)) result = prog.Solve() print prog.GetSolverId().name() if result != SolutionResult.kSolutionFound: print result return None q_path = [] # print signed_dist_fn(prog.GetSolution(q_var[0])) for ii in range(knot_points): q_path.append(prog.GetSolution(q_var[ii])) return q_path
def generate_prog(self): self.nn_conss = [] self.nn_costs = [] plant = PendulumPlant() context = plant.CreateDefaultContext() dircol_constraint = DirectCollocationConstraint(plant, context) prog = MathematicalProgram() # K = prog.NewContinuousVariables(1,7,'K') def final_cost(x): return 100.*(cos(.5*x[0])**2 + x[1]**2) h = []; u = []; x = []; xf = np.array([math.pi, 0.]) # Declare the MathematicalProgram vars up front in a good order, so that # prog.decision_variables will be result of np.hstack(h.flatten(), u.flatten(), x.flatten(), T.flatten()) # for the following shapes: unfortunately, prog.decision_variables() will have these shapes: # h = (num_trajectories, 1) | h = (num_trajectories, 1) # u = (num_trajectories, num_inputs, num_samples) | u = (num_trajectories, num_samples, num_inputs) # x = (num_trajectories, num_states, num_samples) | x = (num_trajectories, num_samples, num_states) # T = (num_params) | T = (num_params) for ti in range(self.num_trajectories): h.append(prog.NewContinuousVariables(1,'h'+str(ti))) for ti in range(self.num_trajectories): u.append(prog.NewContinuousVariables(1, self.num_samples,'u'+str(ti))) for ti in range(self.num_trajectories): x.append(prog.NewContinuousVariables(2, self.num_samples,'x'+str(ti))) # Add in constraints for ti in range(self.num_trajectories): prog.AddBoundingBoxConstraint(self.kMinimumTimeStep, self.kMaximumTimeStep, h[ti]) # prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps x0 = np.array(self.ic_list[ti]) # TODO: hopefully this isn't subtley bad... 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]) # Do optional warm start here if self.warm_start: prog.SetInitialGuess(h[ti], [(self.kMinimumTimeStep+self.kMaximumTimeStep)/2]) for i in range(self.num_samples): prog.SetInitialGuess(u[ti][:,i], [0.]) x_interp = (xf-x0)*i/self.num_samples + x0 prog.SetInitialGuess(x[ti][:,i], x_interp) # prog.SetInitialGuess(u[ti][:,i], np.array(1.0)) for i in range(self.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(self.num_samples): #prog.AddQuadraticCost([[2., 0.], [0., 2.]], [0., 0.], x[ti][:,i]) #prog.AddQuadraticCost([25.], [0.], u[ti][:,i]) u_var = u[ti][:,i] x_var = x[ti][:,i] - xf h_var = h[ti][0] #print(u_var.shape, x_var.shape, xf.shape) prog.AddCost( h_var * (2*x_var.dot(x_var) + 25*u_var.dot(u_var)) ) #prog.AddCost( 2*((x[0]-math.pi)*(x[0]-math.pi) + x[1]*x[1]) + 25*u.dot(u)) kTorqueLimit = 5 prog.AddBoundingBoxConstraint([-kTorqueLimit], [kTorqueLimit], u[ti][:,i]) # prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()])) # 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? # Setting solver options #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") # Save references self.prog = prog self.h = h self.u = u self.x = x self.T = []
def compute_control(self, x_p1, x_p2, x_puck, p_goal, obstacles): """Solve for initial velocity for the puck to bounce the wall once and hit the goal.""" """Currently does not work""" # initialize program N = self.mpc_params.N # length of receding horizon prog = MathematicalProgram() # State and input variables x1 = prog.NewContinuousVariables(N + 1, 4, name='p1_state') # state of player 1 u1 = prog.NewContinuousVariables( N, 2, name='p1_input') # inputs for player 1 xp = prog.NewContinuousVariables( N + 1, 4, name='puck_state') # state of the puck # Slack variables t1_kick = prog.NewContinuousVariables( N + 1, name='kick_time' ) # slack variables that captures if player 1 is kicking or not at the given time step # Defined as continous, but treated as mixed integer. 1 when kicking v1_kick = prog.NewContinuousVariables( N + 1, 2, name='v1_kick') # velocity of player after kick to puck vp_kick = prog.NewContinuousVariables( N + 1, 2, name='vp_kick' ) # velocity of puck after being kicked by the player dist = prog.NewContinuousVariables( N + 1, name='dist_puck_player') # distance between puck and player cost = prog.NewContinuousVariables( N + 1, name='cost') # slack variable to monitor cost # Compute distance between puck and player as slack variable. for k in range(N + 1): r1 = self.sim_params.player_radius rp = self.sim_params.puck_radius prog.AddConstraint( dist[k] == (x1[k, 0:2] - xp[k, 0:2]).dot(x1[k, 0:2] - xp[k, 0:2]) - (r1 + rp)**2) #### COST and final states # TODO: find adequate final velocity x_puck_des = np.concatenate( (p_goal, np.zeros(2)), axis=0) # desired position and vel for puck for k in range(N + 1): Q_dist_puck_goal = 10 * np.eye(2) q_dist_puck_player = 0.1 e1 = x_puck_des[0:2] - xp[k, 0:2] e2 = xp[k, 0:2] - x1[k, 0:2] prog.AddConstraint( cost[k] == e1.dot(np.matmul(Q_dist_puck_goal, e1)) + q_dist_puck_player * dist[k]) prog.AddCost(cost[k]) #prog.AddQuadraticErrorCost(Q=self.mpc_params.Q_puck, x_desired=x_puck_des, vars=xp[k]) # puck in the goal #prog.AddQuadraticErrorCost(Q=np.eye(2), x_desired=x_puck_des[0:2], vars=x1[k, 0:2]) #prog.AddQuadraticErrorCost(Q=10*np.eye(2), x_desired=np.array([2, 2]), vars=x1[k, 0:2]) # TEST: control position of the player instead of puck #for k in range(N): # prog.AddQuadraticCost(1e-2*u1[k].dot(u1[k])) # be wise on control effort # Initial states for puck and player prog.AddBoundingBoxConstraint(x_p1, x_p1, x1[0]) # Intial state for player 1 prog.AddBoundingBoxConstraint(x_puck, x_puck, xp[0]) # Initial state for puck # Compute elastic collision for every possible timestep. for k in range(N + 1): v_puck_bfr = xp[k, 2:4] v_player_bfr = x1[k, 2:4] v_puck_aft, v_player_aft = self.get_reset_velocities( v_puck_bfr, v_player_bfr) prog.AddConstraint(eq(vp_kick[k], v_puck_aft)) prog.AddConstraint(eq(v1_kick[k], v_player_aft)) # Use slack variable to activate guard condition based on distance. M = 15**2 for k in range(N + 1): prog.AddLinearConstraint(dist[k] <= M * (1 - t1_kick[k])) prog.AddLinearConstraint(dist[k] >= -t1_kick[k] * M) # Hybrid dynamics for player #for k in range(N): # A = self.mpc_params.A_player # B = self.mpc_params.B_player # x1_kick = np.concatenate((x1[k][0:2], v1_kick[k]), axis=0) # The state of the player after it kicks the puck # x1_next = np.matmul(A, (1 - t1_kick[k])*x1[k] + t1_kick[k]*x1_kick) + np.matmul(B, u1[k]) # prog.AddConstraint(eq(x1[k+1], x1_next)) # Assuming player dynamics are not affected by collision for k in range(N): A = self.mpc_params.A_player B = self.mpc_params.B_player x1_next = np.matmul(A, x1[k]) + np.matmul(B, u1[k]) prog.AddConstraint(eq(x1[k + 1], x1_next)) # Hybrid dynamics for puck_mass for k in range(N): A = self.mpc_params.A_puck xp_kick = np.concatenate( (xp[k][0:2], vp_kick[k]), axis=0) # State of the puck after it gets kicked xp_next = np.matmul(A, (1 - t1_kick[k]) * xp[k] + t1_kick[k] * xp_kick) prog.AddConstraint(eq(xp[k + 1], xp_next)) # Generate trajectory that is not in direct collision with the puck for k in range(N + 1): eps = 0.1 prog.AddConstraint(dist[k] >= -eps) # Input and arena constraint self.add_input_limits(prog, u1, N) self.add_arena_limits(prog, x1, N) self.add_arena_limits(prog, xp, N) # fake mixed-integer constraint #for k in range(N+1): # prog.AddConstraint(t1_kick[k]*(1-t1_kick[k])==0) # Hot-start guess_u1, guess_x1 = self.get_initial_guess(x_p1, p_goal, x_puck[0:2]) prog.SetInitialGuess(x1, guess_x1) prog.SetInitialGuess(u1, guess_u1) if not self.prev_xp is None: prog.SetInitialGuess(xp, self.prev_xp) #prog.SetInitialGuess(t1_kick, np.ones_like(t1_kick)) # solve for the periods # solver = SnoptSolver() #result = solver.Solve(prog) #if not result.is_success(): # print("Unable to find solution.") # save for hot-start #self.prev_xp = result.GetSolution(xp) #if True: # print("x1:{}".format(result.GetSolution(x1))) # print("u1: {}".format( result.GetSolution(u1))) # print("xp: {}".format( result.GetSolution(xp))) # print('dist{}'.format(result.GetSolution(dist))) # print("t1_kick:{}".format(result.GetSolution(t1_kick))) # print("v1_kick:{}".format(result.GetSolution(v1_kick))) # print("vp_kick:{}".format(result.GetSolution(vp_kick))) # print("cost:{}".format(result.GetSolution(cost))) # return whether successful, and the initial player velocity #u1_opt = result.GetSolution(u1) return True, guess_u1[0, :], np.zeros((2))
g = 9.81 r = l/2 f = 10 I = m * (l**2)/12 for n in range(N): u[:,n] = prog.NewContinuousVariables(2, 'u' + str(n)) dx[:,n] = prog.NewContinuousVariables(3, 'dx' + str(n)) x[:,n] = prog.NewContinuousVariables(3, 'x' + str(n)) s = env.reset().reshape(6, 1) print(s) # Add constraints x0 = [s[0], s[1], s[2]] prog.AddBoundingBoxConstraint(x0, x0, x[:,0]) x1 = [0]*3 prog.AddBoundingBoxConstraint(x1, x1, dx[:, 0]) 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
class FourierCollocationProblem: def __init__(self, system_dynamics, constraints): start_time = time.time() self.prog = MathematicalProgram() self.N = 50 # Number of collocation points self.M = 10 # Number of frequencies self.system_dynamics = system_dynamics self.psi = np.pi * (0.7) # TODO change ( min_height, max_height, min_vel, max_vel, h0, min_travelled_distance, t_f_min, t_f_max, avg_vel_min, avg_vel_max, ) = constraints initial_pos = np.array([0, 0, h0]) # Add state trajectory parameters as decision variables self.coeffs = self.prog.NewContinuousVariables( 3, self.M + 1, "c" ) # (x,y,z) for every frequency self.phase_delays = self.prog.NewContinuousVariables(3, self.M, "eta") self.t_f = self.prog.NewContinuousVariables(1, 1, "t_f")[0, 0] self.avg_vel = self.prog.NewContinuousVariables(1, 1, "V_bar")[0, 0] # Enforce initial conditions residuals = self.get_pos_fourier(collocation_time=0) - initial_pos for residual in residuals: self.prog.AddConstraint(residual == 0) # Enforce dynamics at collocation points for n in range(self.N): collocation_time = (n / self.N) * self.t_f pos = self.get_pos_fourier(collocation_time) vel = self.get_vel_fourier(collocation_time) vel_dot = self.get_vel_dot_fourier(collocation_time) residuals = self.continuous_dynamics(pos, vel, vel_dot) for residual in residuals[3:6]: # TODO only these last three are residuals self.prog.AddConstraint(residual == 0) if True: # Add velocity constraints squared_vel_norm = vel.T.dot(vel) self.prog.AddConstraint(min_vel ** 2 <= squared_vel_norm) self.prog.AddConstraint(squared_vel_norm <= max_vel ** 2) # Add height constraints self.prog.AddConstraint(pos[2] <= max_height) self.prog.AddConstraint(min_height <= pos[2]) # Add constraint on min travelled distance self.prog.AddConstraint(min_travelled_distance <= self.avg_vel * self.t_f) # Add constraints on coefficients if False: for coeff in self.coeffs.T: lb = np.array([-500, -500, -500]) ub = np.array([500, 500, 500]) self.prog.AddBoundingBoxConstraint(lb, ub, coeff) # Add constraints on phase delays if False: for etas in self.phase_delays.T: lb = np.array([0, 0, 0]) ub = np.array([2 * np.pi, 2 * np.pi, 2 * np.pi]) self.prog.AddBoundingBoxConstraint(lb, ub, etas) # Add constraints on final time and avg vel self.prog.AddBoundingBoxConstraint(t_f_min, t_f_max, self.t_f) self.prog.AddBoundingBoxConstraint(avg_vel_min, avg_vel_max, self.avg_vel) # Add objective function self.prog.AddCost(-self.avg_vel) # Set initial guess coeffs_guess = np.zeros((3, self.M + 1)) coeffs_guess += np.random.rand(*coeffs_guess.shape) * 100 self.prog.SetInitialGuess(self.coeffs, coeffs_guess) phase_delays_guess = np.zeros((3, self.M)) phase_delays_guess += np.random.rand(*phase_delays_guess.shape) * 1e-1 self.prog.SetInitialGuess(self.phase_delays, phase_delays_guess) self.prog.SetInitialGuess(self.avg_vel, (avg_vel_max - avg_vel_min) / 2) self.prog.SetInitialGuess(self.t_f, (t_f_max - t_f_min) / 2) print( "Finished formulating the optimization problem in: {0} s".format( time.time() - start_time ) ) start_solve_time = time.time() self.result = Solve(self.prog) print("Found solution: {0}".format(self.result.is_success())) print("Found a solution in: {0} s".format(time.time() - start_solve_time)) # TODO input costs # assert self.result.is_success() return def get_solution(self): coeffs_opt = self.result.GetSolution(self.coeffs) phase_delays_opt = self.result.GetSolution(self.phase_delays) t_f_opt = self.result.GetSolution(self.t_f) avg_vel_opt = self.result.GetSolution(self.avg_vel) sim_N = 100 dt = t_f_opt / sim_N times = np.arange(0, t_f_opt, dt) pos_traj = np.zeros((3, sim_N)) # TODO remove vel traj vel_traj = np.zeros((3, sim_N)) for i in range(sim_N): t = times[i] pos = self.evaluate_pos_traj( coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t ) pos_traj[:, i] = pos vel = self.evaluate_vel_traj( coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t ) vel_traj[:, i] = vel # TODO move plotting out plot_trj_3_wind(pos_traj.T, np.array([np.sin(self.psi), np.cos(self.psi), 0])) plt.show() breakpoint() return def evaluate_pos_traj(self, coeffs, phase_delays, t_f, avg_vel, t): pos_traj = np.copy(coeffs[:, 0]) for m in range(1, self.M): sine_terms = np.array( [ np.sin((2 * np.pi * m * t) / t_f + phase_delays[0, m]), np.sin((2 * np.pi * m * t) / t_f + phase_delays[1, m]), np.sin((2 * np.pi * m * t) / t_f + phase_delays[2, m]), ] ) pos_traj += coeffs[:, m] * sine_terms direction_term = np.array( [ avg_vel * np.sin(self.psi) * t, avg_vel * np.cos(self.psi) * t, 0, ] ) pos_traj += direction_term return pos_traj def evaluate_vel_traj(self, coeffs, phase_delays, t_f, avg_vel, t): vel_traj = np.array([0, 0, 0], dtype=object) for m in range(1, self.M): cos_terms = np.array( [ (2 * np.pi * m) / t_f * np.cos((2 * np.pi * m * t) / t_f + phase_delays[0, m]), (2 * np.pi * m) / t_f * np.cos((2 * np.pi * m * t) / t_f + phase_delays[1, m]), (2 * np.pi * m) / t_f * np.cos((2 * np.pi * m * t) / t_f + phase_delays[2, m]), ] ) vel_traj += coeffs[:, m] * cos_terms direction_term = np.array( [ avg_vel * np.sin(self.psi), avg_vel * np.cos(self.psi), 0, ] ) vel_traj += direction_term return vel_traj def get_pos_fourier(self, collocation_time): pos_traj = np.copy(self.coeffs[:, 0]) for m in range(1, self.M): a = (2 * np.pi * m) / self.t_f sine_terms = np.array( [ np.sin(a * collocation_time + self.phase_delays[0, m]), np.sin(a * collocation_time + self.phase_delays[1, m]), np.sin(a * collocation_time + self.phase_delays[2, m]), ] ) pos_traj += self.coeffs[:, m] * sine_terms direction_term = np.array( [ self.avg_vel * np.sin(self.psi) * collocation_time, self.avg_vel * np.cos(self.psi) * collocation_time, 0, ] ) pos_traj += direction_term return pos_traj def get_vel_fourier(self, collocation_time): vel_traj = np.array([0, 0, 0], dtype=object) for m in range(1, self.M): a = (2 * np.pi * m) / self.t_f cos_terms = np.array( [ a * np.cos(a * collocation_time + self.phase_delays[0, m]), a * np.cos(a * collocation_time + self.phase_delays[1, m]), a * np.cos(a * collocation_time + self.phase_delays[2, m]), ] ) vel_traj += self.coeffs[:, m] * cos_terms direction_term = np.array( [ self.avg_vel * np.sin(self.psi), self.avg_vel * np.cos(self.psi), 0, ] ) vel_traj += direction_term return vel_traj def get_vel_dot_fourier(self, collocation_time): vel_dot_traj = np.array([0, 0, 0], dtype=object) for m in range(1, self.M): a = (2 * np.pi * m) / self.t_f sine_terms = np.array( [ -(a ** 2) * np.sin(a * collocation_time + self.phase_delays[0, m]), -(a ** 2) * np.sin(a * collocation_time + self.phase_delays[1, m]), -(a ** 2) * np.sin(a * collocation_time + self.phase_delays[2, m]), ] ) vel_dot_traj += self.coeffs[:, m] * sine_terms return vel_dot_traj def continuous_dynamics(self, pos, vel, vel_dot): x = np.concatenate((pos, vel)) x_dot = np.concatenate((vel, vel_dot)) # TODO need to somehow implement input to make this work return x_dot - self.system_dynamics(x, u)
K = xuK[3:] uc = K.dot(control_basis(x)) # TODO: snopt doesn't like this. perhaps a softmax? if (uc.value() > 3.0): uc = 3.0 elif (uc.value() < -3.0): uc = -3.0 return [u - uc] h = [] u = [] x = [] for ti in range(num_trajectories): h.append(prog.NewContinuousVariables(1)) prog.AddBoundingBoxConstraint(.01, .2, h[ti]) u.append(prog.NewContinuousVariables(1, num_samples, 'u' + str(ti))) x.append( prog.NewContinuousVariables(num_states, num_samples, 'x' + str(ti))) x0 = (.8 + math.pi - .4 * ti, 0.0) 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):
def PlanGraspPoints(self): # First, extract the bounding geometry of the object. # Generally, our geometry is coming from 3d models, so # we have to do some legwork to extract 2D geometry. For # the examples we'll use in this set, we'll assume # that extracting the convex hull of the first visual element # is a good representation of the object geometry. (This is # not great! But it'll do the job for us, since we're going # to experiment with only simple objects.) kinsol = self.hand.doKinematics( self.x_nom[0:self.hand.get_num_positions()]) self.manipuland_link_index = \ self.hand.FindBody(self.manipuland_link_name).get_body_index() body = self.hand.get_body(self.manipuland_link_index) # For projecting into XY plane Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0., 1.]]) all_points = ExtractPlanarObjectGeometryConvexHull(body, Tview) # For later use: precompute the fingertip positions of the # robot in the nominal posture. nominal_fingertip_points = np.empty((2, self.num_fingers)) for i in range(self.num_fingers): nominal_fingertip_points[:, i] = self.hand.transformPoints( kinsol, self.fingertip_position, self.finger_link_indices[i], 0)[0:2, 0] # Search for an optimal grasp with N points random.seed(42) np.random.seed(42) def get_random_point_and_normal_on_surface(all_points): num_points = all_points.shape[1] i = random.choice(range(num_points - 1)) first_point = np.asarray([all_points[0][i], all_points[1][i]]) second_point = np.asarray( [all_points[0][i + 1], all_points[1][i + 1]]) first_to_second = second_point - first_point # Clip to avoid interpolating close to a corner interpolate_param = np.clip(np.random.rand(), 0.2, 0.8) rand_point = first_point + interpolate_param * first_to_second normal = np.array([-first_to_second[1], first_to_second[0]]) \ / np.linalg.norm(first_to_second) return rand_point, normal best_conv_volume = 0 best_points = [] best_normals = [] best_finger_assignments = [] for i in range(self.n_grasp_search_iters): grasp_points = [] normals = [] for j in range(self.num_fingers): point, normal = \ get_random_point_and_normal_on_surface(all_points) # check for duplicates or close points -- fingertip # radius is 0.2, so require twice that margin to avoid # intersection fingertips. num_rejected = 0 while min([1.0] + [ np.linalg.norm(grasp_point - point, 2) for grasp_point in grasp_points ]) <= 0.4: point, normal = \ get_random_point_and_normal_on_surface(all_points) num_rejected += 1 if num_rejected > 10000: print "Rejected 10000 points in a row due to crowding." \ " Your object is a bit too small for your number of" \ " fingers." break grasp_points.append(point) normals.append(normal) if achieves_force_closure(grasp_points, normals, self.mu): # Test #1: Rank in terms of convex hull volume of grasp points volume = compute_convex_hull_volume(grasp_points) if volume < best_conv_volume: continue # Test #2: Does IK work for this point? self.grasp_points = grasp_points self.grasp_normals = normals # Pick optimal finger assignment that # minimizes distance between fingertips in the # nominal posture, and the chosen grasp points grasp_points_world = self.transform_grasp_points_manipuland( self.x_nom)[0:2, :] prog = MathematicalProgram() # We'd *really* like these to be binary variables, # but unfortunately don't have a MIP solver available in the # course docker container. Instead, we'll solve an LP, # and round the result to the nearest feasible integer # solutions. Intuitively, this LP should probably reach its # optimal value at an extreme point (where the variables # all take integer value). I've not yet observed this # not occuring in practice! assignment_vars = prog.NewContinuousVariables( self.num_fingers, self.num_fingers, "assignment") for grasp_i in range(self.num_fingers): # Every row and column of assignment vars sum to one -- # each finger matches to one grasp position prog.AddLinearConstraint( np.sum(assignment_vars[:, grasp_i]) == 1.) prog.AddLinearConstraint( np.sum(assignment_vars[grasp_i, :]) == 1.) for finger_i in range(self.num_fingers): # If this grasp assignment is active, # add a cost equal to the distance between # nominal pose and grasp position prog.AddLinearCost( assignment_vars[grasp_i, finger_i] * np.linalg.norm(grasp_points_world[:, grasp_i] - nominal_fingertip_points[:, finger_i])) prog.AddBoundingBoxConstraint( 0., 1., assignment_vars[grasp_i, finger_i]) result = Solve(prog) assignments = result.GetSolution(assignment_vars) # Round assignments to nearest feasible set claimed = [False] * self.num_fingers for grasp_i in range(self.num_fingers): order = np.argsort(assignments[grasp_i, :]) fill_i = self.num_fingers - 1 while claimed[order[fill_i]] is not False: fill_i -= 1 if fill_i < 0: raise Exception("Finger association failed. " "Horrible bug. Tell Greg") assignments[grasp_i, :] *= 0. assignments[grasp_i, order[fill_i]] = 1. claimed[order[fill_i]] = True # Populate actual assignments self.grasp_finger_assignments = [] for grasp_i in range(self.num_fingers): for finger_i in range(self.num_fingers): if assignments[grasp_i, finger_i] == 1.: self.grasp_finger_assignments.append( (finger_i, np.array(self.fingertip_position))) continue qinit, info = self.ComputeTargetPosture( self.x_nom, self.x_nom[(self.nq - 3):self.nq], backoff_distance=0.2) if info != 1: continue best_conv_volume = volume best_points = grasp_points best_normals = normals best_finger_assignments = self.grasp_finger_assignments if len(best_points) == 0: print "After %d attempts, couldn't find a good grasp "\ "for this object." % self.n_grasp_search_iters print "Proceeding with a horrible random guess." best_points = [ np.random.uniform(-1., 1., 2) for i in range(self.num_fingers) ] best_normals = [ np.random.uniform(-1., 1., 2) for i in range(self.num_fingers) ] best_finger_assignments = [(i, self.fingertip_position) for i in range(self.num_fingers)] self.grasp_points = best_points self.grasp_normals = best_normals self.grasp_finger_assignments = best_finger_assignments
def solveOptimization(state_init, t_impact, impact_combination, T, u_guess=None, x_guess=None, h_guess=None): prog = MathematicalProgram() h = prog.NewContinuousVariables(T, name='h') u = prog.NewContinuousVariables(rows=T + 1, cols=2 * n_quadrotors, name='u') x = prog.NewContinuousVariables(rows=T + 1, cols=6 * n_quadrotors + 4 * n_balls, name='x') dv = prog.decision_variables() prog.AddBoundingBoxConstraint([h_min] * T, [h_max] * T, h) for i in range(n_quadrotors): sys = Quadrotor2D() context = sys.CreateDefaultContext() dir_coll_constr = DirectCollocationConstraint(sys, context) ind_x = 6 * i ind_u = 2 * i for t in range(T): impact_indices = impact_combination[np.argmax( np.abs(t - t_impact) <= 1)] quad_ind, ball_ind = impact_indices[0], impact_indices[1] if quad_ind == i and np.any( t == t_impact ): # Don't add Direct collocation constraint at impact continue elif quad_ind == i and (np.any(t == t_impact - 1) or np.any(t == t_impact + 1)): prog.AddConstraint( eq( x[t + 1, ind_x:ind_x + 3], x[t, ind_x:ind_x + 3] + h[t] * x[t + 1, ind_x + 3:ind_x + 6])) # Backward euler prog.AddConstraint( eq(x[t + 1, ind_x + 3:ind_x + 6], x[t, ind_x + 3:ind_x + 6]) ) # Zero-acceleration assumption during this time step. Should maybe replace with something less naive else: AddDirectCollocationConstraint( dir_coll_constr, np.array([[h[t]]]), x[t, ind_x:ind_x + 6].reshape(-1, 1), x[t + 1, ind_x:ind_x + 6].reshape(-1, 1), u[t, ind_u:ind_u + 2].reshape(-1, 1), u[t + 1, ind_u:ind_u + 2].reshape(-1, 1), prog) for i in range(n_balls): sys = Ball2D() context = sys.CreateDefaultContext() dir_coll_constr = DirectCollocationConstraint(sys, context) ind_x = 6 * n_quadrotors + 4 * i for t in range(T): impact_indices = impact_combination[np.argmax( np.abs(t - t_impact) <= 1)] quad_ind, ball_ind = impact_indices[0], impact_indices[1] if ball_ind == i and np.any( t == t_impact ): # Don't add Direct collocation constraint at impact continue elif ball_ind == i and (np.any(t == t_impact - 1) or np.any(t == t_impact + 1)): prog.AddConstraint( eq( x[t + 1, ind_x:ind_x + 2], x[t, ind_x:ind_x + 2] + h[t] * x[t + 1, ind_x + 2:ind_x + 4])) # Backward euler prog.AddConstraint( eq(x[t + 1, ind_x + 2:ind_x + 4], x[t, ind_x + 2:ind_x + 4] + h[t] * np.array([0, -9.81]))) else: AddDirectCollocationConstraint( dir_coll_constr, np.array([[h[t]]]), x[t, ind_x:ind_x + 4].reshape(-1, 1), x[t + 1, ind_x:ind_x + 4].reshape(-1, 1), u[t, 0:0].reshape(-1, 1), u[t + 1, 0:0].reshape(-1, 1), prog) # Initial conditions prog.AddLinearConstraint(eq(x[0, :], state_init)) # Final conditions prog.AddLinearConstraint(eq(x[T, 0:14], state_final[0:14])) # Quadrotor final conditions (full state) for i in range(n_quadrotors): ind = 6 * i prog.AddLinearConstraint( eq(x[T, ind:ind + 6], state_final[ind:ind + 6])) # Ball final conditions (position only) for i in range(n_balls): ind = 6 * n_quadrotors + 4 * i prog.AddLinearConstraint( eq(x[T, ind:ind + 2], state_final[ind:ind + 2])) # Input constraints for i in range(n_quadrotors): prog.AddLinearConstraint(ge(u[:, 2 * i], -20.0)) prog.AddLinearConstraint(le(u[:, 2 * i], 20.0)) prog.AddLinearConstraint(ge(u[:, 2 * i + 1], -20.0)) prog.AddLinearConstraint(le(u[:, 2 * i + 1], 20.0)) # Don't allow quadrotor to pitch more than 60 degrees for i in range(n_quadrotors): prog.AddLinearConstraint(ge(x[:, 6 * i + 2], -np.pi / 3)) prog.AddLinearConstraint(le(x[:, 6 * i + 2], np.pi / 3)) # Ball position constraints # for i in range(n_balls): # ind_i = 6*n_quadrotors + 4*i # prog.AddLinearConstraint(ge(x[:,ind_i],-2.0)) # prog.AddLinearConstraint(le(x[:,ind_i], 2.0)) # prog.AddLinearConstraint(ge(x[:,ind_i+1],-3.0)) # prog.AddLinearConstraint(le(x[:,ind_i+1], 3.0)) # Impact constraint quad_temp = Quadrotor2D() for i in range(n_quadrotors): for j in range(n_balls): ind_q = 6 * i ind_b = 6 * n_quadrotors + 4 * j for t in range(T): if np.any( t == t_impact ): # If quad i and ball j impact at time t, add impact constraint impact_indices = impact_combination[np.argmax( t == t_impact)] quad_ind, ball_ind = impact_indices[0], impact_indices[1] if quad_ind == i and ball_ind == j: # At impact, witness function == 0 prog.AddConstraint(lambda a: np.array([ CalcClosestDistanceQuadBall(a[0:3], a[3:5]) ]).reshape(1, 1), lb=np.zeros((1, 1)), ub=np.zeros((1, 1)), vars=np.concatenate( (x[t, ind_q:ind_q + 3], x[t, ind_b:ind_b + 2])).reshape( -1, 1)) # At impact, enforce discrete collision update for both ball and quadrotor prog.AddConstraint( CalcPostCollisionStateQuadBallResidual, lb=np.zeros((6, 1)), ub=np.zeros((6, 1)), vars=np.concatenate( (x[t, ind_q:ind_q + 6], x[t, ind_b:ind_b + 4], x[t + 1, ind_q:ind_q + 6])).reshape(-1, 1)) prog.AddConstraint( CalcPostCollisionStateBallQuadResidual, lb=np.zeros((4, 1)), ub=np.zeros((4, 1)), vars=np.concatenate( (x[t, ind_q:ind_q + 6], x[t, ind_b:ind_b + 4], x[t + 1, ind_b:ind_b + 4])).reshape(-1, 1)) # rough constraints to enforce hitting center-ish of paddle prog.AddLinearConstraint( x[t, ind_q] - x[t, ind_b] >= -0.01) prog.AddLinearConstraint( x[t, ind_q] - x[t, ind_b] <= 0.01) continue # Everywhere else, witness function must be > 0 prog.AddConstraint(lambda a: np.array([ CalcClosestDistanceQuadBall(a[ind_q:ind_q + 3], a[ ind_b:ind_b + 2]) ]).reshape(1, 1), lb=np.zeros((1, 1)), ub=np.inf * np.ones((1, 1)), vars=x[t, :].reshape(-1, 1)) # Don't allow quadrotor collisions # for t in range(T): # for i in range(n_quadrotors): # for j in range(i+1, n_quadrotors): # prog.AddConstraint((x[t,6*i]-x[t,6*j])**2 + (x[t,6*i+1]-x[t,6*j+1])**2 >= 0.65**2) # Quadrotors stay on their own side # prog.AddLinearConstraint(ge(x[:, 0], 0.3)) # prog.AddLinearConstraint(le(x[:, 6], -0.3)) ############################################################################### # Set up initial guesses initial_guess = np.empty(prog.num_vars()) # # initial guess for the time step prog.SetDecisionVariableValueInVector(h, h_guess, initial_guess) x_init[0, :] = state_init prog.SetDecisionVariableValueInVector(x, x_guess, initial_guess) prog.SetDecisionVariableValueInVector(u, u_guess, initial_guess) solver = SnoptSolver() print("Solving...") result = solver.Solve(prog, initial_guess) # print(GetInfeasibleConstraints(prog,result)) # be sure that the solution is optimal assert result.is_success() print(f'Solution found? {result.is_success()}.') ################################################################################# # Extract results # get optimal solution h_opt = result.GetSolution(h) x_opt = result.GetSolution(x) u_opt = result.GetSolution(u) time_breaks_opt = np.array([sum(h_opt[:t]) for t in range(T + 1)]) u_opt_poly = PiecewisePolynomial.ZeroOrderHold(time_breaks_opt, u_opt.T) # x_opt_poly = PiecewisePolynomial.Cubic(time_breaks_opt, x_opt.T, False) x_opt_poly = PiecewisePolynomial.FirstOrderHold( time_breaks_opt, x_opt.T ) # Switch to first order hold instead of cubic because cubic was taking too long to create ################################################################################# # Create list of K matrices for time varying LQR context = quad_plant.CreateDefaultContext() breaks = copy.copy( time_breaks_opt) #np.linspace(0, x_opt_poly.end_time(), 100) K_samples = np.zeros((breaks.size, 12 * n_quadrotors)) for i in range(n_quadrotors): K = None for j in range(breaks.size): context.SetContinuousState( x_opt_poly.value(breaks[j])[6 * i:6 * (i + 1)]) context.FixInputPort( 0, u_opt_poly.value(breaks[j])[2 * i:2 * (i + 1)]) linear_system = FirstOrderTaylorApproximation(quad_plant, context) A = linear_system.A() B = linear_system.B() try: K, _, _ = control.lqr(A, B, Q, R) except: assert K is not None, "Failed to calculate initial K for quadrotor " + str( i) print("Warning: Failed to calculate K at timestep", j, "for quadrotor", i, ". Using K from previous timestep") K_samples[j, 12 * i:12 * (i + 1)] = K.reshape(-1) K_poly = PiecewisePolynomial.ZeroOrderHold(breaks, K_samples.T) return u_opt_poly, x_opt_poly, K_poly, h_opt
def static_controller(self, qref, verbose=False): """ Generates a controller to maintain a static pose Arguments: qref: (N,) numpy array, the static pose to be maintained Return Values: u: (M,) numpy array, actuations to best achieve static pose f: (C,) numpy array, associated normal reaction forces static_controller generates the actuations and reaction forces assuming the velocity and accelerations are zero. Thus, the equation to solve is: N(q) = B*u + J*f where N is a vector of gravitational and conservative generalized forces, B is the actuation selection matrix, and J is the contact-Jacobian transpose. Currently, static_controller only considers the effects of the normal forces. Frictional forces are not yet supported """ #TODO: Solve for friction forces as well # Check inputs if qref.ndim > 1 or qref.shape[0] != self.multibody.num_positions(): raise ValueError( f"Reference position mut be ({self.multibody.num_positions(),},) array" ) # Set the context context = self.multibody.CreateDefaultContext() self.multibody.SetPositions(context, qref) # Get the necessary properties G = self.multibody.CalcGravityGeneralizedForces(context) Jn, _ = self.GetContactJacobians(context) phi = self.GetNormalDistances(context) B = self.multibody.MakeActuationMatrix() #Collect terms A = np.concatenate([B, Jn.transpose()], axis=1) # Create the mathematical program prog = MathematicalProgram() l_var = prog.NewContinuousVariables(self.num_contacts(), name="forces") u_var = prog.NewContinuousVariables(self.multibody.num_actuators(), name="controls") # Ensure dynamics approximately satisfied prog.AddL2NormCost(A=A, b=-G, vars=np.concatenate([u_var, l_var], axis=0)) # Enforce normal complementarity prog.AddBoundingBoxConstraint(np.zeros(l_var.shape), np.full(l_var.shape, np.inf), l_var) prog.AddConstraint(phi.dot(l_var) == 0) # Solve result = Solve(prog) # Check for a solution if result.is_success(): u = result.GetSolution(u_var) f = result.GetSolution(l_var) if verbose: printProgramReport(result, prog) return (u, f) else: print(f"Optimization failed. Returning zeros") if verbose: printProgramReport(result, prog) return (np.zeros(u_var.shape), np.zeros(l_var.shape))
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