def __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False):
self._env = env
self.r, self.w = params
attr_inds = OrderedDict([
(self.r, [("pose", np.array([0, 1], dtype=np.int))]),
(self.w, [("pose", np.array([0, 1], dtype=np.int))])
])
self._param_to_body = {
self.r: self.lazy_spawn_or_body(self.r, self.r.name, self.r.geom),
self.w: self.lazy_spawn_or_body(self.w, self.w.name, self.w.geom)
}
f = lambda x: -self.distance_from_obj(x)[0]
grad = lambda x: -self.distance_from_obj(x)[1]
## so we have an expr for the negated predicate
def f_neg(x):
d = self.distance_from_obj(x)[0]
# if d > 0:
# import pdb; pdb.set_trace()
# self.distance_from_obj(x)
return d
def grad_neg(x):
# print self.distance_from_obj(x)
return -self.distance_from_obj(x)[1]
N_COLS = 8
col_expr = Expr(f, grad)
val = np.zeros((N_COLS, 1))
e = LEqExpr(col_expr, val)
col_expr_neg = Expr(f_neg, grad_neg)
self.neg_expr = LEqExpr(col_expr_neg, -val)
super(RCollides, self).__init__(name,
e,
attr_inds,
params,
expected_param_types,
ind0=0,
ind1=1)
self.n_cols = N_COLS
def get_expr_mult(coeff, expr):
"""
Multiply expresions with coefficients
"""
new_f = lambda x: coeff * expr.eval(x)
new_grad = lambda x: coeff * expr.grad(x)
return Expr(new_f, new_grad)
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 3
self.target1, self.target2, self.dist = params
attr_inds = OrderedDict([(self.target1, [("xy", np.array([0,1], dtype=np.int))]),
(self.target2, [("xy", np.array([0,1], dtype=np.int))]),
(self.dist, [("value", np.array([0], dtype=np.int))])])
def f(x):
scaled_vec = np.abs((x[2:4] - x[:2]) / np.linalg.norm(x[2:4] - x[:2]) * x[4])
if np.all(scaled_vec < x[2:4] - x[:2]):
return -x[2:4] + x[:2] + scaled_vec
elif np.all(-scaled_vec > x[2:4] - x[:2]):
return -scaled_vec - x[2:4] + x[:2]
else:
return np.zeros((2,))
def grad(x):
return np.c_[-np.eye(2), np.eye(2), np.zeros((1,2))]
val = np.zeros((2, 1))
dynamics_expr = Expr(self.f, self.grad)
e = LEqExpr(dynamics_expr, val)
super(WithinDistance, self).__init__(name, e, attr_inds, params, expected_param_types, sim=sim, priority=1)
self.spacial_anchor = False
def __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False):
self._env = env
self.robot, self.rp, self.targ = params
attr_inds = OrderedDict([
(self.rp, [("value", np.array([0, 1], dtype=np.int))]),
(self.targ, [("value", np.array([0, 1], dtype=np.int))])
])
self._param_to_body = {
self.rp:
self.lazy_spawn_or_body(self.rp, self.rp.name, self.robot.geom),
self.targ:
self.lazy_spawn_or_body(self.targ, self.targ.name, self.targ.geom)
}
INCONTACT_COEFF = 1e1
f = lambda x: INCONTACT_COEFF * self.distance_from_obj(x)[0]
grad = lambda x: INCONTACT_COEFF * self.distance_from_obj(x)[1]
col_expr = Expr(f, grad)
val = np.ones((1, 1)) * dsafe * INCONTACT_COEFF
# val = np.zeros((1, 1))
e = EqExpr(col_expr, val)
super(InContact, self).__init__(name,
e,
attr_inds,
params,
expected_param_types,
debug=debug,
ind0=1,
ind1=2)
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 1
self.obj, = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int))])])
if self.obj.geom.road:
direction = self.obj.geom.road.direction
rot_mat = np.array([[np.cos(direction), -np.sin(direction)],
[np.sin(direction), np.cos(direction)]])
self.dir_vec = rot_mat.dot([1,0])
else:
self.dir_vec = np.zeros((2,))
def f(x):
if not self.obj.geom.road:
return np.zeros((2,))
dist_vec = x[2:4] - x[:2]
return (dist_vec / np.linalg.norm(dist_vec)) - self.dir_vec
def grad(x):
return np.c_[np.eye(2), -np.eye(2)]
val = np.zeros((2, 1))
e = EqExpr(Expr(f, grad), val)
super(ExternalDriveDownRoad, self).__init__(name, e, attr_inds, params, expected_param_types, active_range=(0,1), sim=sim, priority=2)
self.spacial_anchor = False
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 2
self.obj, self.sign = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int))])])
def f(x):
if not self.sign.geom.road.is_on(x[0], x[1]):
return np.zeros((2,))
direction = self.sign.geom.road.direction
rot_mat = np.array([[np.cos(direction), -np.sin(direction)],
[np.sin(direction), np.cos(direction)]])
dist_vec = self.sign.geom.loc - x[:2]
rot_dist_vec = rot_mat.dot(dist_vec)
if np.abs(rot_dist_vec[0]) < self.sign.geom.length / 2. and np.abs(rot_dist_vec[1]) < self.sign.geom.width / 2.:
return x[2:] - x[:2]
return np.zeros((2,))
def grad(x):
return np.c_[np.eye(2), -np.eye(2)]
val = np.zeros((2, 1))
e = EqExpr(Expr(f, grad), val)
super(StopAtStopSign, self).__init__(name, e, attr_inds, params, expected_param_types, active_range=(0,1), sim=sim, priority=2)
self.spacial_anchor = False
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 3
self.v1, self.v2, self.dist = params
attr_inds = OrderedDict([(self.v1, [("xy", np.array([0,1], dtype=np.int)),
("theta", np.array([0], dtype=np.int))]),
(self.v2, [("xy", np.array([0,1], dtype=np.int)),
("theta", np.array([0], dtype=np.int))]),
(self.dist, [("value", np.array([0], dtype=np.int))])])
def f(x):
old_v1_pose = self.v1.geom.update_xy_theta(x[0], x[1], x[5], 0)
front_x, front_y = self.v1.geom.vehicle_front()
target_x = x[3] - np.cos(x[5]) * x[6]
target_y =x[4] - np.sin(x[5]) * x[6]
x_delta = target_x - x[0]
y_delta = target_y - x[1]
theta_delta = x[5] - x[2]
while theta_delta > np.pi:
theta_delta -= 2 * np.pi
while theta_delta < np.pi:
theta_delta += 2 * np.pi
return np.r_[x_delta, y_delta, theta_delta].reshape((3,1))
def grad(x):
return np.c_[np.eye(3), np.zeros((3,3))]
val = np.zeros((3, 1))
e = EqExpr(Expr(f, grad), val)
super(Stationary, self).__init__(name, e, attr_inds, params, expected_param_types, sim=sim, priority=3)
self.spacial_anchor = False
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 2
self.obj1, self.obj2 = params
attr_inds = OrderedDict([(self.obj1, [("xy", np.array([0,1], dtype=np.int)),
("theta", np.array([0], dtype=np.int))]),
(self.obj2, [("xy", np.array([0,1], dtype=np.int)),
("theta", np.array([0], dtype=np.int))])])
def f(x):
old_0_pose1 = self.obj1.geom.update_xy_theta(x[0], x[1], x[2], 0)
old_0_pose2 = self.obj2.geom.update_xy_theta(x[3], x[4], x[5], 0)
old_1_pose1 = self.obj1.geom.update_xy_theta(x[6], x[7], x[8], 1)
old_1_pose2 = self.obj2.geom.update_xy_theta(x[9], x[10], x[11], 1)
obj1_pts = self.obj1.geom.get_points(0, COL_DIST) + self.obj1.geom.get_points(1, COL_DIST)
obj2_pts = self.obj2.geom.get_points(0, COL_DIST) + self.obj2.geom.get_points(1, COL_DIST)
self.obj1.geom.update_xy_theta(0, old_0_pose1[0], old_0_pose1[1], old_0_pose1[2])
self.obj2.geom.update_xy_theta(0, old_0_pose2[0], old_0_pose2[1], old_0_pose2[2])
self.obj1.geom.update_xy_theta(1, old_1_pose1[0], old_1_pose1[1], old_1_pose1[2])
self.obj2.geom.update_xy_theta(1, old_1_pose2[0], old_1_pose2[1], old_1_pose2[2])
return collision_vector(obj1_pts, obj2_pts)
def grad(obj1_body, obj2_body):
grad = np.zeros((2,12))
grad[:, :2] = -np.eye(2)
grad[:, 3:5] = np.eye(2)
grad[:, 5:8] = -np.eye(2)
grad[:, 8:11] = np.eye(2)
val = np.zeros((2, 1))
col_expr = Expr(f, grad)
e = EqExpr(col_expr, val)
super(CollisionPredicate, self).__init__(name, e, attr_inds, params, expected_param_types, active_range=(0,1), sim=sim, priority=3)
self.spacial_anchor = False
def __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False):
self._env = env
r, startp, endp, obstr, held = params
self.r = r
self.startp, self.endp = startp, endp
self.obstr = obstr
self.held = held
self.rs_scale = RS_SCALE
attr_inds = OrderedDict([(r, [("pose", np.array([0, 1],
dtype=np.int))]),
(obstr, [("pose",
np.array([0, 1], dtype=np.int))]),
(held, [("pose", np.array([0, 1],
dtype=np.int))])])
self._param_to_body = {
r: self.lazy_spawn_or_body(r, r.name, r.geom),
obstr: self.lazy_spawn_or_body(obstr, obstr.name, obstr.geom),
held: self.lazy_spawn_or_body(held, held.name, held.geom)
}
f = lambda x: -self.distance_from_obj(x)[0]
grad = lambda x: -self.distance_from_obj(x)[1]
## so we have an expr for the negated predicate
f_neg = lambda x: self.distance_from_obj(x)[0]
grad_neg = lambda x: self.distance_from_obj(x)[1]
col_expr = Expr(f, grad)
val = np.zeros((1, 1))
e = LEqExpr(col_expr, val)
col_expr_neg = Expr(f_neg, grad_neg)
self.neg_expr = LEqExpr(col_expr_neg, val)
super(ObstructsHolding, self).__init__(name, e, attr_inds, params,
expected_param_types)
def __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False):
self._env = env
self.r, self.startp, self.endp, self.c = params
attr_inds = OrderedDict([
(self.r, [("pose", np.array([0, 1], dtype=np.int))]),
(self.c, [("pose", np.array([0, 1], dtype=np.int))])
])
self._param_to_body = {
self.r: self.lazy_spawn_or_body(self.r, self.r.name, self.r.geom),
self.c: self.lazy_spawn_or_body(self.c, self.c.name, self.c.geom)
}
self.rs_scale = RS_SCALE
f = lambda x: -self.distance_from_obj(x)[0]
grad = lambda x: -self.distance_from_obj(x)[1]
## so we have an expr for the negated predicate
f_neg = lambda x: self.distance_from_obj(x)[0]
def grad_neg(x):
# print self.distance_from_obj(x)
return self.distance_from_obj(x)[1]
col_expr = Expr(f, grad)
val = np.zeros((1, 1))
e = LEqExpr(col_expr, val)
col_expr_neg = Expr(f_neg, grad_neg)
self.neg_expr = LEqExpr(col_expr_neg, -val)
super(Obstructs, self).__init__(name,
e,
attr_inds,
params,
expected_param_types,
ind0=0,
ind1=3)
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 2
self.obj, self.surface = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int))])])
f = lambda x: self.surface.geom.to(x[0], x[1])
grad = lambda x: np.eye(2)
val = np.zeros((1, 2))
dynamics_expr = Expr(self.f, self.grad)
e = EqExpr(dynamics_expr, val)
super(OnSurface, self).__init__(name, e, attr_inds, params, expected_param_types, sim=sim, priority=2)
self.spacial_anchor = False
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 1
self.obj, = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int)),
("theta", np.array([0], dtype=np.int)),
("vel", np.array([0], dtype=np.int)),
("phi", np.array([0], dtype=np.int)),
("u1", np.array([0], dtype=np.int)),
("u2", np.array([0], dtype=np.int))])])
val = np.zeros((1, 14))
dynamics_expr = Expr(self.f, self.grad)
e = EqExpr(dynamics_expr, val)
super(DynamicPredicate, self).__init__(name, e, attr_inds, params, expected_param_types, active_range=(0,1), sim=sim, priority=1)
self.spacial_anchor = False
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 3
self.obj, self.road, self.lane_num = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int)),
("theta", np.array([0], dtype=np.int))]),
(self.lane_num, [("value", np.array([0], dtype=np.int))])])
f = lambda x: self.road.geom.to_lane(x[0], x[1], x[2] - 1) if x[2] > 0 else self.road.geom.to_lane(x[0], x[1], x[2])
grad = lambda x: np.eye(3)
val = np.zeros((2, 1))
dynamics_expr = Expr(self.f, self.grad)
e = EqExpr(dynamics_expr, val)
super(LeftOfLane, self).__init__(name, e, attr_inds, params, expected_param_types, sim=sim, priority=2)
self.spacial_anchor = False
def get_expr_mult(coeff, expr):
new_f = lambda x: coeff * expr.eval(x)
new_grad = lambda x: coeff * expr.grad(x)
return Expr(new_f, new_grad)
