def solve(self):
self.result = Solve(self)
# Add a constraint that Vdot is strictly negative away from x0 (but make an
# exception for the upright fixed point by multipling by s^2).
constraint2 = prog.AddSosConstraint(-Vdot - L*(s**2+c**2-1) -
eps*(x-x0).dot(x-x0)*s**2)
# Add V(0) = 0 constraint
constraint3 = prog.AddLinearConstraint(
V.Substitute({s: 0, c: 1, thetadot: 0}) == 0)
# Add V(theta=pi) = mgl, just to set the scale.
constraint4 = prog.AddLinearConstraint(
V.Substitute({s: 1, c: 0, thetadot: 0}) == p.mass()*p.gravity()*p.length())
# Call the solver.
result = Solve(prog)
assert(result.is_success())
# Note that I've added mgl to the potential energy (relative to the textbook),
# so that it would be non-negative... like the Lyapunov function.
mgl = p.mass()*p.gravity()*p.length()
print('Mechanical Energy = ')
print(.5*p.mass()*p.length()**2*thetadot**2 + mgl*(1-c))
print('V =')
Vsol = Polynomial(result.GetSolution(V))
print(Vsol.RemoveTermsWithSmallCoefficients(1e-6))
# Plot the results as contour plots.
nq = 151
final_state.set_thetadot(0.0)
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, :])
import manipulation.meshcat_utils as dut import numpy as np from meshcat import Visualizer from pydrake.all import MathematicalProgram, Solve prog = MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddCost(x.dot(x)) prog.AddBoundingBoxConstraint(-2, 2, x) result = Solve(prog) meshcat = Visualizer() X, Y = np.meshgrid(np.linspace(-3, 3, 35), np.linspace(-3, 3, 31)) dut.plot_mathematical_program(meshcat, prog, X, Y, result)
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)
execution_time = u_trajectory.end_time()-u_trajectory.start_time()
u_knots = np.hstack([
u_trajectory.value(t) for t in np.linspace(u_trajectory.start_time(),
u_trajectory.end_time(), execution_time*50)
])#here the u_knots now takes 50 points every secs === 50Hz
#u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), execution_time*50)
#u_lookup = np.vectorize(u_trajectory.value)
#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(), execution_time*50)
])
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
dircol.final_state())
# 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)
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 __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 compute_trajectory(self, state_initial, minimum_time, maximum_time):
'''
:param: state_initial: :param state_initial: numpy array of length 6, see satellite_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 6 columns. See simulate_states_over_time for more documentation.
input_trajectory: a 2d array with N-1 row, and 4 columns. See simulate_states_over_time for more documentation.
time_array: an array with N rows.
'''
mp = MathematicalProgram()
# use direct transcription
N = 50
#N = 200
# Unpack state bounds
pmax = self.pmax
vmax = self.vmax
hmax = self.hmax
wmax = self.wmax
# Calculate number of states and control input
nq = len(state_initial)
nu = 4
#### BEGIN: Decision Variables ####
# Inputs
k = 0
u = mp.NewContinuousVariables(nu,"u_%d" % k)
u_over_time = u
for k in range(1,N-1):
u = mp.NewContinuousVariables(nu, "u_%d" % k)
u_over_time = np.vstack((u_over_time, u))
# States
k = 0
states = mp.NewContinuousVariables(nq,"states_over_time_%d" % k)
states_over_time = states
for k in range(1,N):
states = mp.NewContinuousVariables(nq, "states_over_time_%d" % k)
states_over_time = np.vstack((states_over_time, states))
# Final Time
tf = mp.NewContinuousVariables(1,"tf")
dt = tf / (N-1)
#### END: Decision Variables ####
#### BEGIN: Input constraints ####
for i in range(N-1):
for j in range(nu):
mp.AddConstraint(u_over_time[i,j] >= 0.0)
mp.AddConstraint(u_over_time[i,j] <= 0.10)
#### END: Input constraints ####
#### BEGIN: Time constraints ####
# Final time must be between minimum_time and maximum_time
mp.AddConstraint(tf[0] <= maximum_time)
mp.AddConstraint(tf[0] >= minimum_time)
#### END: Time constraints ####
#### BEGIN: State constraints ####
# initial state constraint
for j in range(nq):
mp.AddLinearConstraint(states_over_time[0,j] >= state_initial[j])
mp.AddLinearConstraint(states_over_time[0,j] <= state_initial[j])
# state constraints for all time
for i in range(N):
mp.AddLinearConstraint(states_over_time[i,0] <= pmax)
mp.AddLinearConstraint(states_over_time[i,0] >= -pmax)
mp.AddLinearConstraint(states_over_time[i,1] <= pmax)
mp.AddLinearConstraint(states_over_time[i,1] >= -pmax)
mp.AddLinearConstraint(states_over_time[i,2] <= vmax)
mp.AddLinearConstraint(states_over_time[i,2] >= -vmax)
mp.AddLinearConstraint(states_over_time[i,3] <= vmax)
mp.AddLinearConstraint(states_over_time[i,3] >= -vmax)
mp.AddLinearConstraint(states_over_time[i,4] <= hmax)
mp.AddLinearConstraint(states_over_time[i,4] >= -hmax)
mp.AddLinearConstraint(states_over_time[i,5] <= wmax)
mp.AddLinearConstraint(states_over_time[i,5] >= -wmax)
# dynamic constraint
for i in range(N-1):
dx = self.satellite_dynamics(states_over_time[i,:],u_over_time[i,:])
for j in range(nq):
mp.AddConstraint(states_over_time[i+1,j]<=states_over_time[i,j]+dx[j]*dt[0])
mp.AddConstraint(states_over_time[i+1,j]>=states_over_time[i,j]+dx[j]*dt[0])
# Final state constraint
final_state_error = states_over_time[-1,:] - self.goalState
mp.AddConstraint((final_state_error).dot(final_state_error) <= 0.001**2)
#### END: State constraints ####
#### BEGIN: Costs ####
# Quadratic cost on fuel (aka control)
for j in range(nu):
mp.AddQuadraticCost(u_over_time[:,j].dot(u_over_time[:,j]))
#### END: Costs ####
#### Solve the Optimization! ####
result = Solve(mp)
#print result.is_success()
#### Name outputs appropriately ####
# Time - knot points
optimal_tf = result.GetSolution(tf)
time_step = optimal_tf / (N-1)
time_array = np.arange(0.0,optimal_tf+time_step,time_step)
# Allocate to input trajectory
input_trajectory = result.GetSolution(u_over_time)
# Allocate to trajectory output
trajectory = result.GetSolution(states_over_time)
# save to csv
if os.path.exists("traj.csv"):
os.remove("traj.csv")
if os.path.exists("input_traj.csv"):
os.remove("input_traj.csv")
with open('traj.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
for i in range(N):
row = [time_array[i], trajectory[i,0], trajectory[i,1], trajectory[i,2], trajectory[i,3], trajectory[i,4], trajectory[i,5]]
writer.writerow(row)
with open('input_traj.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
for i in range(N-1):
row = [input_trajectory[i,0], input_trajectory[i,1], input_trajectory[i,2], input_trajectory[i,3]]
writer.writerow(row)
csvFile.close()
return trajectory, input_trajectory, time_array
from pydrake.all import MathematicalProgram, Solve
prog = MathematicalProgram()
x = prog.NewIndeterminates(2, "x")
f = [-x[0] - 2 * x[1]**2, -x[1] - x[0] * x[1] - 2 * x[1]**3]
V = x[0]**2 + 2 * x[1]**2
Vdot = V.Jacobian(x).dot(f)
prog.AddSosConstraint(-Vdot)
result = Solve(prog)
assert (result.is_success())
print('Successfully verified Lyapunov candidate')
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
def direct_transcription():
prog = MathematicalProgram()
N = 500
dt = 0.01
# Create decision variables
n_x = 6
n_u = 2
x = np.empty((n_x, N), dtype=Variable)
u = np.empty((n_u, N - 1), dtype=Variable)
for n in range(N - 1):
x[:, n] = prog.NewContinuousVariables(n_x, "x" + str(n))
u[:, n] = prog.NewContinuousVariables(n_u, "u" + str(n))
x[:, N - 1] = prog.NewContinuousVariables(n_x, "x" + str(N))
T = N - 1
# Add constraints
# x0 = np.array([10, -np.pi / 2, 0, 40, 0, 0])
# Slotine dynamics: x = [airspeed, heading, flight_path_angle, z, x, y]
for n in range(N - 1):
# Dynamics
prog.AddConstraint(
eq(x[:, n + 1],
x[:, n] + dt * continuous_dynamics(x[:, n], u[:, n])))
# Never below sea level
prog.AddConstraint(x[3, n + 1] >= 0 + 0.5)
# Always positive velocity
prog.AddConstraint(x[0, n + 1] >= 0)
# TODO use residuals
# Add periodic constraints
prog.AddConstraint(x[0, 0] - x[0, T] == 0)
prog.AddConstraint(x[1, 0] - x[1, T] == 0)
prog.AddConstraint(x[2, 0] - x[2, T] == 0)
prog.AddConstraint(x[3, 0] - x[3, T] == 0)
# Start at 20 meter height
prog.AddConstraint(x[4, 0] == 0)
prog.AddConstraint(x[5, 0] == 0)
h0 = 20
prog.AddConstraint(x[3, 0] == h0)
# Add cost function
p0 = x[4:6, 0]
p1 = x[4:6, T]
travel_angle = np.pi # TODO start along y direction
dir_vector = np.array([np.sin(travel_angle), np.cos(travel_angle)])
prog.AddCost(dir_vector.T.dot(p1 - p0)) # Maximize distance travelled
print("Initialized opt problem")
# Initial guess
V0 = 10
x_guess = np.zeros((n_x, N))
x_guess[:, 0] = np.array([V0, travel_angle, 0, h0, 0, 0])
for n in range(N - 1):
# Constant airspeed, heading and height
x_guess[0, n + 1] = V0
x_guess[1, n + 1] = travel_angle
x_guess[3, n + 1] = h0
# Interpolate position
avg_speed = 10 # conservative estimate
x_guess[4:6, n + 1] = dir_vector * avg_speed * n * dt
# Let the rest of the variables be initialized to zero
prog.SetInitialGuess(x, x_guess)
# solve mathematical program
solver = SnoptSolver()
result = solver.Solve(prog)
# be sure that the solution is optimal
# assert result.is_success()
# retrieve optimal solution
thrust_opt = result.GetSolution(x)
state_opt = result.GetSolution(u)
result = Solve(prog)
x_sol = result.GetSolution(x)
u_sol = result.GetSolution(u)
breakpoint()
# Slotine dynamics: x = [airspeed, heading, flight_path_angle, z, x, y]
z = x_sol[:, 3]
x = x_sol[:, 4]
y = x_sol[:, 5]
plot_trj_3_wind(np.vstack((x, y, z)).T, get_wind_field)
return
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)
from pydrake.all import MathematicalProgram, Solve, Polynomial, Variables
prog = MathematicalProgram()
x = prog.NewIndeterminates(2, "x")
f = [-x[0] - 2*x[1]**2,
-x[1] - x[0]*x[1] - 2*x[1]**3]
V = prog.NewSosPolynomial(Variables(x), 2)[0].ToExpression()
prog.AddLinearConstraint(V.Substitute({x[0]: 0, x[1]: 0}) == 0)
prog.AddLinearConstraint(V.Substitute({x[0]: 1, x[1]: 0}) == 1)
Vdot = V.Jacobian(x).dot(f)
prog.AddSosConstraint(-Vdot)
result = Solve(prog)
assert(result.is_success())
print('V = ' + str(Polynomial(result.GetSolution(V))
.RemoveTermsWithSmallCoefficients(1e-5)))
def generate(self):
self.result = Solve(self.prog)
assert (self.result.is_success())
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)
def __init__(self, start, goal, degree, n_segments, duration, regions,
H): #sample_times, num_vars, continuity_degree):
R = len(regions)
N = n_segments
M = 100
prog = MathematicalProgram()
# Set H
if H is None:
H = prog.NewBinaryVariables(R, N)
for n_idx in range(N):
prog.AddLinearConstraint(np.sum(H[:, n_idx]) == 1)
poly_xs, poly_ys, poly_zs = [], [], []
vars_ = np.zeros((N, )).astype(object)
for n_idx in range(N):
# for each segment
t = prog.NewIndeterminates(1, "t" + str(n_idx))
vars_[n_idx] = t[0]
poly_x = prog.NewFreePolynomial(Variables(t), degree,
"c_x_" + str(n_idx))
poly_y = prog.NewFreePolynomial(Variables(t), degree,
"c_y_" + str(n_idx))
poly_z = prog.NewFreePolynomial(Variables(t), degree,
"c_z_" + str(n_idx))
for var_ in poly_x.decision_variables():
prog.AddLinearConstraint(var_ <= M)
prog.AddLinearConstraint(var_ >= -M)
for var_ in poly_y.decision_variables():
prog.AddLinearConstraint(var_ <= M)
prog.AddLinearConstraint(var_ >= -M)
for var_ in poly_z.decision_variables():
prog.AddLinearConstraint(var_ <= M)
prog.AddLinearConstraint(var_ >= -M)
poly_xs.append(poly_x)
poly_ys.append(poly_y)
poly_zs.append(poly_z)
phi = np.array([poly_x, poly_y, poly_z])
for r_idx, region in enumerate(regions):
# if r_idx == 0:
# break
A = region[0]
b = region[1]
b = b + (1 - H[r_idx, n_idx]) * M
b = [Polynomial(this_b) for this_b in b]
q = b - A.dot(phi)
sigma = []
for q_idx in range(len(q)):
sigma_1 = prog.NewFreePolynomial(Variables(t), degree - 1)
prog.AddSosConstraint(sigma_1)
sigma_2 = prog.NewFreePolynomial(Variables(t), degree - 1)
prog.AddSosConstraint(sigma_2)
sigma.append(
Polynomial(t[0]) * sigma_1 +
(1 - Polynomial(t[0])) * sigma_2)
# for var_ in sigma[q_idx].decision_variables():
# prog.AddLinearConstraint(var_<=M)
# prog.AddLinearConstraint(var_>=-M)
q_coeffs = q[q_idx].monomial_to_coefficient_map()
sigma_coeffs = sigma[q_idx].monomial_to_coefficient_map()
both_coeffs = set(q_coeffs.keys()) & set(
sigma_coeffs.keys())
for coeff in both_coeffs:
# import pdb; pdb.set_trace()
prog.AddConstraint(
q_coeffs[coeff] == sigma_coeffs[coeff])
# res = Solve(prog)
# print("x: " + str(res.GetSolution(poly_xs[0].ToExpression())))
# print("y: " + str(res.GetSolution(poly_ys[0].ToExpression())))
# print("z: " + str(res.GetSolution(poly_zs[0].ToExpression())))
# import pdb; pdb.set_trace()
# for this_q in q:
# prog.AddSosConstraint(this_q)
# import pdb; pdb.set_trace()
# cost = 0
print("Constraint: x0(0)=x0")
prog.AddConstraint(
poly_xs[0].ToExpression().Substitute(vars_[0], 0.0) == start[0])
prog.AddConstraint(
poly_ys[0].ToExpression().Substitute(vars_[0], 0.0) == start[1])
prog.AddConstraint(
poly_zs[0].ToExpression().Substitute(vars_[0], 0.0) == start[2])
for idx, poly_x, poly_y, poly_z in zip(range(N), poly_xs, poly_ys,
poly_zs):
if idx < N - 1:
print("Constraint: x" + str(idx) + "(1)=x" + str(idx + 1) +
"(0)")
next_poly_x, next_poly_y, next_poly_z = poly_xs[
idx + 1], poly_ys[idx + 1], poly_zs[idx + 1]
prog.AddConstraint(
poly_x.ToExpression().Substitute(vars_[idx], 1.0) ==
next_poly_x.ToExpression().Substitute(vars_[idx + 1], 0.0))
prog.AddConstraint(
poly_y.ToExpression().Substitute(vars_[idx], 1.0) ==
next_poly_y.ToExpression().Substitute(vars_[idx + 1], 0.0))
prog.AddConstraint(
poly_z.ToExpression().Substitute(vars_[idx], 1.0) ==
next_poly_z.ToExpression().Substitute(vars_[idx + 1], 0.0))
else:
print("Constraint: x" + str(idx) + "(1)=xf")
prog.AddConstraint(poly_x.ToExpression().Substitute(
vars_[idx], 1.0) == goal[0])
prog.AddConstraint(poly_y.ToExpression().Substitute(
vars_[idx], 1.0) == goal[1])
prog.AddConstraint(poly_z.ToExpression().Substitute(
vars_[idx], 1.0) == goal[2])
# for
cost = Expression()
for var_, polys in zip(vars_, [poly_xs, poly_ys, poly_zs]):
for poly in polys:
for _ in range(2): #range(2):
poly = poly.Differentiate(var_)
poly = poly.ToExpression().Substitute({var_: 1.0})
cost += poly**2
# for dv in poly.decision_variables():
# cost += (Expression(dv))**2
prog.AddCost(cost)
res = Solve(prog)
print("x: " + str(res.GetSolution(poly_xs[0].ToExpression())))
print("y: " + str(res.GetSolution(poly_ys[0].ToExpression())))
print("z: " + str(res.GetSolution(poly_zs[0].ToExpression())))
self.poly_xs = [
res.GetSolution(poly_x.ToExpression()) for poly_x in poly_xs
]
self.poly_ys = [
res.GetSolution(poly_y.ToExpression()) for poly_y in poly_ys
]
self.poly_zs = [
res.GetSolution(poly_z.ToExpression()) for poly_z in poly_zs
]
self.vars_ = vars_
self.degree = degree
from pydrake.all import MathematicalProgram, Solve, Polynomial, Variables
prog = MathematicalProgram()
x = prog.NewIndeterminates(2, "x")
f = [-x[0] - 2 * x[1]**2, -x[1] - x[0] * x[1] - 2 * x[1]**3]
V = prog.NewSosPolynomial(Variables(x), 2)[0].ToExpression()
prog.AddLinearConstraint(V.Substitute({x[0]: 0, x[1]: 0}) == 0)
prog.AddLinearConstraint(V.Substitute({x[0]: 1, x[1]: 0}) == 1)
Vdot = V.Jacobian(x).dot(f)
prog.AddSosConstraint(-Vdot)
result = Solve(prog)
assert result.is_success()
print("V = " + str(
Polynomial(result.GetSolution(V)).RemoveTermsWithSmallCoefficients(1e-5)))
V.Substitute({
s: 0,
c: 1,
thetadot: 0
}) == 0)
# Add V(theta=pi) = mgl, just to set the scale.
constraint4 = prog.AddLinearConstraint(
V.Substitute({
s: 1,
c: 0,
thetadot: 0
}) == p.mass() * p.gravity() * p.length())
# Call the solver.
result = Solve(prog)
assert (result.is_success())
print(result.get_solver_id().name())
# Note that I've added mgl to the potential energy (relative to the textbook),
# so that it would be non-negative... like the Lyapunov function.
mgl = p.mass() * p.gravity() * p.length()
print('Mechanical Energy = ')
print(.5 * p.mass() * p.length()**2 * thetadot**2 + mgl * (1 - c))
print('V =')
Vsol = Polynomial(result.GetSolution(V))
print(Vsol.RemoveTermsWithSmallCoefficients(1e-6))
# Plot the results as contour plots.
nq = 151
x0 = dircol.initial_state()
xf = dircol.final_state()
dircol.AddConstraintToAllKnotPoints(-thrust_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= thrust_limit)
dircol.AddConstraintToAllKnotPoints(-thrust_limit <= u[1])
dircol.AddConstraintToAllKnotPoints(u[1] <= thrust_limit)
# dircol.AddConstraintToAllKnotPoints()
dircol.AddBoundingBoxConstraint(-2., -2., x0[0])
dircol.AddBoundingBoxConstraint(-1, -1., x0[1])
dircol.AddBoundingBoxConstraint(-np.pi / 3, -np.pi / 3, x0[2])
dircol.AddBoundingBoxConstraint(0, 0, xf[0])
dircol.AddBoundingBoxConstraint(0, 0, xf[1])
dircol.AddBoundingBoxConstraint(0, 0, xf[2])
result = Solve(dircol)
assert result.is_success()
x_trajectory = dircol.ReconstructStateTrajectory(result)
u_trajectory = dircol.ReconstructInputTrajectory(result)
x_knots = np.hstack([
x_trajectory.value(t)
for t in np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), N)
])
u_knots = np.hstack([
u_trajectory.value(t)
for t in np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), N)
])
time = np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), N)
# print(x_knots)
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)
# Construct an n-by-n positive semi-definite matrix as the decision
# variables.
num_states = A[0].shape[0]
P = prog.NewSymmetricContinuousVariables(num_states, "P")
prog.AddPositiveSemidefiniteConstraint(P - .01*np.identity(num_states))
# Add the common Lyapunov conditions.
for i in range(len(A)):
prog.AddPositiveSemidefiniteConstraint(-A[i].transpose().dot(P)
- P.dot(A[i])
- .01*np.identity(num_states))
# Add an objective.
prog.AddLinearCost(np.trace(P))
# Run the optimization.
result = Solve(prog)
if result.is_success():
P = result.GetSolution(P)
print("eig(P) =" + str(np.linalg.eig(P)[0]))
for i in range(len(A)):
print("eig(Pdot" + str(i) + ") = " +
str(np.linalg.eig(A[i].transpose().dot(P) + P.dot(A[i]))[0]))
else:
print('Could not find a common Lyapunov function.')
print('This is expected to occur with some probability: not all')
print('random sets of stable matrices will have a common Lyapunov')
print('function.')
def achieves_force_closure(points, normals, mu, 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
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