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 _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 _add_pred_dict(self, pred_dict, effective_timesteps, add_nonlin=True,
priority=MAX_PRIORITY, verbose=False):
"""
This function creates constraints for the predicate and added to
Prob class in sco.
"""
## for debugging
ignore_preds = []
priority = np.maximum(priority, 0)
if not pred_dict['hl_info'] == "hl_state":
start, end = pred_dict['active_timesteps']
active_range = range(start, end+1)
if verbose:
print "pred being added: ", pred_dict
print active_range, effective_timesteps
negated = pred_dict['negated']
pred = pred_dict['pred']
if pred.get_type() in ignore_preds:
return
if pred.priority > priority: return
if DEBUG: assert isinstance(pred, common_predicates.ExprPredicate)
expr = pred.get_expr(negated)
if expr is not None:
if add_nonlin or isinstance(expr.expr, AffExpr):
for t in effective_timesteps:
if t in active_range:
if verbose:
print "expr being added at time ", t
var = self._spawn_sco_var_for_pred(pred, t)
bexpr = BoundExpr(expr, var)
# TODO: REMOVE line below, for tracing back predicate for debugging.
if DEBUG:
bexpr.source = (negated, pred, t)
self._bexpr_to_pred[bexpr] = (negated, pred, t)
groups = ['all']
if self.early_converge:
## this will check for convergence per parameter
## this is good if e.g., a single trajectory quickly
## gets stuck
groups.extend([param.name for param in pred.params])
self._prob.add_cnt_expr(bexpr, groups)
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 _add_pred_dict(self, pred_dict, effective_timesteps, add_nonlin=True, priority=MAX_PRIORITY, verbose=False):
## for debugging
ignore_preds = []
priority = np.maximum(priority, 0)
if not pred_dict["hl_info"] == "hl_state":
start, end = pred_dict["active_timesteps"]
active_range = range(start, end + 1)
if verbose:
print "pred being added: ", pred_dict
print active_range, effective_timesteps
negated = pred_dict["negated"]
pred = pred_dict["pred"]
if pred.get_type() in ignore_preds:
return
if pred.priority > priority:
return
assert isinstance(pred, common_predicates.ExprPredicate)
expr = pred.get_expr(negated)
for t in effective_timesteps:
if t in active_range:
if expr is not None:
if add_nonlin or isinstance(expr.expr, AffExpr):
if verbose:
print "expr being added at time ", t
var = self._spawn_sco_var_for_pred(pred, t)
bexpr = BoundExpr(expr, var)
# TODO: REMOVE line below, for tracing back predicate for debugging.
bexpr.pred = pred
self._bexpr_to_pred[bexpr] = (negated, pred, t)
groups = ["all"]
if self.early_converge:
## this will check for convergence per parameter
## this is good if e.g., a single trajectory quickly
## gets stuck
groups.extend([param.name for param in pred.params])
self._prob.add_cnt_expr(bexpr, groups)
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():
for attr_name in param.__dict__.iterkeys():
attr_type = param.get_attr_type(attr_name)
if issubclass(attr_type, Vector):
param_ll = self._param_to_ll[param]
if param.is_symbol():
T = 1
attr_val = getattr(param, attr_name)
else:
T = param_ll._horizon
attr_val = getattr(param, attr_name)[:, param_ll.active_ts[0]:param_ll.active_ts[1]+1]
K = attr_type.dim
# pose = param.pose
if DEBUG: 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) if not param.is_symbol() else np.eye(KT)
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))
transfer_coeff = self.transfer_coeff/float(plan.horizon)
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2*transfer_coeff*Q,
transfer_coeff*A, transfer_coeff*b)
ll_attr_val = getattr(param_ll, attr_name)
param_ll_grb_vars = ll_attr_val.reshape((KT, 1), order='F')
sco_var = self.create_variable(param_ll_grb_vars, cur_val)
bexpr = BoundExpr(quad_expr, sco_var)
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 _resample(self, plan, preds, sample_all = False):
"""
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
"""
bexprs = []
val, attr_inds = None, None
pred_type = {}
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 pred_type.get(pred.get_type, False):
continue
val, attr_inds = pred.resample(negated, t, plan)
if val is not None: pred_type[pred.get_type] = True
## if no resample defined for that pred, continue
if val is not None:
for p in attr_inds:
## get the ll_param for p and gurobi variables
ll_p = self._param_to_ll[p]
n_vals, i = 0, 0
grb_vars = []
for attr, ind_arr, t in attr_inds[p]:
for j, grb_var in enumerate(getattr(ll_p, attr)[ind_arr, t-ll_p.active_ts[0]].flatten()):
Q = np.eye(1)
A = -2*val[p][i+j]*np.ones((1, 1))
b = np.ones((1, 1))*np.power(val[p][i+j], 2)
resample_coeff = self.rs_coeff/float(plan.horizon)
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2*Q*resample_coeff, A*resample_coeff, b*resample_coeff)
v_arr = np.array([grb_var]).reshape((1, 1), order='F')
init_val = np.ones((1, 1))*val[p][i+j]
sco_var = self.create_variable(v_arr, np.array([val[p][i+j]]).reshape((1, 1)), save = True)
bexpr = BoundExpr(quad_expr, sco_var)
bexprs.append(bexpr)
i += len(ind_arr)
if not sample_all:
break
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 _update(self, plan, update_values):
bexprs = []
for val, attr_inds in update_values:
if val is not None:
for p in attr_inds:
## get the ll_param for p and gurobi variables
ll_p = self._param_to_ll[p]
n_vals, i = 0, 0
grb_vars = []
for attr, ind_arr, t in attr_inds[p]:
for j, grb_var in enumerate(getattr(ll_p, attr)[ind_arr, t-ll_p.active_ts[0]].flatten()):
Q = np.eye(1)
A = -2*val[p][i+j]*np.ones((1, 1))
b = np.ones((1, 1))*np.power(val[p][i+j], 2)
resample_coeff = self.rs_coeff/float(plan.horizon)
# QuadExpr is 0.5*x^Tx + Ax + b
quad_expr = QuadExpr(2*Q*resample_coeff, A*resample_coeff, b*resample_coeff)
v_arr = np.array([grb_var]).reshape((1, 1), order='F')
init_val = np.ones((1, 1))*val[p][i+j]
sco_var = self.create_variable(v_arr, np.array([val[p][i+j]]).reshape((1, 1)), save = True)
bexpr = BoundExpr(quad_expr, sco_var)
bexprs.append(bexpr)
i += len(ind_arr)
return bexprs