def post(self):
K = self.hl_action.K
T = self.hl_action.T
h = lambda x: self.distance_from_obj(x, 0.05, (K,T)) # function inequality constraint g(x) <= 0
h_func = CollisionFn([self.traj, self.loc], h)
fneq_fluent = FnEQFluent('fneq_' + self.name, self.priority, self.hl_action)
fneq_fluent.fn = h_func
coeff = np.zeros((T, 1), dtype=np.float)
coeff[T-1, 0] = 1.0
lhs = AffExpr({self.obj_traj: coeff})
rhs = AffExpr({self.traj: coeff, self.gp: 1.0})
lineq_fluent = LinEqFluent('lineq_' + self.name, self.priority, self.hl_action, lhs, rhs)
self.fluents = [fneq_fluent, lineq_fluent]
def pre(self):
# TODO: remove assumption that grasp is one time step
# import ipdb; ipdb.set_trace()
# self.traj.set_value(self.gp.get_value() + self.obj_traj.get_value())
K = self.hl_action.K
T = self.hl_action.T
h = lambda x: self.distance_from_obj(x, 0.05, (K,T)) # function inequality constraint g(x) <= 0
# h = lambda x: self.distance_from_obj(x, 0.0, (K,T)) # function inequality constraint g(x) <= 0
h_func = CollisionFn([self.traj, self.loc], h)
fneq_fluent = FnEQFluent('fneq_' + self.name, self.priority, self.hl_action)
fneq_fluent.fn = h_func
coeff = np.zeros((T, 1), dtype=np.float)
coeff[0, 0] = 1.0
lhs = AffExpr({self.obj_traj: coeff})
rhs = AffExpr({self.traj: coeff, self.gp: 1.0})
lineq_fluent = LinEqFluent('lineq_' + self.name, self.priority, self.hl_action, lhs, rhs)
self.fluents = [fneq_fluent, lineq_fluent]