def find_feasible_latents(self, observed):
# Build an optimization to estimate the hidden variables
try:
prog = MathematicalProgram()
# Add in all the appropriate variables with their bounds
all_vars = self.prices[0].GetVariables()
for price in self.prices[1:]:
all_vars += price.GetVariables()
mp_vars = prog.NewContinuousVariables(len(all_vars))
subs_dict = {}
for v, mv in zip(all_vars, mp_vars):
subs_dict[v] = mv
lb = []
ub = []
prog.AddBoundingBoxConstraint(0., 1., mp_vars)
prices_mp = [
self.prices[k].Substitute(subs_dict) for k in range(12)
]
# Add the observation constraint
for k, val in enumerate(observed[1:]):
if val != 0:
prog.AddConstraint(prices_mp[k] >= val - 2.)
prog.AddConstraint(prices_mp[k] <= val + 2)
# Find lower bounds
prog.AddCost(sum(prices_mp))
solver = SnoptSolver()
result = solver.Solve(prog)
if result.is_success():
lb = [result.GetSolution(x).Evaluate() for x in prices_mp]
lb_vars = result.GetSolution(mp_vars)
# Find upper bound too
prog.AddCost(-2. * sum(prices_mp))
result = solver.Solve(prog)
if result.is_success():
ub_vars = result.GetSolution(mp_vars)
ub = [result.GetSolution(x).Evaluate() for x in prices_mp]
self.price_ub = ub
self.price_lb = lb
subs_dict = {}
for k, v in enumerate(all_vars):
if lb_vars[k] == ub_vars[k]:
subs_dict[v] = lb_vars[k]
else:
new_var = sym.Variable(
"feasible_%d" % k,
sym.Variable.Type.RANDOM_UNIFORM)
subs_dict[v] = new_var * (ub_vars[k] -
lb_vars[k]) + lb_vars[k]
self.prices = [
self.prices[k].Substitute(subs_dict) for k in range(12)
]
return
except RuntimeError as e:
print("Runtime error: ", e)
self.rollout_probability = 0.
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 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 create_qp1(self, plant_context, V, q_des, v_des, vd_des):
# Determine contact points
contact_positions_per_frame = {}
active_contacts_per_frame = {} # Note this should be in frame space
for frame, contacts in self.contacts_per_frame.items():
contact_positions = self.plant.CalcPointsPositions(
plant_context, self.plant.GetFrameByName(frame), contacts,
self.plant.world_frame())
active_contacts_per_frame[
frame] = contacts[:,
np.where(contact_positions[2, :] <= 0.0)[0]]
N_c = sum([
active_contacts.shape[1]
for active_contacts in active_contacts_per_frame.values()
]) # num contact points
if N_c == 0:
print("Not in contact!")
return None
''' Eq(7) '''
H = self.plant.CalcMassMatrixViaInverseDynamics(plant_context)
# Note that CalcGravityGeneralizedForces assumes the form Mv̇ + C(q, v)v = tau_g(q) + tau_app
# while Eq(7) assumes gravity is accounted in C (on the left hand side)
C_7 = self.plant.CalcBiasTerm(
plant_context) - self.plant.CalcGravityGeneralizedForces(
plant_context)
B_7 = self.B_7
# TODO: Double check
Phi_foots = []
for frame, active_contacts in active_contacts_per_frame.items():
if active_contacts.size:
Phi_foots.append(
self.plant.CalcJacobianTranslationalVelocity(
plant_context, JacobianWrtVariable.kV,
self.plant.GetFrameByName(frame), active_contacts,
self.plant.world_frame(), self.plant.world_frame()))
Phi = np.vstack(Phi_foots)
''' Eq(8) '''
v_idx_act = self.v_idx_act
H_f = H[0:v_idx_act, :]
H_a = H[v_idx_act:, :]
C_f = C_7[0:v_idx_act]
C_a = C_7[v_idx_act:]
B_a = self.B_a
Phi_f_T = Phi.T[0:v_idx_act:, :]
Phi_a_T = Phi.T[v_idx_act:, :]
''' Eq(9) '''
# Assume flat ground for now
n = np.array([[0], [0], [1.0]])
d = np.array([[1.0, -1.0, 0.0, 0.0], [0.0, 0.0, 1.0, -1.0],
[0.0, 0.0, 0.0, 0.0]])
v = np.zeros((N_d, N_c, N_f))
for i in range(N_d):
for j in range(N_c):
v[i, j] = (n + mu * d)[:, i]
''' Quadratic Program I '''
prog = MathematicalProgram()
qdd = prog.NewContinuousVariables(
self.plant.num_velocities(),
name="qdd") # To ignore 6 DOF floating base
self.qdd = qdd
beta = prog.NewContinuousVariables(N_d, N_c, name="beta")
self.beta = beta
lambd = prog.NewContinuousVariables(N_f * N_c, name="lambda")
self.lambd = lambd
# Jacobians ignoring the 6DOF floating base
J_foots = []
for frame, active_contacts in active_contacts_per_frame.items():
if active_contacts.size:
num_active_contacts = active_contacts.shape[1]
J_foot = np.zeros((N_f * num_active_contacts, Atlas.TOTAL_DOF))
# TODO: Can this be simplified?
for i in range(num_active_contacts):
J_foot[N_f * i:N_f *
(i +
1), :] = self.plant.CalcJacobianSpatialVelocity(
plant_context, JacobianWrtVariable.kV,
self.plant.GetFrameByName(frame),
active_contacts[:,
i], self.plant.world_frame(),
self.plant.world_frame())[3:]
J_foots.append(J_foot)
J = np.vstack(J_foots)
assert (J.shape == (N_c * N_f, Atlas.TOTAL_DOF))
eta = prog.NewContinuousVariables(J.shape[0], name="eta")
self.eta = eta
x = prog.NewContinuousVariables(
self.x_size, name="x") # x_com, y_com, x_com_d, y_com_d
self.x = x
u = prog.NewContinuousVariables(self.u_size,
name="u") # x_com_dd, y_com_dd
self.u = u
''' Eq(10) '''
w = 0.01
epsilon = 1.0e-8
K_p = 10.0
K_d = 4.0
frame_weights = np.ones((Atlas.TOTAL_DOF))
q = self.plant.GetPositions(plant_context)
qd = self.plant.GetVelocities(plant_context)
# Convert q, q_nom to generalized velocities form
q_err = self.plant.MapQDotToVelocity(plant_context, q_des - q)
# print(f"Pelvis error: {q_err[0:3]}")
## FIXME: Not sure if it's a good idea to ignore the x, y, z position of pelvis
# ignored_pose_indices = {3, 4, 5} # Ignore x position, y position
ignored_pose_indices = {} # Ignore x position, y position
relevant_pose_indices = list(
set(range(Atlas.TOTAL_DOF)) - set(ignored_pose_indices))
self.relevant_pose_indices = relevant_pose_indices
qdd_ref = K_p * q_err + K_d * (v_des - qd) + vd_des # Eq(27) of [1]
qdd_err = qdd_ref - qdd
qdd_err = qdd_err * frame_weights
qdd_err = qdd_err[relevant_pose_indices]
prog.AddCost(
V(x, u) + w * ((qdd_err).dot(qdd_err)) +
epsilon * np.sum(np.square(beta)) + eta.dot(eta))
''' Eq(11) - 0.003s '''
eq11_lhs = H_f.dot(qdd) + C_f
eq11_rhs = Phi_f_T.dot(lambd)
prog.AddLinearConstraint(eq(eq11_lhs, eq11_rhs))
''' Eq(12) - 0.005s '''
alpha = 0.1
# TODO: Double check
Jd_qd_foots = []
for frame, active_contacts in active_contacts_per_frame.items():
if active_contacts.size:
Jd_qd_foot = self.plant.CalcBiasTranslationalAcceleration(
plant_context, JacobianWrtVariable.kV,
self.plant.GetFrameByName(frame), active_contacts,
self.plant.world_frame(), self.plant.world_frame())
Jd_qd_foots.append(Jd_qd_foot.flatten())
Jd_qd = np.concatenate(Jd_qd_foots)
assert (Jd_qd.shape == (N_c * 3, ))
eq12_lhs = J.dot(qdd) + Jd_qd
eq12_rhs = -alpha * J.dot(qd) + eta
prog.AddLinearConstraint(eq(eq12_lhs, eq12_rhs))
''' Eq(13) - 0.015s '''
def tau(qdd, lambd):
return self.B_a_inv.dot(H_a.dot(qdd) + C_a - Phi_a_T.dot(lambd))
self.tau = tau
eq13_lhs = self.tau(qdd, lambd)
prog.AddLinearConstraint(ge(eq13_lhs, -self.sorted_max_efforts))
prog.AddLinearConstraint(le(eq13_lhs, self.sorted_max_efforts))
''' Eq(14) '''
for j in range(N_c):
beta_v = beta[:, j].dot(v[:, j])
prog.AddLinearConstraint(eq(lambd[N_f * j:N_f * j + 3], beta_v))
''' Eq(15) '''
for b in beta.flat:
prog.AddLinearConstraint(b >= 0.0)
''' Eq(16) '''
prog.AddLinearConstraint(ge(eta, eta_min))
prog.AddLinearConstraint(le(eta, eta_max))
''' Enforce x as com '''
com = self.plant.CalcCenterOfMassPosition(plant_context)
com_d = self.plant.CalcJacobianCenterOfMassTranslationalVelocity(
plant_context, JacobianWrtVariable.kV, self.plant.world_frame(),
self.plant.world_frame()).dot(qd)
prog.AddLinearConstraint(x[0] == com[0])
prog.AddLinearConstraint(x[1] == com[1])
prog.AddLinearConstraint(x[2] == com_d[0])
prog.AddLinearConstraint(x[3] == com_d[1])
''' Enforce u as com_dd '''
com_dd = (
self.plant.CalcBiasCenterOfMassTranslationalAcceleration(
plant_context, JacobianWrtVariable.kV,
self.plant.world_frame(), self.plant.world_frame()) +
self.plant.CalcJacobianCenterOfMassTranslationalVelocity(
plant_context, JacobianWrtVariable.kV,
self.plant.world_frame(), self.plant.world_frame()).dot(qdd))
prog.AddLinearConstraint(u[0] == com_dd[0])
prog.AddLinearConstraint(u[1] == com_dd[1])
''' Respect joint limits '''
for name, limit in Atlas.JOINT_LIMITS.items():
# Get the corresponding joint value
joint_pos = self.plant.GetJointByName(name).get_angle(
plant_context)
# Get the corresponding actuator index
act_idx = getActuatorIndex(self.plant, name)
# Use the actuator index to find the corresponding generalized coordinate index
# q_idx = np.where(B_7[:,act_idx] == 1)[0][0]
q_idx = getJointIndexInGeneralizedVelocities(self.plant, name)
if joint_pos >= limit.upper:
# print(f"Joint {name} max reached")
prog.AddLinearConstraint(qdd[q_idx] <= 0.0)
elif joint_pos <= limit.lower:
# print(f"Joint {name} min reached")
prog.AddLinearConstraint(qdd[q_idx] >= 0.0)
return prog
# Print the newly added cost
print(cost_1)
# The newly added cost is stored in prog.linear_costs()
print(prog.linear_costs()[0])
cost_2 = prog.AddLinearCost(2 * x[1] + 3)
print(f"number of linear cost objects: {len(prog.linear_costs())}")
# Can also add costs in a vector coefficient form
cost_3 = prog.AddLinearCost([3., 4.], 5.0, x)
print(cost_3)
# Can also just call the generic "AddCost".
# If drake thinks its linear, gets added to linear costs list
cost_4 = prog.AddCost(x[0] + 3 * x[1] + 5)
print(f"Number of linear cost objects after calling AddCost: {len(prog.linear_costs())}")
# New program, now with constraints
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
y = prog.NewContinuousVariables(3, "y")
bounding_box = prog.AddLinearConstraint(x[1] <= 2)
linear_eq = prog.AddLinearConstraint(x[1] + 2 * y[2] == 1)
linear_ineq = prog.AddLinearConstraint(x[1] + 4 * y[1] >= 2)
# New program, all the way to solving...
prog = MathematicalProgram()
# Declare x as decision variables.
x = prog.NewContinuousVariables(4)
# WARNING: If you return a scalar for a constraint, or a vector for
# a cost, you may get the following cryptic error:
# "Unable to cast Python instance to c++ type"
link_7_distance_to_target_vector = lambda q: [link_7_distance_to_target(q)]
# New program: Using a custom evaluator
prog = MathematicalProgram()
q = prog.NewContinuousVariables(plant_f.num_positions())
# Define nominal configuration
q0 = np.zeros(plant_f.num_positions())
# Add basic cost. (This will be parsed into a quadratic cost)
prog.AddCost((q - q0).dot(q - q0))
# Add constraint based on custom evaluator.
prog.AddConstraint(link_7_distance_to_target_vector,
lb=[0.1],
ub=[0.2],
vars=q)
result = Solve(prog, initial_guess=q0)
print(f"Success? {result.is_success()}")
print(result.get_solution_result())
q_sol = result.GetSolution(q)
print(q_sol)
prog = MathematicalProgram()
Adapted from the MathematicalProgram tutorial on the Drake website: https://drake.mit.edu
"""
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np
import matplotlib.pyplot as plt
# Create empty MathematicalProgram
prog = MathematicalProgram()
# Add 2 continuous decision variables
# x is a numpy array - we can use an optional second argument to name the variables
x = prog.NewContinuousVariables(2)
#print(x)
prog.AddConstraint(x[0] + x[1] == 1)
prog.AddConstraint(x[0] <= x[1])
prog.AddCost(x[0]**2 + x[1]**2)
# Make and add a visualization callback
fig = plt.figure()
curve_x = np.linspace(1, 10, 100)
ax = plt.gca()
ax.plot(curve_x, 9. / curve_x)
ax.plot(-curve_x, -9. / curve_x)
ax.plot(0, 0, 'o')
x_init = [4., 5.]
point_x, = ax.plot(x_init[0], x_init[1], 'x')
ax.axis('equal')
def update(x):
global iter_count
def compute_input(self, x, xd, initial_guess=None):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda1')
z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
prog.AddConstraint(z[0, 0] == 0)
prog.AddConstraint(z[0, 1] == 0)
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints with big M = self.contact
prog.AddConstraint(ge(contact[t], 0))
prog.AddConstraint(contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0)
prog.AddConstraint(contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0)
# Mixed Integer Constraints
M = self.contact_max
prog.AddConstraint(contact[t, 0] <= M)
prog.AddConstraint(contact[t, 1] <= M)
prog.AddConstraint(contact[t, 0] <= M * z[t, 0])
prog.AddConstraint(contact[t, 1] <= M * z[t, 1])
prog.AddConstraint(
contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) <= M *
(1 - z[t, 0]))
prog.AddConstraint(
contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) <= M *
(1 - z[t, 1]))
prog.AddConstraint(z[t, 0] + z[t, 1] == 1)
# Add Input Constraints. Contact Constraints already enforced in big-M
# prog.AddConstraint(le(u[t], self.input_max))
# prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver_id = IpoptSolver().solver_id()
elif (self.solver == "snopt"):
solver_id = SnoptSolver().solver_id()
elif (self.solver == "osqp"):
solver_id = OsqpSolver().solver_id()
elif (self.solver == "mosek"):
solver_id = MosekSolver().solver_id()
elif (self.solver == "gurobi"):
solver_id = GurobiSolver().solver_id()
solver = MixedIntegerBranchAndBound(prog, solver_id)
#result = solver.Solve(prog, initial_guess)
result = solver.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem')
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
current_state, current_tau234, is_symbolic=False)
diff = desired_state - next_state
ret = np.zeros_like(stacked)
ret[0:STATE_SIZE] = diff
return ret
stacked = np.concatenate([next_state, state_over_time[i], tau234])
bounds = np.ones(stacked.shape) * DYNAMICS_EPSILON
mp.AddConstraint(next_state_constraint, -bounds, bounds,
stacked).evaluator().set_description(
"Constrain next_state_constraint %d" % i)
tau234_over_time[i] = tau234
state_over_time[i + 1] = next_state
mp.AddCost(0.1 * tau234_over_time[:, 0].dot(tau234_over_time[:, 0]))
mp.AddCost(0.1 * tau234_over_time[:, 1].dot(tau234_over_time[:, 1]))
mp.AddCost(0.1 * tau234_over_time[:, 2].dot(tau234_over_time[:, 2]))
# This cost is incorrect, different case for if front wheel is left / right of back wheel
# mp.AddCost(-(state_over_time[:,0].dot(state_over_time[:,0])))
final_state = state_over_time[-1]
# Constrain final velocity to be 0
# for j in range(4, 8):
# mp.AddConstraint(final_state[j] <= 0.0)
# mp.AddConstraint(final_state[j] >= 0.0)
# Constrain final front wheel position
final_front_wheel_pos = eom.findFrontWheelPosition(final_state[0],
final_state[1],
final_state[2])
def compute_input(self, x, xd, initial_guess=None, tol=0.0):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda')
#
alpha = prog.NewContinuousVariables(rows=self.T, cols=2, name='alpha')
beta = prog.NewContinuousVariables(rows=self.T, cols=2, name='beta')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints
prog.AddConstraint(ge(alpha[t], 0))
prog.AddConstraint(ge(beta[t], 0))
prog.AddConstraint(alpha[t, 0] == contact[t, 0])
prog.AddConstraint(alpha[t, 1] == contact[t, 1])
prog.AddConstraint(
beta[t, 0] == (contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d'])))
prog.AddConstraint(
beta[t, 1] == (contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d'])))
# Complementarity constraints. Start with relaxed version and start constraining.
prog.AddConstraint(alpha[t, 0] * beta[t, 0] <= tol)
prog.AddConstraint(alpha[t, 1] * beta[t, 1] <= tol)
# Add Input Constraints and Contact Constraints
prog.AddConstraint(le(contact[t], self.contact_max))
prog.AddConstraint(ge(contact[t], -self.contact_max))
prog.AddConstraint(le(u[t], self.input_max))
prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver = IpoptSolver()
elif (self.solver == "snopt"):
solver = SnoptSolver()
result = solver.Solve(prog, initial_guess)
if (self.solver == "ipopt"):
print("Ipopt Solver Status: ",
result.get_solver_details().status, ", meaning ",
result.get_solver_details().ConvertStatusToString())
elif (self.solver == "snopt"):
val = result.get_solver_details().info
status = self.snopt_status(val)
print("Snopt Solver Status: ",
result.get_solver_details().info, ", meaning ", status)
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
# Do forward kinematics
plant.SetPositions(context, iiwa, q)
X_WL7 = plant.CalcRelativeTransform(context, resolve_frame(plant, W), resolve_frame(plant, L7))
p_TL7 = X_WL7.translation() - p_WT
# Return scalar squared distance
return p_TL7.dot(p_TL7)
# NOTE: Returning a scalar for a constraint or a vector for a cost could result in cryptic errors
# Create a mathematical program
prog = MathematicalProgram()
q = prog.NewContinuousVariables(plant_f.num_positions())
# define nominal configuration
q0 = np.zeros(plant_f.num_positions())
# Add the custom cost
prog.AddCost(link_7_distance_to_target, vars=q, description="IK_Cost")
# Solve the problem
CheckProgram(prog)
print('Start optimization')
result = Solve(prog, initial_guess=q0)
# Number of autodiff and float calls during optimization
print(f"Number float evaluations: {float_calls}")
print(f"Number of autodiff evaluations: {autodiff_calls}")
print(f"Success? {result.is_success()}")
print(result.get_solution_result())
qsol = result.GetSolution(q)
print(qsol)
print(f"Initial distance to target: {link_7_distance_to_target(q0):.3f}")
print(f"Final distance to target: {link_7_distance_to_target(qsol):.3f}")