def __init__(self): Device.__init__(self, 'robot') self.monitor = RobotMonitor(self) self.collision = RobotCollision(self) self.dirty_push_command = False self.lock = threading.RLock() #Prevent status thread from triggering motor sync prematurely self.status = {'pimu': {}, 'base': {}, 'lift': {}, 'arm': {}, 'head': {}, 'wacc': {}, 'end_of_arm': {}} self.pimu=pimu.Pimu() self.status['pimu']=self.pimu.status self.base=base.Base() self.status['base']=self.base.status self.lift=lift.Lift() self.status['lift']=self.lift.status self.arm=arm.Arm() self.status['arm']=self.arm.status self.head=head.Head() self.status['head']=self.head.status if 'custom_wacc' in self.params: module_name = self.params['custom_wacc']['py_module_name'] class_name = self.params['custom_wacc']['py_class_name'] self.wacc=getattr(importlib.import_module(module_name), class_name)(self) else: self.wacc=wacc.Wacc() self.status['wacc']=self.wacc.status tool_name = self.params['tool'] module_name = self.robot_params[tool_name]['py_module_name'] class_name = self.robot_params[tool_name]['py_class_name'] self.end_of_arm = getattr(importlib.import_module(module_name), class_name)() self.status['end_of_arm'] = self.end_of_arm.status self.devices={ 'pimu':self.pimu, 'base':self.base, 'lift':self.lift, 'arm': self.arm, 'head': self.head, 'wacc':self.wacc, 'end_of_arm':self.end_of_arm} self.non_dxl_thread=None self.dxl_thread=None
#!/usr/bin/env python import stretch_body.head as head from stretch_body.hello_utils import * import sys import argparse print_stretch_re_use() parser = argparse.ArgumentParser(description='Jog the head from the keyboard') args = parser.parse_args() h = head.Head() h.startup() v_des = [ h.motors['head_pan'].params['motion']['default']['vel'], h.motors['head_tilt'].params['motion']['default']['vel'] ] a_des = [ h.motors['head_pan'].params['motion']['default']['accel'], h.motors['head_tilt'].params['motion']['default']['accel'] ] def menu_top(): print('------ MENU -------') print('m: menu') print('a: increment pan 10 deg') print('b: decrement pan 10 deg') print('c: increment tilt 10 deg') print('d: decrement tilt 10 deg')
def __init__(self): Device.__init__(self) self.params = self.robot_params['robot'] self.pimu = None self.wacc = None self.arm = None self.lift = None self.head = None self.base = None self.end_of_arm = None self.monitor = RobotMonitor(self) self.sentry = RobotSentry(self) self.dirty_push_command = False self.lock = threading.RLock( ) #Prevent status thread from triggering motor sync prematurely self.status = { 'pimu': {}, 'base': {}, 'lift': {}, 'arm': {}, 'head': {}, 'wacc': {}, 'end_of_arm': {} } if self.params['use_pimu']: self.pimu = pimu.Pimu() self.status['pimu'] = self.pimu.status if self.params['use_base']: self.base = base.Base() self.status['base'] = self.base.status if self.params['use_lift']: self.lift = lift.Lift() self.status['lift'] = self.lift.status if self.params['use_arm']: self.arm = arm.Arm() self.status['arm'] = self.arm.status if self.params['use_head']: self.head = head.Head() self.status['head'] = self.head.status if self.params['use_wacc']: if self.params.has_key('custom_wacc'): module_name = self.params['custom_wacc']['py_module_name'] class_name = self.params['custom_wacc']['py_class_name'] self.wacc = getattr(importlib.import_module(module_name), class_name)(self) else: self.wacc = wacc.Wacc() self.status['wacc'] = self.wacc.status if self.params['use_end_of_arm']: self.end_of_arm = end_of_arm.EndOfArm() self.status['end_of_arm'] = self.end_of_arm.status self.devices = { 'pimu': self.pimu, 'base': self.base, 'lift': self.lift, 'arm': self.arm, 'head': self.head, 'wacc': self.wacc, 'end_of_arm': self.end_of_arm } self.rt = None self.dt = None