def pre(self):
# h = lambda x: self.error(x) # function inequality constraint h(x) = 0
# self.fn = CollisionFn([self.obj_traj], h)
h_pos = lambda x: self.pos_error(x) # function inequality constraint h(x) = 0
h_rot = lambda x: self.rot_error(x) # function inequality constraint h(x) = 0
# h_rot = lambda x: self.rot_error_num(x) # function inequality constraint h(x) = 0
self.pos_fn = CollisionFn([self.obj_traj], h_pos)
self.rot_fn = CollisionFn([self.obj_traj], h_rot)
pos_fluent = FnEQFluent('pos_' + self.name, self.priority, self.hl_action)
pos_fluent.fn = self.pos_fn
rot_fluent = FnEQFluent('up_' + self.name, self.priority, self.hl_action)
rot_fluent.fn = self.rot_fn
self.fluents = [pos_fluent, rot_fluent]
def pre(self):
traj = self.traj
h_pos = lambda x: self.pos_error(x) # function inequality constraint h(x) = 0
# h_pos = lambda x: self.num_check(self.pos_error, x) # numerical gradient check on pos error
h_rot = lambda x: self.rot_error(x) # function inequality constraint h(x) = 0
# h_rot = lambda x: self.num_check(self.rot_error, x) # numerical gradient check on rot error
self.pos_fn = CollisionFn([self.traj, self.obj_traj], h_pos)
self.rot_fn = CollisionFn([self.traj, self.obj_traj], h_rot)
pos_fluent = FnEQFluent("pos_" + self.name, self.priority, self.hl_action)
pos_fluent.fn = self.pos_fn
rot_fluent = FnEQFluent("up_" + self.name, self.priority, self.hl_action)
rot_fluent.fn = self.rot_fn
self.fluents = [pos_fluent, rot_fluent]