def SOS_compute_3(self, K, l_coeff, rho_max=10.): prog = MathematicalProgram() # fix u and lbda, search for V and rho x = prog.NewIndeterminates(2, "x") # get lbda from before l = np.array([x[1], x[0], 1]) lbda = l.dot(np.dot(l_coeff, l)) # rho is decision variable now rho = prog.NewContinuousVariables(1, "rho")[0] # create lyap V s = prog.NewContinuousVariables(4, "s") S = np.array([[s[0], s[1]], [s[2], s[3]]]) V = x.dot(np.dot(S, x)) Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0] prog.AddSosConstraint(V) prog.AddSosConstraint(-Vdot - lbda * (rho - V)) prog.AddLinearCost(-rho) prog.AddLinearConstraint(rho <= rho_max) prog.Solve() rho = prog.GetSolution(rho) s = prog.GetSolution(s) return s, rho
def SOS_compute_2(self, l_coeff, S, rho_max=10.): prog = MathematicalProgram() # fix V and lbda, searcu for u and rho x = prog.NewIndeterminates(2, "x") # get lbda from before l = np.array([x[1], x[0], 1]) lbda = l.dot(np.dot(l_coeff, l)) # Define u K = prog.NewContinuousVariables(2, "K") # Fixed Lyapunov V = x.dot(np.dot(S, x)) Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0] # rho is decision variable now rho = prog.NewContinuousVariables(1, "rho")[0] prog.AddSosConstraint(-Vdot - lbda * (rho - V)) prog.AddLinearConstraint(rho <= rho_max) prog.AddLinearCost(-rho) prog.Solve() rho = prog.GetSolution(rho) K = prog.GetSolution(K) return rho, K
def run_simple_mathematical_program(): print "\n\nsimple mathematical program" mp = MathematicalProgram() x = mp.NewContinuousVariables(1, "x") mp.AddLinearCost(x[0] * 1.0) mp.AddLinearConstraint(x[0] >= 1) print mp.Solve() print mp.GetSolution(x) print "(finished) simple mathematical program"
# no need to change it, but if you really want to, # keep l_deg even and do not set l_deg greater than 10 l_deg = 4 assert l_deg % 2 == 0 # SOS Lagrange multipliers l = prog2.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression() # level set as optimization variable rho = prog2.NewContinuousVariables(1, 'rho')[0] # write here the SOS condition described in the "Not quite there yet..." section above prog2.AddSosConstraint(x.dot(x)*(V-rho) - l*V_dot) # insert here the objective function (maximize rho) prog2.AddLinearCost(-rho) # solve program only if the lines above are filled if len(prog2.GetAllConstraints()) != 0: # solve SOS program result = Solve(prog2) # get maximum rho assert result.is_success() rho_method_2 = result.GetSolution(rho) # print maximum rho print(f'Method 2 verified rho = {rho_method_2}.') """## Method 3: Smarter Single-Shot SOS Program
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 MILP_compute_traj(self, obst_idx, x_out, y_out, dx, dy, pose_initial=[0., 0.]): ''' Find trajectory with MILP Outputs trajectory (waypoints) and new K for control ''' mp = MathematicalProgram() N = 8 k = 0 # define state traj st = mp.NewContinuousVariables(2, "state_%d" % k) # # binary variables for obstalces constraint c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = c states = st for k in range(1, N): st = mp.NewContinuousVariables(2, "state_%d" % k) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) st = mp.NewContinuousVariables(2, "state_%d" % (N + 1)) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) # variables encoding max x y dist from obstacle x_margin = mp.NewContinuousVariables(1, "x_margin") y_margin = mp.NewContinuousVariables(1, "y_margin") ### define cost for i in range(N): mp.AddLinearCost(-states[i, 0]) # go as far forward as possible mp.AddLinearCost(-states[-1, 0]) mp.AddLinearCost(-x_margin[0]) mp.AddLinearCost(-y_margin[0]) # bound x y margin so it doesn't explode mp.AddLinearConstraint(x_margin[0] <= 3.) mp.AddLinearConstraint(y_margin[0] <= 3.) # x y is non ngative adn at least above robot radius mp.AddLinearConstraint(x_margin[0] >= 0.05) mp.AddLinearConstraint(y_margin[0] >= 0.05) M = 1000 # slack var for integer things # state constraint for i in range(2): # initial state constraint x y mp.AddLinearConstraint(states[0, i] <= pose_initial[i]) mp.AddLinearConstraint(states[0, i] >= pose_initial[i]) for i in range(N): mp.AddQuadraticCost((states[i + 1, 1] - states[i, 1])**2) mp.AddLinearConstraint(states[i + 1, 0] <= states[i, 0] + dx) mp.AddLinearConstraint(states[i + 1, 0] >= states[i, 0] - dx) mp.AddLinearConstraint(states[i + 1, 1] <= states[i, 1] + dy) mp.AddLinearConstraint(states[i + 1, 1] >= states[i, 1] - dy) # obstacle constraint for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[i, 0] <= rng_min - x_margin[0] + M * obs[i, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[i, 0] >= rng_max + x_margin[0] - M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[i, 1] <= states[i, 0] * np.tan(ang_min) - y_margin[0] + M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[i, 1] >= states[i, 0] * np.tan(ang_max) + y_margin[0] - M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c # obstacle constraint for last state for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[N, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[N, 0] <= rng_min - x_margin[0] + M * obs[N, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[N, 0] >= rng_max + x_margin[0] - M * obs[N, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[N, 1] <= states[N, 0] * np.tan(ang_min) - y_margin[0] + M * obs[N, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[N, 1] >= states[N, 0] * np.tan(ang_max) + y_margin[0] - M * obs[N, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c mp.Solve() trajectory = mp.GetSolution(states) xm = mp.GetSolution(x_margin) ym = mp.GetSolution(y_margin) x_out[:] = trajectory[:, 0] y_out[:] = trajectory[:, 1] return trajectory, xm[0], ym[0]
def compute_trajectory(self, obst_idx, x_out, y_out, ux_out, uy_out, pose_initial=[0., 0., 0., 0.], dt=0.05): ''' Find trajectory with MILP input u are tyhe velocities (xd, yd) dt 0.05 according to a rate of 20 Hz ''' mp = MathematicalProgram() N = 30 k = 0 # define input trajectory and state traj u = mp.NewContinuousVariables(2, "u_%d" % k) # xd yd input_trajectory = u st = mp.NewContinuousVariables(4, "state_%d" % k) # # binary variables for obstalces constraint c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = c states = st for k in range(1, N): u = mp.NewContinuousVariables(2, "u_%d" % k) input_trajectory = np.vstack((input_trajectory, u)) st = mp.NewContinuousVariables(4, "state_%d" % k) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) st = mp.NewContinuousVariables(4, "state_%d" % (N + 1)) states = np.vstack((states, st)) c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k) obs = np.vstack((obs, c)) ### define cost mp.AddLinearCost(100 * (-states[-1, 0])) # go as far forward as possible # mp.AddQuadraticCost(states[-1,1]*states[-1,1]) # time constraint M = 1000 # slack var for obst costraint # state constraint for i in range(2): # initial state constraint x y yaw mp.AddLinearConstraint(states[0, i] <= pose_initial[i]) mp.AddLinearConstraint(states[0, i] >= pose_initial[i]) for i in range(2): # initial state constraint xd yd yawd mp.AddLinearConstraint(states[0, i] <= pose_initial[2 + i] + 1) mp.AddLinearConstraint(states[0, i] >= pose_initial[2 + i] - 1) for i in range(N): # state update according to dynamics state_next = self.quad_dynamics(states[i, :], input_trajectory[i, :], dt) for j in range(4): mp.AddLinearConstraint(states[i + 1, j] <= state_next[j]) mp.AddLinearConstraint(states[i + 1, j] >= state_next[j]) # obstacle constraint for j in range(self.ang_discret - 1): mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3) ang_min = self.angles[j] # lower angle bound of obstacle ang_max = self.angles[j + 1] # higher angle bound of obstaclee if int(obst_idx[j]) < (self.rng_discret - 1): # less than max range measured rng_min = self.ranges[int( obst_idx[j])] # where the obst is at at this angle rng_max = self.ranges[int(obst_idx[j] + 1)] mp.AddLinearConstraint( states[i, 0] <= rng_min - 0.05 + M * obs[i, 4 * j]) # xi <= xobs,low + M*c mp.AddLinearConstraint( states[i, 0] >= rng_max + 0.005 - M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c mp.AddLinearConstraint( states[i, 1] <= states[i, 0] * np.tan(ang_min) - 0.05 + M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c mp.AddLinearConstraint( states[i, 1] >= states[i, 0] * np.tan(ang_max) + 0.05 - M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c # environmnt constraint, dont leave fov mp.AddLinearConstraint( states[i, 1] >= states[i, 0] * np.tan(-self.theta)) mp.AddLinearConstraint( states[i, 1] <= states[i, 0] * np.tan(self.theta)) # bound the inputs # mp.AddConstraint(input_trajectory[i,:].dot(input_trajectory[i,:]) <= 2.5*2.5) # dosnt work with multi int mp.AddLinearConstraint(input_trajectory[i, 0] <= 2.5) mp.AddLinearConstraint(input_trajectory[i, 0] >= -2.5) mp.AddLinearConstraint(input_trajectory[i, 1] <= 0.5) mp.AddLinearConstraint(input_trajectory[i, 1] >= -0.5) mp.Solve() input_trajectory = mp.GetSolution(input_trajectory) trajectory = mp.GetSolution(states) x_out[:] = trajectory[:, 0] y_out[:] = trajectory[:, 1] ux_out[:] = input_trajectory[:, 0] uy_out[:] = input_trajectory[:, 1] return trajectory, input_trajectory
def linear_program(f, A, b, C=None, d=None, tol=1.e-5, **kwargs): """ Solves the linear program min_x f^T x s.t. A x <= b, C x = d. Arguments ---------- f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. tol : float Maximum value of a residual of an inequality to consider the related constraint active. Returns ---------- sol : dict Dictionary with the solution of the LP. Fields ---------- min : float Minimum of the LP (None if the problem is unfeasible or unbounded). argmin : numpy.ndarray Argument that minimizes the LP (None if the problem is unfeasible or unbounded). active_set : list of int Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded). multiplier_inequality : numpy.ndarray Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded). multiplier_equality : numpy.ndarray Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # build program prog = MathematicalProgram() x = prog.NewContinuousVariables(n_x) [prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)] [prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)] prog.AddLinearCost(f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0) [prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()] result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None, 'active_set': None, 'multiplier_inequality': None, 'multiplier_equality': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x) sol['min'] = f.dot(sol['argmin']) sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist() # retrieve multipliers through KKT conditions M = A[sol['active_set']].T if n_eq > 0: M = np.hstack((M, C.T)) m = -np.linalg.pinv(M).dot(f) sol['multiplier_inequality'] = np.zeros(n_ineq) for i, j in enumerate(sol['active_set']): sol['multiplier_inequality'][j] = m[i] if n_eq > 0: sol['multiplier_equality'] = m[len(sol['active_set']):] return sol
from pydrake.all import Jacobian, MathematicalProgram, Solve def dynamics(x): return -x + x**3 prog = MathematicalProgram() x = prog.NewIndeterminates(1, "x") rho = prog.NewContinuousVariables(1, "rho")[0] # Define the Lyapunov function. V = x.dot(x) Vdot = Jacobian([V], x).dot(dynamics(x))[0] # Define the Lagrange multiplier. lambda_ = prog.NewContinuousVariables(1, "lambda")[0] prog.AddConstraint(lambda_ >= 0) prog.AddSosConstraint((V - rho) * x.dot(x) - lambda_ * Vdot) prog.AddLinearCost(-rho) result = Solve(prog) assert result.is_success() print("Verified that " + str(V) + " < " + str(result.GetSolution(rho)) + " is in the region of attraction.") assert math.fabs(result.GetSolution(rho) - 1) < 1e-5
prog = MathematicalProgram() # Construct an n-by-n positive semi-definite matrix as the decision # variables. num_states = A[0].shape[0] P = prog.NewSymmetricContinuousVariables(num_states, "P") prog.AddPositiveSemidefiniteConstraint(P - .01 * np.identity(num_states)) # Add the common Lyapunov conditions. for i in range(len(A)): prog.AddPositiveSemidefiniteConstraint(-A[i].transpose().dot(P) - P.dot(A[i]) - .01 * np.identity(num_states)) # Add an objective. prog.AddLinearCost(np.trace(P)) # Run the optimization. result = Solve(prog) if result.is_success(): P = result.GetSolution(P) print("eig(P) =" + str(np.linalg.eig(P)[0])) for i in range(len(A)): print("eig(Pdot" + str(i) + ") = " + str(np.linalg.eig(A[i].transpose().dot(P) + P.dot(A[i]))[0])) else: print('Could not find a common Lyapunov function.') print('This is expected to occur with some probability: not all') print('random sets of stable matrices will have a common Lyapunov') print('function.')
def achieves_force_closure(points, normals, mu): """ Determines whether the given forces achieve force closure. Note that each of points and normals are lists of variable length, but should be the same length. Here's an example valid set of input args: points = [np.asarray([0.1, 0.2]), np.asarray([0.5,-0.1])] normals = [np.asarray([-1.0,0.0]), np.asarray([0.0, 1.0])] mu = 0.2 achieves_force_closure(points, normals, mu) NOTE: your normals should be normalized (be unit vectors) :param points: a list of arrays where each array is the position of the force points relative to the center of mass. :type points: a list of 1-d numpy arrays of shape (2,) :param normals: a list of arrays where each array is the normal of the local surface of the object, pointing in towards the object :type normals: a list of 1-d numpy arrays of shape (2,) :param mu: coulombic kinetic friction coefficient :type mu: float, greater than 0. :return: whether or not the given parameters achieves force closure :rtype: bool (True or False) """ assert len(points) == len(normals) assert mu >= 0.0 # print('Number of Points: ' + str(len(points))) from pydrake.all import MathematicalProgram, Solve mp = MathematicalProgram() forces = mp.NewContinuousVariables(2 * len(points), "forces") y = mp.NewContinuousVariables(1, "y") g = get_G(points, normals) # print('Is Full Row Rank? => ' + str(is_full_row_rank(g))) if (not is_full_row_rank(g)): return False w = g.dot(forces) mp.AddLinearConstraint(w[0] == 0) mp.AddLinearConstraint(w[1] == 0) mp.AddLinearConstraint(w[2] == 0) # mp.AddLinearEqualityConstraint(g, np.zeros((3, 1)), forces) mp.AddLinearConstraint(y[0] >= -1) mp.AddLinearConstraint(y[0] <= 0) for i in range(len(points)): mp.AddLinearConstraint(mu * forces[2 * i + 1] >= forces[2 * i]) mp.AddLinearConstraint(mu * forces[2 * i + 1] >= -forces[2 * i]) mp.AddLinearConstraint(forces[2 * i + 1] >= -y[0]) mp.AddLinearCost(y[0]) result = Solve(mp) forces_solution = result.GetSolution(forces) y_solution = result.GetSolution(y) # print('forces_solution: ' + str(forces_solution)) # print('w: ' + str(g.dot(forces_solution))) # print('y_solution: ' + str(y_solution)) if y_solution == 0: return False else: return True
def achieves_force_closure(points, normals, mu, debug=False): """ Determines whether the given forces achieve force closure. Note that each of points and normals are lists of variable length, but should be the same length. Here's an example valid set of input args: points = [np.asarray([0.1, 0.2]), np.asarray([0.5,-0.1])] normals = [np.asarray([-1.0,0.0]), np.asarray([0.0, 1.0])] mu = 0.2 achieves_force_closure(points, normals, mu) NOTE: your normals should be normalized (be unit vectors) :param points: a list of arrays where each array is the position of the force points relative to the center of mass. :type points: a list of 1-d numpy arrays of shape (2,) :param normals: a list of arrays where each array is the normal of the local surface of the object, pointing in towards the object :type normals: a list of 1-d numpy arrays of shape (2,) :param mu: coulombic kinetic friction coefficient :type mu: float, greater than 0. :return: whether or not the given parameters achieves force closure :rtype: bool (True or False) """ assert len(points) == len(normals) assert mu >= 0.0 assert_unit_normals(normals) ## YOUR CODE HERE G = get_G(points, normals) if not is_full_row_rank(G): print("G is not full row rank") return False N_points = len(points) max_force = 1000 mp = MathematicalProgram() ## Decision vars # force tangent to surface forces_x = mp.NewContinuousVariables(N_points, "forces_x") # force normal to surface forces_z = mp.NewContinuousVariables(N_points, "forces_z") # -1<=slack_var<=0 used to convert inequalities to strict inequalities slack_var = mp.NewContinuousVariables(1, "slack_var") # put force vars in single array forces = [None] * 2 * N_points for point_idx in range(N_points): forces[point_idx * 2] = forces_x[point_idx] forces[point_idx * 2 + 1] = forces_z[point_idx] # ensure forces/moments add to zero for row_idx in range(np.shape(G)[0]): # 3 rows (LMB_x, LMB_z, AMB) = 0 for static system mp.AddLinearConstraint(G[row_idx, :].dot(forces) == 0) if debug: print("Static force/moment constraints = 0:") print(G[row_idx, :].dot(forces)) # ensure forces stay within friction cone and use slack to avoid trivial 0 solution for point_idx in range(N_points): # normal force must be positive (slack allows us to catch if it's zero) mp.AddLinearConstraint(forces_z[point_idx] + slack_var[0] >= 0) mp.AddLinearConstraint( forces_x[point_idx] <= mu * forces_z[point_idx] + slack_var[0]) mp.AddLinearConstraint( forces_x[point_idx] >= -(mu * forces_z[point_idx] + slack_var[0])) # restrict slack mp.AddLinearConstraint(slack_var[0] <= 0) mp.AddLinearConstraint(slack_var[0] >= -1) mp.AddLinearCost(slack_var[0]) # restrict solution within bounds for force in forces: mp.AddLinearConstraint(force >= -max_force) mp.AddLinearConstraint(force <= max_force) result = Solve(mp) if (not result.is_success()): print("solver failed to find solution") return False gamma = result.GetSolution(slack_var) if (gamma < 0): print("acheived force closure, gamma = {}".format(gamma)) if debug: x = result.GetSolution(forces_x) z = result.GetSolution(forces_z) print("solution forces_x: {}".format(x)) print("solution forces_z: {}".format(z)) return True else: print("only trivial solution with 0 forces found") return False
def linear_program(f, A, b, C=None, d=None, tol=1.e-5): """ Solves the linear program min_x f^T x s.t. A x <= b, C x = d. Arguments ---------- f : numpy.ndarray Gradient of the cost function. A : numpy.ndarray Left-hand side of the inequality constraints. b : numpy.ndarray Right-hand side of the inequality constraints. C : numpy.ndarray Left-hand side of the equality constraints. d : numpy.ndarray Right-hand side of the equality constraints. tol : float Maximum value of a residual of an inequality to consider the related constraint active. Returns ---------- sol : dict Dictionary with the solution of the LP. Fields ---------- min : float Minimum of the LP (None if the problem is unfeasible or unbounded). argmin : numpy.ndarray Argument that minimizes the LP (None if the problem is unfeasible or unbounded). active_set : list of int Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded). multiplier_inequality : numpy.ndarray Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded). multiplier_equality : numpy.ndarray Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints). """ # check equalities if (C is None) != (d is None): raise ValueError('missing C or d.') # problem size n_ineq, n_x = A.shape if C is not None: n_eq = C.shape[0] else: n_eq = 0 # reshape inputs if len(f.shape) == 2: f = np.reshape(f, f.shape[0]) # build program prog = MathematicalProgram() x = prog.NewContinuousVariables(n_x) inequalities = [] for i in range(n_ineq): lhs = A[i, :] + 1.e-20 * np.random.rand( (n_x) ) # drake raises a RuntimeError if the in the expression x does not appear (e.g.: 0 x <= 1) rhs = b[i] + 1.e-15 * np.random.rand( 1 ) # in case the constraint is 0 x <= 0 the previous trick ends up adding the constraint x <= 0 to the program... inequalities.append(prog.AddLinearConstraint(lhs.dot(x) <= rhs)) for i in range(n_eq): prog.AddLinearConstraint(C[i, :].dot(x) == d[i]) prog.AddLinearCost(f.dot(x)) # solve solver = GurobiSolver() prog.SetSolverOption(solver.solver_type(), "OutputFlag", 0) result = prog.Solve() # initialize output sol = { 'min': None, 'argmin': None, 'active_set': None, 'multiplier_inequality': None, 'multiplier_equality': None } if result == SolutionResult.kSolutionFound: sol['argmin'] = prog.GetSolution(x).reshape(n_x, 1) sol['min'] = f.dot(sol['argmin'])[0] sol['active_set'] = np.where( A.dot(sol['argmin']) - b > -tol)[0].tolist() # retrieve multipliers through KKT conditions M = A[sol['active_set'], :].T if n_eq > 0: M = np.hstack((M, C.T)) m = np.linalg.pinv(M).dot(-f.reshape(n_x, 1)) sol['multiplier_inequality'] = np.zeros((n_ineq, 1)) for i, j in enumerate(sol['active_set']): sol['multiplier_inequality'][j, 0] = m[i, :] if n_eq > 0: sol['multiplier_equality'] = m[len(sol['active_set']):, :] return sol
def achieves_force_closure(points, normals, mu): """ Determines whether the given forces achieve force closure. Note that each of points and normals are lists of variable length, but should be the same length. Here's an example valid set of input args: points = [np.asarray([0.1, 0.2]), np.asarray([0.5,-0.1])] normals = [np.asarray([-1.0,0.0]), np.asarray([0.0, 1.0])] mu = 0.2 achieves_force_closure(points, normals, mu) NOTE: your normals should be normalized (be unit vectors) :param points: a list of arrays where each array is the position of the force points relative to the center of mass. :type points: a list of 1-d numpy arrays of shape (2,) :param normals: a list of arrays where each array is the normal of the local surface of the object, pointing in towards the object :type normals: a list of 1-d numpy arrays of shape (2,) :param mu: coulombic static friction coefficient :type mu: float, greater than or equal to 0 :return: whether or not the given parameters achieves force closure :rtype: bool (True or False) """ assert len(points) == len(normals) assert mu >= 0.0 G = get_G(points, normals) if np.linalg.matrix_rank(G) != 3: return False from pydrake.all import MathematicalProgram from pydrake.solvers import mathematicalprogram as mpa mp = MathematicalProgram() f = mp.NewContinuousVariables(2 * len(points), "f") gamma = mp.NewContinuousVariables(1, "gamma") mp.AddLinearConstraint(gamma[0] <= 0) mp.AddLinearConstraint(gamma[0] >= -1) for i in range(len(points)): mp.AddLinearConstraint(f[2 * i] <= mu * f[2 * i + 1] + gamma[0]) mp.AddLinearConstraint(-f[2 * i] <= mu * f[2 * i + 1] + gamma[0]) mp.AddLinearConstraint(f[2 * i] <= 1000) mp.AddLinearConstraint(f[2 * i] >= -1000) mp.AddLinearConstraint(f[2 * i + 1] + gamma[0] >= 0) mp.AddLinearConstraint(f[2 * i + 1] <= 1000) gf = G.dot(f) #print(gf.shape) assert (gf.shape == (3, )) for i in range(3): mp.AddLinearConstraint(gf[i] == 0) mp.AddLinearCost(gamma[0]) #mp.AddQuadraticCost(np.sum(f**2)) #print(mp) x = mp.Solve() #print(mp.Solve()) if x == mpa.SolutionResult.kSolutionFound: #print(mp.GetSolution(f)) g = mp.GetSolution(gamma) #print(g) #print(g[0] < 0) return g[0] < 0 return False