def findMaxForce(theta1, theta2, theta3):
theta12 = theta2 + theta1
theta123 = theta3 + theta12
s1 = sin(theta1)
s2 = sin(theta2)
s3 = sin(theta3)
s12 = sin(theta12)
s123 = sin(theta123)
c1 = cos(theta1)
c2 = cos(theta2)
c3 = cos(theta3)
c12 = cos(theta12)
c123 = cos(theta123)
J = np.array([[
-l_1 * s1 - l_2 * s12 - l_3 * s123, -l_2 * s12 - l_3 * s123,
-l_3 * s123
], [l_1 * c1 + l_2 * c12 + l_3 * c123, l_2 * c12 + l_3 * c123,
l_3 * c123]])
tau1 = (
-l_1 * c1 * m_2 * g # torque due to l_1 - l_2 link motor
+ (-l_1 * c1 + l_2 / 2.0 * c12) * m_m *
g # torque due to battery link motor
+ (-l_1 * c1 + l_2 / 2.0 * c12 + l_b * cos(theta12 + np.pi / 2.0)) *
m_b * g # torque due to battery
+ (-l_1 * c1 + l_2) * m_3 * g # torque due to l_2 - l_3 link motor
+ (-l_1 * c1 + l_2 + l_3 * c3) * m_w * g # torque due to front wheel
)
# print("tau1 = " + str(tau1))
mp = MathematicalProgram()
tau23 = mp.NewContinuousVariables(2, "tau23")
tau123 = np.append(tau1, tau23)
mp.AddCost(J.dot(tau123)[0])
mp.AddConstraint(tau23[0]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
mp.AddConstraint(tau23[1]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
mp.AddLinearConstraint(J.dot(tau123)[1] >= -HUB_MOTOR_FORCE / 2.0)
result = Solve(mp)
is_success = result.is_success()
torques = result.GetSolution(tau23)
full_tau = np.append(tau1, torques)
output_force = J.dot(full_tau)
# print("is_success = " + str(is_success))
# print("tau = " + str(full_tau))
# print("F = " + str(J.dot(full_tau)))
return is_success, output_force, torques
def calcTorqueOutput(self, context, output):
q_v = self.EvalVectorInput(context, self.input_q_v_idx).get_value()
if self.is_wbc:
r = self.EvalVectorInput(context, self.input_r_des_idx).get_value()
rd = self.EvalVectorInput(context,
self.input_rd_des_idx).get_value()
rdd = self.EvalVectorInput(context,
self.input_rdd_des_idx).get_value()
V = lambda x, u: self.V_full(x, u, r, rd, rdd)
q_des = self.EvalVectorInput(context,
self.input_q_des_idx).get_value()
v_des = self.EvalVectorInput(context,
self.input_v_des_idx).get_value()
vd_des = self.EvalVectorInput(context,
self.input_vd_des_idx).get_value()
else:
y_des = self.EvalVectorInput(context,
self.input_y_des_idx).get_value()
V = lambda x, u: self.V_full(x, u, y_des)
q_des = self.q_nom
v_des = [0.0] * self.plant.num_velocities()
vd_des = [0.0] * self.plant.num_velocities()
current_plant_context = self.plant.CreateDefaultContext()
self.plant.SetPositionsAndVelocities(current_plant_context, q_v)
start_formulate_time = time.time()
prog = self.create_qp1(current_plant_context, V, q_des, v_des, vd_des)
if not prog:
print("Invalid program!")
output.SetFromVector([0] * self.plant.num_actuated_dofs())
return
# print(f"Formulate time: {time.time() - start_formulate_time}s")
start_solve_time = time.time()
result = Solve(prog)
# print(f"Solve time: {time.time() - start_solve_time}s")
if not result.is_success():
print(f"FAILED")
output.SetFromVector([0] * self.plant.num_actuated_dofs())
# print(f"Cost: {result.get_optimal_cost()}")
qdd_sol = result.GetSolution(self.qdd)
lambd_sol = result.GetSolution(self.lambd)
x_sol = result.GetSolution(self.x)
u_sol = result.GetSolution(self.u)
beta_sol = result.GetSolution(self.beta)
eta_sol = result.GetSolution(self.eta)
tau = self.tau(qdd_sol, lambd_sol)
output.SetFromVector(tau)
def minimize_height(diagram_f, plant_f, d_context_f, frame_list):
"""Fragile and slow :("""
context_f = plant_f.GetMyContextFromRoot(d_context_f)
diagram_ad = diagram_f.ToAutoDiffXd()
plant_ad = diagram_ad.GetSubsystemByName(plant_f.get_name())
d_context_ad = diagram_ad.CreateDefaultContext()
d_context_ad.SetTimeStateAndParametersFrom(d_context_f)
context_ad = plant_ad.GetMyContextFromRoot(d_context_ad)
def prepare_plant_and_context(z):
if z.dtype == float:
plant, context = plant_f, context_f
else:
plant, context = plant_ad, context_ad
set_frame_heights(plant, context, frame_list, z)
return plant, context
prog = MathematicalProgram()
num_z = len(frame_list)
z_vars = prog.NewContinuousVariables(num_z, "z")
q0 = plant_f.GetPositions(context_f)
z0 = get_frame_heights(plant_f, context_f, frame_list)
cost = prog.AddCost(np.sum(z_vars))
prog.AddBoundingBoxConstraint([0.01] * num_z, [5.] * num_z, z_vars)
# # N.B. Cannot use AutoDiffXd due to cylinders.
distance = MinimumDistanceConstraint(plant=plant_f,
plant_context=context_f,
minimum_distance=0.05)
def distance_with_z(z):
plant, context = prepare_plant_and_context(z)
q = plant.GetPositions(context)
return distance.Eval(q)
prog.AddConstraint(distance_with_z,
lb=distance.lower_bound(),
ub=distance.upper_bound(),
vars=z_vars)
result = Solve(prog, initial_guess=z0)
assert result.is_success()
z = result.GetSolution(z_vars)
set_frame_heights(plant_f, context_f, frame_list, z)
q = plant_f.GetPositions(context_f)
return q
def main():
parser = ArgumentParser()
parser.add_argument('--datapath', default='../out/data.npy')
parser.add_argument('--normalize', dest='normalize', action='store_true')
parser.add_argument('--no-normalize',
dest='normalize',
action='store_false')
parser.set_defaults(normalize=True)
opts = parser.parse_args()
data = dynamics.unmarshal_data(np.load(opts.datapath))
n = data.poslambdas.shape[0]
lambdas = np.vstack((data.poslambdas, data.neglambdas, data.gammas))
ds = np.vstack((data.xdots, data.us, np.ones(data.xdots.shape)))
mp = MathematicalProgram()
M = mp.NewContinuousVariables(3, 3, "M")
W = mp.NewContinuousVariables(3, 3, "W")
W[0, 2] = 0
W[1, 2] = 0
R = M.dot(lambdas) + W.dot(ds)
elementwise_positive_constraint(mp, R)
lambdas_vec = np.expand_dims(np.ndarray.flatten(lambdas), 0)
R_vec = np.ndarray.flatten(R)
mp.AddLinearCost(lambdas_vec.dot(R_vec)[0])
result = Solve(mp)
print(result.is_success())
Msol = result.GetSolution(M)
# When mix 0's in with W, need to evaluate expression
evaluator = np.vectorize(lambda x: x.Evaluate())
Wsol = evaluator(result.GetSolution(W))
print(Msol)
print(Wsol)
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 PPTrajectory():
def __init__(self, sample_times, num_vars, degree, continuity_degree):
self.sample_times = sample_times
self.n = num_vars
self.degree = degree
self.prog = MathematicalProgram()
self.coeffs = []
for i in range(len(sample_times)):
self.coeffs.append(self.prog.NewContinuousVariables(
num_vars, degree+1, "C"))
self.result = None
# Add continuity constraints
for s in range(len(sample_times)-1):
trel = sample_times[s+1]-sample_times[s]
coeffs = self.coeffs[s]
for var in range(self.n):
for deg in range(continuity_degree+1):
# Don't use eval here, because I want left and right
# values of the same time
left_val = 0
for d in range(deg, self.degree+1):
left_val += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
right_val = self.coeffs[s+1][var, deg]*math.factorial(deg)
self.prog.AddLinearConstraint(left_val == right_val)
# Add cost to minimize highest order terms
for s in range(len(sample_times)-1):
self.prog.AddQuadraticCost(np.eye(num_vars),
np.zeros((num_vars, 1)),
self.coeffs[s][:, -1])
def eval(self, t, derivative_order=0):
if derivative_order > self.degree:
return 0
s = 0
while s < len(self.sample_times)-1 and t >= self.sample_times[s+1]:
s += 1
trel = t - self.sample_times[s]
if self.result is None:
coeffs = self.coeffs[s]
else:
coeffs = self.result.GetSolution(self.coeffs[s])
deg = derivative_order
val = 0*coeffs[:, 0]
for var in range(self.n):
for d in range(deg, self.degree+1):
val[var] += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
return val
def add_constraint(self, t, derivative_order, lb, ub=None):
'''
Adds a constraint of the form d^deg lb <= x(t) / dt^deg <= ub
'''
if ub is None:
ub = lb
assert(derivative_order <= self.degree)
val = self.eval(t, derivative_order)
self.prog.AddLinearConstraint(val, lb, ub)
def generate(self):
self.result = Solve(self.prog)
assert(self.result.is_success())
def __init__(self,
A,
B,
C,
K,
T,
Sigma,
x0,
x_lb,
x_ub,
check_states=[],
noise_trunc=[]):
n = A.shape[0] # n states
if (len(check_states) == 0):
check_states = range(0, n)
if (len(noise_trunc) == 0):
# set default to 3 std's
noise_trunc = np.diag(Sigma)**(0.5) * 3.
invsig = np.linalg.inv(Sigma)
#Q = block_diag(*([invsig]*T)) # [n1,n2,...,nL]*[Q]*[n1;n2;...;nL]
b = np.zeros((n, 1)) #n*T
costs = []
eval_n_is = []
for prob_len in range(1, T):
for bound in ['lo', 'hi']:
for state in check_states:
self.prog = MathematicalProgram()
# the noise decision variables, we search for the first time it falsifies.
# to not overconstrain the problem, if we are checking to falsify at t=i,
# we do not want more constraints in i:T because maybe a better probability cost will be
# that i,i+1,i+2 will go out of bounds, and then i+3 comes back, etc ...
self.n_i = self.prog.NewContinuousVariables(
n * prob_len, 'n')
# the states as a function of time
self.x = self.prog.NewIndeterminates(n * prob_len, 'x')
# the maximum value allowed for a noise measurement (truncated normal distribution)
for i, noise in enumerate(self.n_i):
self.prog.AddConstraint(noise, -noise_trunc[i % n],
noise_trunc[i % n])
# simulate the dynamics forward to obtain the constraints on the states
# Drake will convert them to constraints on the decision variables
x_t = x0.copy()
for t in range(1, prob_len + 1):
# which noise variables affect this step
ind = np.arange((t - 1) * n, t * n)
#import pdb; pdb.set_trace()
x_t = (A - B * K.dot(C)).dot(x_t) - B.dot(K).dot(
self.n_i[ind].reshape(n, -1))
# the falsifying constraint:
if (prob_len == t):
for state_i in range(n):
dv_exist = (len(x_t[state_i][0].GetVariables())
> 0)
if (state_i == state and dv_exist):
if (bound == 'lo'):
#print('adding a lower violation constraint state=%d t=%d' %(state_i, t))
# hits the lower bound
self.prog.AddConstraint(
x_t[state_i][0] <= x_lb[state_i][0]
)
else:
#print('adding an upper violation constraint state=%d t=%d' %(state_i, t))
# hits the upper bound
self.prog.AddConstraint(
x_t[state_i][0] >= x_ub[state_i][0]
)
elif (state_i != state and dv_exist):
# keep it within the bounds (satisfying)
self.prog.AddConstraint(
x_t[state_i][0] >= x_lb[state_i][0])
self.prog.AddConstraint(
x_t[state_i][0] <= x_ub[state_i][0])
else:
# dv_exist == False =>
# drake cannot get a constraint in the form of const <=> const
pass
else:
for state_i in range(n):
dv_exist = (len(x_t[state_i][0].GetVariables())
> 0)
if (dv_exist):
# the "box"/corridor constraint
self.prog.AddConstraint(
x_t[state_i][0], x_lb[state_i][0],
x_ub[state_i][0])
# quadratic cost of the log likelihood
self.prog.AddQuadraticCost(invsig, b, self.n_i[ind])
# solve the mathematical program
result = Solve(self.prog)
if (result.is_success()):
eval_n_i = result.GetSolution(self.n_i)
eval_n_is.append(eval_n_i.reshape(prob_len, n).T)
# minus because the real cost is -1/2*x'Sx and for the prog I multiply by -1 to get max
# this is the true multivariate pdf
Q = block_diag(
*([invsig] *
prob_len)) # [n1,n2,...,nL]*[Q]*[n1;n2;...;nL]
cost = (2.*np.pi)**(-(n*prob_len)/2.) * np.linalg.det(Q)**(-0.5) * \
np.exp(-0.5*eval_n_i.T.dot(Q).dot(eval_n_i))
costs.append(cost)
print(
'found solution (lh/step/state/cost %s/%d/%d/%.2e)'
% (bound, prob_len, state, cost))
else:
print('nope :( (lh/step/state %s/%d/%d)' %
(bound, prob_len, state))
costs = np.array(costs)
if (len(costs) > 0):
ind = np.argmax(costs)
# the index step, the actual cost and the whole series of noises
self.ind = ind
self.max_cost = costs[ind]
self.noise_seq = eval_n_is[ind]
else:
self.ind = -1
self.max_cost = -1
self.noise_seq = np.array([])
b=np.array([1., 1., 1.]),
c=1.,
vars=P[:3])
#Initial Trajectory
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
[0., 20.], np.column_stack((initial_state, final_state)))
print(initial_x_trajectory)
dircol.SetInitialTrajectory(PiecewisePolynomial(),
initial_x_trajectory)
#Solve the DirCol
result = Solve(dircol)
print("\n\n")
print("THE FUNCTION RETURN")
print(result.is_success())
print("\n\n")
assert (result.is_success())
#Final Trajectory
print("\n\n")
print("THE STATE TRAJ PRINT")
u_trajectory = dircol.ReconstructInputTrajectory(result)
x_trajectory = dircol.ReconstructStateTrajectory(result)
#For the input u times 100
times_input = np.linspace(u_trajectory.start_time(),
u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
times_input = np.linspace(x_trajectory.start_time(),
point_x, = ax.plot(x_init[0], x_init[1], 'x')
ax.axis('equal')
def update(x):
global iter_count
point_x.set_xdata(x[0])
point_x.set_ydata(x[1])
ax.set_title(f"iteration {iter_count}")
fig.canvas.draw()
fig.canvas.flush_events()
# Update iter_counter
iter_count += 1
plt.pause(1.0)
# Add a visualization callback - it does more than just visualization
iter_count = 0
prog.AddVisualizationCallback(update, x)
# Solve the optimization problem - optional second argument is initial guess. Third argument is solver parameters
result = Solve(prog, x_init, None)
# Print the result
print("Success? ", result.is_success())
# Print the solution
print("x* = ", result.GetSolution(x))
# Print the optimal cost
print('optimal cost = ', result.get_optimal_cost())
# Print the name of the solver
print('solver is: ', result.get_solver_id().name())
def plot_sublevelset_expression(ax, e, vertices=51, **kwargs):
"""
Plots the 2D sub-level set e(x) <= 1, which must contain the origin.
Args:
ax: the matplotlib axis to receive the plot
e: a symbolic expression in two variables
vertices: number of sample points along the boundary
kwargs: are passed to the matplotlib fill method
Returns:
the return values from matplotlib's fill command.
"""
x = list(e.GetVariables())
assert len(x) == 2, "e must be an expression in two variables"
# Handle the special case where e is a degree 2 polynomial.
if e.is_polynomial():
p = Polynomial(e)
if p.TotalDegree() == 2:
env = {a: 0 for a in x}
c = e.Evaluate(env)
e1 = e.Jacobian(x)
b = Evaluate(e1, env)
e2 = Jacobian(e1, x)
A = 0.5 * Evaluate(e2, env)
return plot_sublevelset_quadratic(ax, A, b, c, vertices, **kwargs)
# Find the level-set in polar coordinates, by sampling theta and
# root-finding (on the scalar expression) to find a rplus and rminus.
Xplus = np.empty((2, vertices))
Xminus = np.empty((2, vertices))
i = 0
for theta in np.linspace(0, np.pi, vertices):
prog = MathematicalProgram()
r = prog.NewContinuousVariables(1, "r")[0]
env = {x[0]: r * np.cos(theta), x[1]: r * np.sin(theta)}
scalar = e.Substitute(env)
b = prog.AddBoundingBoxConstraint(0, np.inf, r)
prog.AddConstraint(scalar == 1)
prog.AddQuadraticCost([1], [0], [r])
prog.SetInitialGuess(r, 0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rplus = result.GetSolution(r)
Xplus[0, i] = rplus * np.cos(theta)
Xplus[1, i] = rplus * np.sin(theta)
b.evaluator().UpdateLowerBound([-np.inf])
b.evaluator().UpdateUpperBound([0])
prog.SetInitialGuess(r, -0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rminus = result.GetSolution(r)
Xminus[0, i] = rminus * np.cos(theta)
Xminus[1, i] = rminus * np.sin(theta)
i = i + 1
return ax.fill(np.hstack((Xplus[0, :], Xminus[0, :])),
np.hstack((Xplus[1, :], Xminus[1, :])), **kwargs)
# Constrain final front wheel position
final_front_wheel_pos = eom.findFrontWheelPosition(final_state[0],
final_state[1],
final_state[2])
# mp.AddConstraint(final_front_wheel_pos[1] <= STEP_HEIGHT - w_r)
mp.AddConstraint(final_front_wheel_pos[1] >= STEP_HEIGHT - w_r).evaluator(
).set_description("Constrain final_front_wheel_pos[1] >= 0.01")
print("Begin solving...")
t = time.time()
result = Solve(mp)
solve_traj_opt_duration = time.time() - t
print("Done solving in " + str(solve_traj_opt_duration) + "s (" +
str(solve_traj_opt_duration / 60.0) + "m)")
is_success = result.is_success()
print("is_success = " + str(is_success))
torque_over_time_star = result.GetSolution(tau234_over_time)
state_over_time_star = result.GetSolution(state_over_time)
time_step = 0
last_input = 'c'
max_time_step = len(state_over_time_star)
while time_step < max_time_step:
state = state_over_time_star[time_step]
torque = np.array(
[i.Evaluate() for i in torque_over_time_star[time_step]])
state_d = derivs(state, torque, is_symbolic=False)
proposed_next_state = state + TIME_INTERVAL * state_d
print("time_step = " + str(time_step))
print("theta = " + str(state[0:int(STATE_SIZE / 2)]))