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 is_verified(rho): # initialize optimization problem # (with Drake there is no need to specify that # this is going to be a SOS program!) prog = MathematicalProgram() # SOS indeterminates x = prog.NewIndeterminates(2, 'x') # Lyapunov function V = x.dot(P).dot(x) V_dot = 2*x.dot(P).dot(f(x)) # degree of the polynomial lambda(x) # no need to change it, but if you really want to, # keep l_deg even (why?) and do not set l_deg greater than 10 # (otherwise optimizations will take forever) l_deg = 4 assert l_deg % 2 == 0 # SOS Lagrange multipliers l = prog.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression() # main condition above eps = 1e-3 # do not change prog.AddSosConstraint(- V_dot - l * (rho - V) - eps*x.dot(x)) # solve SOS program # no objective function in this formulation result = Solve(prog) # return True if feasible, False if infeasible return result.is_success()
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)
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 min_time_bounce_kick_traj(self, p0, v0, p0_puck, v0_puck, v_puck_desired): """Use direct transcription to calculate player's trajectory for bounce kick. The elastic collision is enforced when robot reaches the desired position at specified time.""" T = 1 x0 = np.concatenate((p0, v0), axis=0) prog = DirectTranscription(self.sys, self.sys.CreateDefaultContext(), int(T/self.params.dt)) prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state()) self.add_final_state_constraint_elastic_collision(prog, p0_puck, v0_puck, v_puck_desired) self.add_input_limits(prog) self.add_arena_limits(prog) prog.AddFinalCost(prog.time()) result = Solve(prog) u_sol = prog.ReconstructInputTrajectory(result) if not result.is_success(): print("Minimum time bounce kick trajectory: optimization failed") return False, np.zeros((2, 1)) u_values = u_sol.vector_values(u_sol.get_segment_times()) return result.is_success(), u_values
def intercepting_traj(self, p0, v0, pf, vf, T): """Trajectory planning given initial state, final state, and final time.""" x0 = np.concatenate((p0, v0), axis=0) xf = np.concatenate((pf, vf), axis=0) prog = DirectTranscription(self.sys, self.sys.CreateDefaultContext(), int(T/self.params.dt)) prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state()) prog.AddBoundingBoxConstraint(xf, xf, prog.final_state()) self.add_input_limits(prog) self.add_arena_limits(prog) # cost prog.AddRunningCost(prog.input()[0]*prog.input()[0] + prog.input()[1]*prog.input()[1]) # solve optimization result = Solve(prog) u_sol = prog.ReconstructInputTrajectory(result) if not result.is_success(): print("Intercepting trajectory: optimization failed") return False, np.zeros((2, 1)) u_values = u_sol.vector_values(u_sol.get_segment_times()) return result.is_success(), u_values
def min_time_traj_transcription(self, p0, v0, pf, vf, xlim=None, ylim=None): """Minimum time traj using directi transcription.""" T = 2 N = int(T/self.params.dt) x0 = np.concatenate((p0, v0), axis=0) xf = np.concatenate((pf, vf), axis=0) prog = DirectTranscription(self.sys, self.sys.CreateDefaultContext(), N) prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state()) # initial states prog.AddBoundingBoxConstraint(xf, xf, prog.final_state()) self.add_input_limits(prog) self.add_arena_limits(prog) prog.AddFinalCost(prog.time()) result = Solve(prog) u_sol = prog.ReconstructInputTrajectory(result) if not result.is_success(): print("Minimum time trajectory: optimization failed") return False, np.zeros((2, 1)) u_values = u_sol.vector_values(u_sol.get_segment_times()) return result.is_success(), u_values
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 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 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 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))
# 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.')
class FourierCollocationProblem: def __init__(self, system_dynamics, constraints): start_time = time.time() self.prog = MathematicalProgram() self.N = 50 # Number of collocation points self.M = 10 # Number of frequencies self.system_dynamics = system_dynamics self.psi = np.pi * (0.7) # TODO change ( min_height, max_height, min_vel, max_vel, h0, min_travelled_distance, t_f_min, t_f_max, avg_vel_min, avg_vel_max, ) = constraints initial_pos = np.array([0, 0, h0]) # Add state trajectory parameters as decision variables self.coeffs = self.prog.NewContinuousVariables( 3, self.M + 1, "c" ) # (x,y,z) for every frequency self.phase_delays = self.prog.NewContinuousVariables(3, self.M, "eta") self.t_f = self.prog.NewContinuousVariables(1, 1, "t_f")[0, 0] self.avg_vel = self.prog.NewContinuousVariables(1, 1, "V_bar")[0, 0] # Enforce initial conditions residuals = self.get_pos_fourier(collocation_time=0) - initial_pos for residual in residuals: self.prog.AddConstraint(residual == 0) # Enforce dynamics at collocation points for n in range(self.N): collocation_time = (n / self.N) * self.t_f pos = self.get_pos_fourier(collocation_time) vel = self.get_vel_fourier(collocation_time) vel_dot = self.get_vel_dot_fourier(collocation_time) residuals = self.continuous_dynamics(pos, vel, vel_dot) for residual in residuals[3:6]: # TODO only these last three are residuals self.prog.AddConstraint(residual == 0) if True: # Add velocity constraints squared_vel_norm = vel.T.dot(vel) self.prog.AddConstraint(min_vel ** 2 <= squared_vel_norm) self.prog.AddConstraint(squared_vel_norm <= max_vel ** 2) # Add height constraints self.prog.AddConstraint(pos[2] <= max_height) self.prog.AddConstraint(min_height <= pos[2]) # Add constraint on min travelled distance self.prog.AddConstraint(min_travelled_distance <= self.avg_vel * self.t_f) # Add constraints on coefficients if False: for coeff in self.coeffs.T: lb = np.array([-500, -500, -500]) ub = np.array([500, 500, 500]) self.prog.AddBoundingBoxConstraint(lb, ub, coeff) # Add constraints on phase delays if False: for etas in self.phase_delays.T: lb = np.array([0, 0, 0]) ub = np.array([2 * np.pi, 2 * np.pi, 2 * np.pi]) self.prog.AddBoundingBoxConstraint(lb, ub, etas) # Add constraints on final time and avg vel self.prog.AddBoundingBoxConstraint(t_f_min, t_f_max, self.t_f) self.prog.AddBoundingBoxConstraint(avg_vel_min, avg_vel_max, self.avg_vel) # Add objective function self.prog.AddCost(-self.avg_vel) # Set initial guess coeffs_guess = np.zeros((3, self.M + 1)) coeffs_guess += np.random.rand(*coeffs_guess.shape) * 100 self.prog.SetInitialGuess(self.coeffs, coeffs_guess) phase_delays_guess = np.zeros((3, self.M)) phase_delays_guess += np.random.rand(*phase_delays_guess.shape) * 1e-1 self.prog.SetInitialGuess(self.phase_delays, phase_delays_guess) self.prog.SetInitialGuess(self.avg_vel, (avg_vel_max - avg_vel_min) / 2) self.prog.SetInitialGuess(self.t_f, (t_f_max - t_f_min) / 2) print( "Finished formulating the optimization problem in: {0} s".format( time.time() - start_time ) ) start_solve_time = time.time() self.result = Solve(self.prog) print("Found solution: {0}".format(self.result.is_success())) print("Found a solution in: {0} s".format(time.time() - start_solve_time)) # TODO input costs # assert self.result.is_success() return def get_solution(self): coeffs_opt = self.result.GetSolution(self.coeffs) phase_delays_opt = self.result.GetSolution(self.phase_delays) t_f_opt = self.result.GetSolution(self.t_f) avg_vel_opt = self.result.GetSolution(self.avg_vel) sim_N = 100 dt = t_f_opt / sim_N times = np.arange(0, t_f_opt, dt) pos_traj = np.zeros((3, sim_N)) # TODO remove vel traj vel_traj = np.zeros((3, sim_N)) for i in range(sim_N): t = times[i] pos = self.evaluate_pos_traj( coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t ) pos_traj[:, i] = pos vel = self.evaluate_vel_traj( coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t ) vel_traj[:, i] = vel # TODO move plotting out plot_trj_3_wind(pos_traj.T, np.array([np.sin(self.psi), np.cos(self.psi), 0])) plt.show() breakpoint() return def evaluate_pos_traj(self, coeffs, phase_delays, t_f, avg_vel, t): pos_traj = np.copy(coeffs[:, 0]) for m in range(1, self.M): sine_terms = np.array( [ np.sin((2 * np.pi * m * t) / t_f + phase_delays[0, m]), np.sin((2 * np.pi * m * t) / t_f + phase_delays[1, m]), np.sin((2 * np.pi * m * t) / t_f + phase_delays[2, m]), ] ) pos_traj += coeffs[:, m] * sine_terms direction_term = np.array( [ avg_vel * np.sin(self.psi) * t, avg_vel * np.cos(self.psi) * t, 0, ] ) pos_traj += direction_term return pos_traj def evaluate_vel_traj(self, coeffs, phase_delays, t_f, avg_vel, t): vel_traj = np.array([0, 0, 0], dtype=object) for m in range(1, self.M): cos_terms = np.array( [ (2 * np.pi * m) / t_f * np.cos((2 * np.pi * m * t) / t_f + phase_delays[0, m]), (2 * np.pi * m) / t_f * np.cos((2 * np.pi * m * t) / t_f + phase_delays[1, m]), (2 * np.pi * m) / t_f * np.cos((2 * np.pi * m * t) / t_f + phase_delays[2, m]), ] ) vel_traj += coeffs[:, m] * cos_terms direction_term = np.array( [ avg_vel * np.sin(self.psi), avg_vel * np.cos(self.psi), 0, ] ) vel_traj += direction_term return vel_traj def get_pos_fourier(self, collocation_time): pos_traj = np.copy(self.coeffs[:, 0]) for m in range(1, self.M): a = (2 * np.pi * m) / self.t_f sine_terms = np.array( [ np.sin(a * collocation_time + self.phase_delays[0, m]), np.sin(a * collocation_time + self.phase_delays[1, m]), np.sin(a * collocation_time + self.phase_delays[2, m]), ] ) pos_traj += self.coeffs[:, m] * sine_terms direction_term = np.array( [ self.avg_vel * np.sin(self.psi) * collocation_time, self.avg_vel * np.cos(self.psi) * collocation_time, 0, ] ) pos_traj += direction_term return pos_traj def get_vel_fourier(self, collocation_time): vel_traj = np.array([0, 0, 0], dtype=object) for m in range(1, self.M): a = (2 * np.pi * m) / self.t_f cos_terms = np.array( [ a * np.cos(a * collocation_time + self.phase_delays[0, m]), a * np.cos(a * collocation_time + self.phase_delays[1, m]), a * np.cos(a * collocation_time + self.phase_delays[2, m]), ] ) vel_traj += self.coeffs[:, m] * cos_terms direction_term = np.array( [ self.avg_vel * np.sin(self.psi), self.avg_vel * np.cos(self.psi), 0, ] ) vel_traj += direction_term return vel_traj def get_vel_dot_fourier(self, collocation_time): vel_dot_traj = np.array([0, 0, 0], dtype=object) for m in range(1, self.M): a = (2 * np.pi * m) / self.t_f sine_terms = np.array( [ -(a ** 2) * np.sin(a * collocation_time + self.phase_delays[0, m]), -(a ** 2) * np.sin(a * collocation_time + self.phase_delays[1, m]), -(a ** 2) * np.sin(a * collocation_time + self.phase_delays[2, m]), ] ) vel_dot_traj += self.coeffs[:, m] * sine_terms return vel_dot_traj def continuous_dynamics(self, pos, vel, vel_dot): x = np.concatenate((pos, vel)) x_dot = np.concatenate((vel, vel_dot)) # TODO need to somehow implement input to make this work return x_dot - self.system_dynamics(x, u)
dircol.AddBoundingBoxConstraint(final_state.get_value(), final_state.get_value(), dircol.final_state()) # dircol.AddLinearConstraint(dircol.final_state() == final_state.get_value()) R = 10 # Cost on input "effort". dircol.AddRunningCost(R*u[0]**2) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], [initial_state.get_value(), final_state.get_value()]) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) result = Solve(dircol) assert result.is_success() x_trajectory = dircol.ReconstructStateTrajectory(result) fig, ax = plt.subplots(1, 2) vis = PendulumVisualizer(ax[0]) ani = vis.animate(x_trajectory, repeat=True) x_knots = np.hstack([x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), 100)]) ax[1].plot(x_knots[0, :], x_knots[1, :]) plt.show()
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)
def drake_trajectory_generation(file_name): global x_cmd_drake global u_cmd_drake print(file_name) Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() global dircol dircol= DirectCollocation( plant, context, num_time_samples=11, minimum_timestep=0.1, maximum_timestep=0.4, input_port_index=plant.get_actuation_input_port().get_index()) dircol.AddEqualTimeIntervalsConstraints() initial_state = (0., 0., 0., 0.) dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) final_state = (0., math.pi, 0., 0.) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) R = 10 # Cost on input "effort".weight u = dircol.input() 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))) # yapf: disable dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) dircol.AddConstraintToAllKnotPoints(dircol.input()[1] <= 0) dircol.AddConstraintToAllKnotPoints(dircol.input()[1] >= 0) global result global u_values result = Solve(dircol) assert result.is_success() #plotphase_portrait() fig1, ax1 = plt.subplots() u_trajectory = dircol.ReconstructInputTrajectory(result) u_knots = np.hstack([ u_trajectory.value(t) for t in np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400) ])#here the u_knots now is 2x400 #u_trajectory = dircol.ReconstructInputTrajectory(result) times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400) #u_lookup = np.vectorize(u_trajectory.value) #now we have ndarray of u_values with 400 points for 4 seconds w/ 100hz pub frequency #u_values = u_lookup(times) #ax1.plot(times, u_values) ax1.plot(times, u_knots[0]) ax1.plot(times, u_knots[1]) ax1.set_xlabel("time (seconds)") ax1.set_ylabel("force (Newtons)") ax1.set_title(' Direct collocation for Cartpole ') print('here2') plt.show() print('here3') #x_knots = np.hstack([ # x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(), # x_trajectory.end_time(), 100) #]) x_trajectory = dircol.ReconstructStateTrajectory(result) x_knots = np.hstack([ x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), 400) ]) print(x_trajectory.start_time()) print(x_trajectory.end_time()) fig, ax = plt.subplots(4,1,figsize=(8,8)) plt.subplots_adjust(wspace =0, hspace =0.4) #plt.tight_layout(3)#adjust total space ax[0].set_title('state of direct collocation for Cartpole') ax[0].plot(x_knots[0, :], x_knots[2, :], linewidth=2, color='b', linestyle='-') ax[0].set_xlabel("state_dot(theta1_dot and t|heta2_dot)") ax[0].set_ylabel("state(theta1 and theta2)"); ax[0].plot(x_knots[1, :], x_knots[3, :],color='r',linewidth=2,linestyle='--') ax[0].legend(('theta1&theta1dot','theta2&theta2dot')); ax[1].set_title('input u(t) of direct collocation for Cartpole') # ax[1].plot(times,u_values, 'g') ax[1].plot(times, u_knots[0]) ax[1].plot(times, u_knots[1]) ax[1].legend(('input u(t)')) ax[1].set_xlabel("time") ax[1].set_ylabel("u(t)") ax[1].legend(('x joint ','thetajoint')) ax[1].set_title('input x(t) of direct collocation for Cartpole') ax[2].plot(times, x_knots[0, :]) ax[2].set_xlabel("time") ax[2].set_ylabel("x(t)") ax[2].set_title('input theta(t) of direct collocation for Cartpole') ax[3].set_title('input theta(t) of direct collocation for Cartpole') ax[3].plot(times, x_knots[1, :]) ax[3].set_xlabel("time") ax[3].set_ylabel("theta(t)") print('here4') plt.show() print('here5') x_cmd_drake=x_knots #return x_knots[0, :]#u_values # u_cmd_drake=u_values u_cmd_drake=u_knots
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 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
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) plt.figure() plt.plot(times, u_values)
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
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)
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 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 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
class PPTrajectory(): def __init__(self, sample_times, num_vars, degree, continuity_degree): self.sample_times = sample_times self.n = num_vars self.degree = degree self.prog = MathematicalProgram() self.coeffs = [] for i in range(len(sample_times)): self.coeffs.append( self.prog.NewContinuousVariables(num_vars, degree + 1, "C")) self.result = None # Add continuity constraints for s in range(len(sample_times) - 1): trel = sample_times[s + 1] - sample_times[s] coeffs = self.coeffs[s] for var in range(self.n): for deg in range(continuity_degree + 1): # Don't use eval here, because I want left and right # values of the same time left_val = 0 for d in range(deg, self.degree + 1): left_val += coeffs[var, d]*np.power(trel, d-deg) * \ math.factorial(d)/math.factorial(d-deg) right_val = self.coeffs[s + 1][var, deg] * math.factorial(deg) self.prog.AddLinearConstraint(left_val == right_val) # Add cost to minimize highest order terms for s in range(len(sample_times) - 1): self.prog.AddQuadraticCost(np.eye(num_vars), np.zeros( (num_vars, 1)), self.coeffs[s][:, -1]) def eval(self, t, derivative_order=0): if derivative_order > self.degree: return 0 s = 0 while s < len(self.sample_times) - 1 and t >= self.sample_times[s + 1]: s += 1 trel = t - self.sample_times[s] if self.result is None: coeffs = self.coeffs[s] else: coeffs = self.result.GetSolution(self.coeffs[s]) deg = derivative_order val = 0 * coeffs[:, 0] for var in range(self.n): for d in range(deg, self.degree + 1): val[var] += coeffs[var, d]*np.power(trel, d-deg) * \ math.factorial(d)/math.factorial(d-deg) return val def add_constraint(self, t, derivative_order, lb, ub=None): ''' Adds a constraint of the form d^deg lb <= x(t) / dt^deg <= ub ''' if ub is None: ub = lb assert (derivative_order <= self.degree) val = self.eval(t, derivative_order) self.prog.AddLinearConstraint(val, lb, ub) def generate(self): self.result = Solve(self.prog) assert (self.result.is_success())
def direct_collocation_zhao_glider(): print("Running direct collocation") plant = SlotineGlider() context = plant.CreateDefaultContext() N = 21 initial_guess = True max_dt = 0.5 max_tf = N * max_dt dircol = DirectCollocation( plant, context, num_time_samples=N, minimum_timestep=0.05, maximum_timestep=max_dt, ) # Constrain all timesteps, $h[k]$, to be equal, so the trajectory breaks are evenly distributed. dircol.AddEqualTimeIntervalsConstraints() # Add input constraints u = dircol.input() dircol.AddConstraintToAllKnotPoints(0 <= u[0]) dircol.AddConstraintToAllKnotPoints(u[0] <= 3) dircol.AddConstraintToAllKnotPoints(-np.pi / 2 <= u[1]) dircol.AddConstraintToAllKnotPoints(u[1] <= np.pi / 2) # Add state constraints x = dircol.state() min_speed = 5 dircol.AddConstraintToAllKnotPoints(x[0] >= min_speed) min_height = 0.5 dircol.AddConstraintToAllKnotPoints(x[3] >= min_height) # Add initial state travel_angle = (3 / 2) * np.pi h0 = 10 dir_vector = np.array([np.cos(travel_angle), np.sin(travel_angle)]) # Start at initial position x0_pos = np.array([h0, 0, 0]) dircol.AddBoundingBoxConstraint(x0_pos, x0_pos, dircol.initial_state()[3:6]) # Periodicity constraints dircol.AddLinearConstraint( dircol.final_state()[0] == dircol.initial_state()[0]) dircol.AddLinearConstraint( dircol.final_state()[1] == dircol.initial_state()[1]) dircol.AddLinearConstraint( dircol.final_state()[2] == dircol.initial_state()[2]) dircol.AddLinearConstraint( dircol.final_state()[3] == dircol.initial_state()[3]) # Always end in right direction # NOTE this assumes that we always are starting in origin if travel_angle % np.pi == 0: # Travel along x-axis dircol.AddConstraint( dircol.final_state()[5] == dircol.initial_state()[5]) elif travel_angle % ((1 / 2) * np.pi) == 0: # Travel along y-axis dircol.AddConstraint( dircol.final_state()[4] == dircol.initial_state()[4]) else: dircol.AddConstraint( dircol.final_state()[5] == dircol.final_state()[4] * np.tan(travel_angle)) # Maximize distance travelled in desired direction p0 = dircol.initial_state() p1 = dircol.final_state() Q = 1 dist_travelled = np.array([p1[4], p1[5]]) # NOTE assume starting in origin dircol.AddFinalCost(-(dir_vector.T.dot(dist_travelled)) * Q) if True: # Cost on input effort R = 0.1 dircol.AddRunningCost(R * (u[0])**2 + R * u[1]**2) # Initial guess is a straight line from x0 in direction if initial_guess: avg_vel_guess = 10 # Guess for initial velocity x0_guess = np.array([avg_vel_guess, travel_angle, 0, h0, 0, 0]) guessed_total_dist_travelled = 200 xf_guess = np.array([ avg_vel_guess, travel_angle, 0, h0, dir_vector[0] * guessed_total_dist_travelled, dir_vector[1] * guessed_total_dist_travelled, ]) initial_x_trajectory = PiecewisePolynomial.FirstOrderHold( [0.0, 4.0], np.column_stack((x0_guess, xf_guess))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) # Solve direct collocation result = Solve(dircol) assert result.is_success() print("Found a solution!") # PLOTTING N_plot = 200 # Plot trajectory x_trajectory = dircol.ReconstructStateTrajectory(result) times = np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), N_plot) x_knots = np.hstack([x_trajectory.value(t) for t in times]) z = x_knots[3, :] x = x_knots[4, :] y = x_knots[5, :] plot_trj_3_wind(np.vstack((x, y, z)).T, dir_vector) # Plot input u_trajectory = dircol.ReconstructInputTrajectory(result) u_knots = np.hstack([u_trajectory.value(t) for t in times]) plot_input_zhao_glider(times, u_knots.T) plt.show() return 0
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) fig = plt.figure() h, = plt.plot([], [], ".-") plt.xlim((-2.5, 2.5)) plt.ylim((-3., 3.)) def draw_trajectory(t, x): h.set_xdata(x[0, :]) h.set_ydata(x[1, :]) fig.canvas.draw() if plt.get_backend() == u"MacOSX": plt.pause(1e-10) dircol.AddStateTrajectoryCallback(draw_trajectory) result = Solve(dircol) assert result.is_success() x_trajectory = dircol.ReconstructStateTrajectory(result) x_knots = np.hstack([ x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), 100) ]) plt.plot(x_knots[0, :], x_knots[1, :]) plt.show()
# 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 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
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 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. ''' #Determine a time within the middle of the range for (min time, max time) N = 50 time_diff = ((maximum_time - minimum_time) / 2.) if (maximum_time - minimum_time) % 2 == 1: time_used = (time_diff - .5) + minimum_time + 3 else: time_used = (time_diff) + minimum_time + 3 print(time_used) time_array = np.arange(0.0, time_used, time_used / (1.0 * N)) dt = time_used / (1.0 * N) mp = MathematicalProgram() #Base case for state/control variables k = 0 state = mp.NewContinuousVariables(4, "state_%d" % k) #x,y,xdot,ydot state_over_time = state u = mp.NewContinuousVariables(2, "u_%d" % k) u_over_time = u num_time_steps = N #int(time_used) #Set Nx4 vector to represent the state variables; (N-1)x2 vector to represent the control inputs for k in range(1, num_time_steps): state = mp.NewContinuousVariables(4, "state_%d" % k) #x,y,xdot,ydot state_over_time = np.vstack((state_over_time, state)) for k in range(1, num_time_steps - 1): u = mp.NewContinuousVariables(2, "u_%d" % k) u_over_time = np.vstack((u_over_time, u)) #constrain initial states print(state_initial) mp.AddLinearConstraint(state_over_time[ 0, 0] == state_initial[0]).evaluator().set_description('start [0]') mp.AddLinearConstraint(state_over_time[ 0, 1] == state_initial[1]).evaluator().set_description('start [1]') mp.AddLinearConstraint(state_over_time[ 0, 2] == state_initial[2]).evaluator().set_description('start [2]') mp.AddLinearConstraint(state_over_time[0, 3] <= state_initial[3] ).evaluator().set_description('start [3], less') mp.AddLinearConstraint( state_over_time[0, 3] >= state_initial[3]).evaluator( ).set_description('start [3], greater') #find dynamics constraints for each state to make standard direct transcription constraints for i in range(0, num_time_steps - 1): next_state_change = self.rocket_dynamics(state_over_time[i, :], u_over_time[i, :]) #constrain every state via dynamics mp.AddLinearConstraint( state_over_time[i + 1, 0] == state_over_time[i, 0] + next_state_change[0] * dt).evaluator().set_description('dynamics[0]' + str(i)) mp.AddLinearConstraint( state_over_time[i + 1, 1] == state_over_time[i, 1] + next_state_change[1] * dt + np.cos(u_over_time[i, 0] / 100.)).evaluator().set_description('dynamics[1]' + str(i)) mp.AddConstraint( state_over_time[i + 1, 2] == state_over_time[i, 2] + next_state_change[2] * dt + np.sin(state_over_time[i, 0] / 100.)).evaluator().set_description('dynamics[2]' + str(i)) mp.AddConstraint(state_over_time[i + 1, 3] == state_over_time[i, 3] + next_state_change[3] * dt).evaluator().set_description('dynamics[3]' + str(i)) #Add a quadratic cost for the amount of fuel used over the course of the trajectory fuel_cost_gain = 100.0 mp.AddQuadraticCost(fuel_cost_gain * u_over_time[:, 0].dot(u_over_time[:, 0])) mp.AddQuadraticCost(fuel_cost_gain * u_over_time[:, 1].dot(u_over_time[:, 1])) #constrain final states world_2 = self.world_2_position mp.AddConstraint( (state_over_time[-1, 2]**2 + state_over_time[-1, 3]**2) <= 1.0).evaluator().set_description( 'speed constraint') #final vel < 1m/s mp.AddConstraint(((state_over_time[-1, 0] - world_2[0])**2 + (state_over_time[-1, 1] - world_2[1])**2) <= .5**2 ).evaluator().set_description('distance constraint') #constrain system to not stray to far from planets world_1 = self.world_1_position dist_between_planets = (world_1[0] - world_2[0])**2 + (world_1[1] - world_2[1])**2 kp = 5.0 for i in range(num_time_steps): mp.AddConstraint(((state_over_time[i, 0] - world_2[0])**2 + (state_over_time[i, 1] - world_2[1])**2) <= dist_between_planets * kp) result = Solve(mp) trajectory = result.GetSolution(state_over_time) input_trajectory = result.GetSolution(u_over_time) print(result.is_success()) if not result.is_success(): infeasible = GetInfeasibleConstraints(mp, result) print "Infeasible constraints:" for i in range(len(infeasible)): print infeasible[i] return trajectory, input_trajectory, time_array