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 not self.obj.geom.road:
A = np.zeros((2,2))
b = np.zeros((2,1))
val = np.zeros((2, 1))
aff_e = AffExpr(A, b)
e = EqExpr(aff_e, val)
else:
direction = self.obj.geom.road
rot_mat = np.array([[np.cos(direction), -np.sin(direction)],
[np.sin(direction), np.cos(direction)]])
road_len = self.obj.geom.road.length
self.road_end = np.array([self.obj.geom.road.x, self.obj.geom.road.y]) + rot_mat.dot([road_len + END_DIST, 0])
A = np.eye(2)
b = -self.road_end
val = np.zeros((2, 1))
aff_e = AffExpr(A, b)
e = EqExpr(aff_e, val)
super(ExternalVehiclePastRoadEnd, self).__init__(name, e, attr_inds, params, expected_param_types, sim=sim, priority=-2)
self.spacial_anchor = True
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
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):
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int))])])
A = np.zeros((2,2))
b = np.zeros((2,1))
val = np.zeros((2, 1))
aff_e = AffExpr(A, b)
e = EqExpr(aff_e, val)
super(HLPred, self).__init__(name, e, attr_inds, params, expected_param_types, sim=sim, priority=-2)
def __init__(self, name, params, expected_param_types, sim=None):
assert len(params) == 1
self.limit, = params
attr_inds = OrderedDict([(self.limit, [("value", np.array([0], dtype=np.int))])])
A = np.c_[1, -1]
b, val = np.zeros((1, 1)), np.zeros((1, 1))
e = EqExpr(AffExpr(A, b), val)
super(StationaryLimit, 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) == 1
self.obj, = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int)),
("theta", np.array([0], dtype=np.int))])])
A = np.c_[np.eye(3), -np.eye(3)]
b, val = np.zeros((3, 1)), np.zeros((3, 1))
e = EqExpr(AffExpr(A, b), val)
super(Stationary, 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.target = params
attr_inds = OrderedDict([(self.obj, [("vel", np.array([0], dtype=np.int))]),
(self.target, [("value", np.array([0], dtype=np.int))])])
A = np.c_[1, -1]
b, val = np.zeros((1, 1)), np.zeros((1, 1))
aff_e = AffExpr(A, b)
e = EqExpr(aff_e, val)
super(VelAt, self).__init__(name, e, attr_inds, params, expected_param_types, sim=sim, priority=-2)
self.spacial_anchor = True
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, env=None):
## At Robot RobotPose
self.r, self.rp = params
attr_inds = OrderedDict([
(self.r, [("pose", np.array([0, 1], dtype=np.int))]),
(self.rp, [("value", np.array([0, 1], dtype=np.int))])
])
A = np.c_[np.eye(2), -np.eye(2)]
b = np.zeros((2, 1))
val = np.zeros((2, 1))
aff_e = AffExpr(A, b)
e = EqExpr(aff_e, val)
super(At, self).__init__(name, e, attr_inds, params,
expected_param_types)
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 __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False):
self.w, = params
attr_inds = OrderedDict([(self.w, [("pose",
np.array([0, 1], dtype=np.int))])])
A = np.array([[1, 0, -1, 0], [0, 1, 0, -1]])
b = np.zeros((2, 1))
e = EqExpr(AffExpr(A, b), b)
super(StationaryW, self).__init__(name,
e,
attr_inds,
params,
expected_param_types,
active_range=(0, 1))
def __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False):
self.rp, self.target, self.grasp = params
attr_inds = OrderedDict([
(self.rp, [("value", np.array([0, 1], dtype=np.int))]),
(self.target, [("value", np.array([0, 1], dtype=np.int))]),
(self.grasp, [("value", np.array([0, 1], dtype=np.int))])
])
# want x0 - x2 = x4, x1 - x3 = x5
A = np.array([[1, 0, -1, 0, -1, 0], [0, 1, 0, -1, 0, -1]])
b = np.zeros((2, 1))
e = AffExpr(A, b)
e = EqExpr(e, np.zeros((2, 1)))
super(GraspValid, self).__init__(name, e, attr_inds, params,
expected_param_types)
def __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False):
self.c, self.c_held = params
attr_inds = OrderedDict([(self.c, [("pose",
np.array([0, 1], dtype=np.int))])])
if self.c.name == self.c_held.name:
A = np.zeros((1, 4))
b = np.zeros((1, 1))
else:
A = np.array([[1, 0, -1, 0], [0, 1, 0, -1]])
b = np.zeros((2, 1))
e = EqExpr(AffExpr(A, b), b)
super(StationaryNEq, self).__init__(name,
e,
attr_inds,
params,
expected_param_types,
dynamic=True)
