def _get_trajopt_obj(self, plan, active_ts=None):
if active_ts == None:
active_ts = (0, plan.horizon - 1)
start, end = active_ts
traj_objs = []
for param in plan.params.values():
if param not in self._param_to_ll:
continue
if param._type in ['Robot', 'Can']:
for attr_name in param.__dict__.iterkeys():
attr_type = param.get_attr_type(attr_name)
if issubclass(attr_type, Vector):
T = end - start + 1
K = attr_type.dim
attr_val = getattr(param, attr_name)
KT = K * T
v = -1 * np.ones((KT - K, 1))
d = np.vstack((np.ones((KT - K, 1)), np.zeros((K, 1))))
# [:,0] allows numpy to see v and d as one-dimensional so
# that numpy will create a diagonal matrix with v and d as a diagonal
P = np.diag(v[:, 0], K) + np.diag(d[:, 0])
Q = np.dot(np.transpose(P), P)
quad_expr = QuadExpr(Q, np.zeros((1, KT)),
np.zeros((1, 1)))
robot_ll = self._param_to_ll[param]
ll_attr_val = getattr(robot_ll, attr_name)
robot_ll_grb_vars = ll_attr_val.reshape((KT, 1),
order='F')
# init_val = attr_val[:, start:end+1].reshape((KT, 1), order='F')
# bexpr = BoundExpr(quad_expr, Variable(robot_ll_grb_vars, init_val))
bexpr = BoundExpr(quad_expr,
Variable(robot_ll_grb_vars))
traj_objs.append(bexpr)
return traj_objs
def create_variable(self, grb_vars, init_vals, save=False):
"""
if save is Ture
Update the grb_init_mapping so that each grb_var is mapped to
the right initial values.
Then find the sco variables that includes the grb variables we are updating and change the corresponding initial values inside of it.
if save is False
Iterate the var_list and use the last initial value used for each gurobi, and construct the sco variables
"""
sco_var, grb_val_map, ret_val = None, {}, []
for grb, v in zip(grb_vars.flatten(), init_vals.flatten()):
grb_name = grb.VarName
if save: self.grb_init_mapping[grb_name] = v
grb_val_map[grb_name] = self.grb_init_mapping.get(grb_name, v)
ret_val.append(grb_val_map[grb_name])
if grb_name in self._grb_to_var_ind.keys():
for var, i in self._grb_to_var_ind[grb_name]:
var._value[i] = grb_val_map[grb_name]
if np.all(var._grb_vars is grb_vars):
sco_var = var
if sco_var is None:
sco_var = Variable(grb_vars, np.array(ret_val).reshape((len(ret_val), 1)))
self.var_list.append(sco_var)
for i, grb in enumerate(grb_vars.flatten()):
index_val_list = self._grb_to_var_ind.get(grb.VarName, [])
index_val_list.append((sco_var, i))
self._grb_to_var_ind[grb.VarName] = index_val_list
if DEBUG: self.check_sync()
return sco_var
def _get_trajopt_obj(self, plan, active_ts=None):
if active_ts == None:
active_ts = (0, plan.horizon - 1)
start, end = active_ts
traj_objs = []
for param in plan.params.values():
if param not in self._param_to_ll:
continue
if param._type in ['Robot', 'Can']:
T = end - start + 1
K = 2
pose = param.pose
KT = K * T
v = -1 * np.ones((KT - K, 1))
d = np.vstack((np.ones((KT - K, 1)), np.zeros((K, 1))))
# [:,0] allows numpy to see v and d as one-dimensional so
# that numpy will create a diagonal matrix with v and d as a diagonal
P = np.diag(v[:, 0], K) + np.diag(d[:, 0])
Q = np.dot(np.transpose(P), P)
Q *= TRAJOPT_COEFF
quad_expr = QuadExpr(Q, np.zeros((1, KT)), np.zeros((1, 1)))
robot_ll = self._param_to_ll[param]
robot_ll_grb_vars = robot_ll.pose.reshape((KT, 1), order='F')
init_val = param.pose[:, start:end + 1].reshape((KT, 1),
order='F')
bexpr = BoundExpr(quad_expr,
Variable(robot_ll_grb_vars, init_val))
traj_objs.append(bexpr)
return traj_objs
def _get_transfer_obj(self, plan, norm):
transfer_objs = []
if norm == 'min-vel':
for param in plan.params.values():
if param._type in ['Robot', 'Can']:
T = plan.horizon
K = 2
pose = param.pose
assert (K, T) == pose.shape
KT = K * T
v = -1 * np.ones((KT - K, 1))
d = np.vstack((np.ones((KT - K, 1)), np.zeros((K, 1))))
# [:,0] allows numpy to see v and d as one-dimensional so
# that numpy will create a diagonal matrix with v and d as a diagonal
P = np.diag(v[:, 0], K) + np.diag(d[:, 0])
Q = np.dot(np.transpose(P), P)
cur_pose = pose.reshape((KT, 1), order='F')
A = -2 * cur_pose.T.dot(Q)
b = cur_pose.T.dot(Q.dot(cur_pose))
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2 * self.transfer_coeff * Q,
self.transfer_coeff * A,
self.transfer_coeff * b)
ll_param = self._param_to_ll[param]
ll_grb_vars = ll_param.pose.reshape((KT, 1), order='F')
bexpr = BoundExpr(quad_expr,
Variable(ll_grb_vars, cur_pose))
transfer_objs.append(bexpr)
else:
raise NotImplemented
return transfer_objs
def _resample(self, plan, preds):
val, attr_inds = None, None
for negated, pred, t in preds:
## returns a vector of new values and an
## attr_inds (OrderedDict) that gives the mapping
## to parameter attributes
if (self.early_converge and self._failed_groups != ['all']
and self._failed_groups != [] and not np.any([
param.name in self._failed_groups
for param in pred.params
])):
## using early converge and one group didn't converge
## but no parameter from this pred in that group
continue
val, attr_inds = pred.resample(negated, t, plan)
## no resample defined for that pred
if val is not None: break
if val is None:
# import pdb; pdb.set_trace()
return []
t_local = t
bexprs = []
i = 0
for p in attr_inds:
## get the ll_param for p and gurobi variables
ll_p = self._param_to_ll[p]
if p.is_symbol(): t_local = 0
n_vals = 0
grb_vars = []
for attr, ind_arr in attr_inds[p]:
n_vals += len(ind_arr)
grb_vars.extend(list(getattr(ll_p, attr)[ind_arr, t_local]))
for j, grb_var in enumerate(grb_vars):
## create an objective saying stay close to this value
## e(x) = x^2 - 2*val[i+j]*x + val[i+j]^2
Q = np.eye(1)
A = -2 * val[i + j] * np.ones((1, 1))
b = np.ones((1, 1)) * np.power(val[i + j], 2)
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2 * Q * self.rs_coeff, A * self.rs_coeff,
b * self.rs_coeff)
v_arr = np.array([grb_var]).reshape((1, 1), order='F')
init_val = np.ones((1, 1)) * val[i + j]
bexpr = BoundExpr(quad_expr,
Variable(v_arr, val[i + j].reshape((1, 1))))
bexprs.append(bexpr)
i += n_vals
return bexprs
def _get_trajopt_obj(self, plan, active_ts=None):
"""
This function selects parameter of type Robot and Can and returns
the expression e(x) = |Px|^2
Which optimize trajectory so that robot and can's attributes in
current timestep is close to that of next timestep.
forming a straight line between each end points.
Where P is the KT x KT matrix, where Px is the difference of
value in current timestep compare to next timestep
"""
if active_ts == None:
active_ts = (0, plan.horizon-1)
start, end = active_ts
traj_objs = []
for param in plan.params.values():
if param not in self._param_to_ll:
continue
if param._type in ['Robot', 'Can']:
for attr_name in param.__dict__.iterkeys():
attr_type = param.get_attr_type(attr_name)
if issubclass(attr_type, Vector):
T = end - start + 1
K = attr_type.dim
attr_val = getattr(param, attr_name)
KT = K*T
v = -1 * np.ones((KT - K, 1))
d = np.vstack((np.ones((KT - K, 1)), np.zeros((K, 1))))
# [:,0] allows numpy to see v and d as one-dimensional so
# that numpy will create a diagonal matrix with v and d as a diagonal
P = np.diag(v[:, 0], K) + np.diag(d[:, 0])
Q = np.dot(np.transpose(P), P)
Q *= TRAJOPT_COEFF
quad_expr = None
if attr_name == 'pose' and param._type == 'Robot':
quad_expr = QuadExpr(BASE_MOVE_COEFF*Q,
np.zeros((1,KT)),
np.zeros((1,1)))
else:
quad_expr = QuadExpr(Q, np.zeros((1,KT)), np.zeros((1,1)))
param_ll = self._param_to_ll[param]
ll_attr_val = getattr(param_ll, attr_name)
param_ll_grb_vars = ll_attr_val.reshape((KT, 1), order='F')
attr_val = getattr(param, attr_name)
init_val = attr_val[:, start:end+1].reshape((KT, 1), order='F')
bexpr = BoundExpr(quad_expr, Variable(param_ll_grb_vars, init_val))
# bexpr = BoundExpr(quad_expr, Variable(param_ll_grb_vars))
traj_objs.append(bexpr)
return traj_objs
def _get_transfer_obj(self, plan, norm):
"""
This function returns the expression e(x) = P|x - cur|^2
Which says the optimized trajectory should be close to the
previous trajectory.
Where P is the KT x KT matrix, where Px is the difference of parameter's attributes' current value and parameter's next timestep value
"""
transfer_objs = []
if norm == 'min-vel':
for param in plan.params.values():
# if param._type in ['Robot', 'Can', 'EEPose']:
for attr_name in param.__dict__.iterkeys():
attr_type = param.get_attr_type(attr_name)
if issubclass(attr_type, Vector):
if param.is_symbol():
T = 1
else:
T = plan.horizon
K = attr_type.dim
attr_val = getattr(param, attr_name)
# pose = param.pose
assert (K, T) == attr_val.shape
KT = K*T
v = -1 * np.ones((KT - K, 1))
d = np.vstack((np.ones((KT - K, 1)), np.zeros((K, 1))))
# [:,0] allows numpy to see v and d as one-dimensional so
# that numpy will create a diagonal matrix with v and d as a diagonal
P = np.diag(v[:, 0], K) + np.diag(d[:, 0])
# P = np.eye(KT)
Q = np.dot(np.transpose(P), P)
cur_val = attr_val.reshape((KT, 1), order='F')
A = -2*cur_val.T.dot(Q)
b = cur_val.T.dot(Q.dot(cur_val))
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2*self.transfer_coeff*Q,
self.transfer_coeff*A, self.transfer_coeff*b)
param_ll = self._param_to_ll[param]
ll_attr_val = getattr(param_ll, attr_name)
param_ll_grb_vars = ll_attr_val.reshape((KT, 1), order='F')
bexpr = BoundExpr(quad_expr, Variable(param_ll_grb_vars, cur_val))
transfer_objs.append(bexpr)
else:
raise NotImplemented
return transfer_objs
def _resample(self, plan, preds):
"""
This function first calls fail predicate's resample function,
then, uses the resampled value to create a square difference cost
function e(x) = |x - rs_val|^2 that will be minimized later.
rs_val is the resampled value
"""
val, attr_inds = None, None
for negated, pred, t in preds:
## returns a vector of new values and an
## attr_inds (OrderedDict) that gives the mapping
## to parameter attributes
val, attr_inds = pred.resample(negated, t, plan)
## if no resample defined for that pred, continue
if val is not None: break
if val is None:
return []
bexprs, i = [], 0
for p in attr_inds:
## get the ll_param for p and gurobi variables
ll_p = self._param_to_ll[p]
n_vals = 0
grb_vars = []
for attr, ind_arr, t in attr_inds[p]:
n_vals += len(ind_arr)
grb_vars.extend(
list(getattr(ll_p, attr)[ind_arr, t].flatten()))
for j, grb_var in enumerate(grb_vars):
## create an objective saying stay close to the resampled value
## e(x) = (x - val[i+j])**2
## e(x) = x^2 - 2*val[i+j]*x + val[i+j]^2
Q = np.eye(1)
A = -2*val[i+j]*np.ones((1, 1))
b = np.ones((1, 1))*np.power(val[i+j], 2)
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2*Q*self.rs_coeff, A*self.rs_coeff, b*self.rs_coeff)
v_arr = np.array([grb_var]).reshape((1, 1), order='F')
init_val = np.ones((1, 1))*val[i+j]
bexpr = BoundExpr(quad_expr,
Variable(v_arr, val[i+j].reshape((1, 1))))
bexprs.append(bexpr)
i += n_vals
return bexprs
def _get_transfer_obj(self, plan, norm):
transfer_objs = []
if norm == 'min-vel':
for param in plan.params.values():
# if param._type in ['Robot', 'Can', 'EEPose']:
for attr_name in param.__dict__.iterkeys():
attr_type = param.get_attr_type(attr_name)
if issubclass(attr_type, Vector):
if param.is_symbol():
T = 1
else:
T = plan.horizon
K = attr_type.dim
attr_val = getattr(param, attr_name)
# pose = param.pose
assert (K, T) == attr_val.shape
KT = K * T
v = -1 * np.ones((KT - K, 1))
d = np.vstack((np.ones((KT - K, 1)), np.zeros((K, 1))))
# [:,0] allows numpy to see v and d as one-dimensional so
# that numpy will create a diagonal matrix with v and d as a diagonal
P = np.diag(v[:, 0], K) + np.diag(d[:, 0])
Q = np.dot(np.transpose(P), P)
cur_val = attr_val.reshape((KT, 1), order='F')
A = -2 * cur_val.T.dot(Q)
b = cur_val.T.dot(Q.dot(cur_val))
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2 * self.transfer_coeff * Q,
self.transfer_coeff * A,
self.transfer_coeff * b)
param_ll = self._param_to_ll[param]
ll_attr_val = getattr(param_ll, attr_name)
param_ll_grb_vars = ll_attr_val.reshape((KT, 1),
order='F')
bexpr = BoundExpr(quad_expr,
Variable(param_ll_grb_vars, cur_val))
transfer_objs.append(bexpr)
else:
raise NotImplemented
return transfer_objs
def _spawn_sco_var_for_pred(self, pred, t):
x = np.empty(pred.x_dim, dtype=object)
v = np.empty(pred.x_dim)
i = 0
start, end = pred.active_range
for rel_t in range(start, end + 1):
for p in pred.attr_inds:
for attr, ind_arr in pred.attr_inds[p]:
n_vals = len(ind_arr)
ll_p = self._param_to_ll[p]
if p.is_symbol():
x[i:i + n_vals] = getattr(ll_p, attr)[ind_arr, 0]
v[i:i + n_vals] = getattr(p, attr)[ind_arr, 0]
else:
x[i:i + n_vals] = getattr(
ll_p, attr)[ind_arr, t + rel_t - self.ll_start]
v[i:i + n_vals] = getattr(p, attr)[ind_arr, t + rel_t]
i += n_vals
assert i >= pred.x_dim
x = x.reshape((pred.x_dim, 1))
v = v.reshape((pred.x_dim, 1))
return Variable(x, v)
def _spawn_sco_var_for_pred(self, pred, t):
i = 0
x = np.empty(pred.x_dim, dtype=object)
v = np.empty(pred.x_dim)
for p in pred.attr_inds:
for attr, ind_arr in pred.attr_inds[p]:
n_vals = len(ind_arr)
ll_p = self._param_to_ll[p]
if p.is_symbol():
x[i:i + n_vals] = getattr(ll_p, attr)[ind_arr, 0]
v[i:i + n_vals] = getattr(p, attr)[ind_arr, 0]
else:
x[i:i + n_vals] = getattr(ll_p, attr)[ind_arr,
t - self.ll_start]
v[i:i + n_vals] = getattr(p, attr)[ind_arr, t]
i += n_vals
if pred.dynamic:
## include the variables from the next time step
for p in pred.attr_inds:
for attr, ind_arr in pred.attr_inds[p]:
n_vals = len(ind_arr)
ll_p = self._param_to_ll[p]
if p.is_symbol():
x[i:i + n_vals] = getattr(ll_p, attr)[ind_arr, 0]
v[i:i + n_vals] = getattr(p, attr)[ind_arr, 0]
else:
x[i:i + n_vals] = getattr(ll_p,
attr)[ind_arr,
t + 1 - self.ll_start]
v[i:i + n_vals] = getattr(p, attr)[ind_arr, t + 1]
i += n_vals
assert i >= pred.x_dim
x = x.reshape((pred.x_dim, 1))
v = v.reshape((pred.x_dim, 1))
return Variable(x, v)