def optimization(self, quad_plants, ball_plants, contexts): prog = MathematicalProgram() #Time steps likely requires a line search. #Find the number of time steps until a ball reaches a threshold so that a quadrotor can catch it. N = 200 #Number of time steps n_u = 2*len(plants) n_x = 6*len(quad_plants) + 4*len(ball_plants) n_quads = len(quad_plants) n_balls = len(ball_plants) u = np.empty((self.n_u, N-1), dtype=Variable) x = np.empty((self.n_x, N), dtype=Variable) I = 0.00383 r = 0.25 mass = 0.486 g = 9.81 #Add all the decision variables for n in range(N-1): u[:,n] = prog.NewContinuousVariables(self.n_u, 'u' + str(n)) x[:,n] = prog.NewContinuousVariables(self.n_x, 'x' + str(n)) x[:,N-1] = prog.NewContinuousVariables(self.n_x, 'x' + str(N)) #Start at the correct initial conditions #Connect quadrotor and ball state variables to the initial condition of mathprog x0 = np.empty((self.n_x,1)) for k in range(n_quads + n_balls): if k < n_quads: x0[6*k:6*k+5] = self.quad_plants[k] #??? prog.AddBoundingBoxConstraint(x0, x0, x[:,0]) for n in range(N-1): for k in range(n_quads + n_balls): #Handles no collisions if k < n_quads: #Quad dynamic constraints dynamics_constraint_vel = #? dynamics_constraint_acc = #? prog.AddConstraint(dynamics_constraint_vel[0, 0]) prog.AddConstraint(dynamics_constraint_vel[1, 0]) prog.AddConstraint(dynamics_constraint_vel[2, 0]) prog.AddConstraint(dynamics_constraint_acc[0, 0]) prog.AddConstraint(dynamics_constraint_acc[1, 0]) prog.AddConstraint(dynamics_constraint_acc[2, 0]) else: #Ball dynamic constraints dynamics_constraint_vel = # dynamics_constraint_acc = # prog.AddConstraint(dynamics_constraint_vel[0, 0]) prog.AddConstraint(dynamics_constraint_vel[1, 0]) prog.AddConstraint(dynamics_constraint_acc[0, 0]) prog.AddConstraint(dynamics_constraint_acc[1, 0])
def initialize_problem(n_x, n_u, N, x0):
''' returns base MathematicalProgram object and empty decision vars '''
prog = MathematicalProgram()
# Decision Variables
x = prog.NewContinuousVariables(n_x, N, 'x')
u = prog.NewContinuousVariables(n_u, N-1, 'u')
z = [] # placeholder for obstacle binary variables
q = [] # placeholder for obstacle slack buffer variables
# initial conditions constraints
prog.AddBoundingBoxConstraint(x0, x0, x[:, 0])
decision_vars = [x, u, z, q]
return prog, decision_vars
def __init__(self, rtree, q_nom, control_period=0.001):
self.rtree = rtree
self.nq = rtree.get_num_positions()
self.nv = rtree.get_num_velocities()
self.nu = rtree.get_num_actuators()
dim = 3 # 3D
nd = 4 # for friction cone approx, hard coded for now
self.nc = 4 # number of contacts; TODO don't hard code
self.nf = self.nc * nd # number of contact force variables
self.ncf = 2 # number of loop constraint forces; TODO don't hard code
self.neps = self.nc * dim # number of slack variables for contact
self.q_nom = q_nom
self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom))
self.u_des = CassieFixedPointTorque()
self.lfoot = rtree.FindBody("toe_left")
self.rfoot = rtree.FindBody("toe_right")
self.springs = 2 + np.array([
rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"),
rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right")
])
umin = np.zeros(self.nu)
umax = np.zeros(self.nu)
ii = 0
for motor in rtree.actuators:
umin[ii] = motor.effort_limit_min
umax[ii] = motor.effort_limit_max
ii += 1
slack_limit = 10.0
self.initialized = False
#------------------------------------------------------------
# Add Decision Variables ------------------------------------
prog = MathematicalProgram()
qddot = prog.NewContinuousVariables(self.nq, "joint acceleration")
u = prog.NewContinuousVariables(self.nu, "input")
bar = prog.NewContinuousVariables(self.ncf, "four bar forces")
beta = prog.NewContinuousVariables(self.nf, "friction forces")
eps = prog.NewContinuousVariables(self.neps, "slack variable")
nparams = prog.num_vars()
#------------------------------------------------------------
# Problem Constraints ---------------------------------------
self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax,
u).evaluator()
self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000,
beta).evaluator()
self.con_slack_lim = prog.AddBoundingBoxConstraint(
-slack_limit, slack_limit, eps).evaluator()
bar_con = np.zeros((self.ncf, self.nq))
self.con_4bar = prog.AddLinearEqualityConstraint(
bar_con, np.zeros(self.ncf), qddot).evaluator()
if self.nc > 0:
dyn_con = np.zeros(
(self.nq, self.nq + self.nu + self.ncf + self.nf))
dyn_vars = np.concatenate((qddot, u, bar, beta))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
foot_con = np.zeros((self.neps, self.nq + self.neps))
foot_vars = np.concatenate((qddot, eps))
self.con_foot = prog.AddLinearEqualityConstraint(
foot_con, np.zeros(self.neps), foot_vars).evaluator()
else:
dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf))
dyn_vars = np.concatenate((qddot, u, bar))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
#------------------------------------------------------------
# Problem Costs ---------------------------------------------
self.Kp_com = 50
self.Kd_com = 2.0 * sqrt(self.Kp_com)
# self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H)))
self.Kp_qdd = np.diag(
np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6))))
self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd)
self.Kp_qdd[self.springs, self.springs] = 0
self.Kd_qdd[self.springs, self.springs] = 0
com_ddot_des = np.zeros(dim)
qddot_des = np.zeros(self.nq)
self.w_com = 0
self.w_qdd = np.zeros(self.nq)
self.w_qdd[:6] = 10
self.w_qdd[8:10] = 1
self.w_qdd[3:6] = 1000
# self.w_qdd[self.springs] = 0
self.w_u = 0.001 * np.ones(self.nu)
self.w_u[2:4] = 0.01
self.w_slack = 0.1
self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd),
qddot_des,
qddot).evaluator()
self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des,
u).evaluator()
self.cost_slack = prog.AddQuadraticErrorCost(
self.w_slack * np.eye(self.neps), np.zeros(self.neps),
eps).evaluator()
# self.cost_com = prog.AddQuadraticCost().evaluator()
# self.cost_qdd = prog.AddQuadraticCost(
# 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator()
# self.cost_u = prog.AddQuadraticCost(
# 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator()
# self.cost_slack = prog.AddQuadraticCost(
# 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator()
REG = 1e-8
self.cost_reg = prog.AddQuadraticErrorCost(
REG * np.eye(nparams), np.zeros(nparams),
prog.decision_variables()).evaluator()
# self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams),
# np.zeros(nparams), prog.decision_variables()).evaluator()
#------------------------------------------------------------
# Solver settings -------------------------------------------
self.solver = GurobiSolver()
# self.solver = OsqpSolver()
prog.SetSolverOption(SolverType.kGurobi, "Method", 2)
#------------------------------------------------------------
# Save MathProg Variables -----------------------------------
self.qddot = qddot
self.u = u
self.bar = bar
self.beta = beta
self.eps = eps
self.prog = prog
def __init__(self,
rtree,
q_nom,
control_period=0.001,
print_period=1.0,
sim=True,
cost_pub=False):
LeafSystem.__init__(self)
self.rtree = rtree
self.nq = rtree.get_num_positions()
self.nv = rtree.get_num_velocities()
self.nu = rtree.get_num_actuators()
self.cost_pub = cost_pub
dim = 3 # 3D
nd = 4 # for friction cone approx, hard coded for now
self.nc = 4 # number of contacts; TODO don't hard code
self.nf = self.nc * nd # number of contact force variables
self.ncf = 2 # number of loop constraint forces; TODO don't hard code
self.neps = self.nc * dim # number of slack variables for contact
if sim:
self.sim = True
self._DeclareInputPort(PortDataType.kVectorValued,
self.nq + self.nv)
self._DeclareDiscreteState(self.nu)
self._DeclareVectorOutputPort(BasicVector(self.nu),
self.CopyStateOutSim)
self.print_period = print_period
self.last_print_time = -print_period
self.last_v = np.zeros(3)
self.lcmgl = lcmgl("Balancing-plan", true_lcm.LCM())
# self.lcmgl = None
else:
self.sim = False
self._DeclareInputPort(PortDataType.kVectorValued, 1)
self._DeclareInputPort(PortDataType.kVectorValued,
kCassiePositions)
self._DeclareInputPort(PortDataType.kVectorValued,
kCassieVelocities)
self._DeclareInputPort(PortDataType.kVectorValued, 2)
self._DeclareDiscreteState(kCassieActuators * 8 + 1)
self._DeclareVectorOutputPort(
BasicVector(1),
lambda c, o: self.CopyStateOut(c, o, 8 * kCassieActuators, 1))
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 0, kCassieActuators)) #torque
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, kCassieActuators, kCassieActuators)) #motor_pos
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 2 * kCassieActuators, kCassieActuators)) #motor_vel
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 3 * kCassieActuators, kCassieActuators)) #kp
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 4 * kCassieActuators, kCassieActuators)) #kd
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 5 * kCassieActuators, kCassieActuators)) #ki
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 6 * kCassieActuators, kCassieActuators)) #leak
self._DeclareVectorOutputPort(
BasicVector(kCassieActuators), lambda c, o: self.CopyStateOut(
c, o, 7 * kCassieActuators, kCassieActuators)) #clamp
self._DeclarePeriodicDiscreteUpdate(0.0005)
self.q_nom = q_nom
self.com_des = rtree.centerOfMass(self.rtree.doKinematics(q_nom))
self.u_des = CassieFixedPointTorque()
self.lfoot = rtree.FindBody("toe_left")
self.rfoot = rtree.FindBody("toe_right")
self.springs = 2 + np.array([
rtree.FindIndexOfChildBodyOfJoint("knee_spring_left_fixed"),
rtree.FindIndexOfChildBodyOfJoint("knee_spring_right_fixed"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_left"),
rtree.FindIndexOfChildBodyOfJoint("ankle_spring_joint_right")
])
umin = np.zeros(self.nu)
umax = np.zeros(self.nu)
ii = 0
for motor in rtree.actuators:
umin[ii] = motor.effort_limit_min
umax[ii] = motor.effort_limit_max
ii += 1
slack_limit = 10.0
self.initialized = False
#------------------------------------------------------------
# Add Decision Variables ------------------------------------
prog = MathematicalProgram()
qddot = prog.NewContinuousVariables(self.nq, "joint acceleration")
u = prog.NewContinuousVariables(self.nu, "input")
bar = prog.NewContinuousVariables(self.ncf, "four bar forces")
beta = prog.NewContinuousVariables(self.nf, "friction forces")
eps = prog.NewContinuousVariables(self.neps, "slack variable")
nparams = prog.num_vars()
#------------------------------------------------------------
# Problem Constraints ---------------------------------------
self.con_u_lim = prog.AddBoundingBoxConstraint(umin, umax,
u).evaluator()
self.con_fric_lim = prog.AddBoundingBoxConstraint(0, 100000,
beta).evaluator()
self.con_slack_lim = prog.AddBoundingBoxConstraint(
-slack_limit, slack_limit, eps).evaluator()
bar_con = np.zeros((self.ncf, self.nq))
self.con_4bar = prog.AddLinearEqualityConstraint(
bar_con, np.zeros(self.ncf), qddot).evaluator()
if self.nc > 0:
dyn_con = np.zeros(
(self.nq, self.nq + self.nu + self.ncf + self.nf))
dyn_vars = np.concatenate((qddot, u, bar, beta))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
foot_con = np.zeros((self.neps, self.nq + self.neps))
foot_vars = np.concatenate((qddot, eps))
self.con_foot = prog.AddLinearEqualityConstraint(
foot_con, np.zeros(self.neps), foot_vars).evaluator()
else:
dyn_con = np.zeros((self.nq, self.nq + self.nu + self.ncf))
dyn_vars = np.concatenate((qddot, u, bar))
self.con_dyn = prog.AddLinearEqualityConstraint(
dyn_con, np.zeros(self.nq), dyn_vars).evaluator()
#------------------------------------------------------------
# Problem Costs ---------------------------------------------
self.Kp_com = 50
self.Kd_com = 2.0 * sqrt(self.Kp_com)
# self.Kp_qdd = 10*np.eye(self.nq)#np.diag(np.diag(H)/np.max(np.diag(H)))
self.Kp_qdd = np.diag(
np.concatenate(([10, 10, 10, 5, 5, 5], np.zeros(self.nq - 6))))
self.Kd_qdd = 1.0 * np.sqrt(self.Kp_qdd)
self.Kp_qdd[self.springs, self.springs] = 0
self.Kd_qdd[self.springs, self.springs] = 0
com_ddot_des = np.zeros(dim)
qddot_des = np.zeros(self.nq)
self.w_com = 0
self.w_qdd = np.zeros(self.nq)
self.w_qdd[:6] = 10
self.w_qdd[8:10] = 1
self.w_qdd[3:6] = 1000
# self.w_qdd[self.springs] = 0
self.w_u = 0.001 * np.ones(self.nu)
self.w_u[2:4] = 0.01
self.w_slack = 0.1
self.cost_qdd = prog.AddQuadraticErrorCost(np.diag(self.w_qdd),
qddot_des,
qddot).evaluator()
self.cost_u = prog.AddQuadraticErrorCost(np.diag(self.w_u), self.u_des,
u).evaluator()
self.cost_slack = prog.AddQuadraticErrorCost(
self.w_slack * np.eye(self.neps), np.zeros(self.neps),
eps).evaluator()
# self.cost_com = prog.AddQuadraticCost().evaluator()
# self.cost_qdd = prog.AddQuadraticCost(
# 2*np.diag(self.w_qdd), -2*np.matmul(qddot_des, np.diag(self.w_qdd)), qddot).evaluator()
# self.cost_u = prog.AddQuadraticCost(
# 2*np.diag(self.w_u), -2*np.matmul(self.u_des, np.diag(self.w_u)) , u).evaluator()
# self.cost_slack = prog.AddQuadraticCost(
# 2*self.w_slack*np.eye(self.neps), np.zeros(self.neps), eps).evaluator()
REG = 1e-8
self.cost_reg = prog.AddQuadraticErrorCost(
REG * np.eye(nparams), np.zeros(nparams),
prog.decision_variables()).evaluator()
# self.cost_reg = prog.AddQuadraticCost(2*REG*np.eye(nparams),
# np.zeros(nparams), prog.decision_variables()).evaluator()
#------------------------------------------------------------
# Solver settings -------------------------------------------
self.solver = GurobiSolver()
# self.solver = OsqpSolver()
prog.SetSolverOption(SolverType.kGurobi, "Method", 2)
#------------------------------------------------------------
# Save MathProg Variables -----------------------------------
self.qddot = qddot
self.u = u
self.bar = bar
self.beta = beta
self.eps = eps
self.prog = prog
# Create a mathematical program
prog = MathematicalProgram()
# Number of knot points
N = 284
# Create decision variables
u = np.empty((1, N - 1), dtype=Variable)
x = np.empty((2, N), dtype=Variable)
for n in range(0, N - 1):
u[:, n] = prog.NewContinuousVariables(1, 'u' + str(n))
x[:, n] = prog.NewContinuousVariables(2, 'x' + str(n))
x[:, N - 1] = prog.NewContinuousVariables(2, 'x' + str(N))
# Add constraints at every knot point
x0 = [-2, 0]
prog.AddBoundingBoxConstraint(x0, x0, x[:, 0])
for n in range(0, N - 1):
# Add the dynamics as an equality constraint
# prog.AddConstraint(eq(x[:,n+1], A.dot(x[:,n]) + B.dot(u[:,n])))
# Add the dynamics as a function handle constraint
prog.AddConstraint(dynamicsCstr,
lb=np.array([0., 0.]),
ub=np.array([0., 0.]),
vars=np.concatenate((x[:, n], u[:, n], x[:, n + 1]),
axis=0),
description="dynamics")
prog.AddBoundingBoxConstraint(-1, 1, u[:, n])
xf = [0, 0]
prog.AddBoundingBoxConstraint(xf, xf, x[:, N - 1])
# Solve the problem
import manipulation.meshcat_cpp_utils as dut import numpy as np from pydrake.all import MathematicalProgram, Solve, Meshcat prog = MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddCost(x.dot(x)) prog.AddBoundingBoxConstraint(-2, 2, x) result = Solve(prog) meshcat = Meshcat() X, Y = np.meshgrid(np.linspace(-3, 3, 35), np.linspace(-3, 3, 31)) dut.plot_mathematical_program(meshcat, "test", prog, X, Y, result)
# stance-foot force
f = prog.NewContinuousVariables(rows=T, cols=nf, name='f')
# heel strike impulse for the swing leg
imp = prog.NewContinuousVariables(nf, name='imp')
# generalized velocity after the heel strike
# (if "mirrored", this velocity must coincide with the
# initial velocity qd[0] to ensure periodicity)
qd_post = prog.NewContinuousVariables(nq, name='qd_post')
"""Here are part of the constraints of the optimization problem."""
# lower and upper bound on the time steps for all t
prog.AddBoundingBoxConstraint([h_min] * T, [h_max] * T, h)
# link the configurations, velocities, and accelerations
# uses implicit Euler method, https://en.wikipedia.org/wiki/Backward_Euler_method
for t in range(T):
prog.AddConstraint(eq(q[t+1], q[t] + h[t] * qd[t+1]))
prog.AddConstraint(eq(qd[t+1], qd[t] + h[t] * qdd[t]))
# manipulator equations for all t (implicit Euler)
for t in range(T):
vars = np.concatenate((q[t+1], qd[t+1], qdd[t], f[t]))
prog.AddConstraint(manipulator_equations, lb=[0]*nq, ub=[0]*nq, vars=vars)
# velocity reset across heel strike
# see http://underactuated.mit.edu/multibody.html#impulsive_collision
vars = np.concatenate((q[-1], qd[-1], qd_post, imp))
def optimal_trajectory(joints,
start_position,
end_position,
signed_dist_fn,
nc,
time=10,
knot_points=100):
assert len(joints) == len(start_position)
assert len(joints) == len(end_position)
h = time / (knot_points - 1)
nq = len(joints)
prog = MathematicalProgram()
q_var = []
v_var = []
for ii in range(knot_points):
q_var.append(prog.NewContinuousVariables(nq, "q[" + str(ii) + "]"))
v_var.append(prog.NewContinuousVariables(nq, "v[" + str(ii) + "]"))
# ---------------------------------------------------------------
# Initial & Final Pose Constraints ------------------------------
x_i = np.append(start_position, np.zeros(nq))
x_i_vars = np.append(q_var[0], v_var[0])
prog.AddLinearEqualityConstraint(np.eye(2 * nq), x_i, x_i_vars)
tol = 0.01 * np.ones(2 * nq)
x_f = np.append(end_position, np.zeros(nq))
x_f_vars = np.append(q_var[-1], v_var[-1])
prog.AddBoundingBoxConstraint(x_f - tol, x_f + tol, x_f_vars)
# ---------------------------------------------------------------
# Dynamics Constraints ------------------------------------------
for ii in range(knot_points - 1):
dyn_con1 = np.hstack((np.eye(nq), np.eye(nq), -np.eye(nq)))
dyn_var1 = np.concatenate((q_var[ii], v_var[ii], q_var[ii + 1]))
prog.AddLinearEqualityConstraint(dyn_con1, np.zeros(nq), dyn_var1)
# ---------------------------------------------------------------
# Joint Limit Constraints ---------------------------------------
q_min = np.array([j.lower_limits() for j in joints])
q_max = np.array([j.upper_limits() for j in joints])
for ii in range(knot_points):
prog.AddBoundingBoxConstraint(q_min, q_max, q_var[ii])
# ---------------------------------------------------------------
# Collision Constraints -----------------------------------------
for ii in range(knot_points):
prog.AddConstraint(signed_dist_fn, np.zeros(nc), 1e8 * np.ones(nc),
q_var[ii])
# ---------------------------------------------------------------
# Dynamics Constraints ------------------------------------------
for ii in range(knot_points):
prog.AddQuadraticErrorCost(np.eye(nq), np.zeros(nq), v_var[ii])
xi = np.array(start_position)
xf = np.array(end_position)
for ii in range(knot_points):
prog.SetInitialGuess(q_var[ii],
ii * (xf - xi) / (knot_points - 1) + xi)
prog.SetInitialGuess(v_var[ii], np.zeros(nq))
result = prog.Solve()
print prog.GetSolverId().name()
if result != SolutionResult.kSolutionFound:
print result
return None
q_path = []
# print signed_dist_fn(prog.GetSolution(q_var[0]))
for ii in range(knot_points):
q_path.append(prog.GetSolution(q_var[ii]))
return q_path
def generate_prog(self):
self.nn_conss = []
self.nn_costs = []
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol_constraint = DirectCollocationConstraint(plant, context)
prog = MathematicalProgram()
# K = prog.NewContinuousVariables(1,7,'K')
def final_cost(x):
return 100.*(cos(.5*x[0])**2 + x[1]**2)
h = [];
u = [];
x = [];
xf = np.array([math.pi, 0.])
# Declare the MathematicalProgram vars up front in a good order, so that
# prog.decision_variables will be result of np.hstack(h.flatten(), u.flatten(), x.flatten(), T.flatten())
# for the following shapes: unfortunately, prog.decision_variables() will have these shapes:
# h = (num_trajectories, 1) | h = (num_trajectories, 1)
# u = (num_trajectories, num_inputs, num_samples) | u = (num_trajectories, num_samples, num_inputs)
# x = (num_trajectories, num_states, num_samples) | x = (num_trajectories, num_samples, num_states)
# T = (num_params) | T = (num_params)
for ti in range(self.num_trajectories):
h.append(prog.NewContinuousVariables(1,'h'+str(ti)))
for ti in range(self.num_trajectories):
u.append(prog.NewContinuousVariables(1, self.num_samples,'u'+str(ti)))
for ti in range(self.num_trajectories):
x.append(prog.NewContinuousVariables(2, self.num_samples,'x'+str(ti)))
# Add in constraints
for ti in range(self.num_trajectories):
prog.AddBoundingBoxConstraint(self.kMinimumTimeStep, self.kMaximumTimeStep, h[ti])
# prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps
x0 = np.array(self.ic_list[ti]) # TODO: hopefully this isn't subtley bad...
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:,0])
# nudge = np.array([.2, .2])
# prog.AddBoundingBoxConstraint(xf-nudge, xf+nudge, x[ti][:,-1])
prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1])
# Do optional warm start here
if self.warm_start:
prog.SetInitialGuess(h[ti], [(self.kMinimumTimeStep+self.kMaximumTimeStep)/2])
for i in range(self.num_samples):
prog.SetInitialGuess(u[ti][:,i], [0.])
x_interp = (xf-x0)*i/self.num_samples + x0
prog.SetInitialGuess(x[ti][:,i], x_interp)
# prog.SetInitialGuess(u[ti][:,i], np.array(1.0))
for i in range(self.num_samples-1):
AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:,i], x[ti][:,i+1], u[ti][:,i], u[ti][:,i+1], prog)
for i in range(self.num_samples):
#prog.AddQuadraticCost([[2., 0.], [0., 2.]], [0., 0.], x[ti][:,i])
#prog.AddQuadraticCost([25.], [0.], u[ti][:,i])
u_var = u[ti][:,i]
x_var = x[ti][:,i] - xf
h_var = h[ti][0]
#print(u_var.shape, x_var.shape, xf.shape)
prog.AddCost( h_var * (2*x_var.dot(x_var) + 25*u_var.dot(u_var)) )
#prog.AddCost( 2*((x[0]-math.pi)*(x[0]-math.pi) + x[1]*x[1]) + 25*u.dot(u))
kTorqueLimit = 5
prog.AddBoundingBoxConstraint([-kTorqueLimit], [kTorqueLimit], u[ti][:,i])
# prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()]))
# prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x))
# prog.AddCost(final_cost, x[ti][:,-1])
# prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs?
# Setting solver options
#prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation)
#prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out")
# Save references
self.prog = prog
self.h = h
self.u = u
self.x = x
self.T = []
def compute_control(self, x_p1, x_p2, x_puck, p_goal, obstacles):
"""Solve for initial velocity for the puck to bounce the wall once and hit the goal."""
"""Currently does not work"""
# initialize program
N = self.mpc_params.N # length of receding horizon
prog = MathematicalProgram()
# State and input variables
x1 = prog.NewContinuousVariables(N + 1, 4,
name='p1_state') # state of player 1
u1 = prog.NewContinuousVariables(
N, 2, name='p1_input') # inputs for player 1
xp = prog.NewContinuousVariables(
N + 1, 4, name='puck_state') # state of the puck
# Slack variables
t1_kick = prog.NewContinuousVariables(
N + 1, name='kick_time'
) # slack variables that captures if player 1 is kicking or not at the given time step
# Defined as continous, but treated as mixed integer. 1 when kicking
v1_kick = prog.NewContinuousVariables(
N + 1, 2, name='v1_kick') # velocity of player after kick to puck
vp_kick = prog.NewContinuousVariables(
N + 1, 2, name='vp_kick'
) # velocity of puck after being kicked by the player
dist = prog.NewContinuousVariables(
N + 1, name='dist_puck_player') # distance between puck and player
cost = prog.NewContinuousVariables(
N + 1, name='cost') # slack variable to monitor cost
# Compute distance between puck and player as slack variable.
for k in range(N + 1):
r1 = self.sim_params.player_radius
rp = self.sim_params.puck_radius
prog.AddConstraint(
dist[k] == (x1[k, 0:2] - xp[k, 0:2]).dot(x1[k, 0:2] -
xp[k, 0:2]) -
(r1 + rp)**2)
#### COST and final states
# TODO: find adequate final velocity
x_puck_des = np.concatenate(
(p_goal, np.zeros(2)), axis=0) # desired position and vel for puck
for k in range(N + 1):
Q_dist_puck_goal = 10 * np.eye(2)
q_dist_puck_player = 0.1
e1 = x_puck_des[0:2] - xp[k, 0:2]
e2 = xp[k, 0:2] - x1[k, 0:2]
prog.AddConstraint(
cost[k] == e1.dot(np.matmul(Q_dist_puck_goal, e1)) +
q_dist_puck_player * dist[k])
prog.AddCost(cost[k])
#prog.AddQuadraticErrorCost(Q=self.mpc_params.Q_puck, x_desired=x_puck_des, vars=xp[k]) # puck in the goal
#prog.AddQuadraticErrorCost(Q=np.eye(2), x_desired=x_puck_des[0:2], vars=x1[k, 0:2])
#prog.AddQuadraticErrorCost(Q=10*np.eye(2), x_desired=np.array([2, 2]), vars=x1[k, 0:2]) # TEST: control position of the player instead of puck
#for k in range(N):
# prog.AddQuadraticCost(1e-2*u1[k].dot(u1[k])) # be wise on control effort
# Initial states for puck and player
prog.AddBoundingBoxConstraint(x_p1, x_p1,
x1[0]) # Intial state for player 1
prog.AddBoundingBoxConstraint(x_puck, x_puck,
xp[0]) # Initial state for puck
# Compute elastic collision for every possible timestep.
for k in range(N + 1):
v_puck_bfr = xp[k, 2:4]
v_player_bfr = x1[k, 2:4]
v_puck_aft, v_player_aft = self.get_reset_velocities(
v_puck_bfr, v_player_bfr)
prog.AddConstraint(eq(vp_kick[k], v_puck_aft))
prog.AddConstraint(eq(v1_kick[k], v_player_aft))
# Use slack variable to activate guard condition based on distance.
M = 15**2
for k in range(N + 1):
prog.AddLinearConstraint(dist[k] <= M * (1 - t1_kick[k]))
prog.AddLinearConstraint(dist[k] >= -t1_kick[k] * M)
# Hybrid dynamics for player
#for k in range(N):
# A = self.mpc_params.A_player
# B = self.mpc_params.B_player
# x1_kick = np.concatenate((x1[k][0:2], v1_kick[k]), axis=0) # The state of the player after it kicks the puck
# x1_next = np.matmul(A, (1 - t1_kick[k])*x1[k] + t1_kick[k]*x1_kick) + np.matmul(B, u1[k])
# prog.AddConstraint(eq(x1[k+1], x1_next))
# Assuming player dynamics are not affected by collision
for k in range(N):
A = self.mpc_params.A_player
B = self.mpc_params.B_player
x1_next = np.matmul(A, x1[k]) + np.matmul(B, u1[k])
prog.AddConstraint(eq(x1[k + 1], x1_next))
# Hybrid dynamics for puck_mass
for k in range(N):
A = self.mpc_params.A_puck
xp_kick = np.concatenate(
(xp[k][0:2], vp_kick[k]),
axis=0) # State of the puck after it gets kicked
xp_next = np.matmul(A, (1 - t1_kick[k]) * xp[k] +
t1_kick[k] * xp_kick)
prog.AddConstraint(eq(xp[k + 1], xp_next))
# Generate trajectory that is not in direct collision with the puck
for k in range(N + 1):
eps = 0.1
prog.AddConstraint(dist[k] >= -eps)
# Input and arena constraint
self.add_input_limits(prog, u1, N)
self.add_arena_limits(prog, x1, N)
self.add_arena_limits(prog, xp, N)
# fake mixed-integer constraint
#for k in range(N+1):
# prog.AddConstraint(t1_kick[k]*(1-t1_kick[k])==0)
# Hot-start
guess_u1, guess_x1 = self.get_initial_guess(x_p1, p_goal, x_puck[0:2])
prog.SetInitialGuess(x1, guess_x1)
prog.SetInitialGuess(u1, guess_u1)
if not self.prev_xp is None:
prog.SetInitialGuess(xp, self.prev_xp)
#prog.SetInitialGuess(t1_kick, np.ones_like(t1_kick))
# solve for the periods
# solver = SnoptSolver()
#result = solver.Solve(prog)
#if not result.is_success():
# print("Unable to find solution.")
# save for hot-start
#self.prev_xp = result.GetSolution(xp)
#if True:
# print("x1:{}".format(result.GetSolution(x1)))
# print("u1: {}".format( result.GetSolution(u1)))
# print("xp: {}".format( result.GetSolution(xp)))
# print('dist{}'.format(result.GetSolution(dist)))
# print("t1_kick:{}".format(result.GetSolution(t1_kick)))
# print("v1_kick:{}".format(result.GetSolution(v1_kick)))
# print("vp_kick:{}".format(result.GetSolution(vp_kick)))
# print("cost:{}".format(result.GetSolution(cost)))
# return whether successful, and the initial player velocity
#u1_opt = result.GetSolution(u1)
return True, guess_u1[0, :], np.zeros((2))
g = 9.81
r = l/2
f = 10
I = m * (l**2)/12
for n in range(N):
u[:,n] = prog.NewContinuousVariables(2, 'u' + str(n))
dx[:,n] = prog.NewContinuousVariables(3, 'dx' + str(n))
x[:,n] = prog.NewContinuousVariables(3, 'x' + str(n))
s = env.reset().reshape(6, 1)
print(s)
# Add constraints
x0 = [s[0], s[1], s[2]]
prog.AddBoundingBoxConstraint(x0, x0, x[:,0])
x1 = [0]*3
prog.AddBoundingBoxConstraint(x1, x1, dx[:, 0])
def constraint_evaluator1(z):
return np.array([(-(z[6] + z[7])*sin(z[2])/m)*dt + z[3] - z[8],
((z[6] + z[7])*cos(z[2]) - m*g/m)*dt + z[4] - z[9],
((z[6] - z[7])*r/I)*dt + z[5] - z[10]
])
def constraint_evaluator2(z):
return np.array([z[3]*dt + z[0] - z[6], z[4]*dt + z[1] - z[7], z[5]*dt + z[2] - z[8]])
for n in range(N-1):
# Will eventually be prog.AddConstraint(x[:,n+1] == A@x[:,n] + B@u[:,n])
# See drake issues 12841 and 8315
class FourierCollocationProblem:
def __init__(self, system_dynamics, constraints):
start_time = time.time()
self.prog = MathematicalProgram()
self.N = 50 # Number of collocation points
self.M = 10 # Number of frequencies
self.system_dynamics = system_dynamics
self.psi = np.pi * (0.7) # TODO change
(
min_height,
max_height,
min_vel,
max_vel,
h0,
min_travelled_distance,
t_f_min,
t_f_max,
avg_vel_min,
avg_vel_max,
) = constraints
initial_pos = np.array([0, 0, h0])
# Add state trajectory parameters as decision variables
self.coeffs = self.prog.NewContinuousVariables(
3, self.M + 1, "c"
) # (x,y,z) for every frequency
self.phase_delays = self.prog.NewContinuousVariables(3, self.M, "eta")
self.t_f = self.prog.NewContinuousVariables(1, 1, "t_f")[0, 0]
self.avg_vel = self.prog.NewContinuousVariables(1, 1, "V_bar")[0, 0]
# Enforce initial conditions
residuals = self.get_pos_fourier(collocation_time=0) - initial_pos
for residual in residuals:
self.prog.AddConstraint(residual == 0)
# Enforce dynamics at collocation points
for n in range(self.N):
collocation_time = (n / self.N) * self.t_f
pos = self.get_pos_fourier(collocation_time)
vel = self.get_vel_fourier(collocation_time)
vel_dot = self.get_vel_dot_fourier(collocation_time)
residuals = self.continuous_dynamics(pos, vel, vel_dot)
for residual in residuals[3:6]: # TODO only these last three are residuals
self.prog.AddConstraint(residual == 0)
if True:
# Add velocity constraints
squared_vel_norm = vel.T.dot(vel)
self.prog.AddConstraint(min_vel ** 2 <= squared_vel_norm)
self.prog.AddConstraint(squared_vel_norm <= max_vel ** 2)
# Add height constraints
self.prog.AddConstraint(pos[2] <= max_height)
self.prog.AddConstraint(min_height <= pos[2])
# Add constraint on min travelled distance
self.prog.AddConstraint(min_travelled_distance <= self.avg_vel * self.t_f)
# Add constraints on coefficients
if False:
for coeff in self.coeffs.T:
lb = np.array([-500, -500, -500])
ub = np.array([500, 500, 500])
self.prog.AddBoundingBoxConstraint(lb, ub, coeff)
# Add constraints on phase delays
if False:
for etas in self.phase_delays.T:
lb = np.array([0, 0, 0])
ub = np.array([2 * np.pi, 2 * np.pi, 2 * np.pi])
self.prog.AddBoundingBoxConstraint(lb, ub, etas)
# Add constraints on final time and avg vel
self.prog.AddBoundingBoxConstraint(t_f_min, t_f_max, self.t_f)
self.prog.AddBoundingBoxConstraint(avg_vel_min, avg_vel_max, self.avg_vel)
# Add objective function
self.prog.AddCost(-self.avg_vel)
# Set initial guess
coeffs_guess = np.zeros((3, self.M + 1))
coeffs_guess += np.random.rand(*coeffs_guess.shape) * 100
self.prog.SetInitialGuess(self.coeffs, coeffs_guess)
phase_delays_guess = np.zeros((3, self.M))
phase_delays_guess += np.random.rand(*phase_delays_guess.shape) * 1e-1
self.prog.SetInitialGuess(self.phase_delays, phase_delays_guess)
self.prog.SetInitialGuess(self.avg_vel, (avg_vel_max - avg_vel_min) / 2)
self.prog.SetInitialGuess(self.t_f, (t_f_max - t_f_min) / 2)
print(
"Finished formulating the optimization problem in: {0} s".format(
time.time() - start_time
)
)
start_solve_time = time.time()
self.result = Solve(self.prog)
print("Found solution: {0}".format(self.result.is_success()))
print("Found a solution in: {0} s".format(time.time() - start_solve_time))
# TODO input costs
# assert self.result.is_success()
return
def get_solution(self):
coeffs_opt = self.result.GetSolution(self.coeffs)
phase_delays_opt = self.result.GetSolution(self.phase_delays)
t_f_opt = self.result.GetSolution(self.t_f)
avg_vel_opt = self.result.GetSolution(self.avg_vel)
sim_N = 100
dt = t_f_opt / sim_N
times = np.arange(0, t_f_opt, dt)
pos_traj = np.zeros((3, sim_N))
# TODO remove vel traj
vel_traj = np.zeros((3, sim_N))
for i in range(sim_N):
t = times[i]
pos = self.evaluate_pos_traj(
coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t
)
pos_traj[:, i] = pos
vel = self.evaluate_vel_traj(
coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t
)
vel_traj[:, i] = vel
# TODO move plotting out
plot_trj_3_wind(pos_traj.T, np.array([np.sin(self.psi), np.cos(self.psi), 0]))
plt.show()
breakpoint()
return
def evaluate_pos_traj(self, coeffs, phase_delays, t_f, avg_vel, t):
pos_traj = np.copy(coeffs[:, 0])
for m in range(1, self.M):
sine_terms = np.array(
[
np.sin((2 * np.pi * m * t) / t_f + phase_delays[0, m]),
np.sin((2 * np.pi * m * t) / t_f + phase_delays[1, m]),
np.sin((2 * np.pi * m * t) / t_f + phase_delays[2, m]),
]
)
pos_traj += coeffs[:, m] * sine_terms
direction_term = np.array(
[
avg_vel * np.sin(self.psi) * t,
avg_vel * np.cos(self.psi) * t,
0,
]
)
pos_traj += direction_term
return pos_traj
def evaluate_vel_traj(self, coeffs, phase_delays, t_f, avg_vel, t):
vel_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
cos_terms = np.array(
[
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[0, m]),
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[1, m]),
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[2, m]),
]
)
vel_traj += coeffs[:, m] * cos_terms
direction_term = np.array(
[
avg_vel * np.sin(self.psi),
avg_vel * np.cos(self.psi),
0,
]
)
vel_traj += direction_term
return vel_traj
def get_pos_fourier(self, collocation_time):
pos_traj = np.copy(self.coeffs[:, 0])
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
sine_terms = np.array(
[
np.sin(a * collocation_time + self.phase_delays[0, m]),
np.sin(a * collocation_time + self.phase_delays[1, m]),
np.sin(a * collocation_time + self.phase_delays[2, m]),
]
)
pos_traj += self.coeffs[:, m] * sine_terms
direction_term = np.array(
[
self.avg_vel * np.sin(self.psi) * collocation_time,
self.avg_vel * np.cos(self.psi) * collocation_time,
0,
]
)
pos_traj += direction_term
return pos_traj
def get_vel_fourier(self, collocation_time):
vel_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
cos_terms = np.array(
[
a * np.cos(a * collocation_time + self.phase_delays[0, m]),
a * np.cos(a * collocation_time + self.phase_delays[1, m]),
a * np.cos(a * collocation_time + self.phase_delays[2, m]),
]
)
vel_traj += self.coeffs[:, m] * cos_terms
direction_term = np.array(
[
self.avg_vel * np.sin(self.psi),
self.avg_vel * np.cos(self.psi),
0,
]
)
vel_traj += direction_term
return vel_traj
def get_vel_dot_fourier(self, collocation_time):
vel_dot_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
sine_terms = np.array(
[
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[0, m]),
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[1, m]),
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[2, m]),
]
)
vel_dot_traj += self.coeffs[:, m] * sine_terms
return vel_dot_traj
def continuous_dynamics(self, pos, vel, vel_dot):
x = np.concatenate((pos, vel))
x_dot = np.concatenate((vel, vel_dot))
# TODO need to somehow implement input to make this work
return x_dot - self.system_dynamics(x, u)
K = xuK[3:]
uc = K.dot(control_basis(x))
# TODO: snopt doesn't like this. perhaps a softmax?
if (uc.value() > 3.0):
uc = 3.0
elif (uc.value() < -3.0):
uc = -3.0
return [u - uc]
h = []
u = []
x = []
for ti in range(num_trajectories):
h.append(prog.NewContinuousVariables(1))
prog.AddBoundingBoxConstraint(.01, .2, h[ti])
u.append(prog.NewContinuousVariables(1, num_samples, 'u' + str(ti)))
x.append(
prog.NewContinuousVariables(num_states, num_samples, 'x' + str(ti)))
x0 = (.8 + math.pi - .4 * ti, 0.0)
xf = (math.pi, 0.)
if num_states == 4:
x0 += (0., 0.)
xf += (0., 0.)
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0])
prog.AddBoundingBoxConstraint(xf, xf, x[ti][:, -1])
for i in range(num_samples - 1):
def PlanGraspPoints(self):
# First, extract the bounding geometry of the object.
# Generally, our geometry is coming from 3d models, so
# we have to do some legwork to extract 2D geometry. For
# the examples we'll use in this set, we'll assume
# that extracting the convex hull of the first visual element
# is a good representation of the object geometry. (This is
# not great! But it'll do the job for us, since we're going
# to experiment with only simple objects.)
kinsol = self.hand.doKinematics(
self.x_nom[0:self.hand.get_num_positions()])
self.manipuland_link_index = \
self.hand.FindBody(self.manipuland_link_name).get_body_index()
body = self.hand.get_body(self.manipuland_link_index)
# For projecting into XY plane
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0.,
1.]])
all_points = ExtractPlanarObjectGeometryConvexHull(body, Tview)
# For later use: precompute the fingertip positions of the
# robot in the nominal posture.
nominal_fingertip_points = np.empty((2, self.num_fingers))
for i in range(self.num_fingers):
nominal_fingertip_points[:, i] = self.hand.transformPoints(
kinsol, self.fingertip_position, self.finger_link_indices[i],
0)[0:2, 0]
# Search for an optimal grasp with N points
random.seed(42)
np.random.seed(42)
def get_random_point_and_normal_on_surface(all_points):
num_points = all_points.shape[1]
i = random.choice(range(num_points - 1))
first_point = np.asarray([all_points[0][i], all_points[1][i]])
second_point = np.asarray(
[all_points[0][i + 1], all_points[1][i + 1]])
first_to_second = second_point - first_point
# Clip to avoid interpolating close to a corner
interpolate_param = np.clip(np.random.rand(), 0.2, 0.8)
rand_point = first_point + interpolate_param * first_to_second
normal = np.array([-first_to_second[1], first_to_second[0]]) \
/ np.linalg.norm(first_to_second)
return rand_point, normal
best_conv_volume = 0
best_points = []
best_normals = []
best_finger_assignments = []
for i in range(self.n_grasp_search_iters):
grasp_points = []
normals = []
for j in range(self.num_fingers):
point, normal = \
get_random_point_and_normal_on_surface(all_points)
# check for duplicates or close points -- fingertip
# radius is 0.2, so require twice that margin to avoid
# intersection fingertips.
num_rejected = 0
while min([1.0] + [
np.linalg.norm(grasp_point - point, 2)
for grasp_point in grasp_points
]) <= 0.4:
point, normal = \
get_random_point_and_normal_on_surface(all_points)
num_rejected += 1
if num_rejected > 10000:
print "Rejected 10000 points in a row due to crowding." \
" Your object is a bit too small for your number of" \
" fingers."
break
grasp_points.append(point)
normals.append(normal)
if achieves_force_closure(grasp_points, normals, self.mu):
# Test #1: Rank in terms of convex hull volume of grasp points
volume = compute_convex_hull_volume(grasp_points)
if volume < best_conv_volume:
continue
# Test #2: Does IK work for this point?
self.grasp_points = grasp_points
self.grasp_normals = normals
# Pick optimal finger assignment that
# minimizes distance between fingertips in the
# nominal posture, and the chosen grasp points
grasp_points_world = self.transform_grasp_points_manipuland(
self.x_nom)[0:2, :]
prog = MathematicalProgram()
# We'd *really* like these to be binary variables,
# but unfortunately don't have a MIP solver available in the
# course docker container. Instead, we'll solve an LP,
# and round the result to the nearest feasible integer
# solutions. Intuitively, this LP should probably reach its
# optimal value at an extreme point (where the variables
# all take integer value). I've not yet observed this
# not occuring in practice!
assignment_vars = prog.NewContinuousVariables(
self.num_fingers, self.num_fingers, "assignment")
for grasp_i in range(self.num_fingers):
# Every row and column of assignment vars sum to one --
# each finger matches to one grasp position
prog.AddLinearConstraint(
np.sum(assignment_vars[:, grasp_i]) == 1.)
prog.AddLinearConstraint(
np.sum(assignment_vars[grasp_i, :]) == 1.)
for finger_i in range(self.num_fingers):
# If this grasp assignment is active,
# add a cost equal to the distance between
# nominal pose and grasp position
prog.AddLinearCost(
assignment_vars[grasp_i, finger_i] *
np.linalg.norm(grasp_points_world[:, grasp_i] -
nominal_fingertip_points[:,
finger_i]))
prog.AddBoundingBoxConstraint(
0., 1., assignment_vars[grasp_i, finger_i])
result = Solve(prog)
assignments = result.GetSolution(assignment_vars)
# Round assignments to nearest feasible set
claimed = [False] * self.num_fingers
for grasp_i in range(self.num_fingers):
order = np.argsort(assignments[grasp_i, :])
fill_i = self.num_fingers - 1
while claimed[order[fill_i]] is not False:
fill_i -= 1
if fill_i < 0:
raise Exception("Finger association failed. "
"Horrible bug. Tell Greg")
assignments[grasp_i, :] *= 0.
assignments[grasp_i, order[fill_i]] = 1.
claimed[order[fill_i]] = True
# Populate actual assignments
self.grasp_finger_assignments = []
for grasp_i in range(self.num_fingers):
for finger_i in range(self.num_fingers):
if assignments[grasp_i, finger_i] == 1.:
self.grasp_finger_assignments.append(
(finger_i, np.array(self.fingertip_position)))
continue
qinit, info = self.ComputeTargetPosture(
self.x_nom,
self.x_nom[(self.nq - 3):self.nq],
backoff_distance=0.2)
if info != 1:
continue
best_conv_volume = volume
best_points = grasp_points
best_normals = normals
best_finger_assignments = self.grasp_finger_assignments
if len(best_points) == 0:
print "After %d attempts, couldn't find a good grasp "\
"for this object." % self.n_grasp_search_iters
print "Proceeding with a horrible random guess."
best_points = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_normals = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_finger_assignments = [(i, self.fingertip_position)
for i in range(self.num_fingers)]
self.grasp_points = best_points
self.grasp_normals = best_normals
self.grasp_finger_assignments = best_finger_assignments
def solveOptimization(state_init,
t_impact,
impact_combination,
T,
u_guess=None,
x_guess=None,
h_guess=None):
prog = MathematicalProgram()
h = prog.NewContinuousVariables(T, name='h')
u = prog.NewContinuousVariables(rows=T + 1,
cols=2 * n_quadrotors,
name='u')
x = prog.NewContinuousVariables(rows=T + 1,
cols=6 * n_quadrotors + 4 * n_balls,
name='x')
dv = prog.decision_variables()
prog.AddBoundingBoxConstraint([h_min] * T, [h_max] * T, h)
for i in range(n_quadrotors):
sys = Quadrotor2D()
context = sys.CreateDefaultContext()
dir_coll_constr = DirectCollocationConstraint(sys, context)
ind_x = 6 * i
ind_u = 2 * i
for t in range(T):
impact_indices = impact_combination[np.argmax(
np.abs(t - t_impact) <= 1)]
quad_ind, ball_ind = impact_indices[0], impact_indices[1]
if quad_ind == i and np.any(
t == t_impact
): # Don't add Direct collocation constraint at impact
continue
elif quad_ind == i and (np.any(t == t_impact - 1)
or np.any(t == t_impact + 1)):
prog.AddConstraint(
eq(
x[t + 1,
ind_x:ind_x + 3], x[t, ind_x:ind_x + 3] + h[t] *
x[t + 1, ind_x + 3:ind_x + 6])) # Backward euler
prog.AddConstraint(
eq(x[t + 1, ind_x + 3:ind_x + 6], x[t,
ind_x + 3:ind_x + 6])
) # Zero-acceleration assumption during this time step. Should maybe replace with something less naive
else:
AddDirectCollocationConstraint(
dir_coll_constr, np.array([[h[t]]]),
x[t, ind_x:ind_x + 6].reshape(-1, 1),
x[t + 1, ind_x:ind_x + 6].reshape(-1, 1),
u[t, ind_u:ind_u + 2].reshape(-1, 1),
u[t + 1, ind_u:ind_u + 2].reshape(-1, 1), prog)
for i in range(n_balls):
sys = Ball2D()
context = sys.CreateDefaultContext()
dir_coll_constr = DirectCollocationConstraint(sys, context)
ind_x = 6 * n_quadrotors + 4 * i
for t in range(T):
impact_indices = impact_combination[np.argmax(
np.abs(t - t_impact) <= 1)]
quad_ind, ball_ind = impact_indices[0], impact_indices[1]
if ball_ind == i and np.any(
t == t_impact
): # Don't add Direct collocation constraint at impact
continue
elif ball_ind == i and (np.any(t == t_impact - 1)
or np.any(t == t_impact + 1)):
prog.AddConstraint(
eq(
x[t + 1,
ind_x:ind_x + 2], x[t, ind_x:ind_x + 2] + h[t] *
x[t + 1, ind_x + 2:ind_x + 4])) # Backward euler
prog.AddConstraint(
eq(x[t + 1,
ind_x + 2:ind_x + 4], x[t, ind_x + 2:ind_x + 4] +
h[t] * np.array([0, -9.81])))
else:
AddDirectCollocationConstraint(
dir_coll_constr, np.array([[h[t]]]),
x[t, ind_x:ind_x + 4].reshape(-1, 1),
x[t + 1, ind_x:ind_x + 4].reshape(-1, 1),
u[t, 0:0].reshape(-1, 1), u[t + 1, 0:0].reshape(-1,
1), prog)
# Initial conditions
prog.AddLinearConstraint(eq(x[0, :], state_init))
# Final conditions
prog.AddLinearConstraint(eq(x[T, 0:14], state_final[0:14]))
# Quadrotor final conditions (full state)
for i in range(n_quadrotors):
ind = 6 * i
prog.AddLinearConstraint(
eq(x[T, ind:ind + 6], state_final[ind:ind + 6]))
# Ball final conditions (position only)
for i in range(n_balls):
ind = 6 * n_quadrotors + 4 * i
prog.AddLinearConstraint(
eq(x[T, ind:ind + 2], state_final[ind:ind + 2]))
# Input constraints
for i in range(n_quadrotors):
prog.AddLinearConstraint(ge(u[:, 2 * i], -20.0))
prog.AddLinearConstraint(le(u[:, 2 * i], 20.0))
prog.AddLinearConstraint(ge(u[:, 2 * i + 1], -20.0))
prog.AddLinearConstraint(le(u[:, 2 * i + 1], 20.0))
# Don't allow quadrotor to pitch more than 60 degrees
for i in range(n_quadrotors):
prog.AddLinearConstraint(ge(x[:, 6 * i + 2], -np.pi / 3))
prog.AddLinearConstraint(le(x[:, 6 * i + 2], np.pi / 3))
# Ball position constraints
# for i in range(n_balls):
# ind_i = 6*n_quadrotors + 4*i
# prog.AddLinearConstraint(ge(x[:,ind_i],-2.0))
# prog.AddLinearConstraint(le(x[:,ind_i], 2.0))
# prog.AddLinearConstraint(ge(x[:,ind_i+1],-3.0))
# prog.AddLinearConstraint(le(x[:,ind_i+1], 3.0))
# Impact constraint
quad_temp = Quadrotor2D()
for i in range(n_quadrotors):
for j in range(n_balls):
ind_q = 6 * i
ind_b = 6 * n_quadrotors + 4 * j
for t in range(T):
if np.any(
t == t_impact
): # If quad i and ball j impact at time t, add impact constraint
impact_indices = impact_combination[np.argmax(
t == t_impact)]
quad_ind, ball_ind = impact_indices[0], impact_indices[1]
if quad_ind == i and ball_ind == j:
# At impact, witness function == 0
prog.AddConstraint(lambda a: np.array([
CalcClosestDistanceQuadBall(a[0:3], a[3:5])
]).reshape(1, 1),
lb=np.zeros((1, 1)),
ub=np.zeros((1, 1)),
vars=np.concatenate(
(x[t, ind_q:ind_q + 3],
x[t,
ind_b:ind_b + 2])).reshape(
-1, 1))
# At impact, enforce discrete collision update for both ball and quadrotor
prog.AddConstraint(
CalcPostCollisionStateQuadBallResidual,
lb=np.zeros((6, 1)),
ub=np.zeros((6, 1)),
vars=np.concatenate(
(x[t, ind_q:ind_q + 6], x[t, ind_b:ind_b + 4],
x[t + 1, ind_q:ind_q + 6])).reshape(-1, 1))
prog.AddConstraint(
CalcPostCollisionStateBallQuadResidual,
lb=np.zeros((4, 1)),
ub=np.zeros((4, 1)),
vars=np.concatenate(
(x[t, ind_q:ind_q + 6], x[t, ind_b:ind_b + 4],
x[t + 1, ind_b:ind_b + 4])).reshape(-1, 1))
# rough constraints to enforce hitting center-ish of paddle
prog.AddLinearConstraint(
x[t, ind_q] - x[t, ind_b] >= -0.01)
prog.AddLinearConstraint(
x[t, ind_q] - x[t, ind_b] <= 0.01)
continue
# Everywhere else, witness function must be > 0
prog.AddConstraint(lambda a: np.array([
CalcClosestDistanceQuadBall(a[ind_q:ind_q + 3], a[
ind_b:ind_b + 2])
]).reshape(1, 1),
lb=np.zeros((1, 1)),
ub=np.inf * np.ones((1, 1)),
vars=x[t, :].reshape(-1, 1))
# Don't allow quadrotor collisions
# for t in range(T):
# for i in range(n_quadrotors):
# for j in range(i+1, n_quadrotors):
# prog.AddConstraint((x[t,6*i]-x[t,6*j])**2 + (x[t,6*i+1]-x[t,6*j+1])**2 >= 0.65**2)
# Quadrotors stay on their own side
# prog.AddLinearConstraint(ge(x[:, 0], 0.3))
# prog.AddLinearConstraint(le(x[:, 6], -0.3))
###############################################################################
# Set up initial guesses
initial_guess = np.empty(prog.num_vars())
# # initial guess for the time step
prog.SetDecisionVariableValueInVector(h, h_guess, initial_guess)
x_init[0, :] = state_init
prog.SetDecisionVariableValueInVector(x, x_guess, initial_guess)
prog.SetDecisionVariableValueInVector(u, u_guess, initial_guess)
solver = SnoptSolver()
print("Solving...")
result = solver.Solve(prog, initial_guess)
# print(GetInfeasibleConstraints(prog,result))
# be sure that the solution is optimal
assert result.is_success()
print(f'Solution found? {result.is_success()}.')
#################################################################################
# Extract results
# get optimal solution
h_opt = result.GetSolution(h)
x_opt = result.GetSolution(x)
u_opt = result.GetSolution(u)
time_breaks_opt = np.array([sum(h_opt[:t]) for t in range(T + 1)])
u_opt_poly = PiecewisePolynomial.ZeroOrderHold(time_breaks_opt, u_opt.T)
# x_opt_poly = PiecewisePolynomial.Cubic(time_breaks_opt, x_opt.T, False)
x_opt_poly = PiecewisePolynomial.FirstOrderHold(
time_breaks_opt, x_opt.T
) # Switch to first order hold instead of cubic because cubic was taking too long to create
#################################################################################
# Create list of K matrices for time varying LQR
context = quad_plant.CreateDefaultContext()
breaks = copy.copy(
time_breaks_opt) #np.linspace(0, x_opt_poly.end_time(), 100)
K_samples = np.zeros((breaks.size, 12 * n_quadrotors))
for i in range(n_quadrotors):
K = None
for j in range(breaks.size):
context.SetContinuousState(
x_opt_poly.value(breaks[j])[6 * i:6 * (i + 1)])
context.FixInputPort(
0,
u_opt_poly.value(breaks[j])[2 * i:2 * (i + 1)])
linear_system = FirstOrderTaylorApproximation(quad_plant, context)
A = linear_system.A()
B = linear_system.B()
try:
K, _, _ = control.lqr(A, B, Q, R)
except:
assert K is not None, "Failed to calculate initial K for quadrotor " + str(
i)
print("Warning: Failed to calculate K at timestep", j,
"for quadrotor", i, ". Using K from previous timestep")
K_samples[j, 12 * i:12 * (i + 1)] = K.reshape(-1)
K_poly = PiecewisePolynomial.ZeroOrderHold(breaks, K_samples.T)
return u_opt_poly, x_opt_poly, K_poly, h_opt
def static_controller(self, qref, verbose=False):
"""
Generates a controller to maintain a static pose
Arguments:
qref: (N,) numpy array, the static pose to be maintained
Return Values:
u: (M,) numpy array, actuations to best achieve static pose
f: (C,) numpy array, associated normal reaction forces
static_controller generates the actuations and reaction forces assuming the velocity and accelerations are zero. Thus, the equation to solve is:
N(q) = B*u + J*f
where N is a vector of gravitational and conservative generalized forces, B is the actuation selection matrix, and J is the contact-Jacobian transpose.
Currently, static_controller only considers the effects of the normal forces. Frictional forces are not yet supported
"""
#TODO: Solve for friction forces as well
# Check inputs
if qref.ndim > 1 or qref.shape[0] != self.multibody.num_positions():
raise ValueError(
f"Reference position mut be ({self.multibody.num_positions(),},) array"
)
# Set the context
context = self.multibody.CreateDefaultContext()
self.multibody.SetPositions(context, qref)
# Get the necessary properties
G = self.multibody.CalcGravityGeneralizedForces(context)
Jn, _ = self.GetContactJacobians(context)
phi = self.GetNormalDistances(context)
B = self.multibody.MakeActuationMatrix()
#Collect terms
A = np.concatenate([B, Jn.transpose()], axis=1)
# Create the mathematical program
prog = MathematicalProgram()
l_var = prog.NewContinuousVariables(self.num_contacts(), name="forces")
u_var = prog.NewContinuousVariables(self.multibody.num_actuators(),
name="controls")
# Ensure dynamics approximately satisfied
prog.AddL2NormCost(A=A,
b=-G,
vars=np.concatenate([u_var, l_var], axis=0))
# Enforce normal complementarity
prog.AddBoundingBoxConstraint(np.zeros(l_var.shape),
np.full(l_var.shape, np.inf), l_var)
prog.AddConstraint(phi.dot(l_var) == 0)
# Solve
result = Solve(prog)
# Check for a solution
if result.is_success():
u = result.GetSolution(u_var)
f = result.GetSolution(l_var)
if verbose:
printProgramReport(result, prog)
return (u, f)
else:
print(f"Optimization failed. Returning zeros")
if verbose:
printProgramReport(result, prog)
return (np.zeros(u_var.shape), np.zeros(l_var.shape))
def make_multiple_dircol_trajectories(num_trajectories,
num_samples,
initial_conditions=None):
from pydrake.all import (
AutoDiffXd,
Expression,
Variable,
MathematicalProgram,
SolverType,
SolutionResult,
DirectCollocationConstraint,
AddDirectCollocationConstraint,
PiecewisePolynomial,
)
import pydrake.symbolic as sym
from pydrake.examples.pendulum import (PendulumPlant)
# initial_conditions maps (ti) -> [1xnum_states] initial state
if initial_conditions is not None:
initial_conditions = intial_cond_dict[initial_conditions]
assert callable(initial_conditions)
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol_constraint = DirectCollocationConstraint(plant, context)
# num_trajectories = 15;
# num_samples = 15;
prog = MathematicalProgram()
# K = prog.NewContinuousVariables(1,7,'K')
def cos(x):
if isinstance(x, AutoDiffXd):
return x.cos()
elif isinstance(x, Variable):
return sym.cos(x)
return math.cos(x)
def sin(x):
if isinstance(x, AutoDiffXd):
return x.sin()
elif isinstance(x, Variable):
return sym.sin(x)
return math.sin(x)
def final_cost(x):
return 100. * (cos(.5 * x[0])**2 + x[1]**2)
h = []
u = []
x = []
xf = (math.pi, 0.)
for ti in range(num_trajectories):
h.append(prog.NewContinuousVariables(1))
prog.AddBoundingBoxConstraint(.01, .2, h[ti])
# prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps
u.append(prog.NewContinuousVariables(1, num_samples, 'u' + str(ti)))
x.append(prog.NewContinuousVariables(2, num_samples, 'x' + str(ti)))
# Use Russ's initial conditions, unless I pass in a function myself.
if initial_conditions is None:
x0 = (.8 + math.pi - .4 * ti, 0.0)
else:
x0 = initial_conditions(ti)
assert len(x0) == 2 #TODO: undo this hardcoding.
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0])
nudge = np.array([.2, .2])
prog.AddBoundingBoxConstraint(xf - nudge, xf + nudge, x[ti][:, -1])
# prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1])
for i in range(num_samples - 1):
AddDirectCollocationConstraint(dircol_constraint, h[ti],
x[ti][:, i], x[ti][:, i + 1],
u[ti][:, i], u[ti][:, i + 1], prog)
for i in range(num_samples):
prog.AddQuadraticCost([1.], [0.], u[ti][:, i])
# prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()]))
# prog.AddBoundingBoxConstraint([-3.], [3.], u[ti][:,i])
# prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x))
# prog.AddCost(final_cost, x[ti][:,-1])
# prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs?
#prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation)
prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out")
# result = prog.Solve()
# print(result)
# print(prog.GetSolution(K))
# print(prog.GetSolution(K).dot(control_basis(x[0][:,0])))
#if (result != SolutionResult.kSolutionFound):
# f = open('/tmp/snopt.out', 'r')
# print(f.read())
# f.close()
return prog, h, u, x
