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
