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 runDircol(self,x0,xf,tf0): N = 15 # constant #N = np.int(tf0 * 10) # "10Hz" / samples per second context = self.CreateDefaultContext() dircol = DirectCollocation(self, context, num_time_samples=N, minimum_timestep=0.05, maximum_timestep=1.0) u = dircol.input() # set some constraints on inputs dircol.AddEqualTimeIntervalsConstraints() dircol.AddConstraintToAllKnotPoints(u[1] <= self.slewmax) dircol.AddConstraintToAllKnotPoints(u[1] >= -self.slewmax) dircol.AddConstraintToAllKnotPoints(u[0] <= self.umax) dircol.AddConstraintToAllKnotPoints(u[0] >= -self.umax) # constrain the last input to be zero (at least for the u input) #import pdb; pdb.set_trace() dv = dircol.decision_variables() for i in range(3, self.nX*N, 4): alfa_state = dv[i] #u[t_end] dircol.AddBoundingBoxConstraint(-self.alfamax, self.alfamax, alfa_state) #final_u_decision_var = dv[self.nX*N + self.nU*N - 1] #u[t_end] #dircol.AddLinearEqualityConstraint(final_u_decision_var, 0.0) #first_u_decision_var = dv[self.nX*N + 1 ] #u[t_0] #dircol.AddLinearEqualityConstraint(first_u_decision_var, 0.0) # set some constraints on start and final pose eps = 0.0 * np.ones(self.nX) # relaxing factor dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state()) dircol.AddBoundingBoxConstraint(xf-eps, \ xf+eps, dircol.final_state()) R = 1.0*np.eye(self.nU) # Cost on input "effort". dircol.AddRunningCost( u.transpose().dot(R.dot(u)) ) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) # guess initial trajectory initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) # optimize result = Solve(dircol) print('******\nRunning trajectory optimization:') print('w/ solver %s' %(result.get_solver_id().name())) print(result.get_solution_result()) assert(result.is_success()) xtraj = dircol.ReconstructStateTrajectory(result) utraj = dircol.ReconstructInputTrajectory(result) # return nominal trajectory return utraj,xtraj
def runDircol(self, x0, xf, tf0): N = 21 #np.int(tf0 * 10) # "10Hz" samples per second context = self.CreateDefaultContext() dircol = DirectCollocation(self, context, num_time_samples=N, minimum_timestep=0.05, maximum_timestep=1.0) u = dircol.input() dircol.AddEqualTimeIntervalsConstraints() dircol.AddConstraintToAllKnotPoints(u[0] <= 0.5 * self.omegamax) dircol.AddConstraintToAllKnotPoints(u[0] >= -0.5 * self.omegamax) dircol.AddConstraintToAllKnotPoints(u[1] <= 0.5 * self.umax) dircol.AddConstraintToAllKnotPoints(u[1] >= -0.5 * self.umax) eps = 0.0 dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state()) dircol.AddBoundingBoxConstraint(xf - np.array([eps, eps, eps]), xf + np.array([eps, eps, eps]), dircol.final_state()) R = 1.0 * np.eye(2) # Cost on input "effort". dircol.AddRunningCost(u.transpose().dot(R.dot(u))) #dircol.AddRunningCost(R*u[0]**2) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) result = Solve(dircol) print(result.get_solver_id().name()) print(result.get_solution_result()) assert (result.is_success()) #import pdb; pdb.set_trace() xtraj = dircol.ReconstructStateTrajectory(result) utraj = dircol.ReconstructInputTrajectory(result) return utraj, xtraj
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
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)
# dircol.AddLinearConstraint(dircol.final_state() == final_state) R = 10 # Cost on input "effort". dircol.AddRunningCost(R * u[0]**2) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], np.column_stack((initial_state, final_state))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) result = Solve(dircol) print(result.get_solver_id().name()) print(result.get_solution_result()) assert (result.is_success()) x_trajectory = dircol.ReconstructStateTrajectory(result) tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"), FloatingBaseType.kFixed) vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]) ani = vis.animate(x_trajectory, repeat=True) u_trajectory = dircol.ReconstructInputTrajectory(result) times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100) u_lookup = np.vectorize(u_trajectory.value) u_values = u_lookup(times)
# dircol.AddLinearConstraint(dircol.final_state() == final_state) R = 10 # Cost on input "effort". dircol.AddRunningCost(R*u[0]**2) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], np.column_stack((initial_state, final_state))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) result = Solve(dircol) print(result.get_solver_id().name()) print(result.get_solution_result()) assert(result.is_success()) x_trajectory = dircol.ReconstructStateTrajectory(result) tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"), FloatingBaseType.kFixed) vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]) ani = vis.animate(x_trajectory, repeat=True) u_trajectory = dircol.ReconstructInputTrajectory(result) times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100) u_lookup = np.vectorize(u_trajectory.value) u_values = u_lookup(times)
def diff_drive_pd_mp( self, x, x_des): # x_des = [x, theta, yaw, x_dot, theta_dot, yaw_dot] m_s = 0.2 #kg d = 0.085 #m m_c = 0.056 I_3 = 0.000228373 #.0000989844 #kg*m^2 I_2 = .0000989844 R = 0.0333375 g = -9.81 #may need to be set as -9.81; test to see L = 0.03 A_val_theta = (m_s * d * g * (3 * m_c + m_s)) / (3 * m_c * I_3 + 3 * m_c * m_s * d**2 + m_s * I_3) B_val_theta = (-m_s * d / R - 3 * m_c * m_s) / ( 3 * m_c * I_3 + 3 * m_c * m_s * d**2 + m_s * I_3) * math.pi / 180 #multiplicand for u to change in theta_dot B_val_phi = -(2 * L / R) / (6 * m_c * L**2 + m_c * R**2 + 2 * I_2) mp = MathematicalProgram() k = mp.NewContinuousVariables(3, 'k_val') # [kp, kd, ky] u = mp.NewContinuousVariables(2, 'u_val') theta_dot_post = mp.NewContinuousVariables( 1, 'theta_dot_post_val') #estimated value of theta dot after control phi_dot_post = mp.NewContinuousVariables( 1, 'phi_dot_post_val') #estimated value of theta dot after control ''' kp = 1. #regulate theta (pitch) position kd = kp / 20. #regulate theta (pitch) velocity ky = 0.5 #regulate phi (yaw) position ''' actuator_limit = .1 #estimate; 0.1 is probably high #mp.AddConstraint(theta[0] == np.arcsin(2*(x[0]*x[2] - x[1]*x[3]))) #pitch theta = np.arcsin(2 * (x[0] * x[2] - x[1] * x[3])) phi = np.arctan2(2 * (x[0] * x[1] + x[2] * x[3]), 1 - 2 * (x[1]**2 + x[2]**2)) #yaw theta_dot = x[ 10] #Shown to be ~1.5% below true theta_dot on average in experiments mp.AddConstraint(k[0] >= 0.) mp.AddConstraint(k[1] >= 0.) mp.AddConstraint(k[2] >= 0.) mp.AddConstraint(u[0] == k[0] * (x_des[1] - theta + k[1] * (x_des[4] - theta_dot)) + k[2] * (x_des[2] - phi)) mp.AddConstraint(u[0] <= -actuator_limit) mp.AddConstraint(u[0] >= -actuator_limit) mp.AddConstraint(u[1] == -u[0]) mp.AddConstraint(theta_dot_post[0] == theta_dot + B_val_theta * u[0] * 0.0005 + A_val_theta * theta * .0005) mp.AddConstraint(phi_dot_post[0] == x[11] + B_val_phi * u[0] * .0005) theta_dot_des = -(theta - x_des[1]) / (2 * .0005) mp.AddQuadraticCost((theta_dot_post[0] - theta_dot_des)**2) phi_dot_des = -(phi - x_des[2]) / 2 print('theta_dot_des', theta_dot_des) print('theta', theta) mp.AddQuadraticCost(0.001 * (phi_dot_post[0] - phi_dot_des)**2) result = Solve(mp) print('Success: ', result.is_success()) print('Solver id: ', result.get_solver_id().name()) print('k: ', result.GetSolution(k)) print('u: ', result.GetSolution(u)) return result.GetSolution(u)