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):
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) == 2
self.obj, self.limit = params
attr_inds = OrderedDict([(self.obj, [("v", np.array([0], dtype=np.int)),
("u1", np.array([0], dtype=np.int))]),
(self.limit, [("value", np.array([0], dtype=np.int))])])
b, val = np.zeros((1, 1)), np.zeros((1, 1))
e = LEqExpr(AffExpr(self.A, b), val)
super(Stationary, 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))])])
A = np.c_[np.r_[np.eye(3), -np.eye(3)], np.r_[-np.eye(3), np.eye(3)]]
b, val = np.zeros((6, 1)), MOVE_FACTOR * np.ones((6, 1))
e = LEqExpr(AffExpr(A, b), val)
super(IsMP, 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) == 3
self.obj, self.target, self.dist = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0,1], dtype=np.int))]),
(self.target, [("xy", np.array([0,1], dtype=np.int))]),,
(self.dist, [("value", np.array([0], dtype=np.int))])])
A = np.c_[np.r_[np.eye(2), -np.eye(2)], np.r_[-np.eye(2), np.eye(2)], -np.ones((4,1))]
b, val = np.zeros((4, 1)), np.zeros((4, 1))
aff_e = AffExpr(A, b)
e = LEqExpr(aff_e, val)
super(At, 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, 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) == 3
self.obj, self.xlimit, self.ylimit = params
attr_inds = OrderedDict([(self.obj, [("xy", np.array([0, 1], dtype=np.int))]),
(self.xlimit, [("value", np.array([0], dtype=np.int))]),
(self.ylimit, [("value", np.array([0], dtype=np.int))])])
A = np.zeros((4,4))
A[:2,:2] = -np.eye(2)
A[:2,2:4] = np.eye(2)
A[2:4,:2] = -np.eye(2)
b, val = np.zeros((4, 1)), np.zeros((4, 1))
e = LEqExpr(AffExpr(A, b), val)
super(Stationary, 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)
def __init__(self,
name,
params,
expected_param_types,
env=None,
debug=False,
dmove=dmove):
self.r, = params
## constraints |x_t - x_{t+1}| < dmove
## ==> x_t - x_{t+1} < dmove, -x_t + x_{t+a} < dmove
attr_inds = OrderedDict([(self.r, [("pose",
np.array([0, 1], dtype=np.int))])])
A = np.array([[1, 0, -1, 0], [0, 1, 0, -1], [-1, 0, 1, 0],
[0, -1, 0, 1]])
b = np.zeros((4, 1))
e = LEqExpr(AffExpr(A, b), dmove * np.ones((4, 1)))
super(IsMP, self).__init__(name,
e,
attr_inds,
params,
expected_param_types,
active_range=(0, 1))
