def test_gurobi_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] >= 1)
prog.AddLinearConstraint(x[1] >= 1)
prog.AddQuadraticCost(np.eye(2), np.zeros(2), x)
solver = GurobiSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.solver_type(), mp.SolverType.kGurobi)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
def test_gurobi_solver(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
prog.AddLinearConstraint(x[0] >= 1)
prog.AddLinearConstraint(x[1] >= 1)
prog.AddQuadraticCost(np.eye(2), np.zeros(2), x)
solver = GurobiSolver()
self.assertTrue(solver.available())
self.assertEqual(solver.solver_type(), mp.SolverType.kGurobi)
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
x_expected = np.array([1, 1])
self.assertTrue(np.allclose(result.GetSolution(x), x_expected))
self.assertGreater(result.get_solver_details().optimizer_time, 0.)
self.assertEqual(result.get_solver_details().error_code, 0)
self.assertEqual(result.get_solver_details().optimization_status, 2)
self.assertTrue(np.isnan(result.get_solver_details().objective_bound))
def test_api(self):
plant = MultibodyPlant(time_step=0.01)
model_instance = Parser(plant).AddModelFromFile(FindResourceOrThrow(
"drake/bindings/pydrake/multibody/test/two_bodies.sdf"))
plant.Finalize()
context = plant.CreateDefaultContext()
options = ik.GlobalInverseKinematics.Options()
global_ik = ik.GlobalInverseKinematics(plant=plant, options=options)
self.assertIsInstance(global_ik.prog(), mp.MathematicalProgram)
self.assertIsInstance(global_ik.get_mutable_prog(),
mp.MathematicalProgram)
body_index_A = plant.GetBodyIndices(model_instance)[0]
body_index_B = plant.GetBodyIndices(model_instance)[1]
self.assertEqual(
global_ik.body_rotation_matrix(body_index=body_index_A).shape,
(3, 3))
self.assertEqual(
global_ik.body_position(body_index=body_index_A).shape, (3, ))
global_ik.AddWorldPositionConstraint(
body_index=body_index_A,
p_BQ=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldRelativePositionConstraint(
body_index_B=body_index_B,
p_BQ=[0, 0, 0],
body_index_A=body_index_A,
p_AP=[0, 0, 0],
box_lb_F=[-np.inf, -np.inf, -np.inf],
box_ub_F=[np.inf, np.inf, np.inf],
X_WF=RigidTransform())
global_ik.AddWorldOrientationConstraint(
body_index=body_index_A,
desired_orientation=Quaternion(),
angle_tol=np.inf)
global_ik.AddPostureCost(
q_desired=plant.GetPositions(context),
body_position_cost=[1] * plant.num_bodies(),
body_orientation_cost=[1] * plant.num_bodies())
gurobi_solver = GurobiSolver()
if gurobi_solver.available():
global_ik.SetInitialGuess(q=plant.GetPositions(context))
result = gurobi_solver.Solve(global_ik.prog())
self.assertTrue(result.is_success())
global_ik.ReconstructGeneralizedPositionSolution(result=result)
def solveProgram(self):
if (not self.upToDate):
prog = mp.MathematicalProgram()
M = 100
# SET UP FOOTSTEP VARIABLES
f = []
for fNum in range(FootstepPlanner.MAXFOOTSTEPS):
fnR = prog.NewContinuousVariables(self.dim, "fr" +
str(fNum)) # Right Footstep
fnL = prog.NewContinuousVariables(self.dim, "fl" +
str(fNum)) # Left Footstep
f.append(fnR)
f.append(fnL)
n = prog.NewBinaryVariables(
2 * FootstepPlanner.MAXFOOTSTEPS,
"n") # binary variables for nominal regions
# CONSTRAIN WITH REACHABLE REGIONS
# Start position
prog.AddLinearConstraint(f[0][0] == self.startR[0])
prog.AddLinearConstraint(f[0][1] == self.startR[1])
prog.AddLinearConstraint(f[1][0] == self.startL[0])
prog.AddLinearConstraint(f[1][1] == self.startL[1])
# All other footsteps
for fNum in range(1, FootstepPlanner.MAXFOOTSTEPS):
for i in range(self.reachableA.shape[0]):
# Constrain footsteps
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum][0] - f[2 * fNum - 1][0]) +
self.reachableA[i][1] *
(f[2 * fNum][1] - (f[2 * fNum - 1][1] - self.yOffset))
<= self.reachableb[i]
) # Right Footstep (2*fNum) to previous left (2*fNum-1)
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum + 1][0] - f[2 * fNum][0]) +
self.reachableA[i][1] *
(f[2 * fNum + 1][1] -
(f[2 * fNum][1] + self.yOffset)) <= self.reachableb[i]
) # Left Footstep (2*fNum+1) to previous right (2*fNum)
if (self.hasNominal): # Nominal Regions
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum][0] - f[2 * fNum - 1][0]) +
self.reachableA[i][1] *
(f[2 * fNum][1] -
(f[2 * fNum - 1][1] - self.yOffset)) +
n[2 * fNum] * M <=
self.nominal * self.reachableb[i] + M
) # Right Footstep (2*fNum) to previous left (2*fNum-1)
prog.AddLinearConstraint(
self.reachableA[i][0] *
(f[2 * fNum + 1][0] - f[2 * fNum][0]) +
self.reachableA[i][1] *
(f[2 * fNum + 1][1] -
(f[2 * fNum][1] + self.yOffset)) +
n[2 * fNum + 1] * M <=
self.nominal * self.reachableb[i] + M
) # Left Footstep (2*fNum+1) to previous right (2*fNum)
# CONSTRAIN TO OBSTACLE FREE REGIONS
if (self.hasObstacleFree):
H = []
for fNum in range(1, FootstepPlanner.MAXFOOTSTEPS):
hnR = prog.NewBinaryVariables(self.num_regions, "hr" +
str(fNum)) # Right Footstep
hnL = prog.NewBinaryVariables(self.num_regions, "hl" +
str(fNum)) # Left Footstep
# Constrain each footstep to exactly one convex region
prog.AddLinearConstraint(
np.sum(hnR) == 1) # only one is set
prog.AddLinearConstraint(
np.sum(hnL) == 1) # only one is set
H.append(hnR)
H.append(hnL)
# Constrain the footsteps to the regions
for fNum in range(1, FootstepPlanner.MAXFOOTSTEPS - 1):
for i in range(self.num_regions):
for j in range(self.oA[i].shape[0]):
prog.AddLinearConstraint(
self.oA[i][j][0] * f[2 * fNum][0] +
self.oA[i][j][1] * f[2 * fNum][1] +
M * H[2 * fNum][i] <=
self.ob[i][j] + M) # Right footstep constraint
prog.AddLinearConstraint(
self.oA[i][j][0] * f[2 * fNum + 1][0] +
self.oA[i][j][1] * f[2 * fNum + 1][1] +
M * H[2 * fNum + 1][i] <=
self.ob[i][j] + M) # Left footstep constraint
# OPTIMAL NUMBER OF FOOTSTEPS
z = prog.NewBinaryVariables(FootstepPlanner.MAXFOOTSTEPS, "z")
for fNum in range(FootstepPlanner.MAXFOOTSTEPS - 1):
prog.AddLinearConstraint(z[fNum] <= z[fNum + 1])
prog.AddLinearConstraint(z[FootstepPlanner.MAXFOOTSTEPS - 1] -
z[0] == 1)
for fNum in range(
FootstepPlanner.MAXFOOTSTEPS
): # if z[i], then the ith footstep is the same as the final footstep
prog.AddLinearConstraint(
f[2 * fNum][0] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][0] +
M)
prog.AddLinearConstraint(
f[2 * fNum][1] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][1] +
M)
prog.AddLinearConstraint(
-f[2 * fNum][0] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][0] + M)
prog.AddLinearConstraint(
-f[2 * fNum][1] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][1] + M)
prog.AddLinearConstraint(
f[2 * fNum + 1][0] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][0] +
M)
prog.AddLinearConstraint(
f[2 * fNum + 1][1] +
M * z[fNum] <= f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][1] +
M)
prog.AddLinearConstraint(
-f[2 * fNum + 1][0] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][0] + M)
prog.AddLinearConstraint(
-f[2 * fNum + 1][1] + M *
z[fNum] <= -f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][1] + M)
# ADD COSTS
# Cost of consecutive footsteps (with nominal regions considered)
for fNum in range(1, 2 * FootstepPlanner.MAXFOOTSTEPS - 1):
prog.AddQuadraticCost(((f[fNum][0] - f[fNum + 1][0])**2 +
(f[fNum][1] - f[fNum + 1][1])**2) + 2 *
(1 - n[fNum]))
# Cost of number of footsteps
prog.AddLinearCost(-np.sum(z) * 5)
# Cost of distance of final position to goal
prog.AddQuadraticCost(
1 *
((f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][0] - self.goal[0])**2
+ (f[2 * FootstepPlanner.MAXFOOTSTEPS - 1][1] - self.goal[1])
**2))
prog.AddQuadraticCost(
1 *
((f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][0] - self.goal[0])**2
+ (f[2 * FootstepPlanner.MAXFOOTSTEPS - 2][1] - self.goal[1])
**2))
# SOLVE PROBLEM
solver = GurobiSolver()
assert (solver.available())
assert (solver.solver_type() == mp.SolverType.kGurobi)
result = solver.Solve(prog)
assert (result == mp.SolutionResult.kSolutionFound)
# SAVE SOLUTION
self.footsteps = []
self.numFootsteps = 0
for fNum in range(FootstepPlanner.MAXFOOTSTEPS):
self.numFootsteps += 1
self.footsteps.append(prog.GetSolution(f[2 * fNum]))
self.footsteps.append(prog.GetSolution(f[2 * fNum + 1]))
if (prog.GetSolution(z[fNum]) == 1):
break
self.footsteps = np.array(self.footsteps)
self.upToDate = True
prog.AddLinearConstraint(np.sum(z) == 1) # only one is set
# Create M (TODO: calculate this value)
M = 100
# Constrain the points to the regions
for i in range(num_regions):
for j in range(A[i].shape[0]):
prog.AddLinearConstraint(A[i][j][0]*x[0]+A[i][j][1]*x[1] + M*z[i] <= b[i][j] + M)
# Add objective
prog.AddQuadraticCost((x[0]-x_goal[0])**2 + (x[1]-x_goal[1])**2) # distance of x to the goal point
# Solve problem
solver = GurobiSolver()
assert(solver.available())
assert(solver.solver_type()==mp.SolverType.kGurobi)
result = solver.Solve(prog)
assert(result == mp.SolutionResult.kSolutionFound)
print("Goal: " + str(x_goal))
finalx = prog.GetSolution(x)
print("Final Solution: " + str(finalx))
# ********* GRAPH PROBLEM *********
# Create figure
fig = plt.figure(1, (20, 10))
plt.title("Minimize distance of point within " + str(num_regions) + " " + str(dim) + "-D Polytopes to Goal Point")
# Plot regions
for j in range(num_regions):
print("Region " + str(j))
