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 tourn_off_arm(): ar = ArbotiX() print "Disabling servos please wait..." #Set all servos torque limits to 0 for j in xrange(1, 7): ar.setTorqueLimit(j, 0) print("Servos disabled. Windowx_3link node closed.")
def __init__(self, name, host, password, graphplan, actionplan, knowledge): self.communicator = Communicator() print "will init rosnode with name: " + name rospy.init_node(name) # set the speed of each servo device = ArbotiX("/dev/ttyUSB0") for i in range(0, 5): device.setSpeed(i, 20) self.arm = ArmPub(name) self.arm.goToPosition(ArmPub.POSITION_RELAX) behaviours = self.communicator.getBehaviours() behaviours.append(SystemCore()) # does not use the overwritten SpadeAgent # calls directly the constructor of Spade Agent Agent.__init__(self, name, host, password, graphplan, actionplan, behaviours) with open(knowledge) as knowledge: self.setData("knowledge", json.load(knowledge)[self.localName]) self.setData("goals", {"pose": {}}) self.setData("naoAid", spade.AID.aid("nao1@" + host, ["xmpp://nao1@" + host])) other = self.getData("knowledge")["other"] + "@" + host otherAid = spade.AID.aid(other, ["xmpp://" + other]) self.setData("otherTurtleAid", otherAid) self.setData("otherState", Goals.otherStatus["nonReady"]) print self.getData("knowledge") # stores parameters received by waitMessageBehaviour # in case it is a plan self.plan = "" # stores the parameters of an order self.parameters = list() # stores who gave the order self.messageSender = "" self.name = name
def __init__(self, port="/dev/ttyUSB0"): # creating an ArbotiX device (the arm) on port (normally "/dev/ttyUSB0") self.device = ArbotiX(port) joint_defaults = getJointsFromURDF() self.servos = list() # list of the joints joints = rospy.get_param('/arbotix/joints', dict()) for name in sorted(joints.keys()): # pull data to convert from Angle to ticks n = "/arbotix/joints/"+ name +"/" ticks = rospy.get_param(n+"ticks", 1024) neutral = rospy.get_param(n+"neutral", ticks/2) invert = rospy.get_param(n+"invert", False) ranges = rospy.get_param(n+"range", 300) rad_per_tick = radians(ranges)/ticks # pull min and max angles for each servo min_angle, max_angle = getJointLimits(name, joint_defaults) serv = Servo(name, min_angle, max_angle, ticks, neutral, rad_per_tick, invert) self.servos.append(serv)
def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'): super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached) self._redist_rate = rospy.Rate(50) self._arbotix = ArbotiX('/dev/ttyUSB0') assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" self._joint_lock = Lock() self._angles, self._velocities = {}, {} rospy.Subscriber("/joint_states", JointState, self._joint_callback) time.sleep(1) self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]] self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) p.connect(p.DIRECT) widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf' self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi])) self._n_errors = 0
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 main(): CONV_H = 0.37 CONV_V = 0.3 dyna_center = 512 arby = ArbotiX("/dev/ttyUSB0", 115200) pan_rng, tilt_rng = panorama.get_camera_angles(dyna_center, CONV_V, CONV_H) rospy.init_node('brinkmanship_core') signal(SIGINT, handler) camera_pub = rospy.Publisher('image_capture_message', String, queue_size=1) sub = rospy.Subscriber('edge_alert', String, edge_alert_callback) speed = 0.75 turn = 1 # bot_stop(0,0) # time.sleep(2) # bot_forward(speed, turn) # time.sleep(2) # bot_stop(0,0) panorama.set_camera_pose(arby, dyna_center, dyna_center - 51) bot_forward(speed, turn) flag = 0 while not rospy.is_shutdown(): if not alert: pass else: bot_stop(speed, turn) b = datetime.datetime.now() if flag == 0: print("Alert has been published") print("Time required for stopping {} microseconds".format( (b - a).microseconds)) print("Moving Pan Tilt Motors") for pan in pan_rng: for tilt in tilt_rng: panorama.set_camera_pose(arby, pan, tilt) time.sleep(3) camera_pub.publish(str(pan) + '_' + str(tilt)) flag = 1
def panorama_method(): CONV_H = 0.37 CONV_V = 0.3 dyna_center = 512 arby = ArbotiX("/dev/ttyUSB0", 115200) pan_rng, tilt_rng = panorama.get_camera_angles(dyna_center, CONV_V, CONV_H) rospy.init_node('brinkmanship_core') signal(SIGINT, handler) camera_pub = rospy.Publisher('image_capture_message', String, queue_size=1) speed = 0.75 turn = 1 panorama.set_camera_pose(arby, dyna_center, dyna_center - 51) print("Alert has been published") print("Moving Pan Tilt Motors") for pan in pan_rng: for tilt in tilt_rng: panorama.set_camera_pose(arby, pan, tilt) time.sleep(3) camera_pub.publish(str(pan) + '_' + str(tilt)) flag = 1
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()
ticks = serv.angleToTicks(0.7854) print ticks device.setPosition(idserv, ticks) def shutdown(self): # stop turtlebot rospy.loginfo("Stop TurtleBot") # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot self.cmd_vel.publish(Twist()) # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script rospy.sleep(1) if __name__ == "__main__": rospy.init_node('arm') d = ArbotiX(port="/dev/ttyUSB0") arm = Arm() arm.newMoveServo(d, 2) ''' # premiere methode... pas mal mais n'arrive pas a fixer la vitesse arm = Arm() positions = [0,0,0,0,0,0] forward = 5 turn = 5 pos = 0.0 for i in range(0,10): arm.moveServo(pos) time.sleep(0.5) pos = pos + 0.1
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()
#!/usr/bin/env python # license removed for brevity import rospy from arbotix_python.arbotix import ArbotiX from arbotix_python.ax12 import * from sensor_msgs.msg import Joy turret = ArbotiX() def scan(): # Set speed print turret.setSpeed(1, 50) print turret.setSpeed(2, 50) print turret.getSpeed(1) # Put in initial position turret.setPosition(1, 510) turret.setPosition(2, 510) while turret.isMoving(1) or turret.isMoving(2): rospy.sleep(0.5) raw_input("Please enter something") # Do a scan cycle currentPosition = 0 for value in range(400, 600, 50): turret.setPosition(2, value) while turret.isMoving(2): rospy.sleep(0.5) if currentPosition == 0:
# Comment or uncomment prints for reading or writing servo parameters. from arbotix_python.arbotix import ArbotiX from time import sleep port = "/dev/ttyUSB0" arbotix = ArbotiX(port) ### To read one parameter of (one/a lot) servo/s. ### for info visit http://emanual.robotis.com/docs/en/dxl/ax/ax-12a/#control-table-of-eeprom-area # List of servos IDs servos = [1, 2, 3, 4, 5] #servos = [5] # Value of the register (EEPROM or RAM area) to read # parameter to set and its size data_name = 36 size = 2 while True: print arbotix.syncRead(servos, data_name, size) sleep(0.1) #print arbotix.syncRead(servos,data_name,size) # Return example (with Max Torque): # >>> [100, 1, 255, 3, 100, 1, 100, 1, 100, 1] # It means, Servo 1 has a value of 256*1 + 100 # Servo 2 has a value of 256*3 + 255... #----------------------------------------
from arbotix_python.arbotix import ArbotiX SERVO_IDS = [1, 2, 3, 4, 5, 6] arbotix = ArbotiX('/dev/ttyUSB0') err = arbotix.syncWrite(14, [[servo_id, 255, 1] for servo_id in SERVO_IDS]) if err != -1: print('Successfully reconfigured servos') else: print( 'Error writing to servos -- make sure start.sh is not running in the background' )
class WidowXController(RobotController): def __init__(self, robot_name, print_debug, email_cred_file='', log_file='', control_rate=100, gripper_attached='default'): super(WidowXController, self).__init__(robot_name, print_debug, email_cred_file, log_file, control_rate, gripper_attached) self._redist_rate = rospy.Rate(50) self._arbotix = ArbotiX('/dev/ttyUSB0') assert self._arbotix.syncWrite(MAX_TORQUE_L, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" self._joint_lock = Lock() self._angles, self._velocities = {}, {} rospy.Subscriber("/joint_states", JointState, self._joint_callback) time.sleep(1) self._joint_pubs = [rospy.Publisher('/{}/command'.format(name), Float64, queue_size=1) for name in JOINT_NAMES[:-1]] self._gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) p.connect(p.DIRECT) widow_x_urdf = '/'.join(__file__.split('/')[:-1]) + '/widowx/widowx.urdf' self._armID = p.loadURDF(widow_x_urdf, useFixedBase=True) p.resetBasePositionAndOrientation(self._armID, [0, 0, 0], p.getQuaternionFromEuler([np.pi, np.pi, np.pi])) self._n_errors = 0 def _init_gripper(self, gripper_attached): assert gripper_attached == 'default', 'widow_x only supports its default gripper at the moment' def _joint_callback(self, msg): with self._joint_lock: for name, position, velocity in zip(msg.name, msg.position, msg.velocity): self._angles[name] = position self._velocities[name] = velocity def _move_to_target_joints(self, joint_values): for value, pub in zip(joint_values, self._joint_pubs): pub.publish(Float64(value)) def move_to_neutral(self, duration=2): self._n_errors = 0 self._lerp_joints(np.array(NEUTRAL_VALUES), duration) self._gripper_pub.publish(Float64(GRIPPER_DROP[0])) time.sleep(GRIPPER_WAIT) def _reset_pybullet(self): for i, angle in enumerate(self.get_joint_angles()): p.resetJointState(self._armID, i, angle) def _lerp_joints(self, target_joint_pos, duration): start_t, start_joints = rospy.get_time(), self.get_joint_angles() self._control_rate.sleep() cur_joints = self.get_joint_angles() while rospy.get_time() - start_t < 1.2 * duration and not np.isclose(target_joint_pos[:5], cur_joints[:5], atol=CONTROL_TOL).all(): t = min(1.0, (rospy.get_time() - start_t) / duration) target_joints = (1 - t) * start_joints[:5] + t * target_joint_pos[:5] self._move_to_target_joints(target_joints) self._control_rate.sleep() cur_joints = self.get_joint_angles() logging.getLogger('robot_logger').debug('Lerped for {} seconds'.format(rospy.get_time() - start_t)) self._reset_pybullet() delta = np.linalg.norm(target_joints[:5] - cur_joints[:5]) if delta > MAX_FINAL_ERR: assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring" self._n_errors += 1 if self._n_errors > MAX_ERRORS: logging.getLogger('robot_logger').error('More than {} errors! WidowX probably crashed.'.format(MAX_ERRORS)) raise Environment_Exception logging.getLogger('robot_logger').debug('Delta at end of lerp is {}'.format(delta)) def move_to_eep(self, target_pose, duration=1.5): """ :param target_pose: Cartesian pose (x,y,z, quat). :param duration: Total time trajectory will take before ending """ target_joint_pos = self._calculate_ik(target_pose[:3], target_pose[3:])[0] target_joint_pos = np.clip(np.array(target_joint_pos)[:6], JOINT_MIN, JOINT_MAX) self._lerp_joints(target_joint_pos, duration) def move_to_ja(self, waypoints, duration=1.5): """ :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point. Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint :param duration: Total time trajectory will take before ending """ for waypoint in waypoints: # Move to each waypoint in sequence. See move_to_target_joint_position # in controller.py self._move_to_target_joints(waypoint) def redistribute_objects(self): """ Play pre-recorded trajectory that sweeps objects into center of bin """ logging.getLogger('robot_logger').info('redistribute...') file = '/'.join(str.split(robot_envs.__file__, "/")[ :-1]) + '/recorded_trajectories/pushback_traj_{}.pkl'.format(self._robot_name) joint_pos = pkl.load(open(file, "rb")) self._redist_rate.sleep() for joint_t in joint_pos: print(joint_t) self._move_to_target_joints(joint_t) self._redist_rate.sleep() def get_joint_angles(self): #returns current joint angles with self._joint_lock: joints_ret = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'gripper_revolute_joint'] try: return np.array([self._angles[k] for k in joints_ret]) except KeyError: return None def get_joint_angles_velocity(self): #returns current joint angles #No velocities for widowx? :( with self._joint_lock: joints_ret = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'gripper_revolute_joint'] try: return np.array([self._velocities[k] for k in joints_ret]) except KeyError: return None def get_cartesian_pose(self): #Returns cartesian end-effector pose self._reset_pybullet() position, quat = p.getLinkState(self._armID, 5, computeForwardKinematics=1)[4:6] return np.array(list(position) + list(quat)) def quat_2_euler(self, quat): roll, pitch, yaw = Quaternion(quat).yaw_pitch_roll return np.pi - yaw, pitch, roll def euler_2_quat(self, yaw=0.0, pitch=np.pi, roll=0.0): roll_matrix = np.array([[np.cos(roll), -np.sin(roll), 0.0],[np.sin(roll), np.cos(roll), 0.0], [0, 0, 1.0]]) pitch_matrix = np.array([[np.cos(pitch), 0., np.sin(pitch)], [0.0, 1.0, 0.0], [-np.sin(pitch), 0, np.cos(pitch)]]) yaw_matrix = np.array([[1.0, 0, 0], [0, np.cos(yaw), -np.sin(yaw)], [0, np.sin(yaw), np.cos(yaw)]]) rot_mat = roll_matrix.dot(pitch_matrix.dot(yaw_matrix)) return Quaternion(matrix=rot_mat).elements def get_gripper_state(self, integrate_force=False): # returns gripper joint angle, force reading (none if no force) with self._joint_lock: joint_angle = self._angles['gripper_prismatic_joint_1'] return joint_angle, None def open_gripper(self, wait = False): # should likely wrap separate gripper control class for max re-usability self._gripper_pub.publish(Float64(self.GRIPPER_OPEN)) time.sleep(GRIPPER_WAIT) def close_gripper(self, wait = False): # should likely wrap separate gripper control class for max re-usability self._gripper_pub.publish(Float64(self.GRIPPER_CLOSE)) time.sleep(GRIPPER_WAIT) @property def GRIPPER_CLOSE(self): return GRIPPER_CLOSED[0] @property def GRIPPER_OPEN(self): return GRIPPER_OPEN[1] def _calculate_ik(self, targetPos, targetQuat, threshold=1e-5, maxIter=1000, nJoints=6): closeEnough = False iter_count = 0 dist2 = None best_ret, best_dist = None, float('inf') while (not closeEnough and iter_count < maxIter): jointPoses = list(p.calculateInverseKinematics(self._armID, 5, targetPos, targetQuat, JOINT_MIN, JOINT_MAX)) for i in range(nJoints): jointPoses[i] = max(min(jointPoses[i], JOINT_MAX[i]), JOINT_MIN[i]) p.resetJointState(self._armID, i, jointPoses[i]) ls = p.getLinkState(self._armID, 5, computeForwardKinematics=1) newPos, newQuat = ls[4], ls[5] dist2 = sum([(targetPos[i] - newPos[i]) ** 2 for i in range(3)]) closeEnough = dist2 < threshold iter_count += 1 if dist2 < best_dist: best_ret, best_dist = (jointPoses, newPos, newQuat), dist2 return best_ret
if ('Utah_Pit' in file_locations['robot_simulation_env']): TIME_OUT = 226*3+ 162*12+ 355*3+ 500*5-300 #TIME_OUT = 2200*4.1 #one shot 2200*1.3, #3 shots = 2200*3 if ('Utah_BIG' in file_locations['robot_simulation_env']): TIME_OUT = 2200*1 #normal = 1.3 big = 1 if ('Lacus_Mortis_Pit' in file_locations['robot_simulation_env']): TIME_OUT = 2200*10 if ('Pit_Edge_Test' in file_locations['robot_simulation_env']): TIME_OUT = 4*160+160 #normal = 1.3 big = 1 else: # sys.path.insert(1,file_locations['robot_simulation_env'][0:file_locations['robot_simulation_env'].rfind("/",0,-1)]+"/Brinkmanship") # from cloud_process import CloudSubscriber # from brinkmanship import Brinkmanship from arbotix_python.arbotix import ArbotiX TIME_OUT = 80+3*160+240 + rospy.get_rostime().secs arb = ArbotiX("/dev/ttyUSB0",115200) arb = ArbotiX("/dev/ttyUSB1",115200) img_capture_pub = rospy.Publisher('/image_number', Int32, queue_size = 10) map_resolution = rospy.get_param("/resolution") halfway_point = len(smach_helper.read_csv_around_pit(file_locations['file_around_pit'],map_resolution))/2 waypoint_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size = 1) pit_edge_dist_pub = rospy.Publisher('/robot_at_edge_position', Odometry, queue_size = 1) move_base_client = actionlib.SimpleActionClient('move_base',MoveBaseAction) sub_where_to_see = rospy.Subscriber('where_to_see',Float32,smach_helper.update_sun)
class Arm: DEFAULT_SPEED = 20 # predifined positions. More an be added the same way # a list of angles for each servo (first one is the gripper then the motors in order) # from these positions we can define variants: # with gripper closed. First parameter = 2.6 # on the left 90. Second parameter = PI/2 # on the right 90. Second parameter = -PI/2 POSITION_1 = [0,0,0.38,0.28, 0.88, -1.7] POSITION_2 = [0,0,1.07,-0.59, 0.88, -1.7] POSITION_RELAX = [0,0,-1.21,1.5,0.64, 0] POSITION_BRING = [2.6,0,-0.57,0.12,1.5,-1.7] def __init__(self, port="/dev/ttyUSB0"): # creating an ArbotiX device (the arm) on port (normally "/dev/ttyUSB0") self.device = ArbotiX(port) joint_defaults = getJointsFromURDF() self.servos = list() # list of the joints joints = rospy.get_param('/arbotix/joints', dict()) for name in sorted(joints.keys()): # pull data to convert from Angle to ticks n = "/arbotix/joints/"+ name +"/" ticks = rospy.get_param(n+"ticks", 1024) neutral = rospy.get_param(n+"neutral", ticks/2) invert = rospy.get_param(n+"invert", False) ranges = rospy.get_param(n+"range", 300) rad_per_tick = radians(ranges)/ticks # pull min and max angles for each servo min_angle, max_angle = getJointLimits(name, joint_defaults) serv = Servo(name, min_angle, max_angle, ticks, neutral, rad_per_tick, invert) self.servos.append(serv) # moves a single servo at a given speed to a given angle (in radians) # by default the speed is set at 20 def moveServo(self, idserv, angle , speed=DEFAULT_SPEED): serv = self.servos[idserv] self.device.setSpeed(idserv, speed) ticks = serv.angleToTicks(angle) self.device.setPosition(idserv, ticks) def goToPosition(self, position): i = 0 for pos in position: print "moving servo: ", i self.moveServo(i,pos) i = i+1 def closeGripper(self): min_angle = self.servos[0].min_angle print " min angle for gripper = " , min_angle self.moveServo(0,min_angle) def openGripper(self): max_angle = self.servos[0].max_angle print " max angle for gripper = " , max_angle self.moveServo(0,max_angle) def take(self): self.openGripper() time.sleep(2) self.goToPosition(self.POSITION_1) # waits 2 seconds before closing the gripper time.sleep(2) self.closeGripper() # waits 2 seconds before taking the object off the support time.sleep(2) self.goToPosition(Arm.POSITION_BRING) def put(self): self.goToPosition(self.POSITION_1) # waits 2 seconds before closing the gripper time.sleep(2) self.openGripper() # waits 2 seconds before taking the object off the support time.sleep(2) self.goToPosition(Arm.POSITION_BRING)
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