def main():
parser = ArgumentParser()
parser.add_argument('--datapath', default='../out/data.npy')
parser.add_argument('--normalize', dest='normalize', action='store_true')
parser.add_argument('--no-normalize',
dest='normalize',
action='store_false')
parser.set_defaults(normalize=True)
opts = parser.parse_args()
data = dynamics.unmarshal_data(np.load(opts.datapath))
n = data.poslambdas.shape[0]
lambdas = np.vstack((data.poslambdas, data.neglambdas, data.gammas))
ds = np.vstack((data.xdots, data.us, np.ones(data.xdots.shape)))
mp = MathematicalProgram()
M = mp.NewContinuousVariables(3, 3, "M")
W = mp.NewContinuousVariables(3, 3, "W")
W[0, 2] = 0
W[1, 2] = 0
R = M.dot(lambdas) + W.dot(ds)
elementwise_positive_constraint(mp, R)
lambdas_vec = np.expand_dims(np.ndarray.flatten(lambdas), 0)
R_vec = np.ndarray.flatten(R)
mp.AddLinearCost(lambdas_vec.dot(R_vec)[0])
result = Solve(mp)
print(result.is_success())
Msol = result.GetSolution(M)
# When mix 0's in with W, need to evaluate expression
evaluator = np.vectorize(lambda x: x.Evaluate())
Wsol = evaluator(result.GetSolution(W))
print(Msol)
print(Wsol)
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np
# Creat an empty MathematicalProgram named prog (with no decision variables, constraints or costs)
prog = MathematicalProgram()
# Add two decision variables x[0], x[1]
x = prog.NewContinuousVariables(2, "x")
# Add a symbolic linear expression as the cost
cost_1 = prog.AddLinearCost(x[0] + 3 * x[1] + 2)
# 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()