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 test_snopt_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] + x[1] == 1)
prog.AddBoundingBoxConstraint(0, 1, x[1])
prog.AddLinearCost(x[0])
solver = SnoptSolver()
self.assertEqual(solver.solver_id(), SnoptSolver.id())
if solver.available():
self.assertTrue(solver.enabled())
self.assertEqual(solver.solver_type(), mp.SolverType.kSnopt)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
numpy_compare.assert_float_allclose(result.GetSolution(x),
[0., 1.],
atol=1E-7)
self.assertEqual(result.get_solver_details().info, 1)
np.testing.assert_allclose(result.get_solver_details().xmul,
np.array([0., -1]))
np.testing.assert_allclose(result.get_solver_details().F,
np.array([0, 1.]))
np.testing.assert_allclose(result.get_solver_details().Fmul,
np.array([0, 1.]))
def test_two_boxes(self):
# test with 2 boxes.
masses = [1., 2]
box_sizes = [np.array([0.1, 0.1, 0.1]), np.array([0.1, 0.1, 0.2])]
plant, scene_graph, diagram, boxes, ground_geometry_id,\
boxes_geometry_id = construct_environment(
masses, box_sizes)
diagram_context = diagram.CreateDefaultContext()
plant_context = diagram.GetMutableSubsystemContext(
plant, diagram_context)
# Ignore the collision between box-box, since currently Drake doesn't
# support box-box collision with AutoDiffXd.
ignored_collision_pairs = set()
for i in range(len(boxes_geometry_id)):
ignored_collision_pairs.add(
tuple((ground_geometry_id, boxes_geometry_id[i])))
for j in range(i + 1, len(boxes_geometry_id)):
ignored_collision_pairs.add(
tuple((boxes_geometry_id[i], boxes_geometry_id[j])))
dut = StaticEquilibriumProblem(plant, plant_context,
ignored_collision_pairs)
# Add the constraint that the quaternion should have unit length.
ik.AddUnitQuaternionConstraintOnPlant(plant, dut.q_vars(),
dut.get_mutable_prog())
for i in range(len(boxes)):
box_quaternion_start = 7 * i
box_quat, box_xyz = split_se3(
dut.q_vars()[box_quaternion_start:box_quaternion_start + 7])
dut.get_mutable_prog().SetInitialGuess(
box_quat, np.array([0.5, -0.5, 0.5, 0.5]))
dut.get_mutable_prog().SetInitialGuess(
box_xyz, np.array([0, 0, 0.3 + 0.2 * i]))
dut.UpdateComplementarityTolerance(0.1)
snopt_solver = SnoptSolver()
result = snopt_solver.Solve(dut.prog())
self.assertTrue(result.is_success())
# Now progressively tighten the tolerance and solve it again.
for tol in (0.05, 0.01, 0.005, 0.001):
dut.UpdateComplementarityTolerance(tol)
dut.get_mutable_prog().SetInitialGuessForAllVariables(
result.get_x_val())
result = snopt_solver.Solve(dut.prog())
self.assertTrue(result.is_success())
def test_one_box(self):
# Test with a single box.
masses = [1.]
box_sizes = [np.array([0.1, 0.1, 0.1])]
env = construct_environment(masses, box_sizes)
diagram_context = env.diagram.CreateDefaultContext()
plant_context = env.diagram.GetMutableSubsystemContext(
env.plant, diagram_context)
# Ignore the collision between box-box, since currently Drake doesn't
# support box-box collision with AutoDiffXd.
ignored_collision_pairs = {(
env.ground_geometry_id, env.boxes_geometry_id[0])}
dut = StaticEquilibriumProblem(
env.plant, plant_context, ignored_collision_pairs)
# Add the constraint that the quaternion should have unit length.
box_quat, box_xyz = split_se3(dut.q_vars())
ik.AddUnitQuaternionConstraintOnPlant(
env.plant, dut.q_vars(), dut.get_mutable_prog())
dut.get_mutable_prog().SetInitialGuess(
box_quat, np.array([0.5, 0.5, 0.5, 0.5]))
dut.get_mutable_prog().SetInitialGuess(
box_xyz, np.array([0, 0, 0.3]))
# Now set the complementarity tolerance.
dut.UpdateComplementarityTolerance(0.002)
solver = SnoptSolver()
result = solver.Solve(dut.prog())
self.assertTrue(result.is_success())
q_sol = result.GetSolution(dut.q_vars())
box_quat_sol, box_xyz_sol = split_se3(q_sol)
self.assertAlmostEqual(np.linalg.norm(box_quat_sol), 1.)
np.testing.assert_allclose(box_xyz_sol[2], 0.05, atol=0.002)
contact_wrenches = dut.GetContactWrenchSolution(result)
self.assertEqual(len(contact_wrenches), 8)
SolverId, SolverInterface)
from functools import partial
import unittest
import warnings
import numpy as np
import pydrake
from pydrake.autodiffutils import AutoDiffXd
from pydrake.common.test_utilities import numpy_compare
from pydrake.forwarddiff import jacobian
from pydrake.math import ge
import pydrake.symbolic as sym
SNOPT_NO_GUROBI = SnoptSolver().available() and not GurobiSolver().available()
class TestQP:
def __init__(self):
# Create a simple QP that uses all deduced linear constraint types,
# along with a quadratic and linear cost.
# The solution should be [1, 1].
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
self.prog = prog
self.x = x
self.constraints = [
# Bounding box
prog.AddLinearConstraint(x[0] >= 1),
# Bounding box
trajopt.add_quadratic_running_cost(Q, xf, [trajopt.x], name="StateCost")
# Add a final cost on the total time
# cost = lambda h: np.sum(h)
# trajopt.add_final_cost(cost, vars=[trajopt.h], name="TotalTime")
# Set the initial trajectory guess
u_init = np.zeros(trajopt.u.shape)
x_init = np.zeros(trajopt.x.shape)
for n in range(0, x_init.shape[0]):
x_init[n, :] = np.linspace(start=x0[n], stop=xf[n], num=101)
l_init = np.zeros(trajopt.l.shape)
trajopt.set_initial_guess(xtraj=x_init, utraj=u_init, ltraj=l_init)
# Get the final program, with all costs and constraints
prog = trajopt.get_program()
# Set the SNOPT solver options
prog.SetSolverOption(SnoptSolver().solver_id(), "Iterations Limit", 1e5)
prog.SetSolverOption(SnoptSolver().solver_id(), "Major Feasibility Tolerance",
1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(), "Major Optimality Tolerance",
1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(), "Scale Option", 2)
solver = SnoptSolver()
# trajopt.enable_cost_display(display='figure')
# Check the problem for bugs in the constraints
if not utils.CheckProgram(prog):
quit()
# Solve the problem
print("Solving trajectory optimization")
start = timeit.default_timer()
result = solver.Solve(prog)
trajopt.add_equal_time_constraints()
# Add control and state costs
# Add a running cost weights on the controls
R = 100 * np.ones((1, 1))
b = np.zeros((1, ))
# Add running cost weight on state
Q = 1 * np.diag([1, 1, 1, 1])
trajopt.add_quadratic_running_cost(R, b, [trajopt.u], name="ControlCost")
trajopt.add_quadratic_running_cost(Q, xf, [trajopt.x], name="StateCost")
u_init = u_init.reshape(trajopt.u.shape)
# Set the initial trajectory guess, might switch out later for a warm start trajectory
trajopt.set_initial_guess(xtraj=x_init, utraj=u_init, ltraj=l_init)
prog = trajopt.get_program()
# Set the SNOPT solver options
prog.SetSolverOption(SnoptSolver().solver_id(), "Iterations Limit", 1e7)
if i == 1:
prog.SetSolverOption(SnoptSolver().solver_id(),
"Major Feasibility Tolerance", 1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(),
"Major Optimality Tolerance", 1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(), "Scale Option", 1)
else:
prog.SetSolverOption(SnoptSolver().solver_id(),
"Major Feasibility Tolerance", 1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(),
"Major Optimality Tolerance", 1e-6)
prog.SetSolverOption(SnoptSolver().solver_id(), "Scale Option", 2)
solver = SnoptSolver()
# Solve the problem
def do_main():
rospy.init_node('run_dishrack_interaction', anonymous=False)
#np.random.seed(42)
for outer_iter in range(200):
try:
builder = DiagramBuilder()
mbp, scene_graph = AddMultibodyPlantSceneGraph(
builder, MultibodyPlant(time_step=0.0025))
# Add ground
world_body = mbp.world_body()
ground_shape = Box(2., 2., 2.)
ground_body = mbp.AddRigidBody(
"ground",
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
mbp.WeldFrames(world_body.body_frame(), ground_body.body_frame(),
RigidTransform(p=[0, 0, -1]))
mbp.RegisterVisualGeometry(ground_body, RigidTransform.Identity(),
ground_shape, "ground_vis",
np.array([0.5, 0.5, 0.5, 1.]))
mbp.RegisterCollisionGeometry(ground_body,
RigidTransform.Identity(),
ground_shape, "ground_col",
CoulombFriction(0.9, 0.8))
parser = Parser(mbp, scene_graph)
dish_bin_model = "/home/gizatt/projects/scene_generation/models/dish_models/bus_tub_01_decomp/bus_tub_01_decomp.urdf"
cupboard_model = "/home/gizatt/projects/scene_generation/models/dish_models/shelf_two_levels.sdf"
candidate_model_files = {
#"mug": "/home/gizatt/drake/manipulation/models/mug/mug.urdf",
"mug_1":
"/home/gizatt/projects/scene_generation/models/dish_models/mug_1_decomp/mug_1_decomp.urdf",
#"plate_11in": "/home/gizatt/drake/manipulation/models/dish_models/plate_11in_decomp/plate_11in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/mug_big/mug_big.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/bowl_6p25in_decomp/bowl_6p25in_decomp.urdf",
#"/home/gizatt/drake/manipulation/models/dish_models/plate_8p5in_decomp/plate_8p5in_decomp.urdf",
}
# Decide how many of each object to add
max_num_objs = 6
num_objs = [
np.random.randint(0, max_num_objs)
for k in range(len(candidate_model_files.keys()))
]
# Actually produce their initial poses + add them to the sim
poses = [] # [quat, pos]
all_object_instances = []
all_manipulable_body_ids = []
total_num_objs = sum(num_objs)
object_ordering = list(range(total_num_objs))
k = 0
random.shuffle(object_ordering)
print("ordering: ", object_ordering)
for class_k, class_entry in enumerate(
candidate_model_files.items()):
for model_instance_k in range(num_objs[class_k]):
class_name, class_path = class_entry
model_name = "%s_%d" % (class_name, model_instance_k)
all_object_instances.append([class_name, model_name])
model_id = parser.AddModelFromFile(class_path,
model_name=model_name)
all_manipulable_body_ids += mbp.GetBodyIndices(model_id)
# Put them in a randomly ordered line, for placing
y_offset = (object_ordering[k] / float(total_num_objs) -
0.5) # RAnge -0.5 to 0.5
poses.append([
RollPitchYaw(np.random.uniform(
0., 2. * np.pi, size=3)).ToQuaternion().wxyz(),
[-0.25, y_offset, 0.1]
])
k += 1
#$poses.append([
# RollPitchYaw(np.random.uniform(0., 2.*np.pi, size=3)).ToQuaternion().wxyz(),
# [np.random.uniform(-0.2, 0.2), np.random.uniform(-0.1, 0.1), np.random.uniform(0.1, 0.3)]])
# Build a desk
parser.AddModelFromFile(cupboard_model)
mbp.WeldFrames(world_body.body_frame(),
mbp.GetBodyByName("shelf_origin_body").body_frame(),
RigidTransform(p=[0.0, 0, 0.0]))
#parser.AddModelFromFile(dish_bin_model)
#mbp.WeldFrames(world_body.body_frame(), mbp.GetBodyByName("bus_tub_01_decomp_body_link").body_frame(),
# RigidTransform(p=[0.0, 0., 0.], rpy=RollPitchYaw(np.pi/2., 0., 0.)))
mbp.AddForceElement(UniformGravityFieldElement())
mbp.Finalize()
hydra_sg_spy = builder.AddSystem(
HydraInteractionLeafSystem(
mbp,
scene_graph,
all_manipulable_body_ids=all_manipulable_body_ids))
#builder.Connect(scene_graph.get_query_output_port(),
# hydra_sg_spy.get_input_port(0))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
hydra_sg_spy.get_input_port(0))
builder.Connect(mbp.get_state_output_port(),
hydra_sg_spy.get_input_port(1))
builder.Connect(hydra_sg_spy.get_output_port(0),
mbp.get_applied_spatial_force_input_port())
visualizer = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url="tcp://127.0.0.1:6000",
draw_period=0.01))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
visualizer.get_input_port(0))
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(
mbp, diagram_context)
sg_context = diagram.GetMutableSubsystemContext(
scene_graph, diagram_context)
q0 = mbp.GetPositions(mbp_context).copy()
for k in range(len(poses)):
offset = k * 7
q0[(offset):(offset + 4)] = poses[k][0]
q0[(offset + 4):(offset + 7)] = poses[k][1]
mbp.SetPositions(mbp_context, q0)
simulator = Simulator(diagram, diagram_context)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(False)
simulator.Initialize()
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
def squaredNorm(x):
return np.array([x[0]**2 + x[1]**2 + x[2]**2 + x[3]**2])
for k in range(len(poses)):
# Quaternion norm
prog.AddConstraint(squaredNorm, [1], [1],
q_dec[(k * 7):(k * 7 + 4)])
# Trivial quaternion bounds
prog.AddBoundingBoxConstraint(-np.ones(4), np.ones(4),
q_dec[(k * 7):(k * 7 + 4)])
# Conservative bounds on on XYZ
prog.AddBoundingBoxConstraint(np.array([-2., -2., -2.]),
np.array([2., 2., 2.]),
q_dec[(k * 7 + 4):(k * 7 + 7)])
def vis_callback(x):
vis_diagram_context = diagram.CreateDefaultContext()
mbp.SetPositions(
diagram.GetMutableSubsystemContext(mbp,
vis_diagram_context), x)
pose_bundle = scene_graph.get_pose_bundle_output_port().Eval(
diagram.GetMutableSubsystemContext(scene_graph,
vis_diagram_context))
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(pose_bundle))
visualizer.Publish(context)
prog.AddVisualizationCallback(vis_callback, q_dec)
prog.AddQuadraticErrorCost(np.eye(q0.shape[0]) * 1.0, q0, q_dec)
ik.AddMinimumDistanceConstraint(0.001, threshold_distance=1.0)
prog.SetInitialGuess(q_dec, q0)
print("Solving")
# print "Initial guess: ", q0
start_time = time.time()
solver = SnoptSolver()
#solver = NloptSolver()
sid = solver.solver_type()
# SNOPT
prog.SetSolverOption(sid, "Print file", "test.snopt")
prog.SetSolverOption(sid, "Major feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Major optimality tolerance", 1e-2)
prog.SetSolverOption(sid, "Minor feasibility tolerance", 1e-3)
prog.SetSolverOption(sid, "Scale option", 0)
#prog.SetSolverOption(sid, "Elastic weight", 1e1)
#prog.SetSolverOption(sid, "Elastic mode", "Yes")
# NLOPT
#prog.SetSolverOption(sid, "initial_step", 0.1)
#prog.SetSolverOption(sid, "xtol_rel", 1E-2)
#prog.SetSolverOption(sid, "xtol_abs", 1E-2)
#prog.SetSolverOption(sid, "Major step limit", 2)
print("Solver opts: ", prog.GetSolverOptions(solver.solver_type()))
result = mp.Solve(prog)
print("Solve info: ", result)
print("Solved in %f seconds" % (time.time() - start_time))
#print(IpoptSolver().Solve(prog))
print(result.get_solver_id().name())
q0_proj = result.GetSolution(q_dec)
# print "Final: ", q0_proj
mbp.SetPositions(mbp_context, q0_proj)
simulator.StepTo(1000)
raise StopIteration()
except StopIteration:
print(colored("Stopped, saving and restarting", 'yellow'))
qf = mbp.GetPositions(mbp_context)
# Decide whether to accept: all objs within bounds
save = True
for k in range(len(all_object_instances)):
offset = k * 7
q = qf[offset:(offset + 7)]
if q[4] <= -0.25 or q[4] >= 0.25 or q[5] <= -0.2 or q[
5] >= 0.2 or q[6] <= -0.1:
save = False
break
if save:
print(colored("Saving", "green"))
save_config(all_object_instances, qf,
"mug_rack_environments_human.yaml")
else:
print(
colored("Not saving due to bounds violation: " + str(q),
"yellow"))
except Exception as e:
print(colored("Suffered other exception " + str(e), "red"))
sys.exit(-1)
except:
print(
colored("Suffered totally unknown exception! Probably sim.",
"red"))
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
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
