def __init__(self, port="/dev/ttyUSB0", baud=1000000): ArbotiX.__init__(self, port, baud) print "--- Test Dynamixel Communication ---" print "This test use arbitix_ros package" while True: print ">>> ", keyboard_cmd = raw_input().split(" ") try: if keyboard_cmd[0] == "ping": print "id:" elif keyboard_cmd[0] == "get": if keyboard_cmd[1] == "position": pos = self.getPosition(int(keyboard_cmd[2])) if pos > 0: print "position:%d" % pos else: print "error:" + str(pos) elif keyboard_cmd[0] == "set": if keyboard_cmd[1] == "position": index = int(keyboard_cmd[2]) pos = int(keyboard_cmd[3]) result = self.setPosition(index, pos) print "callback:" + str(result) except Exception as exce: print "Error!", exce
def __init__(self): #Initialize arbotix comunications print"\nArbotix initialization, wait 10 seconds..." ArbotiX.__init__(self) for x in xrange(1,21): time.sleep(0.5) print(str(x*0.5) + "/10s") #Setupr velocities and positions vectors and messages self.joints_poses = [0,0,0,0,0,0] self.joints_vels = [0,0,0,0,0] self.ee_closed = 0 self.vels_to_pub = Float32MultiArray() self.poses_to_pub = Float32MultiArray() self.poses_layout = MultiArrayDimension('joints_poses', 6, 0) self.vels_layout = MultiArrayDimension('joints_vels', 5, 0) self.poses_to_pub.layout.dim = [self.poses_layout] self.poses_to_pub.layout.data_offset = 0 self.vels_to_pub.layout.dim = [self.vels_layout] self.vels_to_pub.layout.data_offset = 0 #ROS pubblisher for joint velocities and positions self.pos_pub = rospy.Publisher('/windowx_3links/joints_poses', Float32MultiArray, queue_size=1) self.vel_pub = rospy.Publisher('/windowx_3links/joints_vels', Float32MultiArray, queue_size=1) print"Windowx_3link node created, whaiting for messages in:" print" windowx_2links/torque" print"Publishing joints' positions and velocities in:" print" /windowx_3links/joints_poses" print" /windowx_3links/joints_vels" #Start publisher self.publish()
def __init__(self, port="/dev/ttyUSB0", baud=1000000): #Initialize arbotix comunications print"\nArbotix initialization, wait 10 seconds..." ArbotiX.__init__(self, port, baud) for x in xrange(1,21): time.sleep(0.5) print(str(x*0.5) + "/10s") print"Done." #Set inital torque limits print"Limiting torques" mx28_init_torque_limit = int(MX_TORQUE_STEPS/3) mx64_init_torque_limit = int(MX_TORQUE_STEPS/5) ax_init_torque_limit = int(AX_TORQUE_STEPS/5) self.setTorqueLimit(int(1), mx28_init_torque_limit) self.setTorqueLimit(int(2), mx64_init_torque_limit) self.setTorqueLimit(int(3), (mx64_init_torque_limit + 100)) #The 3rd servo needs more torque to contrast initial inertias self.setTorqueLimit(int(4), mx28_init_torque_limit) self.setTorqueLimit(int(5), ax_init_torque_limit) self.setTorqueLimit(int(6), ax_init_torque_limit) #Go to 0 position for each servo print"Going to initialization position..." self.setPosition(int(1), int(MX_POS_CENTER)) self.setPosition(int(4), int(MX_POS_CENTER)) self.setPosition(int(3), int(MX_POS_CENTER)) self.setPosition(int(2), int(MX_POS_CENTER)) self.setPosition(int(5), int(AX_POS_CENTER)) self.setPosition(int(6), int(AX_POS_CENTER)) print"Arm ready, setting up ROS topics..." #Setupr velocities and positions vectors and messages self.joints_poses = [0,0,0,0,0,0] self.joints_vels = [0,0,0,0,0] self.ee_closed = 0 self.vels_to_pub = Float32MultiArray() self.poses_to_pub = Float32MultiArray() self.poses_layout = MultiArrayDimension('joints_poses', 6, 0) self.vels_layout = MultiArrayDimension('joints_vels', 5, 0) self.poses_to_pub.layout.dim = [self.poses_layout] self.poses_to_pub.layout.data_offset = 0 self.vels_to_pub.layout.dim = [self.vels_layout] self.vels_to_pub.layout.data_offset = 0 #ROS pubblisher for joint velocities and positions self.pos_pub = rospy.Publisher('/windowx_3links/joints_poses', Float32MultiArray, queue_size=1) self.vel_pub = rospy.Publisher('/windowx_3links/joints_vels', Float32MultiArray, queue_size=1) #ROS listener for control torues self.torque_sub = rospy.Subscriber('windowx_3links/torques', Float32MultiArray, self._torque_callback, queue_size=1) print"Windowx_3link node created, whaiting for messages in:" print" windowx_2links/torque" print"Publishing joints' positions and velocities in:" print" /windowx_3links/joints_poses" print" /windowx_3links/joints_vels" #Start publisher self.publish()
def __init__(self): # pause = False # load configurations port = rospy.get_param("~port", "/dev/ttyUSB0") baud = int(rospy.get_param("~baud", "115200")) # self.rate = rospy.get_param("~rate", 100.0) # self.fake = rospy.get_param("~sim", False) # self.use_sync_read = rospy.get_param("~sync_read",True) # use sync read? # self.use_sync_write = rospy.get_param("~sync_write",True) # use sync write? # setup publishers # self.diagnostics = DiagnosticsPublisher() # self.joint_state_publisher = JointStatePublisher() # start an arbotix driver # if not self.fake: ArbotiX.__init__(self, port, baud) rospy.sleep(1.0) rospy.loginfo("Started ArbotiX connection on port " + port + ".")
def __init__(self, port="/dev/ttyUSB0", baud=115200): # start ArbotiX.__init__(self, port, baud) print "ArbotiX Terminal --- Version 0.1" print "Copyright 2011 Vanadium Labs LLC" # loop while True: print ">> ", kmd = raw_input().split(" ") try: if kmd[0] == "help": # display help data if len(kmd) > 1: # for a particular command if kmd[1] == "ls": print help[3] elif kmd[1] == "mv": print help[4] elif kmd[1] == "baud": print help[5] elif kmd[1] == "get": print help[6] elif kmd[1] == "set": print help[7] else: print "help: unrecognized command" else: for h in help: print h elif kmd[0] == "ls": # list servos self._ser.timeout = 0.25 if len(kmd) > 2: self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))]) self.query() self.query() elif kmd[0] == "mv": # rename a servo if self.write(int(kmd[1]), P_ID, [ int(kmd[2]), ]) == 0: print self.OKBLUE + "OK" + self.ENDC elif kmd[0] == "baud": # set bus baud rate self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))]) print self.OKBLUE + "OK" + self.ENDC elif kmd[0] == "set": if kmd[1] == "baud": self.write(int(kmd[2]), P_BAUD_RATE, [self.convertBaud(int(kmd[3]))]) print self.OKBLUE + "OK" + self.ENDC elif kmd[1] == "pos" or kmd[1] == "position": self.setPosition(int(kmd[2]), int(kmd[3])) print self.OKBLUE + "OK" + self.ENDC elif kmd[0] == "get": if kmd[1] == "temp": value = self.getTemperature(int(kmd[2])) if value >= 60 or value < 0: print self.FAIL + str(value) + self.ENDC elif value > 40: print self.WARNING + str(value) + self.ENDC else: print self.OKGREEN + str(value) + self.ENDC elif kmd[1] == "pos" or kmd[1] == "position": value = self.getPosition(int(kmd[2])) if value >= 0: print self.OKGREEN + str(value) + self.ENDC else: print self.FAIL + str(value) + self.ENDC except Exception as e: print "error...", e
def __init__(self): pause = False # load configurations port = rospy.get_param("~port", "/dev/ttyUSB0") baud = int(rospy.get_param("~baud", "115200")) self.rate = rospy.get_param("~rate", 100.0) self.fake = rospy.get_param("~sim", False) self.use_sync_read = rospy.get_param("~sync_read",True) # use sync read? self.use_sync_write = rospy.get_param("~sync_write",True) # use sync write? # setup publishers self.diagnostics = DiagnosticsPublisher() self.joint_state_publisher = JointStatePublisher() # start an arbotix driver if not self.fake: ArbotiX.__init__(self, port, baud) rospy.sleep(1.0) rospy.loginfo("Started ArbotiX connection on port " + port + ".") else: rospy.loginfo("ArbotiX being simulated.") # initialize dynamixel & hobby servos self.servos = Servos(self) # setup controllers self.controllers = list() controllers = rospy.get_param("~controllers", dict()) for name, params in controllers.items(): if params["type"] == "follow_controller": self.controllers.append(FollowController(self, name)) if self.controllers[-1].onboard: pause = True elif params["type"] == "diff_controller": self.controllers.append(DiffController(self, name)) pause = True # elif params["type"] == "omni_controller": # self.controllers.append(OmniController(self, name)) # pause = True else: rospy.logerr("Unrecognized controller: " + params["type"]) # wait for arbotix to start up (especially after reset) if not self.fake: if rospy.has_param("~digital_servos") or rospy.has_param("~digital_sensors") or rospy.has_param("~analog_sensors"): pause = True if pause: while self.getDigital(1) == -1: rospy.loginfo("Waiting for response...") rospy.sleep(0.25) rospy.loginfo("ArbotiX connected.") for controller in self.controllers: controller.startup() # services for io rospy.Service('~SetupAnalogIn',SetupChannel, self.analogInCb) rospy.Service('~SetupDigitalIn',SetupChannel, self.digitalInCb) rospy.Service('~SetupDigitalOut',SetupChannel, self.digitalOutCb) # initialize digital/analog IO streams self.io = dict() if not self.fake: for v,t in {"digital_servos":DigitalServo,"digital_sensors":DigitalSensor,"analog_sensors":AnalogSensor}.items(): temp = rospy.get_param("~"+v,dict()) for name in temp.keys(): pin = rospy.get_param('~'+v+'/'+name+'/pin',1) value = rospy.get_param('~'+v+'/'+name+'/value',0) rate = rospy.get_param('~'+v+'/'+name+'/rate',10) self.io[name] = t(name, pin, value, rate, self) r = rospy.Rate(self.rate) # main loop -- do all the read/write here while not rospy.is_shutdown(): # update controllers for controller in self.controllers: controller.update() # update servo positions (via sync_write) self.servos.update(self.use_sync_write) # update io for io in self.io.values(): io.update() # publish self.servos.interpolate(self.use_sync_read) self.joint_state_publisher.update(self.servos, self.controllers) self.diagnostics.update(self.servos, self.controllers) r.sleep() # do shutdown for controller in self.controllers: controller.shutdown()
def __init__(self, port = "/dev/ttyUSB0", baud = 115200): # start ArbotiX.__init__(self, port, baud) print "ArbotiX Terminal --- Version 0.1" print "Copyright 2011 Vanadium Labs LLC" # loop while True: print ">> ", kmd = raw_input().split(" ") try: if kmd[0] == "help": # display help data if len(kmd) > 1: # for a particular command if kmd[1] == "ls": print help[3] elif kmd[1] == "mv": print help[4] elif kmd[1] == "baud": print help[5] elif kmd[1] == "get": print help[6] elif kmd[1] == "set": print help[7] else: print "help: unrecognized command" else: for h in help: print h elif kmd[0] == "ls": # list servos self._ser.timeout = 0.25 if len(kmd) > 2: self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))]) self.query() self.query() elif kmd[0] == "mv": # rename a servo if self.write( int(kmd[1]), P_ID, [int(kmd[2]),] ) == 0: print self.OKBLUE+"OK"+self.ENDC elif kmd[0] == "baud": # set bus baud rate self.write(253, P_BAUD_RATE, [self.convertBaud(int(kmd[1]))]) print self.OKBLUE+"OK"+self.ENDC elif kmd[0] == "set": if kmd[1] == "baud": self.write( int(kmd[2]), P_BAUD_RATE, [self.convertBaud(int(kmd[3]))] ) print self.OKBLUE+"OK"+self.ENDC elif kmd[1] == "pos" or kmd[1] == "position": self.setPosition( int(kmd[2]), int(kmd[3]) ) print self.OKBLUE+"OK"+self.ENDC elif kmd[0] == "get": if kmd[1] == "temp": value = self.getTemperature(int(kmd[2])) if value >= 60 or value < 0: print self.FAIL+str(value)+self.ENDC elif value > 40: print self.WARNING+str(value)+self.ENDC else: print self.OKGREEN+str(value)+self.ENDC elif kmd[1] == "pos" or kmd[1] == "position": value = self.getPosition(int(kmd[2])) if value >= 0: print self.OKGREEN+str(value)+self.ENDC else: print self.FAIL+str(value)+self.ENDC except Exception as e: print "error...", e
def __init__(self): pause = False # load configurations port = rospy.get_param("~port", "/dev/ttyUSB0") baud = int(rospy.get_param("~baud", "115200")) self.rate = rospy.get_param("~rate", 100.0) self.fake = rospy.get_param("~sim", False) self.use_sync_read = rospy.get_param("~sync_read",True) # use sync read? self.use_sync_write = rospy.get_param("~sync_write",True) # use sync write? # setup publishers self.diagnostics = DiagnosticsPublisher() self.joint_state_publisher = JointStatePublisher() # start an arbotix driver if not self.fake: ArbotiX.__init__(self, port, baud) rospy.sleep(1.0) rospy.loginfo("Started ArbotiX connection on port " + port + ".") else: rospy.loginfo("ArbotiX being simulated.") # setup joints self.joints = dict() for name in rospy.get_param("~joints", dict()).keys(): joint_type = rospy.get_param("~joints/"+name+"/type", "dynamixel") if joint_type == "dynamixel": self.joints[name] = DynamixelServo(self, name) elif joint_type == "hobby_servo": self.joints[name] = HobbyServo(self, name) elif joint_type == "calibrated_linear": self.joints[name] = LinearJoint(self, name) # TODO: <BEGIN> REMOVE THIS BEFORE 1.0 if len(rospy.get_param("~dynamixels", dict()).keys()) > 0: rospy.logwarn("Warning: use of dynamixels as a dictionary is deprecated and will be removed in 0.8.0") for name in rospy.get_param("~dynamixels", dict()).keys(): self.joints[name] = DynamixelServo(self, name, "~dynamixels") if len(rospy.get_param("~servos", dict()).keys()) > 0: rospy.logwarn("Warning: use of servos as a dictionary is deprecated and will be removed in 0.8.0") for name in rospy.get_param("~servos", dict()).keys(): self.joints[name] = HobbyServo(self, name, "~servos") # TODO: <END> REMOVE THIS BEFORE 1.0 # setup controller self.controllers = [ServoController(self, "servos"), ] controllers = rospy.get_param("~controllers", dict()) for name, params in controllers.items(): try: controller = controller_types[params["type"]](self, name) self.controllers.append( controller ) pause = pause or controller.pause except Exception as e: if type(e) == KeyError: rospy.logerr("Unrecognized controller: " + params["type"]) else: rospy.logerr(str(type(e)) + str(e)) # wait for arbotix to start up (especially after reset) if not self.fake: if rospy.has_param("~digital_servos") or rospy.has_param("~digital_sensors") or rospy.has_param("~analog_sensors"): pause = True if pause: while self.getDigital(1) == -1 and not rospy.is_shutdown(): rospy.loginfo("Waiting for response...") rospy.sleep(0.25) rospy.loginfo("ArbotiX connected.") for controller in self.controllers: controller.startup() # services for io rospy.Service('~SetupAnalogIn',SetupChannel, self.analogInCb) rospy.Service('~SetupDigitalIn',SetupChannel, self.digitalInCb) rospy.Service('~SetupDigitalOut',SetupChannel, self.digitalOutCb) # initialize digital/analog IO streams self.io = dict() if not self.fake: for v,t in {"digital_servos":DigitalServo,"digital_sensors":DigitalSensor,"analog_sensors":AnalogSensor}.items(): temp = rospy.get_param("~"+v,dict()) for name in temp.keys(): pin = rospy.get_param('~'+v+'/'+name+'/pin',1) value = rospy.get_param('~'+v+'/'+name+'/value',0) rate = rospy.get_param('~'+v+'/'+name+'/rate',10) self.io[name] = t(name, pin, value, rate, self) r = rospy.Rate(self.rate) # main loop -- do all the read/write here while not rospy.is_shutdown(): # update controllers for controller in self.controllers: controller.update() # update io for io in self.io.values(): io.update() # publish feedback self.joint_state_publisher.update(self.joints.values(), self.controllers) self.diagnostics.update(self.joints.values(), self.controllers) r.sleep() # do shutdown for controller in self.controllers: controller.shutdown()