Exemple #1
0
    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]
Exemple #2
0
    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]