def start(self):
        """
    Start running Igor with the provided configuration.
    This can safely be called multiple times
    but will do nothing after the first invocation.
    All processing will be done on a background thread which is spawned
    in this method.
    """
        self._state_lock.acquire()
        if self._started:
            self._state_lock.release()
            return

        group = create_group(self._config, self._has_camera)
        group.command_lifetime = 500
        group.feedback_frequency = 100.0

        self._group = group
        self._group_command = hebi.GroupCommand(group.size)
        self._group_feedback = hebi.GroupFeedback(group.size)
        self._group_info = hebi.GroupInfo(group.size)

        joystick_selector = self._config.joystick_selector
        self._joy = joystick_selector()
        load_gains(self)

        from threading import Condition, Lock, Thread
        start_condition = Condition(Lock())

        def start_routine(start_condition):
            start_condition.acquire()

            # Calling thread holds `__state_lock` AND
            # is also blocked until we call `start_condition.notify_all()`
            # So we can safely modify this here.
            self._started = True

            # Allow the calling thread to continue
            start_condition.notify_all()
            start_condition.release()

            self._start()

        self._proc_thread = Thread(target=start_routine,
                                   name='Igor II Controller',
                                   args=(start_condition, ))

        # We will have started the thread before returning,
        # but make sure the function has begun running before
        # we release the `_state_lock` and return
        start_condition.acquire()
        self._proc_thread.start()
        start_condition.wait()
        start_condition.release()

        self._state_lock.release()
Exemple #2
0
lookup = hebi.Lookup()
#group = lookup.get_group_from_names(['Wheel'], ['Right', 'Left'])
group = lookup.get_group_from_names(['Wheel'], ['Right_light', 'Left_light'])

if group is None:
    print(
        'Group not found: Did you forget to set the module family and names above?'
    )
    exit(1)

for entry in lookup.entrylist:
    print(entry)

group_feedback = hebi.GroupFeedback(group.size)
group_command = hebi.GroupCommand(group.size)
group_info = hebi.GroupInfo(group.size)
group.feedback_frequency = 100


def Setting():
    group_command.reference_effort = 0.0000
    #group_command.reference_position = 0.0000

    new_velocity_kp_gains = [0.01, 0.01]
    new_effort_kp_gains = [0.1, 0.1]

    for new_gain in new_velocity_kp_gains:
        for i in range(group.size):
            group_command.velocity_kp = new_gain
        if not group.send_command_with_acknowledgement(group_command):
            print(
Exemple #3
0
    def start(self):
        with self._state_lock:
            if self._started:
                return

            config = self._config
            group = create_group(config)
            group.feedback_frequency = config.robot_feedback_frequency
            group.command_lifetime = config.robot_command_lifetime

            num_modules = group.size
            self._group = group
            self._auxilary_group_command = hebi.GroupCommand(num_modules)
            self._group_command = hebi.GroupCommand(num_modules)
            self._group_feedback = hebi.GroupFeedback(num_modules)
            self._group_info = hebi.GroupInfo(num_modules)

            # Construction of legs
            from math import radians
            leg_angles = [
                radians(entry)
                for entry in [30.0, -30.0, 90.0, -90.0, 150.0, -150.0]
            ]
            leg_distances = [0.2375, 0.2375, 0.1875, 0.1875, 0.2375, 0.2375]
            leg_configs = ['left', 'right'] * 3
            num_joints_in_leg = Leg.joint_count
            parameters = self._params

            # HACK: The mass is hardcoded as 21Kg. This hardcodes the weight to be the force here (which will be accurate as long as we are on Earth).
            weight = 21.0 * 9.8

            # The legs need to start with valid feedback, so we must wait for a feedback here
            self._group.get_next_feedback(reuse_fbk=self._group_feedback)

            legs = list()
            for i, angle, distance, leg_configuration in zip(
                    range(num_modules), leg_angles, leg_distances,
                    leg_configs):
                start_idx = num_joints_in_leg * i
                indices = [start_idx, start_idx + 1, start_idx + 2]
                leg = Leg(i, angle, distance, parameters, leg_configuration,
                          self._group_command.create_view(indices),
                          self._group_feedback.create_view(indices))
                legs.append(leg)

            self._legs = legs
            self._weight = weight

            joystick_selector = self._config.joystick_selector

            while True:
                try:
                    joy = joystick_selector()
                    if joy is None:
                        raise RuntimeError
                    self._joy = joy
                    break
                except:
                    pass

            load_gains(self)

            from threading import Condition, Lock, Thread
            start_condition = Condition(Lock())

            def start_routine(start_condition):
                with start_condition:
                    # Calling thread holds `_state_lock` AND
                    # is also blocked until we call `start_condition.notify_all()`
                    # So we can safely modify this here.
                    self._started = True

                    # Allow the calling thread to continue
                    start_condition.notify_all()

                self._start()

            self._proc_thread = Thread(target=start_routine,
                                       name='Hexapod Controller',
                                       args=(start_condition, ))

            # We will have started the thread before returning,
            # but make sure the function has begun running before
            # we release the `_state_lock` and return
            with start_condition:
                self._proc_thread.start()
                start_condition.wait()