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)