def intercepting_with_obs_avoidance(self,
p0,
v0,
pf,
vf,
T,
obstacles=None,
p_puck=None):
"""Intercepting trajectory that avoids obs"""
x0 = np.array(np.concatenate((p0, v0), axis=0))
xf = np.concatenate((pf, vf), axis=0)
prog = MathematicalProgram()
# state and control inputs
N = int(T / self.params.dt) # number of time steps
state = prog.NewContinuousVariables(N + 1, 4, 'state')
cmd = prog.NewContinuousVariables(N, 2, 'input')
# Initial and final state
prog.AddLinearConstraint(eq(state[0], x0))
#prog.AddLinearConstraint(eq(state[-1], xf))
prog.AddQuadraticErrorCost(Q=10.0 * np.eye(4),
x_desired=xf,
vars=state[-1])
self.add_dynamics(prog, N, state, cmd)
## Input saturation
self.add_input_limits(prog, cmd, N)
# Arena constraints
self.add_arena_limits(prog, state, N)
for k in range(N):
prog.AddQuadraticCost(cmd[k].flatten().dot(cmd[k]))
# Add non-linear constraints - will solve with SNOPT
# Avoid other players
if obstacles != None:
for obs in obstacles:
self.avoid_other_player_nl(prog, state, obs, N)
# avoid hitting the puck while generating a kicking trajectory
if not p_puck.any(None):
self.avoid_puck_nl(prog, state, N, p_puck)
solver = SnoptSolver()
result = solver.Solve(prog)
solution_found = result.is_success()
if not solution_found:
print("Solution not found for intercepting_with_obs_avoidance")
u_values = result.GetSolution(cmd)
return solution_found, u_values.T
def mixed_integer_quadratic_program(nc, H, f, A, b, C=None, d=None, **kwargs):
"""
Solves the strictly convex (H > 0) mixed-integer quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d.
The first nc variables in x are continuous, the remaining are binaries.
Arguments
----------
nc : int
Number of continuous variables in the problem.
H : numpy.ndarray
Positive definite Hessian of the cost function.
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
Returns
----------
sol : dict
Dictionary with the solution of the MIQP.
Fields
----------
min : float
Minimum of the MIQP (None if the problem is unfeasible).
argmin : numpy.ndarray
Argument that minimizes the MIQP (None if the problem is unfeasible).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# build program
prog = MathematicalProgram()
x = np.hstack((
prog.NewContinuousVariables(nc),
prog.NewBinaryVariables(n_x - nc)
))
[prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)]
[prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)]
prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0)
[prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()]
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x)
sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin'])
return sol
def make_multiple_dircol_trajectories(num_trajectories,
num_samples,
initial_conditions=None):
from pydrake.all import (
AutoDiffXd,
Expression,
Variable,
MathematicalProgram,
SolverType,
SolutionResult,
DirectCollocationConstraint,
AddDirectCollocationConstraint,
PiecewisePolynomial,
)
import pydrake.symbolic as sym
from pydrake.examples.pendulum import (PendulumPlant)
# initial_conditions maps (ti) -> [1xnum_states] initial state
if initial_conditions is not None:
initial_conditions = intial_cond_dict[initial_conditions]
assert callable(initial_conditions)
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol_constraint = DirectCollocationConstraint(plant, context)
# num_trajectories = 15;
# num_samples = 15;
prog = MathematicalProgram()
# K = prog.NewContinuousVariables(1,7,'K')
def cos(x):
if isinstance(x, AutoDiffXd):
return x.cos()
elif isinstance(x, Variable):
return sym.cos(x)
return math.cos(x)
def sin(x):
if isinstance(x, AutoDiffXd):
return x.sin()
elif isinstance(x, Variable):
return sym.sin(x)
return math.sin(x)
def final_cost(x):
return 100. * (cos(.5 * x[0])**2 + x[1]**2)
h = []
u = []
x = []
xf = (math.pi, 0.)
for ti in range(num_trajectories):
h.append(prog.NewContinuousVariables(1))
prog.AddBoundingBoxConstraint(.01, .2, h[ti])
# prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps
u.append(prog.NewContinuousVariables(1, num_samples, 'u' + str(ti)))
x.append(prog.NewContinuousVariables(2, num_samples, 'x' + str(ti)))
# Use Russ's initial conditions, unless I pass in a function myself.
if initial_conditions is None:
x0 = (.8 + math.pi - .4 * ti, 0.0)
else:
x0 = initial_conditions(ti)
assert len(x0) == 2 #TODO: undo this hardcoding.
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0])
nudge = np.array([.2, .2])
prog.AddBoundingBoxConstraint(xf - nudge, xf + nudge, x[ti][:, -1])
# prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1])
for i in range(num_samples - 1):
AddDirectCollocationConstraint(dircol_constraint, h[ti],
x[ti][:, i], x[ti][:, i + 1],
u[ti][:, i], u[ti][:, i + 1], prog)
for i in range(num_samples):
prog.AddQuadraticCost([1.], [0.], u[ti][:, i])
# prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()]))
# prog.AddBoundingBoxConstraint([-3.], [3.], u[ti][:,i])
# prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x))
# prog.AddCost(final_cost, x[ti][:,-1])
# prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs?
#prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation)
prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out")
# result = prog.Solve()
# print(result)
# print(prog.GetSolution(K))
# print(prog.GetSolution(K).dot(control_basis(x[0][:,0])))
#if (result != SolutionResult.kSolutionFound):
# f = open('/tmp/snopt.out', 'r')
# print(f.read())
# f.close()
return prog, h, u, x
def trajopt_simulation(x0, xf, u_max=0.5): # numeric parameters time_interval = .1 time_steps = 400 # initialize optimization prog = MathematicalProgram() # optimization variables state = prog.NewContinuousVariables(time_steps + 1, 3, 'state') u = prog.NewContinuousVariables(time_steps, 1, 'u') # final position constraint # for residual in constraint_state_to_orbit(state[-2], state[-1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual == 0) # initial position constraint prog.AddConstraint(eq(state[0],x0)) # discretized dynamics for t in range(time_steps): residuals = dubins_discrete_dynamics(state[t], state[t+1], u[t], time_interval) for residual in residuals: prog.AddConstraint(residual == 0) # control limits prog.AddConstraint(u[t][0] <= u_max) prog.AddConstraint(-u_max <= u[t][0]) # cost - increases cost if off the orbit p = state[t][:2] - xf[:2] v = (state[t+1][:2] - state[t][:2]) / time_interval # prog.AddCost(time_interval) # prog.AddCost(u[t][0]*u[t][0]*time_interval) # prog.AddCost(p.dot(v)*time_interval) for residual in constraint_state_to_orbit(state[t], state[t+1], xf, 1/u_max, time_interval): # prog.AddConstraint(residual >= 0) prog.AddQuadraticCost(residual) # prog.AddCost((p.dot(p))) # # initial guess state_guess = interpolate_dubins_state( np.array([0, 0, np.pi]), xf, time_steps, time_interval, 1/u_max ) prog.SetInitialGuess(state, state_guess) solver = SnoptSolver() result = solver.Solve(prog) assert result.is_success() # retrieve optimal solution u_opt = result.GetSolution(u).T state_opt = result.GetSolution(state).T return state_opt, u_opt
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
mp = MathematicalProgram()
#control variables = u = self.nu x 1
#ddot{q} as variable w/ q's shape
controls = mp.NewContinuousVariables(self.nu, "controls")
qdd = mp.NewContinuousVariables(q.shape[0], "qdd")
#make variables for lambda's and beta's
k = 0
b = mp.NewContinuousVariables(2, "betas_%d" % k)
betas = b
for i in range(len(self.grasp_points)):
b = mp.NewContinuousVariables(
2, "betas_%d" % k) #pair of betas for each contact point
betas = np.vstack((betas, b))
mp.AddLinearConstraint(
betas[i, 0] >= 0).evaluator().set_description(
"beta_0 greater than 0 for %d" % i)
mp.AddLinearConstraint(
betas[i, 1] >= 0).evaluator().set_description(
"beta_1 greater than 0 for %d" % i)
#c_0=normals+mu*tangent
#c1 = normals-mu*tangent
n = grasp_normals_world_now.T[:, 0:2] #row=contact point?
t = get_perpendicular2d(n[0])
c_i1 = n[0] + np.dot(self.mu, t)
c_i2 = n[0] - np.dot(self.mu, t)
l = c_i1.dot(betas[0, 0]) + c_i2.dot(betas[0, 1])
lambdas = l
for i in range(1, len(self.grasp_points)):
n = grasp_normals_world_now.T[:, 0:
2] #row=contact point? transpose then index
t = get_perpendicular2d(n[i])
c_i1 = n[i] - np.dot(self.mu, t)
c_i2 = n[i] + np.dot(self.mu, t)
l = c_i1.dot(betas[i, 0]) + c_i2.dot(betas[i, 1])
lambdas = np.vstack((lambdas, l))
control_period = 0.0333
#Constrain the dyanmics
dyn = M.dot(qdd) + C - B.dot(controls)
for i in range(q.shape[0]):
j_c = 0
for l in range(len(self.grasp_points)):
j_sub = J_contact[:, l, :].T
j_c += j_sub.dot(lambdas[l])
# print(j_c.shape)
# print(dyn.shape)
mp.AddConstraint(dyn[i] - j_c[i] == 0)
#PD controller using kinematics
Kp = 100.0
Kd = 1.0
control_period = 0.0333
next_q = q + v.dot(control_period) + np.dot(qdd.dot(control_period**2),
.5)
next_q_dot = v + qdd.dot(control_period)
mp.AddQuadraticCost(Kp * (qdes - next_q).dot((qdes - next_q).T) + Kd *
(next_q_dot).dot(next_q_dot.T))
Kp_ext = 100.
mp.AddQuadraticCost(Kp_ext * (qdes - next_q)[-3:].dot(
(qdes - next_q)[-3:].T))
res = Solve(mp)
c = res.GetSolution(controls)
return c
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(
self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(
self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
number_of_grasp_points = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
# print('shape' + str(np.shape(qdes)))
# print('self.nq' + str(self.nq))
# print('self.nu' + str(self.nu))
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
kd = 1
kp = 25
from pydrake.all import MathematicalProgram, Solve
mp = MathematicalProgram()
u = mp.NewContinuousVariables(self.nu, "u")
qdd = mp.NewContinuousVariables(self.nq, "qdd")
x_y_dimensions = 2
lambda_variable = mp.NewContinuousVariables(x_y_dimensions, number_of_grasp_points, "lambda_variable")
forces = np.zeros((self.nq))
for i in range(number_of_grasp_points):
current_forces = np.transpose(J_contact)[:,i,:].dot(lambda_variable[:,i])
forces = forces + current_forces
leftHandSide = M.dot(qdd) + C
rightHandSide = B.dot(u) + forces
for i in range(len(leftHandSide)):
mp.AddConstraint(leftHandSide[i] == rightHandSide[i])
# Calculate Normals
normals = np.array([])
for i in range(number_of_grasp_points):
current_grasp_normal = grasp_normals_world_now[0:2, i]
normals = np.vstack((normals, current_grasp_normal)) if normals.size else current_grasp_normal
# Calculate Tangeants
tangeants = np.array([])
for i in range(number_of_grasp_points):
current_grasp_tangeant = np.array([normals[i, 1], -normals[i, 0]])
tangeants = np.vstack((tangeants, current_grasp_tangeant)) if tangeants.size else current_grasp_tangeant
# Create beta variables
beta = mp.NewContinuousVariables(2, number_of_grasp_points, "b0")
# Create Grasps
for i in range(number_of_grasp_points):
c0 = normals[i] - self.mu * tangeants[i]
c1 = normals[i] + self.mu * tangeants[i]
right_hand_lambda1 = c0 * beta[0, i] + c1 * beta[1, i]
mp.AddConstraint(lambda_variable[0, i] == right_hand_lambda1[0])
mp.AddConstraint(lambda_variable[1, i] == right_hand_lambda1[1])
mp.AddConstraint(beta[0, i] >= 0)
mp.AddConstraint(beta[1, i] >= 0)
mp.AddConstraint(normals[i].dot(lambda_variable[:, i]) >= 0.25)
# Copying the control period of the constructor. Probably not supposed to do this...
next_tick_qd = v + qdd * self.cont¨rol_period
next_tick_q = q + next_tick_qd * self.control_period
q_error = qdes - next_tick_q
proportionalCost = q_error.dot(np.transpose(q_error))
qd_error = 0 - next_tick_qd
diffCost = qd_error.dot(np.transpose(qd_error))
mp.AddQuadraticCost(kp * proportionalCost + kd * diffCost)
result = Solve(mp)
u_solution = result.GetSolution(u)
u = np.zeros(self.nu)
return u_solution
class Optimization:
def __init__(self, config):
self.physical_params = config["physical_params"]
self.T = config["T"]
self.dt = config["dt"]
self.initial_state = config["xinit"]
self.goal_state = config["xgoal"]
self.ellipse_arc = config["ellipse_arc"]
self.deviation_cost = config["deviation_cost"]
self.Qf = config["Qf"]
self.limits = config["limits"]
self.n_state = 6
self.n_nominal_forces = 4
self.tire_model = config["tire_model"]
self.initial_guess_config = config["initial_guess_config"]
self.puddle_model = config["puddle_model"]
self.force_constraint = config["force_constraint"]
self.visualize_initial_guess = config["visualize_initial_guess"]
self.dynamics = Dynamics(self.physical_params.lf,
self.physical_params.lr,
self.physical_params.m,
self.physical_params.Iz, self.dt)
def build_program(self):
self.prog = MathematicalProgram()
# Declare variables.
state = self.prog.NewContinuousVariables(rows=self.T + 1,
cols=self.n_state,
name='state')
nominal_forces = self.prog.NewContinuousVariables(
rows=self.T, cols=self.n_nominal_forces, name='nominal_forces')
steers = self.prog.NewContinuousVariables(rows=self.T, name="steers")
slip_ratios = self.prog.NewContinuousVariables(rows=self.T,
cols=2,
name="slip_ratios")
self.state = state
self.nominal_forces = nominal_forces
self.steers = steers
self.slip_ratios = slip_ratios
# Set the initial state.
xinit = pack_state_vector(self.initial_state)
for i, s in enumerate(xinit):
self.prog.AddConstraint(state[0, i] == s)
# Constrain xdot to always be at least some value to prevent numerical issues with optimizer.
for i in range(self.T + 1):
s = unpack_state_vector(state[i])
self.prog.AddConstraint(s["xdot"] >= self.limits.min_xdot)
# Constrain slip ratio to be at least a certain magnitude.
if i != self.T:
self.prog.AddConstraint(
slip_ratios[i,
0]**2.0 >= self.limits.min_slip_ratio_mag**2.0)
self.prog.AddConstraint(
slip_ratios[i,
1]**2.0 >= self.limits.min_slip_ratio_mag**2.0)
# Control rate limits.
for i in range(self.T - 1):
ddelta = self.dt * (steers[i + 1] - steers[i])
f_dkappa = self.dt * (slip_ratios[i + 1, 0] - slip_ratios[i, 0])
r_dkappa = self.dt * (slip_ratios[i + 1, 1] - slip_ratios[i, 1])
self.prog.AddConstraint(ddelta <= self.limits.max_ddelta)
self.prog.AddConstraint(ddelta >= -self.limits.max_ddelta)
self.prog.AddConstraint(f_dkappa <= self.limits.max_dkappa)
self.prog.AddConstraint(f_dkappa >= -self.limits.max_dkappa)
self.prog.AddConstraint(r_dkappa <= self.limits.max_dkappa)
self.prog.AddConstraint(r_dkappa >= -self.limits.max_dkappa)
# Control value limits.
for i in range(self.T):
self.prog.AddConstraint(steers[i] <= self.limits.max_delta)
self.prog.AddConstraint(steers[i] >= -self.limits.max_delta)
# Add dynamics constraints by constraining residuals to be zero.
for i in range(self.T):
residuals = self.dynamics.nominal_dynamics(state[i], state[i + 1],
nominal_forces[i],
steers[i])
for r in residuals:
self.prog.AddConstraint(r == 0.0)
# Add the cost function.
self.add_cost(state)
# Add a different force constraint depending on the configuration.
if self.force_constraint == ForceConstraint.NO_PUDDLE:
self.add_no_puddle_force_constraints(
state, nominal_forces, steers,
self.tire_model.get_deterministic_model(), slip_ratios)
elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED:
self.add_mean_constrained()
elif self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED:
self.add_chance_constraints()
else:
raise NotImplementedError("ForceConstraint type invalid.")
return
def constant_input_initial_guess(self, state, steers, slip_ratios,
nominal_forces):
# Guess the slip ratio as the minimum allowed value.
gslip_ratios = np.tile(
np.array([
self.limits.min_slip_ratio_mag, self.limits.min_slip_ratio_mag
]), (self.T, 1))
# Guess the slip angle as a linearly ramping steer that then becomes constant.
# This is done by creating an array of values corresponding to end_delta then
# filling in the initial ramp up phase.
gsteers = self.initial_guess_config["end_delta"] * np.ones(self.T)
igc = self.initial_guess_config
for i in range(igc["ramp_steps"]):
gsteers[i] = (i / igc["ramp_steps"]) * (igc["end_delta"] -
igc["start_delta"])
# Create empty arrays for state and forces.
gstate = np.empty((self.T + 1, self.n_state))
gstate[0] = pack_state_vector(self.initial_state)
gforces = np.empty((self.T, 4))
all_slips = self.T * [None]
# Simulate the dynamics for the initial guess differently depending on the force
# constraint being used.
if self.force_constraint == ForceConstraint.NO_PUDDLE:
tire_model = self.tire_model.get_deterministic_model()
for i in range(self.T):
s = unpack_state_vector(gstate[i])
# Simulate the dynamics for one time step.
gstate[i + 1], forces, slips = self.dynamics.simulate(
gstate[i], gsteers[i], gslip_ratios[i, 0],
gslip_ratios[i, 1], tire_model, self.dt)
# Store the results.
gforces[i] = pack_force_vector(forces)
all_slips[i] = slips
elif self.force_constraint == ForceConstraint.MEAN_CONSTRAINED or self.force_constraint == ForceConstraint.CHANCE_CONSTRAINED:
# mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy])
mean_model = self.tire_model.get_mean_model(
self.puddle_model.get_mean_fun())
for i in range(self.T):
# Update the tire model based off the conditions of the world
# at (x, y)
s = unpack_state_vector(gstate[i])
tire_model = lambda slip_ratio, slip_angle: mean_model(
slip_ratio, slip_angle, s["X"], s["Y"])
# Simulate the dynamics for one time step.
gstate[i + 1], forces, slips = self.dynamics.simulate(
gstate[i], gsteers[i], gslip_ratios[i, 0],
gslip_ratios[i, 1], tire_model, self.dt)
# Store the results.
gforces[i] = pack_force_vector(forces)
all_slips[i] = slips
else:
raise NotImplementedError(
"Invalid value for self.force_constraint")
# Declare array for the initial guess and set the values.
initial_guess = np.empty(self.prog.num_vars())
self.prog.SetDecisionVariableValueInVector(state, gstate,
initial_guess)
self.prog.SetDecisionVariableValueInVector(steers, gsteers,
initial_guess)
self.prog.SetDecisionVariableValueInVector(slip_ratios, gslip_ratios,
initial_guess)
self.prog.SetDecisionVariableValueInVector(nominal_forces, gforces,
initial_guess)
if self.visualize_initial_guess:
# TODO: reorganize visualizations
psis = gstate[:, 2]
xs = gstate[:, 4]
ys = gstate[:, 5]
fig, ax = plt.subplots()
if self.force_constraint != ForceConstraint.NO_PUDDLE:
plot_puddles(ax, self.puddle_model)
plot_ellipse_arc(ax, self.ellipse_arc)
plot_planned_trajectory(ax, xs, ys, psis, gsteers,
self.physical_params)
# Plot the slip ratios/angles
plot_slips(all_slips)
plot_forces(gforces)
if self.force_constraint == ForceConstraint.NO_PUDDLE:
generate_animation(xs,
ys,
psis,
gsteers,
self.physical_params,
self.dt,
puddle_model=None)
else:
generate_animation(xs,
ys,
psis,
gsteers,
self.physical_params,
self.dt,
puddle_model=self.puddle_model)
return initial_guess
def solve_program(self):
# Generate initial guess
initial_guess = self.constant_input_initial_guess(
self.state, self.steers, self.slip_ratios, self.nominal_forces)
# Solve the problem.
solver = SnoptSolver()
result = solver.Solve(self.prog, initial_guess)
solver_details = result.get_solver_details()
print("Exit flag: " + str(solver_details.info))
self.visualize_results(result)
def visualize_results(self, result):
state_res = result.GetSolution(self.state)
psis = state_res[:, 2]
xs = state_res[:, 4]
ys = state_res[:, 5]
steers = result.GetSolution(self.steers)
fig, ax = plt.subplots()
plot_ellipse_arc(ax, self.ellipse_arc)
plot_puddles(ax, self.puddle_model)
plot_planned_trajectory(ax, xs, ys, psis, steers, self.physical_params)
generate_animation(xs,
ys,
psis,
steers,
self.physical_params,
self.dt,
puddle_model=self.puddle_model)
plt.show()
def add_cost(self, state):
# Add the final state cost function.
diff_state = pack_state_vector(self.goal_state) - state[-1]
self.prog.AddQuadraticCost(diff_state.T @ self.Qf @ diff_state)
# Get the approx distance function for the ellipse.
fun = self.ellipse_arc.approx_dist_fun()
for i in range(self.T):
s = unpack_state_vector(state[i])
self.prog.AddCost(self.deviation_cost * fun(s["X"], s["Y"]))
def add_mean_constrained(self):
# mean model is a function that maps (slip_ratio, slip_angle, x, y) -> (E[Fx], E[Fy])
mean_model = self.tire_model.get_mean_model(
self.puddle_model.get_mean_fun())
for i in range(self.T):
# Get slip angles and slip ratios.
s = unpack_state_vector(self.state[i])
F = unpack_force_vector(self.nominal_forces[i])
# get the tire model at this position in space.
tire_model = lambda slip_ratio, slip_angle: mean_model(
slip_ratio, slip_angle, s["X"], s["Y"])
# Unpack values
delta = self.steers[i]
alpha_f, alpha_r = self.dynamics.slip_angles(
s["xdot"], s["ydot"], s["psidot"], delta)
kappa_f = self.slip_ratios[i, 0]
kappa_r = self.slip_ratios[i, 1]
# Compute expected forces.
E_Ffx, E_Ffy = tire_model(kappa_f, alpha_f)
E_Frx, E_Fry = tire_model(kappa_r, alpha_r)
# Constrain these expected force values to be equal to the nominal
# forces in the optimization.
self.prog.AddConstraint(E_Ffx - F["f_long"] == 0.0)
self.prog.AddConstraint(E_Ffy - F["f_lat"] == 0.0)
self.prog.AddConstraint(E_Frx - F["r_long"] == 0.0)
self.prog.AddConstraint(E_Fry - F["r_lat"] == 0.0)
def add_chance_constrained(self):
pass
def add_no_puddle_force_constraints(self, state, forces, steers,
pacejka_model, slip_ratios):
"""
Args:
prog:
state:
forces:
steers:
pacejka_model: function with signature (slip_ratio, slip_angle) using pydrake.math
"""
for i in range(self.T):
# Get slip angles and slip ratios.
s = unpack_state_vector(state[i])
F = unpack_force_vector(forces[i])
delta = steers[i]
alpha_f, alpha_r = self.dynamics.slip_angles(
s["xdot"], s["ydot"], s["psidot"], delta)
kappa_f = slip_ratios[i, 0]
kappa_r = slip_ratios[i, 1]
Ffx, Ffy = pacejka_model(kappa_f, alpha_f)
Frx, Fry = pacejka_model(kappa_r, alpha_r)
# Constrain the values from the pacejka model to be equal
# to the desired values in the optimization.
self.prog.AddConstraint(Ffx - F["f_long"] == 0.0)
self.prog.AddConstraint(Ffy - F["f_lat"] == 0.0)
self.prog.AddConstraint(Frx - F["r_long"] == 0.0)
self.prog.AddConstraint(Fry - F["r_lat"] == 0.0)
def compute_opt_trajectory(self, state_initial, goal_func, verbose=True):
'''
nonlinear trajectory optimization to land drone starting at state_initial, on a vehicle target trajectory given by the goal_func
: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.
'''
# initialize math program
import time
start_time = time.time()
mp = MathematicalProgram()
num_time_steps = 40
booster = mp.NewContinuousVariables(3, "booster_0")
booster_over_time = booster[np.newaxis, :]
state = mp.NewContinuousVariables(6, "state_0")
state_over_time = state[np.newaxis, :]
dt = mp.NewContinuousVariables(1, "dt")
for k in range(1, num_time_steps - 1):
booster = mp.NewContinuousVariables(3, "booster_%d" % k)
booster_over_time = np.vstack((booster_over_time, booster))
for k in range(1, num_time_steps):
state = mp.NewContinuousVariables(6, "state_%d" % k)
state_over_time = np.vstack((state_over_time, state))
goal_state = goal_func(dt[0] * 39)
# calculate states over time
for i in range(1, num_time_steps):
sim_next_state = state_over_time[
i - 1, :] + dt[0] * self.drone_dynamics(
state_over_time[i - 1, :], booster_over_time[i - 1, :])
# add constraints to restrict the next state to the decision variables
for j in range(6):
mp.AddConstraint(sim_next_state[j] == state_over_time[i, j])
# don't hit ground
mp.AddLinearConstraint(state_over_time[i, 2] >= 0.05)
# enforce that we must be thrusting within some constraints
mp.AddLinearConstraint(booster_over_time[i - 1, 0] <= 5.0)
mp.AddLinearConstraint(booster_over_time[i - 1, 0] >= -5.0)
mp.AddLinearConstraint(booster_over_time[i - 1, 1] <= 5.0)
mp.AddLinearConstraint(booster_over_time[i - 1, 1] >= -5.0)
# keep forces in a reasonable position
mp.AddLinearConstraint(booster_over_time[i - 1, 2] >= 1.0)
mp.AddLinearConstraint(
booster_over_time[i - 1, 0] <= booster_over_time[i - 1, 2])
mp.AddLinearConstraint(
booster_over_time[i - 1, 0] >= -booster_over_time[i - 1, 2])
mp.AddLinearConstraint(
booster_over_time[i - 1, 1] <= booster_over_time[i - 1, 2])
mp.AddLinearConstraint(
booster_over_time[i - 1, 1] >= -booster_over_time[i - 1, 2])
# add constraints on initial state
for i in range(6):
mp.AddLinearConstraint(state_over_time[0, i] == state_initial[i])
# add constraints on dt
mp.AddLinearConstraint(dt[0] >= 0.001)
# add constraints on end state
# end goal velocity
mp.AddConstraint(state_over_time[-1, 0] <= goal_state[0] + 0.01)
mp.AddConstraint(state_over_time[-1, 0] >= goal_state[0] - 0.01)
mp.AddConstraint(state_over_time[-1, 1] <= goal_state[1] + 0.01)
mp.AddConstraint(state_over_time[-1, 1] >= goal_state[1] - 0.01)
mp.AddConstraint(state_over_time[-1, 2] <= goal_state[2] + 0.01)
mp.AddConstraint(state_over_time[-1, 2] >= goal_state[2] - 0.01)
mp.AddConstraint(state_over_time[-1, 3] <= goal_state[3] + 0.01)
mp.AddConstraint(state_over_time[-1, 3] >= goal_state[3] - 0.01)
mp.AddConstraint(state_over_time[-1, 4] <= goal_state[4] + 0.01)
mp.AddConstraint(state_over_time[-1, 4] >= goal_state[4] - 0.01)
mp.AddConstraint(state_over_time[-1, 5] <= goal_state[5] + 0.01)
mp.AddConstraint(state_over_time[-1, 5] >= goal_state[5] - 0.01)
# add the cost function
mp.AddQuadraticCost(
0.01 * booster_over_time[:, 0].dot(booster_over_time[:, 0]))
mp.AddQuadraticCost(
0.01 * booster_over_time[:, 1].dot(booster_over_time[:, 1]))
mp.AddQuadraticCost(
0.01 * booster_over_time[:, 2].dot(booster_over_time[:, 2]))
# add more penalty on dt because minimizing time turns out to be more important
mp.AddQuadraticCost(10000 * dt[0] * dt[0])
solved = mp.Solve()
if verbose:
print solved
# extract
booster_over_time = mp.GetSolution(booster_over_time)
output_states = mp.GetSolution(state_over_time)
dt = mp.GetSolution(dt)
time_array = np.zeros(40)
for i in range(40):
time_array[i] = i * dt
trajectory = self.simulate_states_over_time(state_initial, time_array,
booster_over_time)
durations = time_array[1:len(time_array
)] - time_array[0:len(time_array) - 1]
fuel_consumption = (
np.sum(booster_over_time[:len(time_array)]**2, axis=1) *
durations).sum()
if verbose:
print 'expected remaining fuel consumption', fuel_consumption
print("took %s seconds" % (time.time() - start_time))
print ''
return trajectory, booster_over_time, time_array, fuel_consumption
def compute_trajectory(self,
state_initial,
goal_state,
flight_time,
exact=False,
verbose=True):
'''
nonlinear trajectory optimization to land drone starting at state_initial, to a goal_state, in a specific flight_time
: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.
'''
# initialize math program
import time
start_time = time.time()
mp = MathematicalProgram()
num_time_steps = int(min(40, flight_time / 0.05))
dt = flight_time / num_time_steps
time_array = np.arange(0.0, flight_time - 0.00001,
dt) # hacky way to ensure it goes down
num_time_steps = len(time_array) # to ensure these are equal lenghts
flight_time = dt * num_time_steps
if verbose:
print ''
print 'solving problem with no guess'
print 'initial state', state_initial
print 'goal state', goal_state
print 'flight time', flight_time
print 'num time steps', num_time_steps
print 'exact traj', exact
print 'dt', dt
booster = mp.NewContinuousVariables(3, "booster_0")
booster_over_time = booster[np.newaxis, :]
state = mp.NewContinuousVariables(6, "state_0")
state_over_time = state[np.newaxis, :]
for k in range(1, num_time_steps - 1):
booster = mp.NewContinuousVariables(3, "booster_%d" % k)
booster_over_time = np.vstack((booster_over_time, booster))
for k in range(1, num_time_steps):
state = mp.NewContinuousVariables(6, "state_%d" % k)
state_over_time = np.vstack((state_over_time, state))
# calculate states over time
for i in range(1, len(time_array)):
time_step = time_array[i] - time_array[i - 1]
sim_next_state = state_over_time[
i - 1, :] + time_step * self.drone_dynamics(
state_over_time[i - 1, :], booster_over_time[i - 1, :])
# add constraints to restrict the next state to the decision variables
for j in range(6):
mp.AddLinearConstraint(sim_next_state[j] == state_over_time[i,
j])
# don't hit ground
mp.AddLinearConstraint(state_over_time[i, 2] >= 0.05)
# enforce that we must be thrusting within some constraints
mp.AddLinearConstraint(booster_over_time[i - 1, 0] <= 5.0)
mp.AddLinearConstraint(booster_over_time[i - 1, 0] >= -5.0)
mp.AddLinearConstraint(booster_over_time[i - 1, 1] <= 5.0)
mp.AddLinearConstraint(booster_over_time[i - 1, 1] >= -5.0)
# keep forces in a reasonable position
mp.AddLinearConstraint(booster_over_time[i - 1, 2] >= 1.0)
mp.AddLinearConstraint(booster_over_time[i - 1, 2] <= 15.0)
mp.AddLinearConstraint(
booster_over_time[i - 1, 0] <= booster_over_time[i - 1, 2])
mp.AddLinearConstraint(
booster_over_time[i - 1, 0] >= -booster_over_time[i - 1, 2])
mp.AddLinearConstraint(
booster_over_time[i - 1, 1] <= booster_over_time[i - 1, 2])
mp.AddLinearConstraint(
booster_over_time[i - 1, 1] >= -booster_over_time[i - 1, 2])
# add constraints on initial state
for i in range(6):
mp.AddLinearConstraint(state_over_time[0, i] == state_initial[i])
# add constraints on end state
# 100 should be a lower constant...
mp.AddLinearConstraint(
state_over_time[-1, 0] <= goal_state[0] + flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 0] >= goal_state[0] - flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 1] <= goal_state[1] + flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 1] >= goal_state[1] - flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 2] <= goal_state[2] + flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 2] >= goal_state[2] - flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 3] <= goal_state[3] + flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 3] >= goal_state[3] - flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 4] <= goal_state[4] + flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 4] >= goal_state[4] - flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 5] <= goal_state[5] + flight_time / 100.)
mp.AddLinearConstraint(
state_over_time[-1, 5] >= goal_state[5] - flight_time / 100.)
# add the cost function
mp.AddQuadraticCost(
10. * booster_over_time[:, 0].dot(booster_over_time[:, 0]))
mp.AddQuadraticCost(
10. * booster_over_time[:, 1].dot(booster_over_time[:, 1]))
mp.AddQuadraticCost(
10. * booster_over_time[:, 2].dot(booster_over_time[:, 2]))
for i in range(1, len(time_array) - 1):
cost_multiplier = np.exp(
4.5 * i / (len(time_array) -
1)) # exp starting at 1 and going to around 90
# penalize difference_in_state
dist_to_goal_pos = goal_state[:3] - state_over_time[i - 1, :3]
mp.AddQuadraticCost(cost_multiplier *
dist_to_goal_pos.dot(dist_to_goal_pos))
# penalize difference in velocity
if exact:
dist_to_goal_vel = goal_state[-3:] - state_over_time[i - 1,
-3:]
mp.AddQuadraticCost(cost_multiplier / 2. *
dist_to_goal_vel.dot(dist_to_goal_vel))
else:
dist_to_goal_vel = goal_state[-3:] - state_over_time[i - 1,
-3:]
mp.AddQuadraticCost(cost_multiplier / 6. *
dist_to_goal_vel.dot(dist_to_goal_vel))
solved = mp.Solve()
if verbose:
print solved
# extract
booster_over_time = mp.GetSolution(booster_over_time)
output_states = mp.GetSolution(state_over_time)
durations = time_array[1:len(time_array
)] - time_array[0:len(time_array) - 1]
fuel_consumption = (
np.sum(booster_over_time[:len(time_array)]**2, axis=1) *
durations).sum()
if verbose:
print 'expected remaining fuel consumption', fuel_consumption
print("took %s seconds" % (time.time() - start_time))
print 'goal state', goal_state
print 'end state', output_states[-1]
print ''
return output_states, booster_over_time, time_array
xf = (math.pi, 0.)
if num_states == 4:
x0 += (0., 0.)
xf += (0., 0.)
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0])
prog.AddBoundingBoxConstraint(xf, xf, x[ti][:, -1])
for i in range(num_samples - 1):
AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:, i],
x[ti][:, i + 1], u[ti][:, i],
u[ti][:, i + 1], prog)
for i in range(num_samples):
prog.AddQuadraticCost([1.], [0.], u[ti][:, i])
# prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()]))
# prog.AddBoundingBoxConstraint([0.], [0.], u[ti][:,i])
# prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x))
# prog.AddCost(final_cost, x[ti][:,-1])
#prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation)
prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out")
result = prog.Solve()
print(result)
print(prog.GetSolution(K))
print(prog.GetSolution(K).dot(control_basis(x[0][:, 0])))
#if (result != SolutionResult.kSolutionFound):
# f = open('/tmp/snopt.out', 'r')
def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5):
"""
Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d.
Arguments
----------
H : numpy.ndarray
Positive definite Hessian of the cost function.
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
tol : float
Maximum value of a residual of an inequality to consider the related constraint active.
Returns
----------
sol : dict
Dictionary with the solution of the QP.
Fields
----------
min : float
Minimum of the QP (None if the problem is unfeasible).
argmin : numpy.ndarray
Argument that minimizes the QP (None if the problem is unfeasible).
active_set : list of int
Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible).
multiplier_inequality : numpy.ndarray
Lagrange multipliers for the inequality constraints (None if the problem is unfeasible).
multiplier_equality : numpy.ndarray
Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# reshape inputs
if len(f.shape) == 2:
f = np.reshape(f, f.shape[0])
# build program
prog = MathematicalProgram()
x = prog.NewContinuousVariables(n_x)
inequalities = []
for i in range(n_ineq):
lhs = A[i, :] + 1.e-15 * np.random.rand(
(n_x)
) # drake raises a RuntimeError if the in the expression x does not appear (e.g.: 0 x <= 1)
rhs = b[i] + 1.e-15 * np.random.rand(
1
) # in case the constraint is 0 x <= 0 the previous trick ends up adding the constraint x <= 0 to the program...
inequalities.append(prog.AddLinearConstraint(lhs.dot(x) <= rhs))
for i in range(n_eq):
prog.AddLinearConstraint(C[i, :].dot(x) == d[i])
prog.AddQuadraticCost(.5 * x.dot(H).dot(x) + f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), "OutputFlag", 0)
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None,
'active_set': None,
'multiplier_inequality': None,
'multiplier_equality': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x).reshape(n_x, 1)
sol['min'] = .5 * sol['argmin'].T.dot(H).dot(
sol['argmin'])[0, 0] + f.dot(sol['argmin'])[0]
sol['active_set'] = np.where(
A.dot(sol['argmin']) - b > -tol)[0].tolist()
# retrieve multipliers through KKT conditions
lhs = A[sol['active_set'], :]
rhs = b[sol['active_set'], :]
if n_eq > 0:
lhs = np.vstack((lhs, C))
rhs = np.vstack((rhs, d))
H_inv = np.linalg.inv(H)
M = lhs.dot(H_inv).dot(lhs.T)
m = -np.linalg.inv(M).dot(lhs.dot(H_inv).dot(f.reshape(n_x, 1)) + rhs)
sol['multiplier_inequality'] = np.zeros((n_ineq, 1))
for i, j in enumerate(sol['active_set']):
sol['multiplier_inequality'][j, 0] = m[i]
if n_eq > 0:
sol['multiplier_equality'] = m[len(sol['active_set']):, :]
return sol
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())
class HybridModelPredictiveController(object):
def __init__(self, S, N, Q, R, P, X_N):
# store inputs
self.S = S
self.N = N
self.Q = Q
self.R = R
self.P = P
self.X_N = X_N
# mpMIQP
self.build_mpmiqp()
def build_mpmiqp(self):
# express the constrained dynamics as a list of polytopes in the (x,u,x+)-space
P = graph_representation(self.S)
m = big_m(P)
# initialize program
self.prog = MathematicalProgram()
self.x = []
self.u = []
self.d = []
obj = 0.
self.binaries_lower_bound = []
# initial conditions (set arbitrarily to zero in the building phase)
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
self.initial_condition = []
for k in range(self.S.nx):
self.initial_condition.append(self.prog.AddLinearConstraint(self.x[0][k] == 0.).evaluator())
# loop over time
for t in range(self.N):
# create input, mode and next state variables
self.u.append(self.prog.NewContinuousVariables(self.S.nu))
self.d.append(self.prog.NewBinaryVariables(self.S.nm))
self.x.append(self.prog.NewContinuousVariables(self.S.nx))
# enforce constrained dynamics (big-m methods)
xux = np.concatenate((self.x[t], self.u[t], self.x[t+1]))
for i in range(self.S.nm):
mi_sum = np.sum([m[i][j] * self.d[t][j] for j in range(self.S.nm) if j != i], axis=0)
for k in range(P[i].A.shape[0]):
self.prog.AddLinearConstraint(P[i].A[k].dot(xux) <= P[i].b[k] + mi_sum[k])
# SOS1 on the binaries
self.prog.AddLinearConstraint(sum(self.d[t]) == 1.)
# stage cost to the objective
obj += .5 * self.u[t].dot(self.R).dot(self.u[t])
obj += .5 * self.x[t].dot(self.Q).dot(self.x[t])
# terminal constraint
for k in range(self.X_N.A.shape[0]):
self.prog.AddLinearConstraint(self.X_N.A[k].dot(self.x[self.N]) <= self.X_N.b[k])
# terminal cost
obj += .5 * self.x[self.N].dot(self.P).dot(self.x[self.N])
self.objective = self.prog.AddQuadraticCost(obj)
# set solver
self.solver = GurobiSolver()
self.prog.SetSolverOption(self.solver.solver_type(), 'OutputFlag', 1)
def set_initial_condition(self, x0):
for k, c in enumerate(self.initial_condition):
c.UpdateLowerBound(x0[k:k+1])
c.UpdateUpperBound(x0[k:k+1])
def feedforward(self, x0):
# overwrite initial condition
self.set_initial_condition(x0)
# solve MIQP
result = self.solver.Solve(self.prog)
# check feasibility
if result != SolutionResult.kSolutionFound:
return None, None, None, None
# get cost
obj = self.prog.EvalBindingAtSolution(self.objective)[0]
# store argmin in list of vectors
u = [self.prog.GetSolution(ut) for ut in self.u]
x = [self.prog.GetSolution(xt) for xt in self.x]
d = [self.prog.GetSolution(dt) for dt in self.d]
# retrieve mode sequence and check integer feasibility
ms = [np.argmax(dt) for dt in d]
return u, x, ms, obj
def feedback(self, x0):
# get feedforward and extract first input
u_feedforward = self.feedforward(x0)[0]
if u_feedforward is None:
return None
return u_feedforward[0]
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
def quadratic_program(H, f, A, b, C=None, d=None, tol=1.e-5, **kwargs):
"""
Solves the strictly convex (H > 0) quadratic program min .5 x' H x + f' x s.t. A x <= b, C x = d.
Arguments
----------
H : numpy.ndarray
Positive definite Hessian of the cost function.
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
tol : float
Maximum value of a residual of an inequality to consider the related constraint active.
Returns
----------
sol : dict
Dictionary with the solution of the QP.
Fields
----------
min : float
Minimum of the QP (None if the problem is unfeasible).
argmin : numpy.ndarray
Argument that minimizes the QP (None if the problem is unfeasible).
active_set : list of int
Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible).
multiplier_inequality : numpy.ndarray
Lagrange multipliers for the inequality constraints (None if the problem is unfeasible).
multiplier_equality : numpy.ndarray
Lagrange multipliers for the equality constraints (None if the problem is unfeasible or without equality constraints).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# build program
prog = MathematicalProgram()
x = prog.NewContinuousVariables(n_x)
[prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)]
[prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)]
inequalities = []
prog.AddQuadraticCost(.5*x.dot(H).dot(x) + f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0)
[prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()]
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None,
'active_set': None,
'multiplier_inequality': None,
'multiplier_equality': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x)
sol['min'] = .5*sol['argmin'].dot(H).dot(sol['argmin']) + f.dot(sol['argmin'])
sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist()
# retrieve multipliers through KKT conditions
lhs = A[sol['active_set']]
rhs = b[sol['active_set']]
if n_eq > 0:
lhs = np.vstack((lhs, C))
rhs = np.concatenate((rhs, d))
H_inv = np.linalg.inv(H)
M = lhs.dot(H_inv).dot(lhs.T)
m = - np.linalg.inv(M).dot(lhs.dot(H_inv).dot(f) + rhs)
sol['multiplier_inequality'] = np.zeros(n_ineq)
for i, j in enumerate(sol['active_set']):
sol['multiplier_inequality'][j] = m[i]
if n_eq > 0:
sol['multiplier_equality'] = m[len(sol['active_set']):]
return sol
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)
d[1] = -1
d[2] = 1
d[3] = -1
d = np.array([5.0752e+01, 4.7343e+05, 8.4125e+05, 6.2668e+05])
phi0 = -36332.36234347365
print("Qp : ", Quadratic_Positive_def)
print("Qp det: ", QP_det)
# Quadratic cost : u^TQu + c^Tu
CLF_QP_cost_v_effective = np.dot(u_var, np.dot(Quadratic_Positive_def,
u_var)) + np.dot(c, u_var)
prog.AddQuadraticCost(CLF_QP_cost_v_effective)
prog.AddConstraint(np.dot(d, u_var) + phi0 <= 0)
solver = IpoptSolver()
prog.Solve()
# solver.Solve(prog)
print("Optimal u : ", prog.GetSolution(u_var))
u_CLF_QP = prog.GetSolution(u_var)
# ('A_fl:', )
# ('A_fl_det:', 137180180557.17741)
# ('Qp : ', array([[ 1.0000e+00, -1.5475e-13, 4.0035e-14, 3.7932e-15],
# [-1.5475e-13, 5.7298e+07, 2.1803e+05, -3.3461e+06],
# [ 4.0035e-14, 2.1803e+05, 6.2061e+07, 3.5442e+07],
# [ 3.7932e-15, -3.3461e+06, 3.5442e+07, 2.5742e+07]]))
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
# (This makes sure relevant event handlers get called.)
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_control_input = discrete_state. \
get_mutable_vector().get_mutable_value()
x = self.EvalVectorInput(
context, self.robot_state_input_port.get_index()).get_value()
setpoint = self.EvalAbstractInput(
context, self.setpoint_input_port.get_index()).get_value()
q_full = x[:self.nq]
v_full = x[self.nq:]
q = x[self.controlled_inds]
v = x[self.nq:][self.controlled_inds]
kinsol = self.rbt.doKinematics(q_full, v_full)
M_full = self.rbt.massMatrix(kinsol)
C = self.rbt.dynamicsBiasTerm(kinsol, {}, None)[self.controlled_inds]
# Python slicing doesn't work in 2D, so do it row-by-row
M = np.zeros((self.nq_reduced, self.nq_reduced))
for k in range(self.nq_reduced):
M[:, k] = M_full[self.controlled_inds, k]
# Pick a qdd that results in minimum deviation from the desired
# end effector pose (according to the end effector frame's jacobian
# at the current state)
# v_next = v + control_period * qdd
# q_next = q + control_period * (v + v_next) / 2.
# ee_v = J*v
# ee_p = from forward kinematics
# ee_v_next = J*v_next
# ee_p_next = ee_p + control_period * (ee_v + ee_v_next) / 2.
# min u and qdd
# || q_next - q_des ||_Kq
# + || v_next - v_des ||_Kv
# + || qdd ||_Ka
# + || Kee_v - ee_v_next ||_Kee_pt
# + || Kee_pt - ee_p_next ||_Kee_v
# + the messily-implemented angular ones?
# s.t. M qdd + C = B u
# (no contact modeling for now)
prog = MathematicalProgram()
qdd = prog.NewContinuousVariables(self.nq_reduced, "qdd")
u = prog.NewContinuousVariables(self.nu, "u")
prog.AddQuadraticCost(qdd.T.dot(setpoint.Ka).dot(qdd))
v_next = v + self.control_period * qdd
q_next = q + self.control_period * (v + v_next) / 2.
if setpoint.v_des is not None:
v_err = setpoint.v_des - v_next
prog.AddQuadraticCost(v_err.T.dot(setpoint.Kv).dot(v_err))
if setpoint.q_des is not None:
q_err = setpoint.q_des - q_next
prog.AddQuadraticCost(q_err.T.dot(setpoint.Kq).dot(q_err))
if setpoint.ee_frame is not None and setpoint.ee_pt is not None:
# Convert x to autodiff for Jacobians computation
q_full_ad = np.empty(self.nq, dtype=AutoDiffXd)
for i in range(self.nq):
der = np.zeros(self.nq)
der[i] = 1
q_full_ad.flat[i] = AutoDiffXd(q_full.flat[i], der)
kinsol_ad = self.rbt.doKinematics(q_full_ad)
tf_ad = self.rbt.relativeTransform(kinsol_ad, 0, setpoint.ee_frame)
# Compute errors in EE pt position and velocity (in world frame)
ee_p_ad = tf_ad[0:3, 0:3].dot(setpoint.ee_pt) + tf_ad[0:3, 3]
ee_p = np.hstack([y.value() for y in ee_p_ad])
J_ee = np.vstack([y.derivatives()
for y in ee_p_ad]).reshape(3, self.nq)
J_ee = J_ee[:, self.controlled_inds]
ee_v = J_ee.dot(v)
ee_v_next = J_ee.dot(v_next)
ee_p_next = ee_p + self.control_period * (ee_v + ee_v_next) / 2.
if setpoint.ee_pt_des is not None:
ee_p_err = setpoint.ee_pt_des.reshape(
(3, 1)) - ee_p_next.reshape((3, 1))
prog.AddQuadraticCost(
(ee_p_err.T.dot(setpoint.Kee_pt).dot(ee_p_err))[0, 0])
if setpoint.ee_v_des is not None:
ee_v_err = setpoint.ee_v_des.reshape(
(3, 1)) - ee_v_next.reshape((3, 1))
prog.AddQuadraticCost(
(ee_v_err.T.dot(setpoint.Kee_v).dot(ee_v_err))[0, 0])
# Also compute errors in EE cardinal vector directions vs targets in world frame
for i, vec in enumerate(
(setpoint.ee_x_des, setpoint.ee_y_des, setpoint.ee_z_des)):
if vec is not None:
direction = np.zeros(3)
direction[i] = 1.
ee_dir_ad = tf_ad[0:3, 0:3].dot(direction)
ee_dir_p = np.hstack([y.value() for y in ee_dir_ad])
J_ee_dir = np.vstack([y.derivatives() for y in ee_dir_ad
]).reshape(3, self.nq)
J_ee_dir = J_ee_dir[:, self.controlled_inds]
ee_dir_v = J_ee_dir.dot(v)
ee_dir_v_next = J_ee_dir.dot(v_next)
ee_dir_p_next = ee_dir_p + self.control_period * (
ee_dir_v + ee_dir_v_next) / 2.
ee_dir_p_err = vec.reshape((3, 1)) - ee_dir_p_next.reshape(
(3, 1))
prog.AddQuadraticCost((ee_dir_p_err.T.dot(
setpoint.Kee_xyz).dot(ee_dir_p_err))[0, 0])
prog.AddQuadraticCost((ee_dir_v_next.T.dot(
setpoint.Kee_xyzd).dot(ee_dir_v_next)))
LHS = np.dot(M, qdd) + C
RHS = np.dot(self.B, u)
for i in range(self.nq_reduced):
prog.AddLinearConstraint(LHS[i] == RHS[i])
result = prog.Solve()
u = prog.GetSolution(u)
new_control_input[:] = u
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x)
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(
self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(
self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
normals = grasp_normals_world_now[0:2, :].T
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
'''
YOUR CODE HERE
'''
# This is not proper orthogonal vector...but it somehow makes it work...
# whereas the correct does not
def ortho2(x):
return np.array([x[0],-x[1]])
#return np.array([x[1],-x[0]])
prog = MathematicalProgram()
dt = self.control_period
u = prog.NewContinuousVariables(self.nu, "u")
qdd = prog.NewContinuousVariables(q.shape[0], "qdd")
for i in range(qdd.shape[0]):
prog.AddLinearConstraint(qdd[i] <= 40)
prog.AddLinearConstraint(qdd[i] >= -40)
#prog.AddLinearConstraint(v[i] + qdd[i] * dt <= 80)
#prog.AddLinearConstraint(v[i] - qdd[i] * dt >= -80)
beta = prog.NewContinuousVariables(n_cf, 2, "beta")
#prog.AddQuadraticCost(0.1*np.sum(beta**2))
for i in range(n_cf):
prog.AddLinearConstraint(beta[i,0] >= 0)
prog.AddLinearConstraint(beta[i,1] >= 0)
prog.AddLinearConstraint(beta[i,0] <= 10)
prog.AddLinearConstraint(beta[i,1] <= 10)
c = np.zeros((n_cf,2,2))
for i in range(n_cf):
c[i,0] = normals[i] - self.mu * ortho2(normals[i])
c[i,1] = normals[i] + self.mu * ortho2(normals[i])
const = M.dot(qdd) + C - B.dot(u) ## eq 0
for i in range(n_cf):
lambda_i = c[i,0]*beta[i,0] + c[i,1]*beta[i,1]
tmp = J_contact[:, i, :].T.dot(lambda_i)
const -= tmp
for i in range(const.shape[0]):
prog.AddLinearConstraint(const[i] == 0.0)
Kp = 1000
Kd = 10
#v2 = v + dt * qdd
#q2 = q + v * dt + 0.5 * qdd * dt*dt
v2 = v + dt * qdd
q2 = q + (v+v2)/2 * dt #+ 0.5 * qdd * dt*dt
#v2 = v + dt * qdd
#q2 = q + v2 * dt
prog.AddQuadraticCost(Kp * np.sum((qdes - q2) ** 2) + Kd * np.sum(v2**2))
result = prog.Solve()
u = prog.GetSolution(u)
return u
def constraint_evaluator1(z):
return np.array([(-(z[6] + z[7])*sin(z[2])/m)*dt + z[3] - z[8],
((z[6] + z[7])*cos(z[2]) - m*g/m)*dt + z[4] - z[9],
((z[6] - z[7])*r/I)*dt + z[5] - z[10]
])
def constraint_evaluator2(z):
return np.array([z[3]*dt + z[0] - z[6], z[4]*dt + z[1] - z[7], z[5]*dt + z[2] - z[8]])
for n in range(N-1):
# Will eventually be prog.AddConstraint(x[:,n+1] == A@x[:,n] + B@u[:,n])
# See drake issues 12841 and 8315
# prog.AddConstraint(eq(dx[0,n+1], dx[0,n] + dt*(-(u[0,n] + u[1,n])*sin(x[2,n])/m)))
# prog.AddConstraint(eq(dx[1,n+1], dx[1,n] + dt*((u[0,n] + u[1,n])*cos(x[2,n]) - m*g/m)))
# prog.AddConstraint(eq(dx[2,n+1], dx[2,n] + dt*((u[0,n] - u[1,n])*r/I)))
prog.AddQuadraticCost(sum(x[:,n]**2) + sum(dx[:,n]**2) + sum(u[:,n]**2))
prog.AddConstraint(constraint_evaluator1,
lb=np.array([0]*3),
ub=np.array([0]*3),
vars=[x[0, n], x[1, n], x[2, n],
dx[0, n], dx[1, n], dx[2, n],
u[0, n], u[1, n],
dx[0, n+1], dx[1, n+1], dx[2, n+1],])
prog.AddConstraint(constraint_evaluator2,
lb=np.array([0]*3),
ub=np.array([0]*3),
vars=[x[0, n], x[1, n], x[2, n],
dx[0, n], dx[1, n], dx[2, n],
x[0, n+1], x[1, n+1], x[2, n+1],])
def intercepting_with_obs_avoidance_bb(self,
p0,
v0,
pf,
vf,
T,
obstacles=None,
p_puck=None):
"""kick while avoiding obstacles using big M formulation and branch and bound"""
x0 = np.array(np.concatenate((p0, v0), axis=0))
xf = np.concatenate((pf, vf), axis=0)
prog = MathematicalProgram()
# state and control inputs
N = int(T / self.params.dt) # number of command steps
state = prog.NewContinuousVariables(N + 1, 4, 'state')
cmd = prog.NewContinuousVariables(N, 2, 'input')
# Initial and final state
prog.AddLinearConstraint(eq(state[0], x0))
prog.AddLinearConstraint(eq(state[-1], xf))
self.add_dynamics(prog, N, state, cmd)
## Input saturation
self.add_input_limits(prog, cmd, N)
# Arena constraints
self.add_arena_limits(prog, state, N)
# Add Mixed-integer constraints - will solve with BB
# avoid hitting the puck while generating a kicking trajectory
# MILP formulation with B&B solver
x_obs_puck = prog.NewBinaryVariables(rows=N + 1,
cols=2) # obs x_min, obs x_max
y_obs_puck = prog.NewBinaryVariables(rows=N + 1,
cols=2) # obs y_min, obs y_max
self.avoid_puck_bigm(prog, x_obs_puck, y_obs_puck, state, N, p_puck)
# Avoid other players
if obstacles != None:
x_obs_player = list()
y_obs_player = list()
for i, obs in enumerate(obstacles):
x_obs_player.append(prog.NewBinaryVariables(
rows=N + 1, cols=2)) # obs x_min, obs x_max
y_obs_player.append(prog.NewBinaryVariables(
rows=N + 1, cols=2)) # obs y_min, obs y_max
self.avoid_other_player_bigm(prog, x_obs_player[i],
y_obs_player[i], state, obs, N)
# Solve with simple b&b solver
for k in range(N):
prog.AddQuadraticCost(cmd[k].flatten().dot(cmd[k]))
bb = branch_and_bound.MixedIntegerBranchAndBound(
prog,
OsqpSolver().solver_id())
result = bb.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem.')
u_values = np.array([bb.GetSolution(u) for u in cmd]).T
solution_found = result.kSolutionFound
return solution_found, u_values
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.
'''
from pydrake.all import MathematicalProgram
import numpy as np
import math
N = 100
t = maximum_time
#input_trajectory = np.ones((N,2))*10.0
#time_used = 100.0
#time_array = np.arange(0.0, time_used, time_used/(N+1))
#trajectory = self.simulate_states_over_time(state_initial, time_array, input_trajectory)
#return trajectory, input_trajectory, time_array
mp = MathematicalProgram()
def addLinearEqual(x, y, length):
for i in range(length):
mp.AddLinearConstraint(x[i] == y[i])
def addEpsilonEq(a, b, e):
mp.AddConstraint(a <= b + e)
mp.AddConstraint(a >= b - e)
def addEqual(x, y, length, e):
for i in range(length):
addEpsilonEq(x[i], y[i], e)
def worldTwoDistSquared(x):
return np.sum((self.world_2_position - x) ** 2)
inp_traj = mp.NewContinuousVariables(N-1, 2, "inp")
traj = mp.NewContinuousVariables(N, 4, "traj")
#mp.NewContinuousVariables(1, "t")
#mp.AddLinearConstraint(t[0] == 10.0)
time_step = t / N
addLinearEqual(traj[0], state_initial, 4)
for i in range(N-1):
#todo add constraint forcing orbit to not occur until time t
predicted = traj[i] + time_step * self.rocket_dynamics(traj[i], inp_traj[i])
addEqual(traj[i+1], predicted, 4, 1e-8)
mp.AddQuadraticCost(np.sum(inp_traj[i]**2))
#mp.AddQuadraticCost(time_step*np.sum(inp_traj[i]**2))
addEpsilonEq(worldTwoDistSquared(traj[-1][:2]), 0.5**2, 1e-8)
velocity = traj[-1][2:] / time_step
addEpsilonEq(velocity.dot(velocity), self.G*self.M2/0.5, 1e-8)
addEpsilonEq(velocity.dot(traj[-1][:2] - self.world_2_position), 0, 1e-8)
#mp.AddLinearConstraint(t == 100.0)
#mp.AddLinearConstraint(t >= minimum_time)
#mp.AddLinearConstraint(t <= maximum_time)
print(mp)
print(mp.Solve())
time_used = t # mp.GetSolution(t)
time_array = np.arange(0.0, time_used, time_used/N)
trajectory = mp.GetSolution(traj)
input_trajectory = mp.GetSolution(inp_traj)
#print('time=', time_array)
#print('input=', input_trajectory)
#print('traj=', trajectory)
true_trajectory = self.simulate_states_over_time(state_initial, time_array, input_trajectory)
#print(trajectory - true_trajectory)
#print(trajectory)
#print(true_trajectory)
return trajectory, input_trajectory, time_array
#todo add constraint ensuring orbit to not occurs at time t
#mp.AddLinearCost(x[0]*1.0)
#input_trajectory = np.ones((N,2))*10.0
#time_used = 100.0
#time_array = np.arange(0.0, time_used, time_used/(N+1))
#trajectory = self.simulate_states_over_time(state_initial, time_array, input_trajectory)
#return trajectory, input_trajectory, time_array
def MILP_compute_traj(self,
obst_idx,
x_out,
y_out,
dx,
dy,
pose_initial=[0., 0.]):
'''
Find trajectory with MILP
Outputs trajectory (waypoints) and new K for control
'''
mp = MathematicalProgram()
N = 8
k = 0
# define state traj
st = mp.NewContinuousVariables(2, "state_%d" % k)
# # binary variables for obstalces constraint
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = c
states = st
for k in range(1, N):
st = mp.NewContinuousVariables(2, "state_%d" % k)
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
st = mp.NewContinuousVariables(2, "state_%d" % (N + 1))
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
# variables encoding max x y dist from obstacle
x_margin = mp.NewContinuousVariables(1, "x_margin")
y_margin = mp.NewContinuousVariables(1, "y_margin")
### define cost
for i in range(N):
mp.AddLinearCost(-states[i, 0]) # go as far forward as possible
mp.AddLinearCost(-states[-1, 0])
mp.AddLinearCost(-x_margin[0])
mp.AddLinearCost(-y_margin[0])
# bound x y margin so it doesn't explode
mp.AddLinearConstraint(x_margin[0] <= 3.)
mp.AddLinearConstraint(y_margin[0] <= 3.)
# x y is non ngative adn at least above robot radius
mp.AddLinearConstraint(x_margin[0] >= 0.05)
mp.AddLinearConstraint(y_margin[0] >= 0.05)
M = 1000 # slack var for integer things
# state constraint
for i in range(2): # initial state constraint x y
mp.AddLinearConstraint(states[0, i] <= pose_initial[i])
mp.AddLinearConstraint(states[0, i] >= pose_initial[i])
for i in range(N):
mp.AddQuadraticCost((states[i + 1, 1] - states[i, 1])**2)
mp.AddLinearConstraint(states[i + 1, 0] <= states[i, 0] + dx)
mp.AddLinearConstraint(states[i + 1, 0] >= states[i, 0] - dx)
mp.AddLinearConstraint(states[i + 1, 1] <= states[i, 1] + dy)
mp.AddLinearConstraint(states[i + 1, 1] >= states[i, 1] - dy)
# obstacle constraint
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[i, 0] <= rng_min - x_margin[0] +
M * obs[i, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[i, 0] >= rng_max + x_margin[0] -
M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[i, 1] <=
states[i, 0] * np.tan(ang_min) - y_margin[0] +
M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[i, 1] >=
states[i, 0] * np.tan(ang_max) + y_margin[0] -
M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
# obstacle constraint for last state
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[N, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[N, 0] <= rng_min - x_margin[0] +
M * obs[N, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[N, 0] >= rng_max + x_margin[0] -
M * obs[N, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[N,
1] <= states[N, 0] * np.tan(ang_min) - y_margin[0] +
M * obs[N, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[N,
1] >= states[N, 0] * np.tan(ang_max) + y_margin[0] -
M * obs[N, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
mp.Solve()
trajectory = mp.GetSolution(states)
xm = mp.GetSolution(x_margin)
ym = mp.GetSolution(y_margin)
x_out[:] = trajectory[:, 0]
y_out[:] = trajectory[:, 1]
return trajectory, xm[0], ym[0]
def solve(self, quad_start_q, quad_final_q, time_used):
mp = MathematicalProgram()
# We want to solve this for a certain number of knot points
N = 40 # num knot points
time_increment = time_used / (N + 1)
dt = time_increment
time_array = np.arange(0.0, time_used, time_increment)
quad_u = mp.NewContinuousVariables(2, "u_0")
quad_q = mp.NewContinuousVariables(6, "quad_q_0")
for i in range(1, N):
u = mp.NewContinuousVariables(2, "u_%d" % i)
quad = mp.NewContinuousVariables(6, "quad_q_%d" % i)
quad_u = np.vstack((quad_u, u))
quad_q = np.vstack((quad_q, quad))
for i in range(N):
mp.AddLinearConstraint(quad_u[i][0] <= 3.0) # force
mp.AddLinearConstraint(quad_u[i][0] >= 0.0) # force
mp.AddLinearConstraint(quad_u[i][1] <= 10.0) # torque
mp.AddLinearConstraint(quad_u[i][1] >= -10.0) # torque
mp.AddLinearConstraint(quad_q[i][0] <= 1000.0) # pos x
mp.AddLinearConstraint(quad_q[i][0] >= -1000.0)
mp.AddLinearConstraint(quad_q[i][1] <= 1000.0) # pos y
mp.AddLinearConstraint(quad_q[i][1] >= -1000.0)
mp.AddLinearConstraint(
quad_q[i][2] <= 60.0 * np.pi / 180.0) # pos theta
mp.AddLinearConstraint(quad_q[i][2] >= -60.0 * np.pi / 180.0)
mp.AddLinearConstraint(quad_q[i][3] <= 1000.0) # vel x
mp.AddLinearConstraint(quad_q[i][3] >= -1000.0)
mp.AddLinearConstraint(quad_q[i][4] <= 1000.0) # vel y
mp.AddLinearConstraint(quad_q[i][4] >= -1000.0)
mp.AddLinearConstraint(quad_q[i][5] <= 10.0) # vel theta
mp.AddLinearConstraint(quad_q[i][5] >= -10.0)
for i in range(1, N):
quad_q_dyn_feasible = self.dynamics(quad_q[i - 1, :],
quad_u[i - 1, :], dt)
# Direct transcription constraints on states to dynamics
for j in range(6):
quad_state_err = (quad_q[i][j] - quad_q_dyn_feasible[j])
eps = 0.001
mp.AddConstraint(quad_state_err <= eps)
mp.AddConstraint(quad_state_err >= -eps)
# Initial and final quad and ball states
for j in range(6):
mp.AddLinearConstraint(quad_q[0][j] == quad_start_q[j])
mp.AddLinearConstraint(quad_q[-1][j] == quad_final_q[j])
# Quadratic cost on the control input
R_force = 10.0
R_torque = 100.0
Q_quad_x = 100.0
Q_quad_y = 100.0
mp.AddQuadraticCost(R_force * quad_u[:, 0].dot(quad_u[:, 0]))
mp.AddQuadraticCost(R_torque * quad_u[:, 1].dot(quad_u[:, 1]))
mp.AddQuadraticCost(Q_quad_x * quad_q[:, 0].dot(quad_q[:, 1]))
mp.AddQuadraticCost(Q_quad_y * quad_q[:, 1].dot(quad_q[:, 1]))
# Solve the optimization
print "Number of decision vars: ", mp.num_vars()
# mp.SetSolverOption(SolverType.kSnopt, "Major iterations limit", 100000)
print "Solve: ", mp.Solve()
quad_traj = mp.GetSolution(quad_q)
input_traj = mp.GetSolution(quad_u)
return (quad_traj, input_traj, time_array)
def test_Feedback_Linearization_controller_BS(x, t):
# Output feedback linearization 2
#
# y1= ||r-rf||^2/2
# y2= wx
# y3= wy
# y4= wz
#
# Analysis: The zero dynamics is unstable.
global g, xf, Aout, Bout, Mout, T_period, w_freq, radius
print("%%%%%%%%%%%%%%%%%%%%%")
print("%%CLF-QP %%%%%%%%%%%")
print("%%%%%%%%%%%%%%%%%%%%%")
print("t:", t)
prog = MathematicalProgram()
u_var = prog.NewContinuousVariables(4, "u_var")
c_var = prog.NewContinuousVariables(1, "c_var")
# # # Example 1 : Circle
x_f = radius * math.cos(w_freq * t)
y_f = radius * math.sin(w_freq * t)
# print("x_f:",x_f)
# print("y_f:",y_f)
dx_f = -radius * math.pow(w_freq, 1) * math.sin(w_freq * t)
dy_f = radius * math.pow(w_freq, 1) * math.cos(w_freq * t)
ddx_f = -radius * math.pow(w_freq, 2) * math.cos(w_freq * t)
ddy_f = -radius * math.pow(w_freq, 2) * math.sin(w_freq * t)
dddx_f = radius * math.pow(w_freq, 3) * math.sin(w_freq * t)
dddy_f = -radius * math.pow(w_freq, 3) * math.cos(w_freq * t)
ddddx_f = radius * math.pow(w_freq, 4) * math.cos(w_freq * t)
ddddy_f = radius * math.pow(w_freq, 4) * math.sin(w_freq * t)
# # Example 2 : Lissajous curve a=1 b=2
# ratio_ab=2;
# a=1;
# b=ratio_ab*a;
# delta_lissajous = 0 #math.pi/2;
# x_f = radius*math.sin(a*w_freq*t+delta_lissajous)
# y_f = radius*math.sin(b*w_freq*t)
# # print("x_f:",x_f)
# # print("y_f:",y_f)
# dx_f = radius*math.pow(a*w_freq,1)*math.cos(a*w_freq*t+delta_lissajous)
# dy_f = radius*math.pow(b*w_freq,1)*math.cos(b*w_freq*t)
# ddx_f = -radius*math.pow(a*w_freq,2)*math.sin(a*w_freq*t+delta_lissajous)
# ddy_f = -radius*math.pow(b*w_freq,2)*math.sin(b*w_freq*t)
# dddx_f = -radius*math.pow(a*w_freq,3)*math.cos(a*w_freq*t+delta_lissajous)
# dddy_f = -radius*math.pow(b*w_freq,3)*math.cos(b*w_freq*t)
# ddddx_f = radius*math.pow(a*w_freq,4)*math.sin(a*w_freq*t+delta_lissajous)
# ddddy_f = radius*math.pow(b*w_freq,4)*math.sin(b*w_freq*t)
e1 = np.array([1, 0, 0]) # e3 elementary vector
e2 = np.array([0, 1, 0]) # e3 elementary vector
e3 = np.array([0, 0, 1]) # e3 elementary vector
# epsilonn= 1e-0
# alpha = 100;
# kp1234=alpha*1/math.pow(epsilonn,4) # gain for y
# kd1=4/math.pow(epsilonn,3) # gain for y^(1)
# kd2=12/math.pow(epsilonn,2) # gain for y^(2)
# kd3=4/math.pow(epsilonn,1) # gain for y^(3)
# kp5= 10; # gain for y5
q = np.zeros(7)
qd = np.zeros(6)
q = x[0:8]
qd = x[8:15]
print("qnorm:", np.dot(q[3:7], q[3:7]))
q0 = q[3]
q1 = q[4]
q2 = q[5]
q3 = q[6]
xi1 = q[7]
v = qd[0:3]
w = qd[3:6]
xi2 = qd[6]
xd = xf[0]
yd = xf[1]
zd = xf[2]
wd = xf[11:14]
# Useful vectors and matrices
(Rq, Eq, wIw, I_inv) = robobee_plantBS.GetManipulatorDynamics(q, qd)
F1q = np.zeros((3, 4))
F1q[0, :] = np.array([q2, q3, q0, q1])
F1q[1, :] = np.array([-1 * q1, -1 * q0, q3, q2])
F1q[2, :] = np.array([q0, -1 * q1, -1 * q2, q3])
Rqe3 = np.dot(Rq, e3)
Rqe3_hat = np.zeros((3, 3))
Rqe3_hat[0, :] = np.array([0, -Rqe3[2], Rqe3[1]])
Rqe3_hat[1, :] = np.array([Rqe3[2], 0, -Rqe3[0]])
Rqe3_hat[2, :] = np.array([-Rqe3[1], Rqe3[0], 0])
Rqe1 = np.dot(Rq, e1)
Rqe1_hat = np.zeros((3, 3))
Rqe1_hat[0, :] = np.array([0, -Rqe1[2], Rqe1[1]])
Rqe1_hat[1, :] = np.array([Rqe1[2], 0, -Rqe1[0]])
Rqe1_hat[2, :] = np.array([-Rqe1[1], Rqe1[0], 0])
Rqe1_x = np.dot(e2.T, Rqe1)
Rqe1_y = np.dot(e1.T, Rqe1)
w_hat = np.zeros((3, 3))
w_hat[0, :] = np.array([0, -w[2], w[1]])
w_hat[1, :] = np.array([w[2], 0, -w[0]])
w_hat[2, :] = np.array([-w[1], w[0], 0])
Rw = np.dot(Rq, w)
Rw_hat = np.zeros((3, 3))
Rw_hat[0, :] = np.array([0, -Rw[2], Rw[1]])
Rw_hat[1, :] = np.array([Rw[2], 0, -Rw[0]])
Rw_hat[2, :] = np.array([-Rw[1], Rw[0], 0])
#- Checking the derivation
# print("F1qEqT-(-Rqe3_hat)",np.dot(F1q,Eq.T)-(-Rqe3_hat))
# Rqe3_cal = np.zeros(3)
# Rqe3_cal[0] = 2*(q3*q1+q0*q2)
# Rqe3_cal[1] = 2*(q3*q2-q0*q1)
# Rqe3_cal[2] = (q0*q0+q3*q3-q1*q1-q2*q2)
# print("Rqe3 - Rqe3_cal", Rqe3-Rqe3_cal)
# Four output
y1 = x[0] - x_f
y2 = x[1] - y_f
y3 = x[2] - zd - 0
y4 = math.atan2(Rqe1_x, Rqe1_y) - math.pi / 8
# print("Rqe1_x:", Rqe1_x)
eta1 = np.zeros(3)
eta1 = np.array([y1, y2, y3])
eta5 = y4
# print("y4", y4)
# First derivative of first three output and yaw output
eta2 = np.zeros(3)
eta2 = v - np.array([dx_f, dy_f, 0])
dy1 = eta2[0]
dy2 = eta2[1]
dy3 = eta2[2]
x2y2 = (math.pow(Rqe1_x, 2) + math.pow(Rqe1_y, 2)) # x^2+y^2
eta6_temp = np.zeros(3) #eta6_temp = (ye2T-xe1T)/(x^2+y^2)
eta6_temp = (Rqe1_y * e2.T - Rqe1_x * e1.T) / x2y2
# print("eta6_temp:", eta6_temp)
# Body frame w ( multiply R)
eta6 = np.dot(eta6_temp, np.dot(-Rqe1_hat, np.dot(Rq, w)))
# World frame w
# eta6 = np.dot(eta6_temp,np.dot(-Rqe1_hat,w))
# print("Rqe1_hat:", Rqe1_hat)
# Second derivative of first three output
eta3 = np.zeros(3)
eta3 = -g * e3 + Rqe3 * xi1 - np.array([ddx_f, ddy_f, 0])
ddy1 = eta3[0]
ddy2 = eta3[1]
ddy3 = eta3[2]
# Third derivative of first three output
eta4 = np.zeros(3)
# Body frame w ( multiply R)
eta4 = Rqe3 * xi2 + np.dot(-Rqe3_hat, np.dot(Rq, w)) * xi1 - np.array(
[dddx_f, dddy_f, 0])
# World frame w
# eta4 = Rqe3*xi2+np.dot(np.dot(F1q,Eq.T),w)*xi1 - np.array([dddx_f,dddy_f,0])
dddy1 = eta4[0]
dddy2 = eta4[1]
dddy3 = eta4[2]
# Fourth derivative of first three output
B_qw_temp = np.zeros(3)
# Body frame w
B_qw_temp = xi1 * (-np.dot(Rw_hat, np.dot(Rqe3_hat, Rw)) +
np.dot(Rqe3_hat, np.dot(Rq, np.dot(I_inv, wIw)))
) # np.dot(I_inv,wIw)*xi1-2*w*xi2
B_qw = B_qw_temp + xi2 * (-2 * np.dot(Rqe3_hat, Rw)) - np.array(
[ddddx_f, ddddy_f, 0]) #np.dot(Rqe3_hat,B_qw_temp)
# World frame w
# B_qw_temp = xi1*(-np.dot(w_hat,np.dot(Rqe3_hat,w))+np.dot(Rqe3_hat,np.dot(I_inv,wIw))) # np.dot(I_inv,wIw)*xi1-2*w*xi2
# B_qw = B_qw_temp+xi2*(-2*np.dot(Rqe3_hat,w)) - np.array([ddddx_f,ddddy_f,0]) #np.dot(Rqe3_hat,B_qw_temp)
# B_qw = B_qw_temp - np.dot(w_hat,np.dot(Rqe3_hat,w))*xi1
# Second derivative of yaw output
# Body frame w
dRqe1_x = np.dot(e2, np.dot(-Rqe1_hat, Rw)) # \dot{x}
dRqe1_y = np.dot(e1, np.dot(-Rqe1_hat, Rw)) # \dot{y}
alpha1 = 2 * (Rqe1_x * dRqe1_x +
Rqe1_y * dRqe1_y) / x2y2 # (2xdx +2ydy)/(x^2+y^2)
# World frame w
# dRqe1_x = np.dot(e2,np.dot(-Rqe1_hat,w)) # \dot{x}
# dRqe1_y = np.dot(e1,np.dot(-Rqe1_hat,w)) # \dot{y}
# alpha1 = 2*(Rqe1_x*dRqe1_x+Rqe1_y*dRqe1_y)/x2y2 # (2xdx +2ydy)/(x^2+y^2)
# # alpha2 = math.pow(dRqe1_y,2)-math.pow(dRqe1_x,2)
# Body frame w
B_yaw_temp3 = np.zeros(3)
B_yaw_temp3 = alpha1 * np.dot(Rqe1_hat, Rw) + np.dot(
Rqe1_hat, np.dot(Rq, np.dot(I_inv, wIw))) - np.dot(
Rw_hat, np.dot(Rqe1_hat, Rw))
B_yaw = np.dot(eta6_temp,
B_yaw_temp3) # +alpha2 :Could be an error in math.
g_yaw = np.zeros(3)
g_yaw = -np.dot(eta6_temp, np.dot(Rqe1_hat, np.dot(Rq, I_inv)))
# World frame w
# B_yaw_temp3 =np.zeros(3)
# B_yaw_temp3 = alpha1*np.dot(Rqe1_hat,w)+np.dot(Rqe1_hat,np.dot(I_inv,wIw))-np.dot(w_hat,np.dot(Rqe1_hat,w))
# B_yaw = np.dot(eta6_temp,B_yaw_temp3) # +alpha2 :Could be an error in math.
# g_yaw = np.zeros(3)
# g_yaw = -np.dot(eta6_temp,np.dot(Rqe1_hat,I_inv))
# print("g_yaw:", g_yaw)
# Decoupling matrix A(x)\in\mathbb{R}^4
A_fl = np.zeros((4, 4))
A_fl[0:3, 0] = Rqe3
# Body frame w
A_fl[0:3, 1:4] = -np.dot(Rqe3_hat, np.dot(Rq, I_inv)) * xi1
# World frame w
# A_fl[0:3,1:4] = -np.dot(Rqe3_hat,I_inv)*xi1
A_fl[3, 1:4] = g_yaw
A_fl_inv = np.linalg.inv(A_fl)
A_fl_det = np.linalg.det(A_fl)
# print("I_inv:", I_inv)
print("A_fl:", A_fl)
print("A_fl_det:", A_fl_det)
# Drift in the output dynamics
U_temp = np.zeros(4)
U_temp[0:3] = B_qw
U_temp[3] = B_yaw
# Output dyamics
eta = np.hstack([eta1, eta2, eta3, eta5, eta4, eta6])
eta_norm = np.dot(eta, eta)
# v-CLF QP controller
FP_PF = np.dot(Aout.T, Mout) + np.dot(Mout, Aout)
PG = np.dot(Mout, Bout)
L_FVx = np.dot(eta, np.dot(FP_PF, eta))
L_GVx = 2 * np.dot(eta.T, PG) # row vector
L_fhx_star = U_temp
Vx = np.dot(eta, np.dot(Mout, eta))
# phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+(min_e_Q/max_e_P)*Vx*1 # exponentially stabilizing
# phi0_exp = L_FVx+np.dot(L_GVx,L_fhx_star)+min_e_Q*eta_norm # more exact bound - exponentially stabilizing
phi0_exp = L_FVx + np.dot(
L_GVx, L_fhx_star) # more exact bound - exponentially stabilizing
phi1_decouple = np.dot(L_GVx, A_fl)
# # Solve QP
v_var = np.dot(A_fl, u_var) + L_fhx_star
Quadratic_Positive_def = np.matmul(A_fl.T, A_fl)
QP_det = np.linalg.det(Quadratic_Positive_def)
c_QP = 2 * np.dot(L_fhx_star.T, A_fl)
c_QP_extned = np.hstack([c_QP, -1])
u_var_extended = np.hstack([u_var, c_var])
# CLF_QP_cost_v = np.dot(v_var,v_var) // Exact quadratic cost
CLF_QP_cost_v_effective = np.dot(
u_var, np.dot(Quadratic_Positive_def, u_var)) + np.dot(
c_QP, u_var) - c_var[0] # Quadratic cost without constant term
# CLF_QP_cost_u = np.dot(u_var,u_var)
phi1 = np.dot(phi1_decouple, u_var) + c_var[0] * eta_norm
#----Printing intermediate states
# print("L_fhx_star: ",L_fhx_star)
# print("c_QP:", c_QP)
# print("Qp : ",Quadratic_Positive_def)
# print("Qp det: ", QP_det)
# print("c_QP", c_QP)
# print("phi0_exp: ", phi0_exp)
# print("PG:", PG)
# print("L_GVx:", L_GVx)
# print("eta6", eta6)
# print("d : ", phi1_decouple)
# print("Cost expression:", CLF_QP_cost_v)
#print("Const expression:", phi0_exp+phi1)
#----Different solver option // Gurobi did not work with python at this point (some binding issue 8/8/2018)
solver = IpoptSolver()
# solver = GurobiSolver()
# print solver.available()
# assert(solver.available()==True)
# assertEqual(solver.solver_type(), mp.SolverType.kGurobi)
# solver.Solve(prog)
# assertEqual(result, mp.SolutionResult.kSolutionFound)
# mp.AddLinearConstraint()
# print("x:", x)
# print("phi_0_exp:", phi0_exp)
# print("phi1_decouple:", phi1_decouple)
# print("eta1:", eta1)
# print("eta2:", eta2)
# print("eta3:", eta3)
# print("eta4:", eta4)
# print("eta5:", eta5)
# print("eta6:", eta6)
# Output Feedback Linearization controller
mu = np.zeros(4)
k = np.zeros((4, 14))
k = np.matmul(Bout.T, Mout)
k = np.matmul(np.linalg.inv(R), k)
mu = -1 / 1 * np.matmul(k, eta)
v = -U_temp + mu
U_fl = np.matmul(
A_fl_inv, v
) # Output Feedback controller to comare the result with CLF-QP solver
# Set up the QP problem
prog.AddQuadraticCost(CLF_QP_cost_v_effective)
prog.AddConstraint(phi0_exp + phi1 <= 0)
prog.AddConstraint(c_var[0] >= 0)
prog.AddConstraint(c_var[0] <= 100)
prog.AddConstraint(u_var[0] <= 30)
prog.SetSolverOption(mp.SolverType.kIpopt, "print_level",
5) # CAUTION: Assuming that solver used Ipopt
print("CLF value:", Vx) # Current CLF value
prog.SetInitialGuess(u_var, U_fl)
prog.Solve() # Solve with default osqp
# solver.Solve(prog)
print("Optimal u : ", prog.GetSolution(u_var))
U_CLF_QP = prog.GetSolution(u_var)
C_CLF_QP = prog.GetSolution(c_var)
#---- Printing for debugging
# dx = robobee_plantBS.evaluate_f(U_fl,x)
# print("dx", dx)
# print("\n######################")
# # print("qe3:", A_fl[0,0])
# print("u:", u)
# print("\n####################33")
# deta4 = B_qw+Rqe3*U_fl_zero[0]+np.matmul(-np.matmul(Rqe3_hat,I_inv),U_fl_zero[1:4])*xi1
# deta6 = B_yaw+np.dot(g_yaw,U_fl_zero[1:4])
# print("deta4:",deta4)
# print("deta6:",deta6)
print(C_CLF_QP)
phi1_opt = np.dot(phi1_decouple, U_CLF_QP) + C_CLF_QP * eta_norm
phi1_opt_FL = np.dot(phi1_decouple, U_fl)
print("FL u: ", U_fl)
print("CLF u:", U_CLF_QP)
print("Cost FL: ", np.dot(mu, mu))
v_CLF = np.dot(A_fl, U_CLF_QP) + L_fhx_star
print("Cost CLF: ", np.dot(v_CLF, v_CLF))
print("Constraint FL : ", phi0_exp + phi1_opt_FL)
print("Constraint CLF : ", phi0_exp + phi1_opt)
u = U_CLF_QP
print("eigenvalues minQ maxP:", [min_e_Q, max_e_P])
return u
def compute_trajectory_to_other_world(self, state_initial, minimum_time,
maximum_time):
'''
Your mission is to implement this function.
A successful implementation of this function will compute a dynamically feasible trajectory
which satisfies these criteria:
- Efficiently conserve fuel
- Reach "orbit" of the far right world
- Approximately obey the dynamic constraints
- Begin at the state_initial provided
- Take no more than maximum_time, no less than minimum_time
The above are defined more precisely in the provided notebook.
Please note there are two return args.
:param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation
:param: minimum_time: float, minimum time allowed for trajectory
:param: maximum_time: float, maximum time allowed for trajectory
:return: three return args separated by commas:
trajectory, input_trajectory, time_array
trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation.
input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation.
time_array: an array with N rows.
'''
mp = MathematicalProgram()
max_speed = 0.99
desired_distance = 0.5
u_cost_factor = 1000.
N = 50
# trajectory = np.zeros((N+1,4))
# input_trajectory = np.ones((N,2))*10.0
time_used = (maximum_time + minimum_time) / 2.
time_step = time_used / (N + 1)
time_array = np.arange(0.0, time_used, time_step)
k = 0
# Create continuous variables for u & x
u = mp.NewContinuousVariables(2, "u_%d" % k)
x = mp.NewContinuousVariables(4, "x_%d" % k)
u_over_time = u
x_over_time = x
for k in range(1, N):
u = mp.NewContinuousVariables(2, "u_%d" % k)
x = mp.NewContinuousVariables(4, "x_%d" % k)
u_over_time = np.vstack((u_over_time, u))
x_over_time = np.vstack((x_over_time, x))
# trajectory is N+1 in size
x = mp.NewContinuousVariables(4, "x_%d" % (N + 1))
x_over_time = np.vstack((x_over_time, x))
# Add cost
# for k in range(0, N):
mp.AddQuadraticCost(u_cost_factor *
u_over_time[:, 0].dot(u_over_time[:, 0]))
mp.AddQuadraticCost(u_cost_factor *
u_over_time[:, 1].dot(u_over_time[:, 1]))
# Add constraint for initial state
mp.AddLinearConstraint(x_over_time[0, 0] >= state_initial[0])
mp.AddLinearConstraint(x_over_time[0, 0] <= state_initial[0])
mp.AddLinearConstraint(x_over_time[0, 1] >= state_initial[1])
mp.AddLinearConstraint(x_over_time[0, 1] <= state_initial[1])
mp.AddLinearConstraint(x_over_time[0, 2] >= state_initial[2])
mp.AddLinearConstraint(x_over_time[0, 2] <= state_initial[2])
mp.AddLinearConstraint(x_over_time[0, 3] >= state_initial[3])
mp.AddLinearConstraint(x_over_time[0, 3] <= state_initial[3])
# Add constraint between x & u
for k in range(1, N + 1):
next_step = self.rocket_dynamics(x_over_time[k - 1, :],
u_over_time[k - 1, :])
mp.AddConstraint(x_over_time[k, 0] == x_over_time[k - 1, 0] +
time_step * next_step[0])
mp.AddConstraint(x_over_time[k, 1] == x_over_time[k - 1, 1] +
time_step * next_step[1])
mp.AddConstraint(x_over_time[k, 2] == x_over_time[k - 1, 2] +
time_step * next_step[2])
mp.AddConstraint(x_over_time[k, 3] == x_over_time[k - 1, 3] +
time_step * next_step[3])
# Make sure it never goes too far from the planets
# for k in range(1, N):
# mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_2_position[:]) <= 10)
# mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_1_position[:]) <= 10)
# Make sure u never goes above a threshold
max_u = 6.
for k in range(0, N):
mp.AddLinearConstraint(u_over_time[k, 0] <= max_u)
mp.AddLinearConstraint(-u_over_time[k, 0] <= max_u)
mp.AddLinearConstraint(u_over_time[k, 1] <= max_u)
mp.AddLinearConstraint(-u_over_time[k, 1] <= max_u)
# Make sure it reaches world 2
mp.AddConstraint(
self.two_norm(x_over_time[-1, 0:2] -
self.world_2_position) <= desired_distance)
mp.AddConstraint(
self.two_norm(x_over_time[-1, 0:2] -
self.world_2_position) >= desired_distance)
# Make sure it's speed isn't too high
mp.AddConstraint(self.two_norm(x_over_time[-1, 2:4]) <= max_speed**2.)
# Get result
result = Solve(mp)
x_over_time_result = result.GetSolution(x_over_time)
u_over_time_result = result.GetSolution(u_over_time)
print("Success", result.is_success())
print("Final position", x_over_time_result[-1, :])
print(
"Final distance to world2",
self.two_norm(x_over_time_result[-1, 0:2] - self.world_2_position))
print("Final speed", self.two_norm(x_over_time_result[-1, 2:4]))
print("Fuel consumption", (u_over_time_result**2.).sum())
trajectory = x_over_time_result
input_trajectory = u_over_time_result
return trajectory, input_trajectory, time_array
def compute_trajectory_to_other_world(self, state_initial, minimum_time,
maximum_time):
'''
Your mission is to implement this function.
A successful implementation of this function will compute a dynamically feasible trajectory
which satisfies these criteria:
- Efficiently conserve fuel
- Reach "orbit" of the far right world
- Approximately obey the dynamic constraints
- Begin at the state_initial provided
- Take no more than maximum_time, no less than minimum_time
The above are defined more precisely in the provided notebook.
Please note there are two return args.
:param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation
:param: minimum_time: float, minimum time allowed for trajectory
:param: maximum_time: float, maximum time allowed for trajectory
:return: three return args separated by commas:
trajectory, input_trajectory, time_array
trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation.
input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation.
time_array: an array with N rows.
'''
# length of horizon (excluding init state)
N = 50
trajectory = np.zeros((N + 1, 4))
input_trajectory = np.ones((N, 2)) * 10.0
### My implementation of Direct Transcription
# (states and control are all decision vars using Euler integration)
mp = MathematicalProgram()
# let trajectory duration be a decision var
total_time = mp.NewContinuousVariables(1, "total_time")
dt = total_time[0] / N
# create the control decision var (m*N) and state decision var (n*[N+1])
idx = 0
u_list = mp.NewContinuousVariables(2, "u_{}".format(idx))
state_list = mp.NewContinuousVariables(4, "state_{}".format(idx))
state_list = np.vstack(
(state_list,
mp.NewContinuousVariables(4, "state_{}".format(idx + 1))))
for idx in range(1, N):
u_list = np.vstack(
(u_list, mp.NewContinuousVariables(2, "u_{}".format(idx))))
state_list = np.vstack(
(state_list,
mp.NewContinuousVariables(4, "state_{}".format(idx + 1))))
### Constraints
# set init state as constraint on stage 0 decision vars
for state_idx in range(4):
mp.AddLinearConstraint(
state_list[0, state_idx] == state_initial[state_idx])
# interstage equality constraint on state vars via Euler integration
# note: Direct Collocation could improve accuracy for same computation
for idx in range(1, N + 1):
state_new = state_list[idx - 1, :] + dt * self.rocket_dynamics(
state_list[idx - 1, :], u_list[idx - 1, :])
for state_idx in range(4):
mp.AddConstraint(state_list[idx,
state_idx] == state_new[state_idx])
# constraint on time
mp.AddLinearConstraint(total_time[0] <= maximum_time)
mp.AddLinearConstraint(total_time[0] >= minimum_time)
# constraint on final state distance (squared)to second planet
final_dist_to_world_2_sq = (
self.world_2_position[0] - state_list[-1, 0])**2 + (
self.world_2_position[1] - state_list[-1, 1])**2
mp.AddConstraint(final_dist_to_world_2_sq <= 0.25)
# constraint on final state speed (squared
final_speed_sq = state_list[-1, 2]**2 + state_list[-1, 3]**2
mp.AddConstraint(final_speed_sq <= 1)
### Cost
# equal cost on vertical/horizontal accels, weight shouldn't matter since this is the only cost
mp.AddQuadraticCost(1 * u_list[:, 0].dot(u_list[:, 0]))
mp.AddQuadraticCost(1 * u_list[:, 1].dot(u_list[:, 1]))
### Solve and parse
result = Solve(mp)
trajectory = result.GetSolution(state_list)
input_trajectory = result.GetSolution(u_list)
tf = result.GetSolution(total_time)
time_array = np.linspace(0, tf[0], N + 1)
print "optimization successful: ", result.is_success()
print "total num decision vars (x: (N+1)*4, u: 2N, total_time: 1): {}".format(
mp.num_vars())
print "solver used: ", result.get_solver_id().name()
print "optimal trajectory time: {:.2f} sec".format(tf[0])
return trajectory, input_trajectory, time_array
def 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
