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()
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(
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()