def linear_program_drake(f, A, b, C=None, d=None, x_bound=None, solver='gurobi', **kwargs):
"""
Solves the linear program
minimize f^T * x
s. t. A * x <= b
C * x = d
INPUTS:
f: gradient of the cost function (2D numpy array)
A: left hand side of the constraints (2D numpy array)
b: right hand side of the constraints (2D numpy array)
C: left hand side of the equalities (2D numpy array)
d: right hand side of the equalities (2D numpy array)
OUTPUTS:
x_min: argument which minimizes the cost (its elements are nan if unfeasible or unbounded)
cost_min: minimum of the cost function (nan if unfeasible or unbounded)
"""
# program dimensions
n_variables = f.shape[0]
n_constraints = A.shape[0]
# build program
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(n_variables, "x")
for i in range(0, n_constraints):
prog.AddLinearConstraint((A[i,:] + 1e-15).dot(x) <= b[i])
if C is not None and d is not None:
for i in range(C.shape[0]):
prog.AddLinearConstraint(C[i, :].dot(x) == d[i])
prog.AddLinearCost((f.flatten() + 1e-15).dot(x))
# options
if solver == 'gurobi':
for (key, value) in kwargs.items():
prog.SetSolverOption(mp.SolverType.kGurobi, key, value)
# set bounds to the solution
if x_bound is not None:
for i in range(0, n_variables):
prog.AddLinearConstraint(x[i] <= x_bound)
prog.AddLinearConstraint(x[i] >= -x_bound)
# solve
if solver == 'gurobi':
solver = GurobiSolver()
elif solver == 'mosek':
solver = MosekSolver()
result = solver.Solve(prog)
x_min = np.reshape(prog.GetSolution(x), (n_variables,1))
cost_min = f.T.dot(x_min)
return [x_min, cost_min]
def test_write_to_file(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2)
prog.AddLinearConstraint(x[0] + x[1] == 1)
prog.AddQuadraticCost(x[0] * x[0] + x[1] * x[1])
solver = GurobiSolver()
file_name = temp_directory() + "/gurobi.mps"
options = mp.SolverOptions()
options.SetOption(solver.id(), "GRBwrite", file_name)
result = solver.Solve(prog, None, options)
self.assertTrue(os.path.exists(file_name))
def test_compute_iis(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2)
prog.AddBoundingBoxConstraint(1, np.inf, x)
prog.AddLinearConstraint(x[0] + x[1] == 1)
solver = GurobiSolver()
ilp_file_name = temp_directory() + "/gurobi.ilp"
options = mp.SolverOptions()
options.SetOption(solver.id(), "GRBwrite", ilp_file_name)
options.SetOption(solver.id(), "GRBcomputeIIS", 1)
result = solver.Solve(prog, None, options)
self.assertTrue(os.path.exists(ilp_file_name))
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_socp_dual(self):
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
constraint = prog.AddLorentzConeConstraint(
[2., 2 * x[0], 3 * x[1] + 1])
prog.AddLinearCost(x[1])
solver = GurobiSolver()
options = mp.SolverOptions()
options.SetOption(solver.solver_id(), "QCPDual", 1)
result = solver.Solve(prog, None, options)
np.testing.assert_allclose(result.GetDualSolution(constraint),
np.array([-1. / 12]),
atol=1e-7)
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 test_mixed_integer_optimization(self):
prog = mp.MathematicalProgram()
x = prog.NewBinaryVariables(3, "x")
c = np.array([-1.0, -1.0, -2.0])
prog.AddLinearCost(c.dot(x))
a = np.array([1.0, 2.0, 3.0])
prog.AddLinearConstraint(a.dot(x) <= 4)
prog.AddLinearConstraint(x[0] + x[1], 1, np.inf)
solver = GurobiSolver()
result = solver.Solve(prog, None, None)
self.assertTrue(result.is_success())
# Test that we got the right solution for all x
x_expected = np.array([1.0, 0.0, 1.0])
self.assertTrue(np.all(np.isclose(result.GetSolution(x), x_expected)))
# Also test by asking for the value of each element of x
for i in range(3):
self.assertAlmostEqual(result.GetSolution(x[i]), x_expected[i])
def solve(self):
solver = GurobiSolver()
self.prog.SetSolverOption(mp.SolverType.kGurobi, "LogToConsole", 1)
self.prog.SetSolverOption(mp.SolverType.kGurobi, "OutputFlag", 1)
start_time = time.time()
result = solver.Solve(self.prog)
solve_time = time.time() - start_time
assert result == mp.SolutionResult.kSolutionFound
states, inputs, contact = self.prog.extract_solution(
self.robot, self.vars.qcom, self.vars.qlimb, self.vars.contact,
self.vars.contact_force, self.vars.contact_lambda,
self.vars.contact_sequence_array)
ts = states.components[0].breaks
return SolutionData(opt=self,
states=states,
inputs=inputs,
contact_indicator=contact,
ts=ts,
solve_time=solve_time)
def test_callback(self):
prog = mp.MathematicalProgram()
b = prog.NewBinaryVariables(4)
prog.AddLinearConstraint(b[0] <= 1 - 0.5 * b[1])
prog.AddLinearConstraint(b[1] <= 1 - 0.5 * b[0])
prog.AddLinearCost(-b[0] - b[1])
prog.SetSolverOption(GurobiSolver.id(), "Presolve", 0)
prog.SetSolverOption(GurobiSolver.id(), "Heuristics", 0.)
prog.SetSolverOption(GurobiSolver.id(), "Cuts", 0)
prog.SetSolverOption(GurobiSolver.id(), "NodeMethod", 2)
b_init = np.array([0, 0., 0., 0.])
prog.SetInitialGuess(b, b_init)
solver = GurobiSolver()
explored_node_count = 0
def node_callback(prog, solver_status_info, x, x_vals):
nonlocal explored_node_count
explored_node_count = solver_status_info.explored_node_count
solver.AddMipNodeCallback(
callback=lambda prog, solver_status_info, x, x_vals: node_callback(
prog, solver_status_info, x, x_vals))
best_objectives = []
def sol_callback(prog, callback_info, objectives):
print(f"explored nodes {callback_info.explored_node_count}")
objectives.append(callback_info.best_objective)
solver.AddMipSolCallback(
callback=lambda prog, callback_info: sol_callback(
prog, callback_info, best_objectives))
result = solver.Solve(prog)
self.assertTrue(result.is_success())
self.assertGreater(explored_node_count, 0)
self.assertGreater(len(best_objectives), 0)
def quadratic_program(H, f, A, b, C=None, d=None):
"""
Solves the convex (i.e., H > 0) quadratic program
minimize x^T * H * x + f^T * x
s. t. A * x <= b
C * x = d
INPUTS:
H: Hessian of the cost function (2D numpy array)
f: linear term of the cost function (2D numpy array)
A: left hand side of the inequalities (2D numpy array)
b: right hand side of the inequalities (2D numpy array)
C: left hand side of the equalities (2D numpy array)
d: right hand side of the equalities (2D numpy array)
OUTPUTS:
x_min: argument which minimizes the cost (its elements are nan if unfeasible or unbounded)
cost_min: minimum of the cost function (nan if unfeasible or unbounded)
"""
# program dimensions
n_variables = f.shape[0]
n_constraints = A.shape[0]
# build program
prog = mp.MathematicalProgram()
x = prog.NewContinuousVariables(n_variables, "x")
for i in range(0, n_constraints):
prog.AddLinearConstraint((A[i,:] + 1e-15).dot(x) <= b[i])
if C is not None and d is not None:
for i in range(C.shape[0]):
prog.AddLinearConstraint(C[i, :].dot(x) == d[i])
prog.AddQuadraticCost(H, f, x)
# solve
solver = GurobiSolver()
result = solver.Solve(prog)
x_min = np.reshape(prog.GetSolution(x), (n_variables,1))
cost_min = .5*x_min.T.dot(H.dot(x_min)) + f.T.dot(x_min)
return [x_min, cost_min]
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
class SlamBackend:
def __init__(self, frontend: SlamFrontend):
"""
Indices (consistent with Indelman2015, aka "the paper".)
Constants:
k: current time step.
L: planning horizon.
i: time step.
j: landmark.
l \in [k, k+L]: one of the future time steps up to L.
:param X_WB_e_0: np array of shape (self.dim_X). Initial estimated
robot position, used to anchor the entire trajectory.
"""
self.solver = GurobiSolver()
self.X_WB_e_0 = np.zeros(2)
self.dim_l = 2 # dimension of landmarks.
self.dim_X = 2 # dimension of poses.
self.dim_measurements = 2
# current beliefs
self.X_WB_e_dict = {0: self.X_WB_e_0}
self.X_WB_e_var_dict = {}
self.l_xy_e_dict = dict()
# Information matrix of current robot poses and landmarks, which are
# ordered as follows:
# [X_WB_e0, ... X_WB_e_k, l_xy_e0, ... l_xy_eK]
# The landmarks are ordered as self.l_xy_e_dict.keys().
self.I_all = None
# past measurements
self.odometry_measurements = dict()
# key (int): landmark index
# value: dict of {t: {"distance":d, "bearing": b}}.
# t: index of robot poses from which the landmark is visible.
# d: range measurement for the landmark from the pose indexed by t.
# b: bearing measurement for the landmark from the pose indexed by t.
self.landmark_measurements = dict()
# Taking stuff from frontend
self.frontend = frontend
self.vis = frontend.vis
self.l_xy = frontend.l_xy
# TODO: load from config file?
self.sigma_odometry = frontend.sigma_odometry
self.sigma_range = frontend.sigma_range
self.sigma_bearing = frontend.sigma_bearing
self.sigma_dynamics = self.sigma_odometry
self.r_range_max = frontend.r_range_max
self.r_range_min = frontend.r_range_min
# weights/selection matrices for the cost function.
self.M_u = np.ones(self.dim_l) * 0.1
self.beta = 0.15
self.alpha_LB = 0.6
self.is_uncertainty_reduction_only = False
def update_landmark_measurements(self, t, idx_visible_l_list,
d_l_measured_list, b_l_measured_list):
"""
inputs are the output of Frontend.get_range_measurements()
:return: None.
"""
for i, i_l in enumerate(idx_visible_l_list):
d_l = d_l_measured_list[i]
b_l = b_l_measured_list[i]
if i_l not in self.landmark_measurements:
self.landmark_measurements[i_l] = dict()
self.landmark_measurements[i_l][t] = \
{"distance": d_l, "bearing": b_l}
def update_odometry_measruements(self, t, odometry_measurement):
"""
:param t: current time step (starting from 0).
:param odometry_measurement: odometry measruement between t and t-1.
:return:
"""
self.odometry_measurements[t] = odometry_measurement
def get_X_WB_belief(self):
n = len(self.X_WB_e_dict)
X_WB_e = np.zeros((n, self.dim_X))
for i in range(n):
X_WB_e[i] = self.X_WB_e_dict[i]
return X_WB_e
def get_l_xy_belief(self):
n = len(self.l_xy_e_dict)
l_xy_e = np.zeros((n, self.dim_l))
for i, k in enumerate(self.l_xy_e_dict.keys()):
l_xy_e[i] = self.l_xy_e_dict[k]
return l_xy_e
def get_odometry_measurements(self):
n_o = len(self.odometry_measurements)
odometry_measurements = np.zeros((n_o, self.dim_X))
for i in range(n_o):
odometry_measurements[i] = self.odometry_measurements[i + 1]
return odometry_measurements
def get_X_WB_initial_guess_array(self):
"""
Use self.t_xy_e_dict to create an initial guess for the next bundle
adjustment.
Let t be the current exploration time step. As t_xy_e_dict is the
estimate for time step t-1, its shape is (t - 1, 3), which does
not include an estimate for the pose of the current time step.
The initial guess for the current time step is computed as
self.t_xy_e_dict[t - 1] + self.odometry_measurements[t].
:return:
"""
n_o = len(self.odometry_measurements)
X_WB_e = np.zeros((n_o + 1, self.dim_X))
for i in range(n_o):
X_WB_e[i] = self.X_WB_e_dict[i]
if n_o > 0:
X_WB_e[n_o] = X_WB_e[n_o - 1] + self.odometry_measurements[n_o]
# First ground truth is given.
X_WB_e[0] = self.X_WB_e_0
return X_WB_e
def get_l_xy_initial_guess_array(self):
nl = len(self.landmark_measurements)
l_xy_e = np.ones((nl, 2))
for i_k, k in enumerate(self.landmark_measurements.keys()):
if k in self.l_xy_e_dict:
l_xy_e[i_k] = self.l_xy_e_dict[k]
return l_xy_e
def marginalize_info_matrix(self, Omega, n_p, n_l):
nX = n_p * self.dim_X
m, n = Omega.shape
assert m == n
assert m == nX + n_l * self.dim_l
A = Omega[:nX, :nX]
B = Omega[:nX, nX:]
C = Omega[nX:, nX:]
return A - B.dot(np.linalg.inv(C)).dot(B.T)
def calc_range_derivatives(self,
J,
i_row,
j_start_x,
j_start_l,
d_xy,
sigma=1.0):
"""
:param i_row:
:param j_start_x:
:param j_start_l:
:param d_xy: X_WB - l_xy
:return:
"""
if torch.is_tensor(J):
norm = torch.norm
else:
norm = np.linalg.norm
d = norm(d_xy)
J[i_row, j_start_x:j_start_x + 2] = d_xy / (d * sigma)
J[i_row, j_start_l:j_start_l + self.dim_l] = -d_xy / (d * sigma)
return d
def calc_bearing_derivatives(self,
J,
i_row,
j_start_x,
j_start_l,
X_WB,
l_xy,
sigma=1.0):
xb = X_WB[0]
yb = X_WB[1]
xl = l_xy[0]
yl = l_xy[1]
dx = xl - xb
dy = yl - yb
d_arctan_D_dx = 1 / (1 + (dy / dx)**2) * (-dy / dx**2)
d_arctan_D_dy = 1 / (1 + (dy / dx)**2) * (1 / dx)
J[i_row, j_start_x] = -d_arctan_D_dx / sigma
J[i_row, j_start_x + 1] = -d_arctan_D_dy / sigma
J[i_row, j_start_l] = d_arctan_D_dx / sigma
J[i_row, j_start_l + 1] = d_arctan_D_dy / sigma
return dx, dy
@staticmethod
def calc_num_landmark_measurements(landmark_measurements):
nl_measurements = 0 # number of landmark measurements
for visible_landmarks_i in landmark_measurements.values():
nl_measurements += len(visible_landmarks_i)
return nl_measurements
def calc_jacobian_and_b(self, X_WB_e, l_xy_e):
"""
:param X_WB_e (n_o + 1, 3)
:param l_xy_e (nl, 2): coordinates of landmarks, ordered the same way as
self.landmark_measurements.
:param landmark_measurements:
:return:
"""
dim_l = self.dim_l # dimension of landmarks.
dim_X = self.dim_X # dimension of poses.
nl = len(self.landmark_measurements) # number of landmarks
n_o = len(self.odometry_measurements) # number of odometries
# number of landmark measurements
nl_measurements = self.calc_num_landmark_measurements(
self.landmark_measurements)
n_rows = (n_o + 1) * dim_X + nl_measurements * self.dim_measurements
n_cols = (n_o + 1) * dim_X + nl * dim_l
J = np.zeros((n_rows, n_cols))
b = np.zeros(n_rows)
sigmas = np.zeros(n_rows)
# prior on first pose.
J[-self.dim_X:, :self.dim_X] = np.eye(self.dim_X)
sigmas[-self.dim_X:] = (self.sigma_odometry / 10)
# odometry
for i in range(n_o):
i0 = i * dim_X
i1 = i0 + dim_X
J[i0:i1, i0:i1] = -np.eye(dim_X)
J[i0:i1, i0 + dim_X:i1 + dim_X] = np.eye(dim_X)
bi = b[i0:i1]
sigmas_i = sigmas[i0:i1]
# displacement
bi[:2] = X_WB_e[i + 1, :2] - X_WB_e[i, :2] - \
self.odometry_measurements[i + 1][:2]
sigmas_i[:2] = self.sigma_odometry
# range and bearing measurements.
i_row = n_o * dim_X
for idx_j, (j, visible_robot_poses) in enumerate(
self.landmark_measurements.items()):
for i in visible_robot_poses.keys():
# i: index of robot poses visible from
d_ik_m = visible_robot_poses[i]["distance"]
b_ik_m = visible_robot_poses[i]["bearing"]
# range.
d_xy = X_WB_e[i, :2] - l_xy_e[idx_j]
j_start_x = i * dim_X
j_start_l = (n_o + 1) * dim_X + idx_j * dim_l
d = self.calc_range_derivatives(J, i_row, j_start_x, j_start_l,
d_xy)
b[i_row] = d - d_ik_m
sigmas[i_row] = self.sigma_range
# bearing.
i_row += 1
dx, dy = self.calc_bearing_derivatives(J, i_row, j_start_x,
j_start_l, X_WB_e[i],
l_xy_e[idx_j])
b[i_row] = calc_angle_diff(b_ik_m, np.arctan2(dy, dx))
sigmas[i_row] = self.sigma_bearing
i_row += 1
return J, b, sigmas
def calc_info_matrix(self, X_WB_e, l_xy_e):
J, b, sigmas = self.calc_jacobian_and_b(X_WB_e, l_xy_e)
Omega = 1 / sigmas**2
I = (J.T * Omega).dot(J)
q = J.T.dot(Omega * b)
c = (b * Omega).dot(b)
return I, q, c
def calc_A_lower_half(self, dX_WB, l_xy_e):
"""
Algorithm 4 in the paper.
:param dX_WB: [l, self.dim_X]. Input for time steps [l : ].
:return:
"""
# X_WB_e: belief. e stands for "estimated".
# X_WB_p: prediction. corresponds to quantities with an overbar.
n_p = len(self.X_WB_e_dict)
l = len(dX_WB)
# future predictions.
X_WB_p = self.calc_pose_predictions(dX_WB)
# Find the visible landmarks for every X_WB_p[i].
# Store in {i: [landmark order indices.]}, i.e. the same order as
# self.landmark_measurements.
# future_landmark_measurements = self.find_visible_landmarks(X_WB_p)
nl = len(self.landmark_measurements) # number of landmarks
# number of new landmark measurements
nl_measurements = l * nl
# Structure of new "state"
# [X_WB_0, ... X_WB_{k},
# l_xy (all landmarks),
# X_WB_{k+1}, ... X_WB_{k+l}]
n_Xk = n_p * self.dim_X + nl * self.dim_l
n_rows_F = l * self.dim_X
n_rows_H = 2 * nl_measurements
n_cols = l * self.dim_X + n_Xk
# F_whitened is devided by sigma_w.
if torch.is_tensor(dX_WB):
F_whitened = torch.zeros((n_rows_F, n_cols))
# H is the original Jacobian, before divided by sigma_v.
H = torch.zeros((n_rows_H, n_cols))
# as defined in Eq. (33)
Omega_v_bar_sqrt = torch.zeros(n_rows_H)
Omega_v_sqrt = torch.zeros(n_rows_H)
I_X = torch.eye(self.dim_X)
l_xy_e = torch.from_numpy(l_xy_e)
norm = torch.norm
vstack = torch.vstack
else:
F_whitened = np.zeros((n_rows_F, n_cols), dtype=dX_WB.dtype)
H = np.zeros((n_rows_H, n_cols), dtype=dX_WB.dtype)
Omega_v_bar_sqrt = np.zeros(n_rows_H)
Omega_v_sqrt = np.zeros(n_rows_H)
I_X = np.eye(self.dim_X)
norm = np.linalg.norm
vstack = np.vstack
# dynamics, F_whitened.
for i in range(l):
i0 = i * self.dim_X
i1 = i0 + self.dim_X
F_whitened[i0:i1, n_Xk + i0:n_Xk + i1] = I_X / self.sigma_dynamics
if i == 0:
j0 = (n_p - 1) * self.dim_X
j1 = n_p * self.dim_X
F_whitened[i0: i1, j0: j1] = \
-I_X / self.sigma_dynamics
else:
j0 = n_Xk + i0 - self.dim_X
j1 = n_Xk + i1 - self.dim_X
F_whitened[i0: i1, j0: j1] = \
-I_X / self.sigma_dynamics
# observations, H.
i_row = 0
for idx_j in range(nl):
for i in range(l):
d_xy = X_WB_p[i] - l_xy_e[idx_j]
j_start_x = n_Xk + i * self.dim_X
j_start_l = n_p * self.dim_X + idx_j * self.dim_l
d = self.calc_range_derivatives(H,
i_row,
j_start_x,
j_start_l,
d_xy,
sigma=1)
self.calc_bearing_derivatives(H,
i_row + 1,
j_start_x,
j_start_l,
X_WB_p[i],
l_xy_e[idx_j],
sigma=1)
if d > 5 * self.r_range_max:
p_visible = 0.
else:
p_visible = 1.
Omega_v_bar_sqrt[i_row] = p_visible / self.sigma_range
Omega_v_bar_sqrt[i_row + 1] = p_visible / self.sigma_bearing
Omega_v_sqrt[i_row] = 1 / self.sigma_range
Omega_v_sqrt[i_row + 1] = 1 / self.sigma_bearing
i_row += 2
# A2 is the lower half of \mathcal{A} in Eq. (32),
# i.e. [F_whitened; H / Omega_v_sqrt].
A2 = vstack([F_whitened, (H.T * Omega_v_bar_sqrt).T])
return {
"A2": A2,
"X_WB_p": X_WB_p,
"H": H,
"F_whitened": F_whitened,
"Omega_v_bar_sqrt": Omega_v_bar_sqrt,
"Omega_v_sqrt": Omega_v_sqrt
}
def calc_inner_layer(self, dX_WB, l_xy_e, I_Xk):
"""
Algorithm 4 in the paper.
:param dX_WB:
:param X_WB_e:
:param l_xy_e:
:param I_Xk: information matrix of current trajetory and landmarks.
:return:
"""
if torch.is_tensor(dX_WB):
I_Xk = torch.from_numpy(I_Xk)
n_Xk = I_Xk.shape[0] # size of states up to now.
def calc_I(A2, dtype):
n_Xl = A2.shape[1] - n_Xk # size of states into the future.
n_X = n_Xk + n_Xl
A21 = A2[:, :n_Xk]
A22 = A2[:, n_Xk:]
# I_X{k+l} = A.T.dot(A)
if torch.is_tensor(A2):
I = torch.zeros((n_X, n_X))
I[:n_Xk, :n_Xk] = I_Xk + torch.mm(A21.T, A21)
I[n_Xk:, n_Xk:] = torch.mm(A22.T, A22)
I[:n_Xk, n_Xk:] = torch.mm(A21.T, A22)
else:
I = np.zeros((n_X, n_X), dtype=dtype)
I[:n_Xk, :n_Xk] = I_Xk + A21.T.dot(A21)
I[n_Xk:, n_Xk:] = A22.T.dot(A22)
I[:n_Xk, n_Xk:] = A21.T.dot(A22)
I[n_Xk:, :n_Xk] = I[:n_Xk, n_Xk:].T
return I
# lower half of A
result = self.calc_A_lower_half(dX_WB, l_xy_e)
# including landmarks.
result["I_e"] = calc_I(result["A2"], dtype=object)
# excluding landmarks.
result["I_p"] = calc_I(result["F_whitened"], dtype=np.float)
return result
def calc_pose_predictions(self, dX_WB):
L = len(dX_WB)
k = len(self.X_WB_e_dict) - 1 # current time step.
# robot configuration for time steps k + 1 : (k + L).
if torch.is_tensor(dX_WB):
X_WB_p = torch.zeros((L, self.dim_X))
X_WB_e_k = torch.from_numpy(self.X_WB_e_dict[k])
else:
X_WB_p = np.zeros((L, self.dim_X), dtype=dX_WB.dtype)
X_WB_e_k = self.X_WB_e_dict[k]
for i in range(L):
if i == 0:
X_WB_p[i] = X_WB_e_k + dX_WB[i]
else:
X_WB_p[i] = X_WB_p[i - 1] + dX_WB[i]
return X_WB_p
@staticmethod
def get_inverse(x):
if torch.is_tensor(x):
return torch.inverse
else:
return inv_pydrake
def calc_objective(self, dX_WB, X_WB_e, l_xy_e, X_WB_g, alpha):
"""
Algorithm 3 in the paper.
:return:
"""
L = len(dX_WB)
I_k, _, _ = self.calc_info_matrix(X_WB_e, l_xy_e)
if torch.is_tensor(dX_WB):
M_u = torch.from_numpy(self.M_u)
X_WB_g = torch.from_numpy(X_WB_g)
else:
M_u = self.M_u
inv = self.get_inverse(dX_WB)
# (a) in eq. (41).
c_a = (dX_WB * M_u * dX_WB).sum()
# (b) in eq. (41).
c_b = 0.
for l in range(L):
result_l = self.calc_inner_layer(dX_WB[:l + 1], l_xy_e, I_k)
Cov_e_l = inv(result_l["I_e"])
c_b += Cov_e_l[-self.dim_X:, -self.dim_X:].diagonal().sum()
Cov_e_L = Cov_e_l
Cov_p_L = inv(result_l["I_p"])
X_WB_p_L = result_l["X_WB_p"]
A2_L = result_l["A2"]
Omega_v_sqrt = result_l["Omega_v_sqrt"]
Omega_v_bar_sqrt = result_l["Omega_v_bar_sqrt"]
# (c) in eq. (41)
c_c1 = ((X_WB_p_L[-1] - X_WB_g)**2).sum()
H_whitened_kL = A2_L[L * self.dim_X:]
H_kL = result_l["H"]
# B.shape = (self.dim_X, m2),
# where m2 is the number of range and bearing measurements of
# the trajectory X_WB_p_L.
if torch.is_tensor(dX_WB):
B = torch.mm(Cov_e_L[-self.dim_X:],
H_whitened_kL.T) * Omega_v_bar_sqrt
Q_kL = torch.mm(B.T, B)
D = torch.mm(torch.mm(H_kL, Cov_p_L), H_kL.T)
else:
B = Cov_e_L[-self.dim_X:].dot(H_whitened_kL.T) * Omega_v_bar_sqrt
Q_kL = B.T.dot(B)
D = H_kL.dot(Cov_p_L).dot(H_kL.T)
D[np.diag_indices_from(D)] += 1 / Omega_v_sqrt**2
if torch.is_tensor(dX_WB):
c_c2 = torch.mm(Q_kL, D).diagonal().sum()
else:
c_c2 = Q_kL.dot(D).diagonal().sum()
c = c_a + alpha * (c_b * 50) + (1 - alpha) * (c_c1 + c_c2)
# c = c_c1
return {
"c":
c,
"c_a":
c_a,
"c_b":
c_b,
"c_c1":
c_c1,
"c_c2":
c_c2,
"predicted_uncertainty_L":
Cov_p_L[-self.dim_X:, -self.dim_X:].diagonal().sum()
}
def calc_alpha(self, dX_WB, X_WB_e, l_xy_e):
inv = self.get_inverse(dX_WB)
I_k, _, _ = self.calc_info_matrix(X_WB_e, l_xy_e)
result_L = self.calc_inner_layer(dX_WB, l_xy_e, I_k)
Cov_p_L = inv(result_L["I_p"])
uncertainty_L = Cov_p_L[-self.dim_X:, -self.dim_X:].diagonal().sum()
alpha = min(1, uncertainty_L / self.beta)
if self.is_uncertainty_reduction_only:
if alpha < self.alpha_LB:
self.is_uncertainty_reduction_only = False
else:
alpha = 1
else:
if alpha == 1:
self.is_uncertainty_reduction_only = True
return float(alpha)
def calc_objective_gradient_finite_difference(self, dX_WB, X_WB_e, l_xy_e,
X_WB_g, alpha):
"""
Algorithm 2 in the paper.
:param dX_WB:
:param X_WB_e:
:param l_xy_e:
:param X_WB_g: goal.
:return:
"""
Dc = np.zeros_like(dX_WB.ravel())
h = 1e-3
for i in range(len(Dc)):
dX_WB_new = dX_WB.copy()
dX_WB_new.ravel()[i] += h
c_new1 = self.calc_objective(dX_WB_new, X_WB_e, l_xy_e, X_WB_g,
alpha)["c"]
dX_WB_new.ravel()[i] -= 2 * h
c_new2 = self.calc_objective(dX_WB_new, X_WB_e, l_xy_e, X_WB_g,
alpha)["c"]
Dc[i] = (c_new1 - c_new2) / h / 2
Dc.resize(dX_WB.shape)
return Dc
def run_gradient_descent(self,
dX_WB0,
X_WB_e,
l_xy_e,
X_WB_g,
alpha=None,
backprop=True):
"""
Algorithm 1 in the paper.
:return:
"""
dX_WB = dX_WB0.copy()
dX_WB_torch = torch.from_numpy(dX_WB)
dX_WB_torch.requires_grad_()
iter_count = 0
a = 0.4
b = 0.5
epsilon = 2e-3
if alpha is None:
alpha = self.calc_alpha(dX_WB, X_WB_e, l_xy_e)
while True:
if backprop:
if dX_WB_torch.grad is not None:
dX_WB_torch.grad.data.zero_()
result = self.calc_objective(dX_WB_torch, X_WB_e, l_xy_e,
X_WB_g, alpha)
c0 = result["c"]
c0.backward()
D_dX_WB = dX_WB_torch.grad.numpy()
result = self.convert_cost_from_torch_tensor_to_float(result)
else:
result = self.calc_objective(dX_WB, X_WB_e, l_xy_e, X_WB_g,
alpha)
D_dX_WB = self.calc_objective_gradient_finite_difference(
dX_WB, X_WB_e, l_xy_e, X_WB_g, alpha)
c0 = result["c"]
print("\nIteration {}, cost \n".format(iter_count), result)
# line search
t = 1
line_search_count = 0
while True:
result = self.calc_objective(dX_WB - t * D_dX_WB, X_WB_e,
l_xy_e, X_WB_g, alpha)
c_t = result["c"]
if c_t < c0 - a * t * (D_dX_WB**
2).sum() or line_search_count > 10:
break
# print("count {}, c_t {}, ".format(line_search_count, c_t))
t *= b
line_search_count += 1
dX_WB -= t * D_dX_WB
print("Line search steps {}, gradient {}".format(
line_search_count, np.linalg.norm(D_dX_WB)))
print(result)
iter_count += 1
# termination
is_cost_reduction_small = abs((c_t - c0) / c_t) < epsilon
is_gradient_small = np.linalg.norm(D_dX_WB) < epsilon
if is_cost_reduction_small or is_gradient_small or iter_count > 200:
break
print("alpha = {}".format(alpha), self.is_uncertainty_reduction_only)
result["alpha"] = alpha
return dX_WB, result
@staticmethod
def convert_cost_from_torch_tensor_to_float(result):
result_no_torch = dict()
for key, value in result.items():
result_no_torch[key] = float(value)
return result_no_torch
def initialize_dX_WB_with_goal(self, X_WB_0, X_WB_g, L, max_step=0.2):
"""
:param X_WB_0: current robot pose.
:param X_WB_g: goal robot pose.
:param L: number of time steps.
:return:
"""
step = (X_WB_g - X_WB_0) / L
step_size = np.linalg.norm(step)
step /= step_size
step *= min(step_size, max_step)
dX_WB = np.zeros((L, X_WB_0.size))
dX_WB[:] = step
return dX_WB
def initialize_dX_WB_with_fixed_input(self, X_WB_0, dX_WB0, L):
"""
:param X_WB_0: current robot pose.
:param dX_WB0: fixed input.
:param L: number of time steps.
:return:
"""
dX_WB = np.zeros((L, X_WB_0.size))
dX_WB[:] = dX_WB0
return dX_WB
def run_bundle_adjustment(self, is_printing=False):
dim_l = self.dim_l # dimension of landmarks.
dim_X = self.dim_X # dimension of poses.
X_WB_e0 = self.get_X_WB_initial_guess_array()
l_xy_e0 = self.get_l_xy_initial_guess_array()
X_WB_e = X_WB_e0.copy()
l_xy_e = l_xy_e0.copy()
n_o = len(self.odometry_measurements)
n_p = n_o + 1 # number of poses.
n_l = len(self.landmark_measurements)
steps_counter = 0
while True:
I_k, q, c = self.calc_info_matrix(X_WB_e, l_xy_e)
prog = mp.MathematicalProgram()
dX_WB_e = prog.NewContinuousVariables(n_p, dim_X, "dX_WB_e")
dl_xy_e = prog.NewContinuousVariables(n_l, dim_l, "dl_xy_e")
dx = np.hstack((dX_WB_e.ravel(), dl_xy_e.ravel()))
nx = len(dx)
prog.AddQuadraticCost(I_k, q, 0.5 * c, dx)
prog.AddQuadraticCost(np.eye(nx) * 2, np.zeros(nx), dx)
result = self.solver.Solve(prog)
optimal_cost = result.get_optimal_cost()
assert result.get_solution_result() == \
mp.SolutionResult.kSolutionFound
dX_WB_e_values = result.GetSolution(dX_WB_e)
dl_xy_e_values = result.GetSolution(dl_xy_e)
dx_values = result.GetSolution(dx)
X_WB_e += dX_WB_e_values
l_xy_e += dl_xy_e_values
steps_counter += 1
if steps_counter > 500 or \
np.linalg.norm(dx_values) / (n_p + n_l) < 5e-3:
break
self.update_beliefs(X_WB_e, l_xy_e, I_k)
if is_printing:
print("\nStep ", n_o)
print("optimal cost: ", optimal_cost)
_, b, _ = self.calc_jacobian_and_b(X_WB_e, l_xy_e)
print("b norm recalculated: ", np.linalg.norm(b))
print("total gradient steps: ", steps_counter)
print("dx norm: ", np.linalg.norm(dx_values))
Omega_pose = self.marginalize_info_matrix(I_k, n_p, n_l)
print("Marginalized covariance diagonal\n",
np.linalg.inv(Omega_pose).diagonal())
Cov = np.linalg.inv(I_k)
print("Sqrt covariance trace\n", np.sqrt(Cov.diagonal().sum()))
print("\n")
return X_WB_e, l_xy_e
def update_beliefs(self, X_WB_e_values, l_xy_e_values, I_all):
Cov = np.linalg.inv(I_all)
for i, X_WB_e in enumerate(X_WB_e_values):
i0 = i * self.dim_X
i1 = i0 + self.dim_X
self.X_WB_e_dict[i] = X_WB_e
Cov_i = Cov[i0:i1, i0:i1]
e_values, e_vectors = np.linalg.eig(Cov_i)
X_WB_ellipsoid = np.eye(4)
X_WB_ellipsoid[:2, :2] = e_vectors
X_WB_ellipsoid[:2, 3] = X_WB_e
sigmas = np.sqrt(e_values)
self.X_WB_e_var_dict[i] = {
"sigmas": np.hstack([np.real(sigmas), 0.01]),
"transform": X_WB_ellipsoid,
"cov": Cov_i
}
self.l_xy_e_dict.clear()
for k, l_xy_e_value in zip(self.landmark_measurements.keys(),
l_xy_e_values):
self.l_xy_e_dict[k] = l_xy_e_value
self.I_all = I_all
def calc_sqrt_postion_cov_trace(self):
postion_cov_trace = 0.
for a in self.X_WB_e_var_dict.values():
postion_cov_trace += a["cov"].diagonal().sum()
return np.sqrt(postion_cov_trace)
def find_visible_landmarks(self, X_WB_p):
future_landmark_measurements = dict()
for i, X_WB in enumerate(X_WB_p):
for j in self.l_xy_e_dict.keys():
d_xy = self.l_xy_e_dict[j] - X_WB
d = np.linalg.norm(d_xy)
if self.r_range_min < d:
if j not in future_landmark_measurements:
future_landmark_measurements[j] = dict()
b = np.arctan2(d_xy[1], d_xy[0])
future_landmark_measurements[j][i] = {
"distance": d,
"bearing": b
}
return future_landmark_measurements
def draw_estimated_path(self):
n_p = len(self.X_WB_e_dict)
t_xy_e = np.zeros((n_p, 2))
for i in range(n_p):
t_xy_e[i] = self.X_WB_e_dict[i][:2]
self.draw_robot_path(t_xy_e,
color=[1, 0, 0],
prefix="robot_poses_e",
idx_segment=0,
size=0.2)
def draw_estimated_path_segment(self,
L: int,
idx_segment: int,
covariance_scale=1.):
n_p = len(self.X_WB_e_dict)
name = "robot_poses_e/{}".format(idx_segment)
material = meshcat.geometry.MeshLambertMaterial(color=0x778899,
opacity=0.5)
if L is None:
L = n_p
points = np.zeros((L, 3))
for i in range(L):
idx = n_p - L + i
# Draw covariance ellipses.
sigmas = self.X_WB_e_var_dict[idx]["sigmas"]
sigmas[:2] *= covariance_scale
self.vis[name]["points"][str(i)].set_object(
meshcat.geometry.Ellipsoid(sigmas), material)
self.vis[name]["points"][str(i)].set_transform(
self.X_WB_e_var_dict[idx]["transform"])
points[i, :2] = self.X_WB_e_dict[idx]
# Draw lines.
self.vis[name]["path"].set_object(
meshcat.geometry.Line(meshcat.geometry.PointsGeometry(points.T),
material))
def draw_estimated_landmarks(self):
nl = len(self.l_xy_e_dict)
l_xy_e = np.zeros((nl, 3))
for i, (k, l_xy_e_i) in enumerate(self.l_xy_e_dict.items()):
l_xy_e[i][:2] = l_xy_e_i
points = np.zeros((2, 3))
points[0, :2] = l_xy_e_i
points[1, :2] = self.l_xy[k]
self.vis["landmarks_e"]["corrspondances"][str(k)].set_object(
meshcat.geometry.Line(meshcat.geometry.PointsGeometry(
points.T)))
# draw landmark
prefix = "landmarks_e/points/{}".format(i)
self.frontend.draw_cylinder(prefix, 0.05, 0.11, 0x00ffff, l_xy_e_i)
def draw_robot_path(self,
X_WBs,
color,
prefix: str,
idx_segment: int,
size=0.2):
t_xy = X_WBs[:, :2]
for i in range(1, len(t_xy)):
name = prefix + "/{}/points/{}".format(idx_segment, i)
self.frontend.draw_box(name, [size, size, 0.15], color, t_xy[i])
points = np.zeros((2, 3))
points[0, :2] = t_xy[i - 1]
points[1, :2] = t_xy[i]
self.vis[prefix][str(idx_segment)]["path"][str(i)].set_object(
meshcat.geometry.Line(meshcat.geometry.PointsGeometry(
points.T)))
# 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))
for simplex in chulls[j].simplices:
print(simplex)
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]