def set_pid_values(req): print "motorID = %s. Setting PID values: P = %s, I = %s and D = %s"%( req.ID, req.P, req.I, req.D) motors=dynamixel_io.DynamixelIO('/dev/ttyUSB0',1000000) motors.set_p_gain(req.ID, req.P) motors.set_i_gain(req.ID, req.I) motors.set_d_gain(req.ID, req.D) """ Sets the value of integral action of PID controller. Gain value is in range 0 to 254. """ return True
def main(args=sys.argv): parser = OptionParser( usage='Usage: %prog [options] OLD_ID NEW_ID', description='Changes the unique ID of a Dynamixel servo motor.') parser.add_option( '-p', '--port', metavar='PORT', default='/dev/ttyUSB0', help= 'motors of specified controllers are connected to PORT [default: %default]' ) parser.add_option( '-b', '--baud', metavar='BAUD', type="int", default=1000000, help= 'connection to serial port will be established at BAUD bps [default: %default]' ) (options, args) = parser.parse_args(args) if len(args) < 3: parser.print_help() exit(1) port = options.port baudrate = options.baud old_id = int(args[1]) new_id = int(args[2]) try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError as soe: print('ERROR:{}'.format(soe)) else: print('Changing motor id from %d to %d...' % (old_id, new_id), end='') if dxl_io.ping(old_id): dxl_io.set_id(old_id, new_id) print(' done') print('Verifying new id...', end='') if dxl_io.ping(new_id): print(' done') else: print( 'ERROR: The motor did not respond to a ping to its new id.' ) else: print( 'ERROR: The specified motor did not respond to id %d. Make sure to specify the correct baudrate.' % old_id)
def __init__(self): rospy.init_node('dqn') self.pub = rospy.Publisher('/pan4_controller/command', Float64, queue_size=10) self.init_pub = rospy.Publisher('/tilt4_controller/command', Float64, queue_size=10) rospy.Subscriber('/pan4_controller/state', JointState, self.angle_state) self.dxl_io = dynamixel_io.DynamixelIO('/dev/ttyUSB0', 1000000) self.num_actions = 2 self.frame_num = 1 self.img_dims = [84, 84] self.initial_replay_size = 100 self.gamma = 0.99 self.epsilon = 1.0 self.final_epsilon = 0.1 self.epsilon_step = 0.0002 self.num_replay_memory = 40000 self.target_update_interval = 1000 self.momentum = 0.95 self.min_grad = 0.01 self.lr = 0.00025 self.t = 0 self.rate = rospy.Rate(5) self.replay_memory = deque() self.video = Video(self.img_dims) self.q_network, self.s, self.q = self.build_network() self.target_network, self.st, self.qt = self.build_network() self.q_network_weights = self.q_network.trainable_weights self.target_network_weights = self.target_network.trainable_weights self.update_target_network = [ self.target_network_weights[i].assign(self.q_network_weights[i]) for i in xrange(len(self.target_network_weights)) ] self.batch_size = 32 self.a, self.y, self.loss, self.grad_update = self.build_training_op() self.sess = tf.InteractiveSession() self.sess.run(tf.initialize_all_variables()) self.sess.run(self.update_target_network)
def enable_torque(req): motors = dynamixel_io.DynamixelIO('/dev/ttyUSB0', 1000000) state = False if (req.motor_state == 0) | (req.motor_state == 9) | (req.motor_state == 12): valueTuples = ((4, 1), (5, 1), (3, 1), (1, 1), (2, 1), (6, 1), (7, 1)) motors.set_multi_torque_enabled(valueTuples) state = True elif req.motor_state == 11: valueTuples = ((4, 1), (5, 1), (3, 0), (1, 1), (2, 1), (6, 1), (7, 1)) motors.set_multi_torque_enabled(valueTuples) state = True elif req.motor_state == 8: valueTuples = ((4, 1), (5, 1), (3, 1), (1, 1), (2, 1), (6, 0), (7, 1)) motors.set_multi_torque_enabled(valueTuples) state = True elif (req.motor_state == 2) | (req.motor_state == 3) | ( req.motor_state == 4) | (req.motor_state == 5): valueTuples = ((4, 1), (5, 1), (3, 0), (1, 1), (2, 1), (6, 0), (7, 1)) motors.set_multi_torque_enabled(valueTuples) state = True elif (req.motor_state == 6) | (req.motor_state == 1) | (req.motor_state == 7): valueTuples = ((4, 1), (5, 1), (3, 1), (1, 1), (2, 0), (6, 1), (7, 1)) motors.set_multi_torque_enabled(valueTuples) state = True elif req.motor_state == 10: valueTuples = ((4, 1), (5, 1), (3, 1), (1, 1), (2, 1), (6, 0), (7, 1)) motors.set_multi_torque_enabled(valueTuples) state = True else: valueTuples = ((4, 0), (5, 0), (3, 0), (1, 0), (2, 0), (6, 0), (7, 0)) motors.set_multi_torque_enabled(valueTuples) state = True return state print "State of the motors changed"
def main(args=sys.argv): usage_msg = 'Usage: %prog [options] ID [On|Off]' desc_msg = 'Turns the torque of specified Dynamixel servo motor on or off.' epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 1 Off' % sys.argv[0] parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg) parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0', help='motors of specified controllers are connected to PORT [default: %default]') parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000, help='connection to serial port will be established at BAUD bps [default: %default]') (options, args) = parser.parse_args(args) if len(args) < 3: parser.print_help() exit(1) port = options.port baudrate = options.baud motor_id = int(args[1]) torque_on = args[2] try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError as soe: print('ERROR: {}'.format(soe)) else: print('Turning torque %s for motor %d' % (torque_on, motor_id)) if dxl_io.ping(motor_id): if torque_on.lower() == 'off': torque_on = False elif torque_on.lower() == 'on': torque_on = True else: parser.print_help() exit(1) dxl_io.set_torque_enabled(motor_id, torque_on) print('done') else: print('ERROR: motor %d did not respond. Make sure to specify the correct baudrate.' % motor_id)
max_motor_id = 25 update_rate = 5 diagnostics_rate = 1 error_level_temp = 75 warn_level_temp = 70 readback_echo = False torque_on = True motor_id = 1 r = 0.126 m = 842 I = m * r * r c = r * m * 9.81 sync_write_address = [0x66, 0x00] try: #rospy.init_node('dual_joint', anonymous=True) dxl_io = dynamixel_io.DynamixelIO(port_name, baud_rate) except dynamixel_io.SerialOpenError, soe: print 'ERROR:', soe else: print 'Turning torque %s for motor %d' % (torque_on, motor_id) if dxl_io.ping(motor_id): torque_response = dxl_io.set_torque_enabled(2, 1) print "torque enable2 response", torque_response time.sleep(1) torque_response = dxl_io.set_torque_enabled(1, 1) print "torque enable1 response", torque_response time.sleep(3) print 'done' else: print 'ERROR: motor %d did not respond. Make sure to specify the correct baudrate.' % motor_id
#! /usr/bin/env python import sys sys.path.append( "/home/ubuntu/practice_ws/src/dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers" ) from dynamixel_driver import dynamixel_io import joint_torque_controller as tc dxl_io = dynamixel_io.DynamixelIO('/dev/ttyUSB0', 1000000) jtc = [0, 0, 0, 0, 0, 0] jtc[1] = tc.JointTorqueController(dxl_io, "tilt1_controller", "pan_tilt_port") jtc[2] = tc.JointTorqueController(dxl_io, "tilt2_controller", "pan_tilt_port") jtc[3] = tc.JointTorqueController(dxl_io, "tilt3_controller", "pan_tilt_port") jtc[4] = tc.JointTorqueController(dxl_io, "tilt4_controller", "pan_tilt_port") jtc[5] = tc.JointTorqueController(dxl_io, "tilt5_controller", "pan_tilt_port") for i in range(1, 6): jtc[i].initialize() print(jtc[1].MAX_VELOCITY) print(jtc[1].MIN_VELOCITY) for i in range(1, 6): jtc[i].set_compliance_margin(0) jtc[i].set_compliance_slope(32) jtc[i].set_compliance_punch(32) jtc[2].set_compliance_margin(0)
help= 'connection to serial port will be established at BAUD bps [default: %default]' ) (options, args) = parser.parse_args(sys.argv) if len(args) < 2: parser.print_help() exit(1) port = options.port baudrate = options.baud motor_ids = args[1:] try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError, soe: print 'ERROR:', soe else: responses = 0 print 'Pinging motors:' for motor_id in motor_ids: motor_id = int(motor_id) print '%d ...' % motor_id, p = dxl_io.ping(motor_id) if p: responses += 1 values = dxl_io.get_feedback(motor_id) angles = dxl_io.get_angle_limits(motor_id) model = dxl_io.get_model_number(motor_id) firmware = dxl_io.get_firmware_version(motor_id)
port_name2 = '/dev/ttyUSB2' port_namespace2 = 'ttyUSB2' port_name3 = '/dev/ttyUSB3' port_namespace3 = 'ttyUSB3' baud_rate = 3000000 min_motor_id = 1 max_motor_id = 25 update_rate = 5 diagnostics_rate = 1 error_level_temp = 75 warn_level_temp = 70 readback_echo = False torque_on = True motor_id = 1 motor_id_3 = 7 r = 0.126 m = 842 I = m * r * r c = r * m * 9.81 #i=0 # sync_write_address= [0x66, 0x00] try: dxl_io_0 = dynamixel_io.DynamixelIO(port_name0, baud_rate) #dxl_io_1 = dynamixel_io.DynamixelIO(port_name1, baud_rate) #dxl_io_2 = dynamixel_io.DynamixelIO(port_name2, baud_rate) dxl_io_3 = dynamixel_io.DynamixelIO(port_name3, baud_rate) except dynamixel_io.SerialOpenError, soe: print 'ERROR:', soe else: pos_control2()
def __init__(self): rospy.init_node('ddpg') #self.pub2 = rospy.Publisher('/pan2_controller/command', Float64, queue_size=10) self.pub4 = rospy.Publisher('/pan4_controller/command', Float64, queue_size=10) #self.pub5 = rospy.Publisher('/pan5_controller/command', Float64, queue_size=10) #self.init_pub = rospy.Publisher('/tilt4_controller/command', Float64, queue_size=10) #rospy.Subscriber('/pan2_controller/state', JointState, self.joint2) rospy.Subscriber('/pan4_controller/state', JointState, self.joint4) #rospy.Subscriber('/pan5_controller/state', JointState, self.joint5) COM = '/dev/ttyUSB2' ser = serial.Serial(COM, bytesize=serial.SEVENBITS, parity=serial.PARITY_EVEN, timeout=1.0) self.bad=False self.weight_thread = Weight(ser) self.weight_thread.daemon = True self.weight_thread.start() self.dxl_io = dynamixel_io.DynamixelIO('/dev/ttyUSB0',1000000) self.action_dim = 1 #self.sensor_dim = 6 self.frame_num = 1 self.img_dims = [84,84] self.initial_replay_size = 200 self.gamma = 0.99 self.epsilon = 1.0 self.tau = 0.001 self.final_epsilon = 0.1 self.epsilon_step = 0.0001 self.num_replay_memory = 40000 self.target_update_interval = 1000 self.momentum = 0.95 self.min_grad = 0.01 self.lr = 0.000025 self.t = 0 self.freq = 4 self.rate = rospy.Rate(10) self.replay_memory = deque() self.weight = 0.0 self.angle = np.array([0.0 for i in range(self.action_dim)]).astype('float32') self.velocity = np.array([0.0 for i in range(self.action_dim)]).astype('float32') self.video = Video(self.img_dims) self.actor_network, self.s_i_a, self.act = self.build_actor_network() self.critic_network, self.s_i_c, self.s_a_c, self.q, self.action_grads = self.build_critic_network() self.target_actor_network, self.t_s_i_a, self.t_act = self.build_actor_network() self.target_critic_network, self.t_s_i_c, self.t_s_a_c, self.t_q, self.t_actor_grads = self.build_critic_network() self.actor_network_weights = self.actor_network.trainable_weights self.target_actor_network_weights = self.target_actor_network.trainable_weights self.critic_network_weights = self.critic_network.trainable_weights self.target_critic_network_weights = self.target_critic_network.trainable_weights self.batch_size = 32 self.y, self.loss, self.grad_update, self.a, self.actor_optimize = self.build_training_op() self.sess = tf.InteractiveSession() self.sess.run(tf.initialize_all_variables()) self.update_t_actor() print "nice" self.update_t_critic()
def main(args=sys.argv): usage_msg = 'Usage: %prog [options] IDs' desc_msg = 'Prints the current status of specified Dynamixel servo motors.' epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 1 2 3 4 5' % sys.argv[ 0] parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg) parser.add_option( '-p', '--port', metavar='PORT', default='/dev/ttyUSB0', help= 'motors of specified controllers are connected to PORT [default: %default]' ) parser.add_option( '-b', '--baud', metavar='BAUD', type="int", default=1000000, help= 'connection to serial port will be established at BAUD bps [default: %default]' ) (options, args) = parser.parse_args(args) if len(args) < 2: parser.print_help() exit(1) port = options.port baudrate = options.baud motor_ids = args[1:] try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError as soe: print('ERROR: {}'.format(soe)) else: responses = 0 print('Pinging motors:') for motor_id in motor_ids: motor_id = int(motor_id) print('%d ...' % motor_id, end='') p = dxl_io.ping(motor_id) if p: responses += 1 values = dxl_io.get_feedback(motor_id) angles = dxl_io.get_angle_limits(motor_id) model = dxl_io.get_model_number(motor_id) firmware = dxl_io.get_firmware_version(motor_id) values['model'] = '%s (firmware version: %d)' % ( DXL_MODEL_TO_PARAMS[model]['name'], firmware) values['degree_symbol'] = u"\u00B0" values['min'] = angles['min'] values['max'] = angles['max'] values['voltage'] = values['voltage'] values['moving'] = str(values['moving']) print(' done') if angles['max'] == 0 and angles['min'] == 0: values['freespin'] = True else: values['freespin'] = False print_data(values) else: print(' error') if responses == 0: print( 'ERROR: None of the specified motors responded. Make sure to specify the correct baudrate.' )
def main(args=sys.argv): usage_msg = 'Usage: %prog [options] MOTOR_IDs' desc_msg = 'Sets various configuration options of specified Dynamixel servo motor.' epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 --baud-rate=1 --return-delay=1 5 9 23' % sys.argv[ 0] parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg) parser.add_option( '-p', '--port', metavar='PORT', default='/dev/ttyUSB0', help= 'motors of specified controllers are connected to PORT [default: %default]' ) parser.add_option( '-b', '--baud', metavar='BAUD', type='int', default=1000000, help= 'connection to serial port will be established at BAUD bps [default: %default]' ) parser.add_option('-r', '--baud-rate', type='int', metavar='RATE', dest='baud_rate', help='set servo motor communication speed') parser.add_option('-d', '--return-delay', type='int', metavar='DELAY', dest='return_delay', help='set servo motor return packet delay time') parser.add_option('--cw-angle-limit', type='int', metavar='CW_ANGLE', dest='cw_angle_limit', help='set servo motor CW angle limit') parser.add_option('--ccw-angle-limit', type='int', metavar='CCW_ANGLE', dest='ccw_angle_limit', help='set servo motor CCW angle limit') parser.add_option('--min-voltage-limit', type='int', metavar='MIN_VOLTAGE', dest='min_voltage_limit', help='set servo motor minimum voltage limit') parser.add_option('--max-voltage-limit', type='int', metavar='MAX_VOLTAGE', dest='max_voltage_limit', help='set servo motor maximum voltage limit') (options, args) = parser.parse_args(args) print(options) if len(args) < 2: parser.print_help() exit(1) port = options.port baudrate = options.baud motor_ids = args[1:] try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except dynamixel_io.SerialOpenError as soe: print('ERROR: {}'.format(soe)) else: for motor_id in motor_ids: motor_id = int(motor_id) print('Configuring Dynamixel motor with ID %d' % motor_id) if dxl_io.ping(motor_id): # check if baud rate needs to be changed if options.baud_rate: valid_rates = (1, 3, 4, 7, 9, 16, 34, 103, 207, 250, 251, 252) if options.baud_rate not in valid_rates: print( 'Requested baud rate is invalid, please use one of the following: %s' % str(valid_rates)) if options.baud_rate <= 207: print('Setting baud rate to %d bps' % int(2000000.0 / (options.baud_rate + 1))) elif options.baud_rate == 250: print('Setting baud rate to %d bps' % 2250000) elif options.baud_rate == 251: print('Setting baud rate to %d bps' % 2500000) elif options.baud_rate == 252: print('Setting baud rate to %d bps' % 3000000) dxl_io.set_baud_rate(motor_id, options.baud_rate) # check if return delay time needs to be changed if options.return_delay is not None: if options.return_delay < 0 or options.return_delay > 254: print( 'Requested return delay time is out of valid range (0 - 254)' ) else: print('Setting return delay time to %d us' % (options.return_delay * 2)) dxl_io.set_return_delay_time(motor_id, options.return_delay) # check if CW angle limit needs to be changed if options.cw_angle_limit is not None: print('Setting CW angle limit to %d' % options.cw_angle_limit) dxl_io.set_angle_limit_cw(motor_id, options.cw_angle_limit) # check if CCW angle limit needs to be changed if options.ccw_angle_limit is not None: print('Setting CCW angle limit to %d' % options.ccw_angle_limit) dxl_io.set_angle_limit_ccw(motor_id, options.ccw_angle_limit) # check if minimum voltage limit needs to be changed if options.min_voltage_limit: print('Setting minimum voltage limit to %d' % options.min_voltage_limit) dxl_io.set_voltage_limit_min(motor_id, options.min_voltage_limit) # check if maximum voltage limit needs to be changed if options.max_voltage_limit: print('Setting maximum voltage limit to %d' % options.max_voltage_limit) dxl_io.set_voltage_limit_max(motor_id, options.max_voltage_limit) print('done') else: print('Unable to connect to Dynamixel motor with ID %d' % motor_id)
port_name2='/dev/ttyUSB2' port_namespace2='ttyUSB2' port_name3='/dev/ttyUSB3' port_namespace3='ttyUSB3' baud_rate=3000000 min_motor_id=1 max_motor_id=25 update_rate=5 diagnostics_rate=1 error_level_temp=75 warn_level_temp=70 readback_echo=False torque_on=True motor_id = 9 motor_id_3 = 7 r=0.126 m=842 I=m*r*r c=r*m*9.81 #i=0 # sync_write_address= [0x66, 0x00] try: dxl_io_0 = dynamixel_io.DynamixelIO(port_name0, baud_rate) #left hip # dxl_io_1 = dynamixel_io.DynamixelIO(port_name1, baud_rate) #right hip # dxl_io_2 = dynamixel_io.DynamixelIO(port_name2, baud_rate) #right knee # dxl_io_3 = dynamixel_io.DynamixelIO(port_name3, baud_rate) #left knee except dynamixel_io.SerialOpenError, soe: print 'ERROR:', soe else: cop_test()
port_name2='/dev/ttyUSB2' port_namespace2='ttyUSB2' port_name3='/dev/ttyUSB3' port_namespace3='ttyUSB3' baud_rate=3000000 min_motor_id=1 max_motor_id=25 update_rate=5 diagnostics_rate=1 error_level_temp=75 warn_level_temp=70 readback_echo=False torque_on=True motor_id = 1 motor_id_3 = 7 r=0.126 m=842 I=m*r*r c=r*m*9.81 #i=0 # sync_write_address= [0x66, 0x00] try: dxl_io_0 = dynamixel_io.DynamixelIO(port_name0, baud_rate) #left hip dxl_io_1 = dynamixel_io.DynamixelIO(port_name1, baud_rate) #right hip dxl_io_2 = dynamixel_io.DynamixelIO(port_name2, baud_rate) #right knee dxl_io_3 = dynamixel_io.DynamixelIO(port_name3, baud_rate) #left knee except dynamixel_io.SerialOpenError, soe: print 'ERROR:', soe else: pos_control()
# \date 2014-08-25 # \author (c) Nicolas Alt, TU Muenchen import IPython from dynamixel_driver import dynamixel_io from dynamixel_driver import dynamixel_const from dynamixel_driver.dynamixel_const import * port = "/dev/robot/ttyArm" baud = 400000 id_max = 30 print "Dynamixel shell, opening %s @%d baud..." % (port, baud) try: D = dynamixel_io.DynamixelIO(port, baud, True) except e: print "Failed to open port." exit(1) print "Pinging motors 0..%d..." % (id_max) for id in range(id_max): # max: range(253) if D.ping(id): pos = D.get_position(id) print "ID %3d found, position %4d" % (id, pos) # r = D.read(id, DXL_PRESENT_POSITION_L, 2); pos = r[5] + (r[6] << 8); pos # Read / write the acceleration # r = D.read(id, 73, 1); acc = r[5]; acc # r = D.write(id, 73, (1,)); r
def initialize_motor(chosenPort, chosenBaudRate = 1000000): rospy.loginfo("Initialzing motor at port: " + chosenPort + " and Baud rate: " + str(chosenBaudRate)) motor = dynamixel_io.DynamixelIO( port=chosenPort, baudrate = chosenBaudRate) return motor