def CheckLevelSet(self, prev_x, x0, Vs, Vsdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = Vs.Substitute(dict(zip(prev_x, x))) Vdot = Vsdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] #mapping = dict(zip(x, np.ones(len(x)))) #V_norm = 0.0*V #for i in range(len(x)): # basis = np.ones(len(x)) # V_norm = V_norm + V.Substitute(dict(zip(x, basis))) #V = V/V_norm #Vdot = Vdot/V_norm #prog.AddConstraint(V_norm == 0) # in relative state (lambda(xbar) Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) #import pdb; pdb.set_trace() result = Solve(prog) if(not result.is_success()): print('%s, %s' %(result.get_solver_id().name(),result.get_solution_result()) ) print('slack = %f' %(result.GetSolution(slack)) ) print('Rho = %f' %(rho)) #assert result.is_success() return -1.0 return result.GetSolution(slack)
def FixedLyapunovMaximizeLevelSet(self, prog, x, V, Vdot, multiplier_degree=None): ''' Assumes V>0. Vdot >= 0 => V >= rho (or x=0) via maximize rho subject to (V-rho)*x'*x - Lambda*Vdot is SOS, Lambda is SOS. ''' if multiplier_degree is None: # There are no guarantees... this is a reasonable guess: multiplier_degree = Polynomial(Vdot).TotalDegree() print "Using a degree " + str(multiplier_degree) + " multiplier for the S-procedure" # TODO(russt): implement numerical "balancing" from matlab version. Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() rho = prog.NewContinuousVariables(1, "rho")[0] prog.AddSosConstraint((V-rho)*(x.dot(x)) - Lambda*Vdot) prog.AddCost(-rho) # mosek = MosekSolver() # mosek.set_stream_logging(True, 'mosek.out') # result = mosek.Solve(prog, None, None) result = Solve(prog) print "Using " + result.get_solver_id().name() assert result.is_success() assert result.GetSolution(rho) > 0, "Optimization failed (rho <= 0)." return V/result.GetSolution(rho)
def findLyapunovFunctionSOS(self, x0, deg_V, deg_L): prog = MathematicalProgram() # Declare the "indeterminates", x. These are the variables which define the polynomials z = prog.NewIndeterminates(nZ,'z') x = prog.NewIndeterminates(1, 'x')[0] y = prog.NewIndeterminates(1, 'y')[0] thetadot = prog.NewIndeterminates(1, 'thetadot')[0] X = np.array([s, c, thetadot]) sym_system = self.ToSymbolic() sym_context = sym_system.CreateDefaultContext() sym_context.SetContinuousState(z0+z) sym_context.FixInputPort(0, u0+ucon ) f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() # - dztrajdt.value(t).transpose() # Construct a polynomial V that contains all monomials with s,c,thetadot up to degree n. V = prog.NewFreePolynomial(Variables(X), deg_V).ToExpression() eps = 1e-4 constraint1 = prog.AddSosConstraint(V - eps*(X-x0).dot(X-x0)) # constraint to enforce that V is strictly positive away from x0. Vdot = V.Jacobian(X).dot(f) # Construct the polynomial which is the time derivative of V L = prog.NewFreePolynomial(Variables(X), deg_L).ToExpression() # Construct a polynomial L representing the "Lagrange multiplier". constraint2 = prog.AddSosConstraint(-Vdot - L*(x**2+y**2-1) - eps*(X-x0).dot(X-x0)*y**2) # Add a constraint that Vdot is strictly negative away from x0 # Add V(0) = 0 constraint constraint3 = prog.AddLinearConstraint(V.Substitute({y: 0, x: 1, thetadot: 0}) == 0) # Add V(theta=xxx) = 1, just to set the scale. constraint4 = prog.AddLinearConstraint(V.Substitute({y: 1, x: 0, thetadot: 0}) == 1) # Call the solver. result = Solve(prog) Vsol = Polynomial(result.GetSolution(V)) return Vsol
def test_method_3(self): """Test level set for Method 3""" # test level set value up to fourth digit # (note that the degree of the polynomial does not affect precision # in this particular problem) rho_method_3 = self.notebook_locals['rho_method_3'] self.assertEqual(hash(self.round(rho_method_3, 4) * 2), 1404258392611139588, "The level set from Method 3 is incorrect.") # try to solve again the optimization problem # and test that the optimal value of rho is the same # to retrieve rho, find the only optimization variable in the cost prog = self.notebook_locals['prog3'] costs = prog.GetAllCosts() self.assertEqual(len(costs), 1, "prog3 has more than one cost function.") variables = prog.GetAllCosts()[0].variables() self.assertEqual(len(variables), 1, "The cost function of prog3 is incorrect.") rho = variables[0] result = Solve(prog) self.assertAlmostEqual( result.GetSolution(rho), rho_method_3, msg="Solving prog3 we got a different value of rho_method_3.")
def solve_lcp(P, q): prog = MathematicalProgram() x = prog.NewContinuousVariables(q.size) prog.AddLinearComplementarityConstraint(P, q, x) result = Solve(prog) status = result.is_success() z = result.GetSolution(x) return (z, status)
class SosProgram(SosProgramParent, MathematicalProgram): def __init__(self): MathematicalProgram.__init__(self) self.result = None def add_variables(self, size, name='c'): return self.NewContinuousVariables(size, name) def add_psd_variable(self, size, name='Q'): gram = self.NewSymmetricContinuousVariables(size, name) cons = self.AddPositiveSemidefiniteConstraint(gram) return gram, cons def add_linear_constraint(self, cons): self.AddLinearConstraint(cons) def add_linear_cost(self, expr): self.AddLinearCost(expr) def solve(self): self.result = Solve(self) def minimum(self): if self.result is not None: return self.result.get_optimal_cost() def substitute_minimizer(self, expr): if isinstance(expr, Polynomial): p_opt = Polynomial({}) for v, c in expr: c_opt = self.result.GetSolution(c) if isinstance(c_opt, Expression): c_opt = c_opt.Evaluate() p_opt[v] = c_opt return p_opt else: return self.result.GetSolution(expr)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1)[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) assert result.is_success() return result.GetSolution(slack)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): #import pdb; pdb.set_trace() prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1,'a')[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() #print('V degree: %d' %(Polynomial(V).TotalDegree())) #print('Vdot degree: %d' %(Polynomial(Vdot).TotalDegree())) #print('SOS degree: %d' %(Polynomial(-Vdot + Lambda*(V - rho) - slack*V).TotalDegree())) prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) #assert result.is_success() return result.GetSolution(slack)
def get_ik(self, t): epsilon = 1e-2 ik = InverseKinematics(plant=self.plant, with_joint_limits=True) if self.q_guess is None: context = self.plant.CreateDefaultContext() self.q_guess = self.plant.GetPositions(context) # This helps get the solver out of the saddle point when knee joint are locked (0.0) self.q_guess[getJointIndexInGeneralizedPositions( self.plant, 'l_leg_kny')] = 0.1 self.q_guess[getJointIndexInGeneralizedPositions( self.plant, 'r_leg_kny')] = 0.1 for trajectory in self.trajectories: position = trajectory.position_traj.value(t) ik.AddPositionConstraint( frameB=self.plant.GetFrameByName(trajectory.frame), p_BQ=trajectory.position_offset, frameA=self.plant.world_frame(), p_AQ_upper=position + trajectory.position_tolerance, p_AQ_lower=position - trajectory.position_tolerance) if trajectory.orientation_traj is not None: orientation = trajectory.orientation_traj.value(t) ik.AddOrientationConstraint( frameAbar=self.plant.world_frame(), R_AbarA=RotationMatrix.Identity(), frameBbar=self.plant.GetFrameByName(trajectory.frame), R_BbarB=RotationMatrix(orientation), theta_bound=trajectory.orientation_tolerance) result = Solve(ik.prog(), self.q_guess) if result.is_success(): return result.GetSolution(ik.q()) else: raise Exception("Failed to find IK solution!")
def ComputeControlInput(self, x, t): # Set up things you might want... q = x[0:self.nq] v = x[self.nq:] kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()]) M = self.hand.massMatrix(kinsol) C = self.hand.dynamicsBiasTerm(kinsol, {}, None) B = self.hand.B # Assume grasp points are achieved, and calculate # contact jacobian at those points, as well as the current # grasp points and normals (in WORLD FRAME). grasp_points_world_now = self.transform_grasp_points_manipuland(q) grasp_normals_world_now = self.transform_grasp_normals_manipuland(q) J_manipuland = jacobian( self.transform_grasp_points_manipuland, q) ee_points_now = self.transform_grasp_points_fingers(q) J_manipulator = jacobian( self.transform_grasp_points_fingers, q) # The contact jacobian (Phi), for contact forces coming from # point contacts, describes how the movement of the contact point # maps into the joint coordinates of the robot. We can get this # by combining the jacobians derived from forward kinematics to each # of the two bodies that are in contact, evaluated at the contact point # in each body's frame. J_contact = J_manipuland - J_manipulator # Cut off the 3rd dimension, which should always be 0 J_contact = J_contact[0:2, :, :] # Given these, the manipulator equations enter as a linear constraint # M qdd + C = Bu + J_contact lambda # (the combined effects of lambda on the robot). # The list of grasp points, in manipuland frame, that we'll use. n_cf = len(self.grasp_points) number_of_grasp_points = len(self.grasp_points) # The evaluated desired manipuland posture. manipuland_qdes = self.GetDesiredObjectPosition(t) # The desired robot (arm) posture, calculated via inverse kinematics # to achieve the desired grasp points on the object in its current # target posture. qdes, info = self.ComputeTargetPosture(x, manipuland_qdes) # print('shape' + str(np.shape(qdes))) # print('self.nq' + str(self.nq)) # print('self.nu' + str(self.nu)) if info != 1: if not self.shut_up: print "Warning: target posture IK solve got info %d " \ "when computing goal posture at least once during " \ "simulation. This means the grasp points was hard to " \ "achieve given the current object posture. This is " \ "occasionally OK, but indicates that your controller " \ "is probably struggling a little." % info self.shut_up = True # From here, it's up to you. Following the guidelines in the problem # set, implement a controller to achieve the specified goals. kd = 1 kp = 25 from pydrake.all import MathematicalProgram, Solve mp = MathematicalProgram() u = mp.NewContinuousVariables(self.nu, "u") qdd = mp.NewContinuousVariables(self.nq, "qdd") x_y_dimensions = 2 lambda_variable = mp.NewContinuousVariables(x_y_dimensions, number_of_grasp_points, "lambda_variable") forces = np.zeros((self.nq)) for i in range(number_of_grasp_points): current_forces = np.transpose(J_contact)[:,i,:].dot(lambda_variable[:,i]) forces = forces + current_forces leftHandSide = M.dot(qdd) + C rightHandSide = B.dot(u) + forces for i in range(len(leftHandSide)): mp.AddConstraint(leftHandSide[i] == rightHandSide[i]) # Calculate Normals normals = np.array([]) for i in range(number_of_grasp_points): current_grasp_normal = grasp_normals_world_now[0:2, i] normals = np.vstack((normals, current_grasp_normal)) if normals.size else current_grasp_normal # Calculate Tangeants tangeants = np.array([]) for i in range(number_of_grasp_points): current_grasp_tangeant = np.array([normals[i, 1], -normals[i, 0]]) tangeants = np.vstack((tangeants, current_grasp_tangeant)) if tangeants.size else current_grasp_tangeant # Create beta variables beta = mp.NewContinuousVariables(2, number_of_grasp_points, "b0") # Create Grasps for i in range(number_of_grasp_points): c0 = normals[i] - self.mu * tangeants[i] c1 = normals[i] + self.mu * tangeants[i] right_hand_lambda1 = c0 * beta[0, i] + c1 * beta[1, i] mp.AddConstraint(lambda_variable[0, i] == right_hand_lambda1[0]) mp.AddConstraint(lambda_variable[1, i] == right_hand_lambda1[1]) mp.AddConstraint(beta[0, i] >= 0) mp.AddConstraint(beta[1, i] >= 0) mp.AddConstraint(normals[i].dot(lambda_variable[:, i]) >= 0.25) # Copying the control period of the constructor. Probably not supposed to do this... next_tick_qd = v + qdd * self.cont¨rol_period next_tick_q = q + next_tick_qd * self.control_period q_error = qdes - next_tick_q proportionalCost = q_error.dot(np.transpose(q_error)) qd_error = 0 - next_tick_qd diffCost = qd_error.dot(np.transpose(qd_error)) mp.AddQuadraticCost(kp * proportionalCost + kd * diffCost) result = Solve(mp) u_solution = result.GetSolution(u) u = np.zeros(self.nu) return u_solution
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
def create_q_knots(pose_lst): """Convert end-effector pose list to joint position list using series of InverseKinematics problems. Note that q is 9-dimensional because the last 2 dimensions contain gripper joints, but these should not matter to the constraints. @param: pose_lst (python list): post_lst[i] contains keyframe X_WG at index i. @return: q_knots (python_list): q_knots[i] contains IK solution that will give f(q_knots[i]) \approx pose_lst[i]. """ q_knots = [] plant, _ = CreateIiwaControllerPlant() world_frame = plant.world_frame() gripper_frame = plant.GetFrameByName("body") #q_nominal = np.array([ 0., 0.6, 0., -1.75, 0., 1., 0., 0., 0.]) # nominal joint for joint-centering. q_nominal = np.array( [-1.57, 0.1, 0.00, -1.2, 0.00, 1.60, 0.00, 0.00, 0.00]) def AddOrientationConstraint(ik, R_WG, bounds): """Add orientation constraint to the ik problem. Implements an inequality constraint where the axis-angle difference between f_R(q) and R_WG must be within bounds. Can be translated to: ik.prog().AddBoundingBoxConstraint(angle_diff(f_R(q), R_WG), -bounds, bounds) """ ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=R_WG, frameBbar=gripper_frame, R_BbarB=RotationMatrix(), theta_bound=bounds) def AddPositionConstraint(ik, p_WG_lower, p_WG_upper): """Add position constraint to the ik problem. Implements an inequality constraint where f_p(q) must lie between p_WG_lower and p_WG_upper. Can be translated to ik.prog().AddBoundingBoxConstraint(f_p(q), p_WG_lower, p_WG_upper) """ ik.AddPositionConstraint(frameA=world_frame, frameB=gripper_frame, p_BQ=np.zeros(3), p_AQ_lower=p_WG_lower, p_AQ_upper=p_WG_upper) for i in range(len(pose_lst)): # if i % 100 == 0: # print(i) ik = inverse_kinematics.InverseKinematics(plant) q_variables = ik.q() # Get variables for MathematicalProgram prog = ik.prog() # Get MathematicalProgram #### Modify here ############################### X_WG = pose_lst[i] p_WG = X_WG.translation() r_WG = X_WG.rotation() z_slack = 0 degrees_slack = 0 AddPositionConstraint(ik, p_WG - np.array([0, 0, z_slack]), p_WG + np.array([0, 0, z_slack])) AddOrientationConstraint(ik, r_WG, degrees_slack * np.pi / 180) # initial guess if i == 0: prog.SetInitialGuess(q_variables, q_nominal) diff = q_variables - q_nominal else: prog.SetInitialGuess(q_variables, q_knots[i - 1]) diff = q_variables - q_knots[i - 1] prog.AddCost(np.sum(diff.dot(diff))) ################################################ result = Solve(prog) if not result.is_success(): visualize_transform(meshcat, "FAIL", X_WG, prefix='', length=0.3, radius=0.02) print(f"Failed at {i}") break #raise RuntimeError tmp = result.GetSolution(q_variables) q_knots.append(tmp) return q_knots
def pose_to_jointangles(T_world_robotPose): plant, _ = CreateIiwaControllerPlant() plant_context = plant.CreateDefaultContext() world_frame = plant.world_frame() gripper_frame = plant.GetFrameByName("body") #q_nominal = np.array([ 0., 0.6, 0., -1.75, 0., 1., 0., 0., 0.]) # nominal joint for joint-centering. q_nominal = np.array( [-1.57, 0.1, 0.00, -1.2, 0.00, 1.60, 0.00, 0.00, 0.00]) def AddOrientationConstraint(ik, R_WG, bounds): ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=R_WG, frameBbar=gripper_frame, R_BbarB=RotationMatrix(), theta_bound=bounds) def AddPositionConstraint(ik, p_WG_lower, p_WG_upper): ik.AddPositionConstraint(frameA=world_frame, frameB=gripper_frame, p_BQ=np.zeros(3), p_AQ_lower=p_WG_lower, p_AQ_upper=p_WG_upper) # def AddJacobianConstraint_Joint_To_Plane(ik): # # calculate the jacobian # J_G = plant.CalcJacobianSpatialVelocity( # ik.context(), # JacobianWrtVariable.kQDot, # gripper_frame, # [0,0,0], # world_frame, # world_frame # ) # # ensure that when joints 4 and 6 move, they keep the gripper in the desired plane # prog = ik.get_mutable_prog() # prog.AddConstraint() # joint_4 = J_G[:, 3] # joint_6 = J_G[:, 5] ik = inverse_kinematics.InverseKinematics(plant) q_variables = ik.q() # Get variables for MathematicalProgram prog = ik.prog() # Get MathematicalProgram p_WG = T_world_robotPose.translation() r_WG = T_world_robotPose.rotation() # must be an exact solution z_slack = 0 degrees_slack = 0 AddPositionConstraint(ik, p_WG - np.array([0, 0, z_slack]), p_WG + np.array([0, 0, z_slack])) AddOrientationConstraint(ik, r_WG, degrees_slack * np.pi / 180) # todo: add some sort of constraint so that jacobian is 0 in certain directions # (so just joints 4 and 6 move when swung) # initial guess prog.SetInitialGuess(q_variables, q_nominal) diff = q_variables - q_nominal prog.AddCost(np.sum(diff.dot(diff))) result = Solve(prog) if not result.is_success(): #visualize_transform(meshcat, "FAIL", X_WG, prefix='', length=0.3, radius=0.02) assert (False) # no IK solution for target jas = result.GetSolution(q_variables) return jas
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 TimeVaryingLyapunovSearchRho(self, prev_x, Vs, Vdots, Ts, times, xtraj, utraj, \ rho_f, multiplier_degree=None): C = 1.0 #8.0 #rho_f = 3.0 tries = 40 prev_rhointegral = 0. N = len(times)-1 #Vmin = self.minimumV(prev_x, Vs) #0.05*np.ones((1, len(times))) # need to do something here instead of this!! dt = np.diff(times) #rho = np.flipud(rho_f*np.exp(-C*(np.array(times)-times[0])/(times[-1]-times[0])))# + np.max(Vmin) #rho = np.linspace(0.1, rho_f, N+1) #rho = np.linspace(rho_f/2.0, rho_f, N+1) rho = np.linspace(rho_f*3.0, rho_f, N+1) fig, ax = plt.subplots() fig.suptitle('Rho progression') ax.set(xlabel='index', ylabel='rho') ax.grid(True) plt.show(block = False) need_to_break = False for idx in range(tries): print('starting iteration #%d with rho=' %(idx)) print(rho) ax.plot(rho) plt.pause(0.05) # start of number of iterations if we want rhodot = np.diff(rho)/dt # sampleCheck() Lambda_vec = [] x_vec = [] #import pdb; pdb.set_trace() #fix rho, optimize Lagrange multipliers for i in range(N): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = Vs[i].Substitute(dict(zip(prev_x, x))) Vdot = Vdots[i].Substitute(dict(zip(prev_x, x))) x0 = xtraj.value(times[i]).transpose()[0] Ttrans = np.linalg.inv(Ts[i]) x0 = Ttrans.dot(x0) #xmin, vmin, vdmin = self.SampleCheck(x, V, Vdot) #if(vdmin > rhodot[i]): # print('Vdot is greater than rhodot!') #Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() Lambda = prog.NewFreePolynomial(Variables(x), multiplier_degree).ToExpression() Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar) gamma = prog.NewContinuousVariables(1,'g')[0] # Jdot-rhodot+Lambda*(rho-J) < -gamma prog.AddSosConstraint( -gamma*V - (Vdot-rhodot[i] + Lambda*(rho[i]-V)) ) prog.AddCost(-gamma) #maximize gamma result = Solve(prog) if result.is_success() == False: need_to_break = True print('Solver could not solve anymore') import pdb; pdb.set_trace() break else: Lambda_vec.append(result.GetSolution(Lambda)) slack = result.GetSolution(gamma) print('Slack #%d = %f' %(idx, slack)) x_vec.append(x) if(slack < 0.0): print('In iter#%d, found negative slack so going to end prematurely... :(' %(idx)) need_to_break = True if(need_to_break == True): break; # fix Lagrange multipliers, maximize rho rhointegral = 0.0 prog = MathematicalProgram() xx = prog.NewIndeterminates(len(x),'x') t = prog.NewContinuousVariables(N,'t') #import pdb; pdb.set_trace() #rho = np.concatenate((t,[rho_f])) + Vmin rho_x = np.concatenate((t,[rho[-1]])) #+ rho #import pdb; pdb.set_trace() for i in range(N): #prog.AddConstraint(t[i]>=0.0) # does this mean [prog,rho] = new(prog,N,'pos'); in matlab?? rhod_x = (rho_x[i+1]-rho_x[i])/dt[i] #prog.AddConstraint(rhod_x<=0.0) rhointegral = rhointegral+rho_x[i]*dt[i] + 0.5*rhod_x*(dt[i]**2) V = Vs[i].Substitute(dict(zip(prev_x, xx))) Vdot = Vdots[i].Substitute(dict(zip(prev_x, xx))) #x0 = xtraj.value(times[i]).transpose()[0] L1 = Lambda_vec[i].Substitute(dict(zip(x_vec[i], xx))) #Vdot = Vdot*rho_x[i] - V*rhod_x prog.AddSosConstraint( -(Vdot - rhod_x + L1 * ( rho_x[i]-V ) ) ) prog.AddCost(-rhointegral) result = Solve(prog) assert result.is_success() rhos = result.GetSolution(rho_x) rho = [] for r in rhos: rho.append(r[0].Evaluate()) rhointegral = result.GetSolution(rhointegral).Evaluate() if( (rhointegral-prev_rhointegral)/rhointegral < 1E-5): # 0.1% print('Rho integral converged') need_to_break = True break; else: prev_rhointegral = rhointegral print('End of iteration #%d: rhointegral=%f' %(idx, rhointegral)) if(need_to_break == True): print('In iter#%d, found negative slack so ending prematurely... :(' %(idx)) break; # end of iterations if we want ax.plot(rho) plt.pause(0.05) print('done computing funnel.\nFinal rho= ') print(rho) #import pdb; pdb.set_trace() for i in range(len(rho)): Vs[i] = Vs[i]/rho[i] return Vs
a = 0.5*Jacobian(V1.Jacobian(x),x) b = 0.5*Jacobian(V2.Jacobian(x),x) pxi = np.array([[-0.5,-0.5], [-0.5, 0.5], [0.5,0.5], [0.5,-0.5]]) for pt in pxi: prog.AddConstraint( pt.T.dot(a).dot(pt) <= 1.0) prog.AddConstraint( pt.T.dot(b).dot(pt) <= 1.0) pxi = np.array([[0.0, 1.0] ]) prog.AddConstraint( pxi[-1].T.dot(b).dot(pxi[-1]) <= 1.0) prog.AddMaximizeLogDeterminantSymmetricMatrixCost(a) prog.AddMaximizeLogDeterminantSymmetricMatrixCost(b) result = Solve(prog) assert(result.is_success()) V1 = result.GetSolution(V1) V2 = result.GetSolution(V2) fig1, ax1 = plt.subplots() ax1.set_xlim([-5.0, 5.0]) ax1.set_ylim([-5.0, 5.0]) ax1.set_xlabel('x') ax1.set_ylabel('y') sublevelset(ax1, V2, np.array([0,0]), vertices=51, color=(0.1,0.8,0.8)) sublevelset(ax1, V1, np.array([0,0]), vertices=51, color=(0.1,0.5,0.8)) plt.show(block = True)
# 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.')
# 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 The SOS program we just wrote was already a satisfying solution, but it turns out we can do even better! From the textbook chapter "[Lyapunov analysis with convex optimization](http://underactuated.mit.edu/lyapunov.html#section2)", you know that every SOS constraint brings with it a lot of optimization variables and an SDP constraint. So, whenever we can, removing redundant SOS requirements is always a good thing to do. We claim that in the previous formulation we don't need $\lambda(\mathbf{x})$ to be SOS. How is this possible? We start by noticing that, in a neighborhood of the origin, excluded the origin itself, $\dot{V}(\mathbf{x})$ is strictly negative. (This because $V(\mathbf{x})$ is a Lyapunov function for the linearized system hence, locally, it works also for the nonlinear system.) In light of the latter observation, instead of asking that $\dot{V}(\mathbf{x})$ is negative for all $\mathbf{x} \neq 0$ in $L(\rho)$, we can equivalently ask that all the points $\mathbf{x} \neq 0$ where $\dot{V}(\mathbf{x}) = 0$ must be outside the level set $L(\rho)$.
def ComputeControlInput(self, x, t): # Set up things you might want... q = x[0:self.nq] v = x[self.nq:] kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()]) M = self.hand.massMatrix(kinsol) C = self.hand.dynamicsBiasTerm(kinsol, {}, None) B = self.hand.B # Assume grasp points are achieved, and calculate # contact jacobian at those points, as well as the current # grasp points and normals (in WORLD FRAME). grasp_points_world_now = self.transform_grasp_points_manipuland(q) grasp_normals_world_now = self.transform_grasp_normals_manipuland(q) J_manipuland = jacobian(self.transform_grasp_points_manipuland, q) ee_points_now = self.transform_grasp_points_fingers(q) J_manipulator = jacobian(self.transform_grasp_points_fingers, q) # The contact jacobian (Phi), for contact forces coming from # point contacts, describes how the movement of the contact point # maps into the joint coordinates of the robot. We can get this # by combining the jacobians derived from forward kinematics to each # of the two bodies that are in contact, evaluated at the contact point # in each body's frame. J_contact = J_manipuland - J_manipulator # Cut off the 3rd dimension, which should always be 0 J_contact = J_contact[0:2, :, :] # Given these, the manipulator equations enter as a linear constraint # M qdd + C = Bu + J_contact lambda # (the combined effects of lambda on the robot). # The list of grasp points, in manipuland frame, that we'll use. n_cf = len(self.grasp_points) # The evaluated desired manipuland posture. manipuland_qdes = self.GetDesiredObjectPosition(t) # The desired robot (arm) posture, calculated via inverse kinematics # to achieve the desired grasp points on the object in its current # target posture. qdes, info = self.ComputeTargetPosture(x, manipuland_qdes) if info != 1: if not self.shut_up: print "Warning: target posture IK solve got info %d " \ "when computing goal posture at least once during " \ "simulation. This means the grasp points was hard to " \ "achieve given the current object posture. This is " \ "occasionally OK, but indicates that your controller " \ "is probably struggling a little." % info self.shut_up = True # From here, it's up to you. Following the guidelines in the problem # set, implement a controller to achieve the specified goals. mp = MathematicalProgram() #control variables = u = self.nu x 1 #ddot{q} as variable w/ q's shape controls = mp.NewContinuousVariables(self.nu, "controls") qdd = mp.NewContinuousVariables(q.shape[0], "qdd") #make variables for lambda's and beta's k = 0 b = mp.NewContinuousVariables(2, "betas_%d" % k) betas = b for i in range(len(self.grasp_points)): b = mp.NewContinuousVariables( 2, "betas_%d" % k) #pair of betas for each contact point betas = np.vstack((betas, b)) mp.AddLinearConstraint( betas[i, 0] >= 0).evaluator().set_description( "beta_0 greater than 0 for %d" % i) mp.AddLinearConstraint( betas[i, 1] >= 0).evaluator().set_description( "beta_1 greater than 0 for %d" % i) #c_0=normals+mu*tangent #c1 = normals-mu*tangent n = grasp_normals_world_now.T[:, 0:2] #row=contact point? t = get_perpendicular2d(n[0]) c_i1 = n[0] + np.dot(self.mu, t) c_i2 = n[0] - np.dot(self.mu, t) l = c_i1.dot(betas[0, 0]) + c_i2.dot(betas[0, 1]) lambdas = l for i in range(1, len(self.grasp_points)): n = grasp_normals_world_now.T[:, 0: 2] #row=contact point? transpose then index t = get_perpendicular2d(n[i]) c_i1 = n[i] - np.dot(self.mu, t) c_i2 = n[i] + np.dot(self.mu, t) l = c_i1.dot(betas[i, 0]) + c_i2.dot(betas[i, 1]) lambdas = np.vstack((lambdas, l)) control_period = 0.0333 #Constrain the dyanmics dyn = M.dot(qdd) + C - B.dot(controls) for i in range(q.shape[0]): j_c = 0 for l in range(len(self.grasp_points)): j_sub = J_contact[:, l, :].T j_c += j_sub.dot(lambdas[l]) # print(j_c.shape) # print(dyn.shape) mp.AddConstraint(dyn[i] - j_c[i] == 0) #PD controller using kinematics Kp = 100.0 Kd = 1.0 control_period = 0.0333 next_q = q + v.dot(control_period) + np.dot(qdd.dot(control_period**2), .5) next_q_dot = v + qdd.dot(control_period) mp.AddQuadraticCost(Kp * (qdes - next_q).dot((qdes - next_q).T) + Kd * (next_q_dot).dot(next_q_dot.T)) Kp_ext = 100. mp.AddQuadraticCost(Kp_ext * (qdes - next_q)[-3:].dot( (qdes - next_q)[-3:].T)) res = Solve(mp) c = res.GetSolution(controls) return c
def bounce_pass_wall(self, p_puck, p_goal, which_wall, duration=3, debug=True): """Solve for initial velocity for the puck to bounce the wall once and hit the goal.""" # initialize program prog = MathematicalProgram() # time periods before and after hitting the wall h = prog.NewContinuousVariables(2, name='h') # velocities of the ball at the start of each segment (after collision). vel_start = prog.NewContinuousVariables(2, name='vel_start') vel_after = prog.NewContinuousVariables(2, name='vel_after') # sum of durations cannot exceed the specified value prog.AddConstraint(np.sum(h) <= duration) # Help the solver by telling it initial velocity direction self.add_initial_vel_direction_constraint(prog, which_wall, p_goal, vel_start) # add dynamics constraints for the moment the ball hits the wall p_contact = self.next_p(p_puck, vel_start, h[0]) v_contact = self.next_vel(vel_start, h[0]) # keep the same x vel, but flip the y vel self.add_reset_map_constraint(prog, which_wall, p_contact, v_contact, vel_after) # in the last segment, need to specify bounds for the final position and velocity of the ball p_end = self.next_p(p_contact, vel_after, h[1]) v_end = self.next_vel(vel_after, h[1]) self.add_goal_constraint(prog, which_wall, p_goal, p_end, v_end) # solve for time periods h result = Solve(prog) if not result.is_success(): if debug: infeasible = GetInfeasibleConstraints(prog, result) print("Infeasible constraints in ContactOptimizer:") for i in range(len(infeasible)): print(infeasible[i]) # return directly return else: if debug: print("vel_start:{}".format(result.GetSolution(vel_start))) print("vel_after: {}".format(result.GetSolution(vel_after))) print("h: {}".format(result.GetSolution(h))) p1 = self.next_p(p_puck, result.GetSolution(vel_start), result.GetSolution(h[0])) p2 = self.next_p(p1, result.GetSolution(vel_after), result.GetSolution(h[1])) print("p1:{}".format(p1)) print("p2:{}".format(p2)) v_end = self.next_vel(result.GetSolution(vel_after), result.GetSolution(h[1])) print("v_end:{}".format(v_end)) # return whether successful, and the initial puck velocity return result.is_success(), result.GetSolution(vel_start)
# minima, two of them being global minima. p = 4*x**2+x*y - 4*y**2 - 2.1*x**4 + 4*y**4+x**6/3 # Find the minimum value by adding a sums of squares constraint, via # for all x, p(x) >= pmin # which we write as # p(x) - pmin is sos. pmin = prog.NewContinuousVariables(1, "pmin")[0] prog.AddSosConstraint(p-pmin) # Maximize pmin. prog.AddCost(-pmin) result = Solve(prog) assert(result.is_success()) print("Minimum value (lower bound): " + str(result.GetSolution(pmin))) # Now, let's plot it. fig = plt.figure(figsize=(10, 5)) ax0 = fig.add_subplot(121, projection='3d') ax1 = fig.add_subplot(122) xs = np.linspace(-2.2, 2.2, 51) ys = np.linspace(-1.2, 1.2, 51) [X, Y] = np.meshgrid(xs, ys) P = X.copy() for i in range(len(xs)): for j in range(len(ys)): P[i, j] = p.Evaluate({x: X[i, j], y: Y[i, j]}) ax0.plot_surface(X, Y, P) ax1.contour(X, Y, P, 100)
# Add V(theta=pi) = mgl, just to set the scale. constraint4 = prog.AddLinearConstraint( V.Substitute({s: 1, c: 0, thetadot: 0}) == p.mass()*p.gravity()*p.length()) # Call the solver. result = Solve(prog) assert(result.is_success()) # Note that I've added mgl to the potential energy (relative to the textbook), # so that it would be non-negative... like the Lyapunov function. mgl = p.mass()*p.gravity()*p.length() print('Mechanical Energy = ') print(.5*p.mass()*p.length()**2*thetadot**2 + mgl*(1-c)) print('V =') Vsol = Polynomial(result.GetSolution(V)) print(Vsol.RemoveTermsWithSmallCoefficients(1e-6)) # Plot the results as contour plots. nq = 151 nqd = 151 q = np.linspace(-2*np.pi, 2*np.pi, nq) qd = np.linspace(-2*mgl, 2*mgl, nqd) Q, QD = np.meshgrid(q, qd) Energy = .5*p.mass()*p.length()**2*QD**2 + mgl*(1-np.cos(Q)) Vplot = Q.copy() env = {s: 0., c: 1., thetadot: 0} for i in range(nq): for j in range(nqd): env[s] = np.sin(Q[i, j])
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 compute_trajectory_to_other_world(self, state_initial, minimum_time, maximum_time): ''' Your mission is to implement this function. A successful implementation of this function will compute a dynamically feasible trajectory which satisfies these criteria: - Efficiently conserve fuel - Reach "orbit" of the far right world - Approximately obey the dynamic constraints - Begin at the state_initial provided - Take no more than maximum_time, no less than minimum_time The above are defined more precisely in the provided notebook. Please note there are two return args. :param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation :param: minimum_time: float, minimum time allowed for trajectory :param: maximum_time: float, maximum time allowed for trajectory :return: three return args separated by commas: trajectory, input_trajectory, time_array trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation. input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation. time_array: an array with N rows. ''' mp = MathematicalProgram() max_speed = 0.99 desired_distance = 0.5 u_cost_factor = 1000. N = 50 # trajectory = np.zeros((N+1,4)) # input_trajectory = np.ones((N,2))*10.0 time_used = (maximum_time + minimum_time) / 2. time_step = time_used / (N + 1) time_array = np.arange(0.0, time_used, time_step) k = 0 # Create continuous variables for u & x u = mp.NewContinuousVariables(2, "u_%d" % k) x = mp.NewContinuousVariables(4, "x_%d" % k) u_over_time = u x_over_time = x for k in range(1, N): u = mp.NewContinuousVariables(2, "u_%d" % k) x = mp.NewContinuousVariables(4, "x_%d" % k) u_over_time = np.vstack((u_over_time, u)) x_over_time = np.vstack((x_over_time, x)) # trajectory is N+1 in size x = mp.NewContinuousVariables(4, "x_%d" % (N + 1)) x_over_time = np.vstack((x_over_time, x)) # Add cost # for k in range(0, N): mp.AddQuadraticCost(u_cost_factor * u_over_time[:, 0].dot(u_over_time[:, 0])) mp.AddQuadraticCost(u_cost_factor * u_over_time[:, 1].dot(u_over_time[:, 1])) # Add constraint for initial state mp.AddLinearConstraint(x_over_time[0, 0] >= state_initial[0]) mp.AddLinearConstraint(x_over_time[0, 0] <= state_initial[0]) mp.AddLinearConstraint(x_over_time[0, 1] >= state_initial[1]) mp.AddLinearConstraint(x_over_time[0, 1] <= state_initial[1]) mp.AddLinearConstraint(x_over_time[0, 2] >= state_initial[2]) mp.AddLinearConstraint(x_over_time[0, 2] <= state_initial[2]) mp.AddLinearConstraint(x_over_time[0, 3] >= state_initial[3]) mp.AddLinearConstraint(x_over_time[0, 3] <= state_initial[3]) # Add constraint between x & u for k in range(1, N + 1): next_step = self.rocket_dynamics(x_over_time[k - 1, :], u_over_time[k - 1, :]) mp.AddConstraint(x_over_time[k, 0] == x_over_time[k - 1, 0] + time_step * next_step[0]) mp.AddConstraint(x_over_time[k, 1] == x_over_time[k - 1, 1] + time_step * next_step[1]) mp.AddConstraint(x_over_time[k, 2] == x_over_time[k - 1, 2] + time_step * next_step[2]) mp.AddConstraint(x_over_time[k, 3] == x_over_time[k - 1, 3] + time_step * next_step[3]) # Make sure it never goes too far from the planets # for k in range(1, N): # mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_2_position[:]) <= 10) # mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_1_position[:]) <= 10) # Make sure u never goes above a threshold max_u = 6. for k in range(0, N): mp.AddLinearConstraint(u_over_time[k, 0] <= max_u) mp.AddLinearConstraint(-u_over_time[k, 0] <= max_u) mp.AddLinearConstraint(u_over_time[k, 1] <= max_u) mp.AddLinearConstraint(-u_over_time[k, 1] <= max_u) # Make sure it reaches world 2 mp.AddConstraint( self.two_norm(x_over_time[-1, 0:2] - self.world_2_position) <= desired_distance) mp.AddConstraint( self.two_norm(x_over_time[-1, 0:2] - self.world_2_position) >= desired_distance) # Make sure it's speed isn't too high mp.AddConstraint(self.two_norm(x_over_time[-1, 2:4]) <= max_speed**2.) # Get result result = Solve(mp) x_over_time_result = result.GetSolution(x_over_time) u_over_time_result = result.GetSolution(u_over_time) print("Success", result.is_success()) print("Final position", x_over_time_result[-1, :]) print( "Final distance to world2", self.two_norm(x_over_time_result[-1, 0:2] - self.world_2_position)) print("Final speed", self.two_norm(x_over_time_result[-1, 2:4])) print("Fuel consumption", (u_over_time_result**2.).sum()) trajectory = x_over_time_result input_trajectory = u_over_time_result return trajectory, input_trajectory, time_array
def findLyapunovFunctionSOS(self, xtraj, utraj, deg_V, deg_L): times = xtraj.get_segment_times() prog = MathematicalProgram() # Declare the "indeterminates", x. These are the variables which define the polynomials x = prog.NewIndeterminates(3, 'x') ucon = prog.NewIndeterminates(2, 'u') sym_system = self.ToSymbolic() sym_context = sym_system.CreateDefaultContext() sym_context.SetContinuousState(x) sym_context.FixInputPort(0, ucon) f = sym_system.EvalTimeDerivativesTaylor( sym_context).CopyToVector() # - dztrajdt.value(t).transpose() for t in times: x0 = xtraj.value(t).transpose()[0] u0 = utraj.value(t).transpose()[0] f_fb = self.EvalClosedLoopDynamics(x, ucon, f, x0, u0) # Construct a polynomial V that contains all monomials with s,c,thetadot up to degree n. V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression() eps = 1e-4 constraint1 = prog.AddSosConstraint( V - eps * (x - x0).dot(x - x0) ) # constraint to enforce that V is strictly positive away from x0. Vdot = V.Jacobian(x).dot( f_fb ) # Construct the polynomial which is the time derivative of V #L = prog.NewFreePolynomial(Variables(x), deg_L).ToExpression() # Construct a polynomial L representing the # "Lagrange multiplier". # Add a constraint that Vdot is strictly negative away from x0 constraint2 = prog.AddSosConstraint(-Vdot[0] - eps * (x - x0).dot(x - x0)) #import pdb; pdb.set_trace() # Add V(0) = 0 constraint constraint3 = prog.AddLinearConstraint( V.Substitute({ x[0]: 0.0, x[1]: 1.0, x[2]: 0.0 }) == 1.0) # Add V(theta=xxx) = 1, just to set the scale. constraint4 = prog.AddLinearConstraint( V.Substitute({ x[0]: 1.0, x[1]: 0.0, x[2]: 0.0 }) == 1.0) # Call the solver (default). #result = Solve(prog) # Call the solver (specific). #solver = CsdpSolver() #solver = ScsSolver() #solver = IpoptSolver() #result = solver.Solve(prog, None, None) result = Solve(prog) # print out the result. print("Success? ", result.is_success()) print(result.get_solver_id().name()) Vsol = Polynomial(result.GetSolution(V)) print('Time t=%f:\nV=' % (t)) print(Vsol.RemoveTermsWithSmallCoefficients(1e-6)) return Vsol, Vsol.Jacobian(x).dot(f_fb)
def CalcOutput(self, context, output): # ============================ LOAD INPUTS ============================ # Torque inputs tau_g = np.expand_dims( np.array(self.GetInputPort("tau_g").Eval(context)), 1) joint_centering_torque = np.expand_dims( np.array( self.GetInputPort("joint_centering_torque").Eval(context)), 1) # Force inputs # PROGRAMMING: Add zeros here? F_GT = self.GetInputPort("F_GT").Eval(context)[0] F_GN = self.GetInputPort("F_GN").Eval(context)[0] # Positions theta_L = self.GetInputPort("theta_L").Eval(context)[0] theta_MX = self.GetInputPort("theta_MX").Eval(context)[0] theta_MY = self.GetInputPort("theta_MY").Eval(context)[0] theta_MZ = self.GetInputPort("theta_MZ").Eval(context)[0] p_CT = self.GetInputPort("p_CT").Eval(context)[0] p_CN = self.GetInputPort("p_CN").Eval(context)[0] p_LT = self.GetInputPort("p_LT").Eval(context)[0] p_LN = self.GetInputPort("p_LN").Eval(context)[0] d_T = self.GetInputPort("d_T").Eval(context)[0] d_N = self.GetInputPort("d_N").Eval(context)[0] d_X = self.GetInputPort("d_X").Eval(context)[0] p_MConM = np.array([self.GetInputPort("p_MConM").Eval(context)]).T # Velocities d_theta_L = self.GetInputPort("d_theta_L").Eval(context)[0] d_theta_MX = self.GetInputPort("d_theta_MX").Eval(context)[0] d_theta_MY = self.GetInputPort("d_theta_MY").Eval(context)[0] d_theta_MZ = self.GetInputPort("d_theta_MZ").Eval(context)[0] d_d_T = self.GetInputPort("d_d_T").Eval(context)[0] d_d_N = self.GetInputPort("d_d_N").Eval(context)[0] d_d_X = self.GetInputPort("d_d_X").Eval(context)[0] # Manipulator inputs J = np.array(self.GetInputPort("J").Eval(context)).reshape( (6, self.nq)) J_translational = np.array( self.GetInputPort("J_translational").Eval(context)).reshape( (3, self.nq)) J_rotational = np.array( self.GetInputPort("J_rotational").Eval(context)).reshape( (3, self.nq)) Jdot_qdot = np.expand_dims( np.array(self.GetInputPort("Jdot_qdot").Eval(context)), 1) M = np.array(self.GetInputPort("M").Eval(context)).reshape( (self.nq, self.nq)) Cv = np.expand_dims(np.array(self.GetInputPort("Cv").Eval(context)), 1) # Other inputs mu_S = self.GetInputPort("mu_S").Eval(context)[0] hats_T = self.GetInputPort("hats_T").Eval(context)[0] s_hat_X = self.GetInputPort("s_hat_X").Eval(context)[0] # ============================= OTHER PREP ============================ if self.theta_MYd is None: self.theta_MYd = theta_MY if self.theta_MZd is None: self.theta_MZd = theta_MZ # Calculate desired values dd_d_Td = 1000 * (self.d_Td - d_T) - 100 * d_d_T dd_theta_Ld = 10 * (self.d_theta_Ld - d_theta_L) a_MX_d = 100 * (self.d_Xd - d_X) - 10 * d_d_X theta_MXd = theta_L alpha_MXd = 100 * (theta_MXd - theta_MX) + 10 * (d_theta_L - d_theta_MX) alpha_MYd = 10 * (self.theta_MYd - theta_MY) - 5 * d_theta_MY alpha_MZd = 10 * (self.theta_MZd - theta_MZ) - 5 * d_theta_MZ dd_d_Nd = 0 # =========================== SOLVE PROGRAM =========================== ## 1. Define an instance of MathematicalProgram prog = MathematicalProgram() ## 2. Add decision variables # Contact values F_NM = prog.NewContinuousVariables(1, 1, name="F_NM") F_ContactMY = prog.NewContinuousVariables(1, 1, name="F_ContactMY") F_ContactMZ = prog.NewContinuousVariables(1, 1, name="F_ContactMZ") F_NL = prog.NewContinuousVariables(1, 1, name="F_NL") if self.model_friction: F_FMT = prog.NewContinuousVariables(1, 1, name="F_FMT") F_FMX = prog.NewContinuousVariables(1, 1, name="F_FMX") F_FLT = prog.NewContinuousVariables(1, 1, name="F_FLT") F_FLX = prog.NewContinuousVariables(1, 1, name="F_FLX") else: F_FMT = np.array([[0]]) F_FMX = np.array([[0]]) F_FLT = np.array([[0]]) F_FLX = np.array([[0]]) F_ContactM_XYZ = np.array([F_FMX, F_ContactMY, F_ContactMZ])[:, :, 0] # Object forces and torques if not self.measure_joint_wrench: F_OT = prog.NewContinuousVariables(1, 1, name="F_OT") F_ON = prog.NewContinuousVariables(1, 1, name="F_ON") tau_O = -self.sys_consts.k_J*theta_L \ - self.sys_consts.b_J*d_theta_L # Control values tau_ctrl = prog.NewContinuousVariables(self.nq, 1, name="tau_ctrl") # Object accelerations a_MX = prog.NewContinuousVariables(1, 1, name="a_MX") a_MT = prog.NewContinuousVariables(1, 1, name="a_MT") a_MY = prog.NewContinuousVariables(1, 1, name="a_MY") a_MZ = prog.NewContinuousVariables(1, 1, name="a_MZ") a_MN = prog.NewContinuousVariables(1, 1, name="a_MN") a_LT = prog.NewContinuousVariables(1, 1, name="a_LT") a_LN = prog.NewContinuousVariables(1, 1, name="a_LN") alpha_MX = prog.NewContinuousVariables(1, 1, name="alpha_MX") alpha_MY = prog.NewContinuousVariables(1, 1, name="alpha_MY") alpha_MZ = prog.NewContinuousVariables(1, 1, name="alpha_MZ") alpha_a_MXYZ = np.array( [alpha_MX, alpha_MY, alpha_MZ, a_MX, a_MY, a_MZ])[:, :, 0] # Derived accelerations dd_theta_L = prog.NewContinuousVariables(1, 1, name="dd_theta_L") dd_d_N = prog.NewContinuousVariables(1, 1, name="dd_d_N") dd_d_T = prog.NewContinuousVariables(1, 1, name="dd_d_T") ddq = prog.NewContinuousVariables(self.nq, 1, name="ddq") ## Constraints # "set_description" calls gives us useful names for printing prog.AddConstraint(eq( self.sys_consts.m_L * a_LT, F_FLT + F_GT + F_OT)).evaluator().set_description("Link tangential force balance") prog.AddConstraint( eq(self.sys_consts.m_L * a_LN, F_NL + F_GN + F_ON)).evaluator().set_description("Link normal force balance") prog.AddConstraint(eq( self.sys_consts.I_L*dd_theta_L, \ (-self.sys_consts.w_L/2)*F_ON - (p_CN-p_LN) * F_FLT + \ (p_CT-p_LT)*F_NL + tau_O )).evaluator().set_description("Link moment balance") prog.AddConstraint(eq( F_NL, -F_NM)).evaluator().set_description("3rd law normal forces") if self.model_friction: prog.AddConstraint(eq(F_FMT, -F_FLT)).evaluator().set_description( "3rd law friction forces (T hat)") prog.AddConstraint(eq(F_FMX, -F_FLX)).evaluator().set_description( "3rd law friction forces (X hat)") prog.AddConstraint(eq( -dd_theta_L*(self.sys_consts.h_L/2+self.sys_consts.r) + \ d_theta_L**2*self.sys_consts.w_L/2 - a_LT + a_MT, -dd_theta_L*d_N + dd_d_T - d_theta_L**2*d_T - 2*d_theta_L*d_d_N )).evaluator().set_description("d_N derivative is derivative") prog.AddConstraint(eq( -dd_theta_L*self.sys_consts.w_L/2 - \ d_theta_L**2*self.sys_consts.h_L/2 - \ d_theta_L**2*self.sys_consts.r - a_LN + a_MN, dd_theta_L*d_T + dd_d_N - d_theta_L**2*d_N + 2*d_theta_L*d_d_T )).evaluator().set_description("d_N derivative is derivative") prog.AddConstraint(eq(dd_d_N, 0)).evaluator().set_description("No penetration") if self.model_friction: prog.AddConstraint( eq(F_FLT, mu_S * F_NL * self.sys_consts.mu * hats_T)).evaluator().set_description( "Friction relationship LT") prog.AddConstraint( eq(F_FLX, mu_S * F_NL * self.sys_consts.mu * s_hat_X)).evaluator().set_description( "Friction relationship LX") if not self.measure_joint_wrench: prog.AddConstraint( eq(a_LT, -(self.sys_consts.w_L / 2) * d_theta_L** 2)).evaluator().set_description("Hinge constraint (T hat)") prog.AddConstraint(eq(a_LN, (self.sys_consts.w_L / 2) * dd_theta_L)).evaluator().set_description( "Hinge constraint (N hat)") for i in range(6): lhs_i = alpha_a_MXYZ[i, 0] assert not hasattr(lhs_i, "shape") rhs_i = (Jdot_qdot + np.matmul(J, ddq))[i, 0] assert not hasattr(rhs_i, "shape") prog.AddConstraint(lhs_i == rhs_i).evaluator().set_description( "Relate manipulator and end effector with joint " + \ "accelerations " + str(i)) tau_contact_trn = np.matmul(J_translational.T, F_ContactM_XYZ) tau_contact_rot = np.matmul(J_rotational.T, np.cross(p_MConM, F_ContactM_XYZ, axis=0)) tau_contact = tau_contact_trn + tau_contact_rot tau_out = tau_ctrl - tau_g + joint_centering_torque for i in range(self.nq): M_ddq_row_i = (np.matmul(M, ddq) + Cv)[i, 0] assert not hasattr(M_ddq_row_i, "shape") tau_i = (tau_g + tau_contact + tau_out)[i, 0] assert not hasattr(tau_i, "shape") prog.AddConstraint(M_ddq_row_i == tau_i).evaluator( ).set_description("Manipulator equations " + str(i)) # Projection equations prog.AddConstraint( eq(a_MT, np.cos(theta_L) * a_MY + np.sin(theta_L) * a_MZ)) prog.AddConstraint( eq(a_MN, -np.sin(theta_L) * a_MY + np.cos(theta_L) * a_MZ)) prog.AddConstraint( eq(F_FMT, np.cos(theta_L) * F_ContactMY + np.sin(theta_L) * F_ContactMZ)) prog.AddConstraint( eq(F_NM, -np.sin(theta_L) * F_ContactMY + np.cos(theta_L) * F_ContactMZ)) prog.AddConstraint(dd_d_T[0, 0] == dd_d_Td).evaluator().set_description( "Desired dd_d_Td constraint" + str(i)) prog.AddConstraint(dd_theta_L[0, 0] == dd_theta_Ld).evaluator( ).set_description("Desired a_LN constraint" + str(i)) prog.AddConstraint(a_MX[0, 0] == a_MX_d).evaluator().set_description( "Desired a_MX constraint" + str(i)) prog.AddConstraint(alpha_MX[0, 0] == alpha_MXd).evaluator( ).set_description("Desired alpha_MX constraint" + str(i)) prog.AddConstraint(alpha_MY[0, 0] == alpha_MYd).evaluator( ).set_description("Desired alpha_MY constraint" + str(i)) prog.AddConstraint(alpha_MZ[0, 0] == alpha_MZd).evaluator( ).set_description("Desired alpha_MZ constraint" + str(i)) prog.AddConstraint(dd_d_N[0, 0] == dd_d_Nd).evaluator().set_description( "Desired dd_d_N constraint" + str(i)) result = Solve(prog) assert result.is_success() tau_ctrl_result = [] for i in range(self.nq): tau_ctrl_result.append( result.GetSolution()[prog.FindDecisionVariableIndex( tau_ctrl[i, 0])]) tau_ctrl_result = np.expand_dims(tau_ctrl_result, 1) # ======================== UPDATE DEBUG VALUES ======================== self.debug["times"].append(context.get_time()) # control effort self.debug["dd_d_Td"].append(dd_d_Td) self.debug["dd_theta_Ld"].append(dd_theta_Ld) self.debug["a_MX_d"].append(a_MX_d) self.debug["alpha_MXd"].append(alpha_MXd) self.debug["alpha_MYd"].append(alpha_MYd) self.debug["alpha_MZd"].append(alpha_MZd) self.debug["dd_d_Nd"].append(dd_d_Nd) # decision variables if self.model_friction: self.debug["F_FMT"].append( result.GetSolution()[prog.FindDecisionVariableIndex(F_FMT[0, 0])]) self.debug["F_FMX"].append( result.GetSolution()[prog.FindDecisionVariableIndex(F_FMX[0, 0])]) self.debug["F_FLT"].append( result.GetSolution()[prog.FindDecisionVariableIndex(F_FLT[0, 0])]) self.debug["F_FLX"].append( result.GetSolution()[prog.FindDecisionVariableIndex(F_FLX[0, 0])]) else: self.debug["F_FMT"].append(F_FMT) self.debug["F_FMX"].append(F_FMX) self.debug["F_FLT"].append(F_FLT) self.debug["F_FLX"].append(F_FLX) self.debug["F_NM"].append( result.GetSolution()[prog.FindDecisionVariableIndex(F_NM[0, 0])]) self.debug["F_ContactMY"].append( result.GetSolution()[prog.FindDecisionVariableIndex( F_ContactMY[0, 0])]) self.debug["F_ContactMZ"].append( result.GetSolution()[prog.FindDecisionVariableIndex( F_ContactMZ[0, 0])]) self.debug["F_NL"].append( result.GetSolution()[prog.FindDecisionVariableIndex(F_NL[0, 0])]) for i in range(self.nq): self.debug["tau_ctrl_" + str(i)].append( result.GetSolution()[prog.FindDecisionVariableIndex( tau_ctrl[i, 0])]) self.debug["a_MX"].append( result.GetSolution()[prog.FindDecisionVariableIndex(a_MX[0, 0])]) self.debug["a_MT"].append( result.GetSolution()[prog.FindDecisionVariableIndex(a_MT[0, 0])]) self.debug["a_MY"].append( result.GetSolution()[prog.FindDecisionVariableIndex(a_MY[0, 0])]) self.debug["a_MZ"].append( result.GetSolution()[prog.FindDecisionVariableIndex(a_MZ[0, 0])]) self.debug["a_MN"].append( result.GetSolution()[prog.FindDecisionVariableIndex(a_MN[0, 0])]) self.debug["a_LT"].append( result.GetSolution()[prog.FindDecisionVariableIndex(a_LT[0, 0])]) self.debug["a_LN"].append( result.GetSolution()[prog.FindDecisionVariableIndex(a_LN[0, 0])]) self.debug["alpha_MX"].append( result.GetSolution()[prog.FindDecisionVariableIndex(alpha_MX[0, 0])]) self.debug["alpha_MY"].append( result.GetSolution()[prog.FindDecisionVariableIndex(alpha_MY[0, 0])]) self.debug["alpha_MZ"].append( result.GetSolution()[prog.FindDecisionVariableIndex(alpha_MZ[0, 0])]) self.debug["dd_theta_L"].append( result.GetSolution()[prog.FindDecisionVariableIndex( dd_theta_L[0, 0])]) self.debug["dd_d_N"].append( result.GetSolution()[prog.FindDecisionVariableIndex(dd_d_N[0, 0])]) self.debug["dd_d_T"].append( result.GetSolution()[prog.FindDecisionVariableIndex(dd_d_T[0, 0])]) for i in range(self.nq): self.debug["ddq_" + str(i)].append( result.GetSolution()[prog.FindDecisionVariableIndex(ddq[i, 0])]) self.debug["theta_MXd"].append(theta_MXd) output.SetFromVector(tau_ctrl_result.flatten())
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))
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 result = Solve(prog) x_sol = result.GetSolution(x) print(f"Optimization successful? {result.is_success()}") # Display the optimized trajectories plt.figure() plt.plot(x_sol[0, :], x_sol[1, :]) plt.xlabel('q') plt.ylabel('qdot') plt.show()
def compute_trajectory_to_other_world(self, state_initial, minimum_time, maximum_time): ''' Your mission is to implement this function. A successful implementation of this function will compute a dynamically feasible trajectory which satisfies these criteria: - Efficiently conserve fuel - Reach "orbit" of the far right world - Approximately obey the dynamic constraints - Begin at the state_initial provided - Take no more than maximum_time, no less than minimum_time The above are defined more precisely in the provided notebook. Please note there are two return args. :param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation :param: minimum_time: float, minimum time allowed for trajectory :param: maximum_time: float, maximum time allowed for trajectory :return: three return args separated by commas: trajectory, input_trajectory, time_array trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation. input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation. time_array: an array with N rows. ''' # length of horizon (excluding init state) N = 50 trajectory = np.zeros((N + 1, 4)) input_trajectory = np.ones((N, 2)) * 10.0 ### My implementation of Direct Transcription # (states and control are all decision vars using Euler integration) mp = MathematicalProgram() # let trajectory duration be a decision var total_time = mp.NewContinuousVariables(1, "total_time") dt = total_time[0] / N # create the control decision var (m*N) and state decision var (n*[N+1]) idx = 0 u_list = mp.NewContinuousVariables(2, "u_{}".format(idx)) state_list = mp.NewContinuousVariables(4, "state_{}".format(idx)) state_list = np.vstack( (state_list, mp.NewContinuousVariables(4, "state_{}".format(idx + 1)))) for idx in range(1, N): u_list = np.vstack( (u_list, mp.NewContinuousVariables(2, "u_{}".format(idx)))) state_list = np.vstack( (state_list, mp.NewContinuousVariables(4, "state_{}".format(idx + 1)))) ### Constraints # set init state as constraint on stage 0 decision vars for state_idx in range(4): mp.AddLinearConstraint( state_list[0, state_idx] == state_initial[state_idx]) # interstage equality constraint on state vars via Euler integration # note: Direct Collocation could improve accuracy for same computation for idx in range(1, N + 1): state_new = state_list[idx - 1, :] + dt * self.rocket_dynamics( state_list[idx - 1, :], u_list[idx - 1, :]) for state_idx in range(4): mp.AddConstraint(state_list[idx, state_idx] == state_new[state_idx]) # constraint on time mp.AddLinearConstraint(total_time[0] <= maximum_time) mp.AddLinearConstraint(total_time[0] >= minimum_time) # constraint on final state distance (squared)to second planet final_dist_to_world_2_sq = ( self.world_2_position[0] - state_list[-1, 0])**2 + ( self.world_2_position[1] - state_list[-1, 1])**2 mp.AddConstraint(final_dist_to_world_2_sq <= 0.25) # constraint on final state speed (squared final_speed_sq = state_list[-1, 2]**2 + state_list[-1, 3]**2 mp.AddConstraint(final_speed_sq <= 1) ### Cost # equal cost on vertical/horizontal accels, weight shouldn't matter since this is the only cost mp.AddQuadraticCost(1 * u_list[:, 0].dot(u_list[:, 0])) mp.AddQuadraticCost(1 * u_list[:, 1].dot(u_list[:, 1])) ### Solve and parse result = Solve(mp) trajectory = result.GetSolution(state_list) input_trajectory = result.GetSolution(u_list) tf = result.GetSolution(total_time) time_array = np.linspace(0, tf[0], N + 1) print "optimization successful: ", result.is_success() print "total num decision vars (x: (N+1)*4, u: 2N, total_time: 1): {}".format( mp.num_vars()) print "solver used: ", result.get_solver_id().name() print "optimal trajectory time: {:.2f} sec".format(tf[0]) return trajectory, input_trajectory, time_array
# Use IK to find a nonpenetrating configuration of the scene from # which to start simulation. q0 = mbp.GetPositions(mbp_context).copy() ik = InverseKinematics(mbp, mbp_context) q_dec = ik.q() prog = ik.prog() constraint = ik.AddMinimumDistanceConstraint(0.01) prog.AddQuadraticErrorCost(np.eye(q0.shape[0])*1.0, q0, q_dec) mbp.SetPositions(mbp_context, q0) prog.SetInitialGuess(q_dec, q0) print("Solving") print("Initial guess: ", q0) res = Solve(prog) #print(prog.GetSolverId().name()) q0_proj = res.GetSolution(q_dec) print("Final: ", q0_proj) # Run a sim starting from whatever configuration IK figured out. mbp.SetPositions(mbp_context, q0_proj) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.set_target_realtime_rate(1.0) try: simulator.AdvanceTo(2.0) except Exception as e: print("Exception in sim: ", e) scene_k -= 1