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
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
from __future__ import print_function
import sys, tty, termios
import time
import stretch_body.arm as arm
import argparse
import stretch_body.hello_utils as hu
hu.print_stretch_re_use()
parser = argparse.ArgumentParser(
description='Jog the arm motion from the keyboard')
args = parser.parse_args()
small_move_m = .01
large_move_m = 0.1
a = arm.Arm()
if not a.startup():
exit()
a.motor.disable_sync_mode()
a.push_command()
def get_keystroke():
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)