Exemple #1
0
 def _execute_impl(self, door_position, handle_position, locking_tolerance,
                   handle_release_angle):
     door_vel = gm.DiffSymbol(door_position)
     is_unlocked = gm.alg_not(
         gm.alg_and(gm.less_than(door_position, locking_tolerance),
                    gm.less_than(handle_position, handle_release_angle)))
     self.constraints = {
         f'lock {door_position}': Constraint(-1e9, is_unlocked * 1e9,
                                             door_vel)
     }
if __name__ == '__main__':

    a, b = [gm.Position(x) for x in 'ab']

    l = 2
    a_in_w = gm.dot(gm.translation3(0, 0, 2), gm.translation3(0, 0, -a))
    d_in_a = gm.rotation3_axis_angle(gm.vector3(0, 1, 0), gm.acos(a / l))
    d_in_w = gm.dot(a_in_w, d_in_a)
    A = gm.pos_of(a_in_w)
    B = gm.dot(d_in_w, gm.point3(0, 0, l))
    C = gm.dot(d_in_w, gm.point3(0, 0, l * 0.5))
    D = gm.dot(d_in_w, gm.point3(0, 0, l * 0.25))
    E = gm.dot(d_in_w, gm.point3(0, 0, l * 0.75))

    lock_bound = gm.alg_not(
        gm.alg_and(gm.less_than(b, 0.3), gm.less_than(1.99, a)))

    # PLOT OF MOVEMENT

    As = []
    Bs = []
    Cs = []
    Ds = []
    Es = []

    for x in np.linspace(0, l, 20):
        q = {a: x}
        As.append(np.take(gm.subs(A, q).flatten(), (0, 2)))
        Bs.append(np.take(gm.subs(B, q).flatten(), (0, 2)))
        Cs.append(np.take(gm.subs(C, q).flatten(), (0, 2)))
        Ds.append(np.take(gm.subs(D, q).flatten(), (0, 2)))
                goal_lin_vel_sq_norm - lin_vel_alignment, 2, lin_vel_alignment)

            qp = GQPB(
                world,
                constraints,
                {
                    'grasp_constraint_lin': goal_grasp_lin,
                    'grasp_constraint_ang': goal_grasp_ang,
                    'goal_door_angle': goal_door_angle,
                    'goal_handle_angle': goal_handle_angle,
                    # 'goal_lin_vel_alignment': goal_lin_vel_alignment
                },
                controlled_values,
                visualizer=visualizer)

            is_unlocked = gm.alg_and(gm.greater_than(door_position, 0.4),
                                     gm.less_than(handle_position, 0.15))

            integrator = CommandIntegrator(
                qp,
                start_state=q_ik_goal,
                recorded_terms={
                    'is_unlocked': is_unlocked,
                    'handle_position': handle_position,
                    'door_position': door_position
                },
                printed_exprs={'is_unlocked': is_unlocked})

            try:
                integrator.restart(title='Door opening generator')
                integrator.run(dt=0.05,
                               max_iterations=500,
    def __init__(self, name):
        super(Lockbox, self).__init__('Lockbox - {}'.format(name))

        self.lock_a_p = Position('lock_a')
        self.lock_b_p = Position('lock_b')
        self.lock_c_p = Position('lock_c')
        self.lock_d_p = Position('lock_d')
        self.lock_e_p = Position('lock_e')
        self.lock_f_p = Position('lock_f')
        pos_symbols = {
            self.lock_a_p, self.lock_b_p, self.lock_c_p, self.lock_d_p,
            self.lock_e_p, self.lock_f_p
        }
        self.pos_symbols_str = {str(s) for s in pos_symbols}

        self.lock_a_v = DiffSymbol(self.lock_a_p)
        self.lock_b_v = DiffSymbol(self.lock_b_p)
        self.lock_c_v = DiffSymbol(self.lock_c_p)
        self.lock_d_v = DiffSymbol(self.lock_d_p)
        self.lock_e_v = DiffSymbol(self.lock_e_p)
        self.lock_f_v = DiffSymbol(self.lock_f_p)
        vel_symbols = {
            self.lock_a_v, self.lock_b_v, self.lock_c_v, self.lock_d_v,
            self.lock_e_v, self.lock_f_v
        }
        self.vel_symbols_str = {str(s) for s in vel_symbols}
        self.int_rules = {s: s for s in vel_symbols}

        b_open_threshold = 0.4
        c_open_threshold = 0.6
        d_open_threshold = 0.8
        e_open_threshold = 1.0
        f_open_threshold = 1.2

        # Locking rules
        # b and c lock a
        # d locks b and c
        # e locks c and d
        # f locks e
        a_open_condition = alg_and(
            greater_than(self.lock_b_p, b_open_threshold),
            greater_than(self.lock_c_p, c_open_threshold))
        b_open_condition = greater_than(self.lock_d_p, d_open_threshold)
        c_open_condition = alg_and(
            greater_than(self.lock_d_p, b_open_threshold + 0.1),
            greater_than(self.lock_e_p, e_open_threshold))
        d_open_condition = greater_than(self.lock_e_p, e_open_threshold - 0.1)
        e_open_condition = greater_than(self.lock_f_p, f_open_threshold)

        self.lock_str_labels = [
            'a open = $b \\succ {} \\curlywedge c \\succ {}$'.format(
                b_open_threshold, c_open_threshold),
            'b open = $d \\succ {}$'.format(d_open_threshold),
            'c open = $d \\succ {} \\curlywedge e \\succ {}$'.format(
                b_open_threshold + 0.1, e_open_threshold),
            'd open = $e \\succ {}$'.format(e_open_threshold - 0.1),
            'e open = $f \\succ {}$'.format(f_open_threshold)
        ]

        self.recorded_terms = dict(
            zip(self.lock_str_labels, [
                a_open_condition.expr, b_open_condition.expr,
                c_open_condition.expr, d_open_condition.expr,
                e_open_condition.expr
            ]))

        # Velocity constraints
        self.km.add_constraint(
            'lock_a_velocity',
            Constraint(-0.4 * a_open_condition, 0.4 * a_open_condition,
                       self.lock_a_v))
        self.km.add_constraint(
            'lock_b_velocity',
            Constraint(-0.1, 0.1 * b_open_condition, self.lock_b_v))
        self.km.add_constraint(
            'lock_c_velocity',
            Constraint(-0.1, 0.1 * c_open_condition, self.lock_c_v))
        self.km.add_constraint(
            'lock_d_velocity',
            Constraint(-0.1, 0.1 * d_open_condition, self.lock_d_v))
        self.km.add_constraint(
            'lock_e_velocity',
            Constraint(-0.1, 0.1 * e_open_condition, self.lock_e_v))
        self.km.add_constraint('lock_f_velocity',
                               Constraint(-0.4, 0.4, self.lock_f_v))

        # Configuration space
        self.km.add_constraint(
            'lock_b_position',
            Constraint(-self.lock_b_p, 0.7 - self.lock_b_p, self.lock_b_p))
        self.km.add_constraint(
            'lock_c_position',
            Constraint(-self.lock_c_p, 0.8 - self.lock_c_p, self.lock_c_p))
        self.km.add_constraint(
            'lock_d_position',
            Constraint(-self.lock_d_p, 0.9 - self.lock_d_p, self.lock_d_p))
        self.km.add_constraint(
            'lock_e_position',
            Constraint(-self.lock_e_p, 1.1 - self.lock_e_p, self.lock_e_p))