def main(settings):
portName = settings['port'] #searches for a usb2dyn
baudRate = 1000000 #this is set to max can be changed to =>settings['baudRate']<= to be asked for a speed
highestServoId = settings['highestServoId']#asked for highest servo id
seriall = serial_stream.SerialStream(port=portName, baudrate=baudRate, timeout=1)
net = dynamixel_network.DynamixelNetwork(seriall) # Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId) #scans for all attached servos
myActuators = [] #starts a list
for dyn in net.get_dynamixels(): #makes a list of all dyn's
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators: #if the list has nothing show error
print 'No Dynamixels Found!'
sys.exit(0)
else: #if it found some it prints done
print "...Done"
for actuator in myActuators: #this is just an initializing loop to tell the servos to move as fast as we want
actuator.moving_speed = 1023
actuator.synchronized = True
actuator.torque_enable = True
actuator._set_torque_limit(0)
ovr_Initialize() #this part is form the oculus rift(OR) sdk this gets the pan and tilt
hmd = ovrHmd_Create(0)
hmdDesc = ovrHmdDesc()
ovrHmd_GetDesc(hmd, byref(hmdDesc))
ovrHmd_StartSensor(hmd,ovrSensorCap_Orientation | ovrSensorCap_YawCorrection,0)#end this part of OR code
while True: #the main part of our code
actuator.read_all()
ss = ovrHmd_GetSensorState(hmd, ovr_GetTimeInSeconds())#more OR code
pose = ss.Predicted.Pose
tilt = rad2dyn(pose.Orientation.x*np.pi); #gets tilt
pan = rad2dyn(pose.Orientation.y*np.pi); #gets pan
pos = '' #this is the start of a string because that is how info is sent over the network
for actuator in myActuators: #loop for actuators
pos = pos + str(actuator._get_current_position()) + ' ' #adds the current position to the string we made
pos = pos + str(pan) + ' ' + str(tilt) #adds the pan and tilt to the end of the list
pos.join(' ') #adds a space at the end
#print 'sent', pos
s.sendto(pos, server) #sends the string made above
data, addr = s.recvfrom(1024) #waits to get back the current psoition and load
#print data
data=data.split() #this splits the string into a list
counter=0 #just a counter
for actuator in myActuators: #loop for out actuators
#time.sleep(1)
#print actuator.id
if(abs(int(data[counter+1])) < 180):
#actuator._set_torque_limit(0) #this makes it so we dont feel feedback
else: #this makes it feel feedback
actuator._set_torque_limit(int(data[counter+1]))
actuator._set_goal_position(int(data[counter]))
counter+=2 #this counts through the list of servos coming in
if counter==len(data): #breaks the for loop if needs to
break
net.synchronize() #sends the information to each servo to update
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName, baudrate=baudRate, timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 1023
actuator.synchronized = True
actuator.torque_enable = True
while True:
actuator.read_all()
time.sleep(0.01)
for actuator in myActuators:
if ( actuator.id == 11):
myActuators[0]._set_goal_position(actuator.current_position)
if ( actuator.id == 12):
myActuators[1]._set_goal_position(actuator.current_position)
if ( actuator.id == 13):
myActuators[2]._set_goal_position(actuator.current_position)
if ( actuator.id == 14):
myActuators[3]._set_goal_position(actuator.current_position)
if ( actuator.id == 15):
myActuators[4]._set_goal_position(actuator.current_position)
if ( actuator.id == 16):
myActuators[5]._set_goal_position(actuator.current_position)
if ( actuator.id == 17):
myActuators[6]._set_goal_position(actuator.current_position)
if ( actuator.id == 18):
myActuators[7]._set_goal_position(actuator.current_position)
if ( actuator.id == 19):
myActuators[8]._set_goal_position(actuator.current_position)
if ( actuator.id == 20):
myActuators[9]._set_goal_position(actuator.current_position)
net.synchronize()
def test_scan(self):
""" Test a scan of registers"""
istream = MockStream()
iface = dynamixel_network.DynamixelNetwork(istream)
iface.dynamixel_error += self.error_handler
self.reset()
# ping response
istream.append(make_packet(1, 0, []))
# read register response
istream.append(make_packet(1, 0, [1, 2, 3, 4]))
# scan for one servo
iface.scan(1, 1)
# get by id
dyn = iface[1]
# assert initial read registers are ok
self.assertEqual(dyn.moving_speed, 1027)
self.assertEqual(dyn.goal_position, 513)
self.assertFalse(self.has_errors)
def test_get_attr(self):
""" Test getting an attribute"""
istream = MockStream()
iface = dynamixel_network.DynamixelNetwork(istream)
iface.dynamixel_error += self.error_handler
self.reset()
# ping response
istream.append(make_packet(1, 0, []))
# read register response
istream.append(make_packet(1, 0, [1, 2, 3, 4]))
# scan for one servo
iface.scan(1, 1)
# get by id
dyn = iface[1]
dyn.synchronized = False
istream.obuffer = ''
istream.append(make_packet(0x01, 0x00, [0x20, 0x00]))
value = dyn.current_position
self.assertEqual(value, 0x20)
def test_reset(self):
""" Test a reset call"""
istream = MockStream()
iface = dynamixel_network.DynamixelNetwork(istream)
iface.dynamixel_error += self.error_handler
self.reset()
# ping response
istream.append(make_packet(1, 0, []))
# read register response
istream.append(make_packet(1, 0, [1, 2, 3, 4]))
# scan for one servo
iface.scan(1, 1)
# get by id
dyn = iface[1]
istream.obuffer = ''
istream.append(make_packet(0x01, 0x00, []))
dyn.reset(1)
self.assertEqual(istream.obuffer,
''.join(map(chr, [0xff, 0xff, 0x1, 0x2, 0x6, 0xf6])))
self.assertFalse(self.has_errors)
def test_set_attr(self):
""" Test setting an attribute"""
istream = MockStream()
iface = dynamixel_network.DynamixelNetwork(istream)
iface.dynamixel_error += self.error_handler
self.reset()
# ping response
istream.append(make_packet(1, 0, []))
# read register response
istream.append(make_packet(1, 0, [1, 2, 3, 4]))
# scan for one servo
iface.scan(1, 1)
# get by id
dyn = iface[1]
dyn.synchronized = False
istream.obuffer = ''
istream.append(make_packet(0x01, 0x00, []))
dyn.goal_position = 0x3FF
self.assertEqual(
istream.obuffer, ''.join(
map(chr,
[0xFF, 0xFF, 0x01, 0x05, 0x03, 0x1E, 0xFF, 0x03, 0xD6])))
self.assertFalse(self.has_errors)
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
'''actuator.moving_speed = 80
actuator.synchronized = True
actuator.torque_enable = True'''
#actuator.torque_limit = 400
#actuator.max_torque = 400
actuator.moving_speed = 1023
#print "actuator speed %d" %(actuator._get_moving_speed());
actuator.synchronized = True
#actuator._set_torque_limit(0)
actuator.torque_enable = True
#myActuators[1]._set_moving_speed(1023);
'''Oculus Rift stuff'''
ovr_Initialize()
hmd = ovrHmd_Create(0)
hmdDesc = ovrHmdDesc()
ovrHmd_GetDesc(hmd, byref(hmdDesc))
print hmdDesc.ProductName
ovrHmd_StartSensor( \
hmd,
ovrSensorCap_Orientation |
ovrSensorCap_YawCorrection,
0
)
#read data in real-time
timeout = time.time() + 60 * 180 #Terminate 180 minutes from now
while True:
actuator.read_all()
time.sleep(0.016)
ss = ovrHmd_GetSensorState(hmd, ovr_GetTimeInSeconds())
pose = ss.Predicted.Pose
tilt = rad2dyn(pose.Orientation.x * np.pi)
#tiltscaled=tilt*1.00;
#myActuators[8]._set_goal_position(tilt);
pan = rad2dyn(pose.Orientation.y * np.pi)
#panscaled=pan*1.20;
#myActuators[9]._set_goal_position(pan);
#tiltlist.append(rad2dyn(pose.Orientation.x*np.pi));
#panlist.append(rad2dyn(pose.Orientation.y*np.pi));
#panlist.pop(0);
#tiltlist.pop(0);
#tilt = np.mean(tiltlist);
#pan = np.mean(panlist); '''
#myActuators[0]._set_synchronized(1);
myActuators[0]._set_goal_position(pan)
myActuators[1]._set_goal_position(tilt)
'''print "myact 0 speed %d" %(myActuators[0]._get_moving_speed());
print "myact 1 speed %d" %(myActuators[1]._get_moving_speed());
#print "myact 0 load %d" %(myActuators[0]._get_current_load());
#print "myact 1 load %d" %(myActuators[1]._get_current_load());
print "myact 0 load %d" %(myActuators[0]._get_torque_limit());
print "myact 1 load %d" %(myActuators[1]._get_torque_limit());
#print "myact %d" %(myActuators[1]._get_moving_speed());
print "myact 0 pos %d" %(myActuators[0]._get_goal_position());
print "myact 1 pos %d" %(myActuators[1]._get_goal_position()); '''
print "tilt %d pan %d" % (tilt, pan)
net.synchronize()
def main(settings):
RSP = 0
RSY = 0
RSR = 0
REB = 0
RF1 = 0
RF2 = 0
RHP = 0
LHP = 0
RKN = 0
LKN = 0
RAP = 0
LAP = 0
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 1023
actuator.synchronized = True
actuator.torque_enable = False
print "bending knees"
for i in range(0, 70):
RHP = RHP - 0.015
ref.ref[ha.RHP] = RHP
r.put(ref)
LHP = LHP - 0.015
ref.ref[ha.LHP] = LHP
r.put(ref)
RKN = RKN + 0.015
ref.ref[ha.RKN] = RKN
r.put(ref)
LKN = LKN + 0.015
ref.ref[ha.LKN] = LKN
r.put(ref)
RAP = RAP - 0.005
ref.ref[ha.RAP] = RAP
r.put(ref)
LAP = LAP - 0.005
ref.ref[ha.LAP] = LAP
r.put(ref)
time.sleep(0.15)
#send commands to robot
while True:
actuator.read_all()
time.sleep(0.01)
for actuator in myActuators:
if (actuator.id == 1):
ref.ref[
ha.RSP] = -dyn2rad(actuator.current_position) + 1.859185
#print "RSP radian value = %f" %dyn2rad(actuator.current_position)
r.put(ref)
if (actuator.id == 2):
#print "RWY radian value = %f" %dyn2rad(actuator.current_position)
ref.ref[ha.RWY] = dyn2rad(actuator.current_position) + 1.93
r.put(ref)
if (actuator.id == 3):
ref.ref[ha.RSR] = dyn2rad(actuator.current_position) - 1.546253
#print "RSR radian value = %f" %dyn2rad(actuator.current_position)
r.put(ref)
if (actuator.id == 4):
ref.ref[ha.REB] = dyn2rad(actuator.current_position) + .312932
#print "REB radian value = %f" %dyn2rad(actuator.current_position)
r.put(ref)
if (actuator.id == 5):
#print "RF1 radian value = %f" %dyn2rad(actuator.current_position)
if (dyn2rad(actuator.current_position) >= 0.5):
ref.ref[ha.RF1] = -1
r.put(ref)
ref.ref[ha.RF2] = -1
r.put(ref)
if (dyn2rad(actuator.current_position) <= 0.5):
ref.ref[ha.RF1] = 1
r.put(ref)
ref.ref[ha.RF2] = 1
r.put(ref)
net.synchronize()
def main(settings):
RSP = 0
RSY = 0
RSR = 0
REB = 0
RF1 = 0
RF2 = 0
RHP = 0
LHP = 0
RKN = 0
LKN = 0
RAP = 0
LAP = 0
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 1023
actuator.synchronized = True
actuator.torque_enable = False
#send commands to robot
ref.ref[ha.RWY] = -1.4
r.put(ref)
while True:
actuator.read_all()
time.sleep(0.01)
for actuator in myActuators:
if (actuator.id == 11):
ref.ref[
ha.RSP] = -dyn2rad(actuator.current_position) + 1.859185
print "RSP radian value = %f" % dyn2rad(
actuator.current_position)
r.put(ref)
#myActuators[5]._set_goal_position(actuator.current_position)
'''disabled - second elbow bend not in DRC
if ( actuator.id == 12):
print "RSY radian value = %f" %dyn2rad(actuator.current_position)
ref.ref[ha.RSY] = dyn2rad(actuator.current_position) + 1.9575767
r.put(ref)
myActuators[6]._set_goal_position(actuator.current_position)'''
if (actuator.id == 13):
ref.ref[ha.RSR] = dyn2rad(actuator.current_position) - 1.546253
print "RSR radian value = %f" % dyn2rad(
actuator.current_position)
r.put(ref)
#myActuators[7]._set_goal_position(actuator.current_position)
if (actuator.id == 14):
ref.ref[ha.REB] = dyn2rad(actuator.current_position) + .312932
print "REB radian value = %f" % dyn2rad(
actuator.current_position)
r.put(ref)
#myActuators[8]._set_goal_position(actuator.current_position)
if (actuator.id == 15):
print "RF1 radian value = %f" % dyn2rad(
actuator.current_position)
if (dyn2rad(actuator.current_position) >= 0.4):
ref.ref[ha.RF1] = -1
r.put(ref)
ref.ref[ha.RF2] = -1
r.put(ref)
if (dyn2rad(actuator.current_position) <= 0.4):
ref.ref[ha.RF1] = 1
r.put(ref)
ref.ref[ha.RF2] = 1
r.put(ref)
#myActuators[9]._set_goal_position(actuator.current_position)
net.synchronize()
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 80
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 0
actuator.max_torque = 0
while True:
actuator.read_all()
time.sleep(0.01)
for actuator in myActuators:
#if ( actuator.id == 16): #to stop the base from moving
# myActuators[15]._set_goal_position(1024)
if (actuator.id == 8):
ref.ref[ha.RSP] = -dyn2rad(actuator.current_position)
print "%f RSP" % (ref.ref[ha.RSP])
if (actuator.id == 9):
ref.ref[ha.RSR] = -dyn2rad(actuator.current_position) - .177942
print "%f RSR" % (ref.ref[ha.RSR])
#for debugging: print "[ID] radian value = %f" %dyn2rad(actuator.current_position)
if (actuator.id == 10):
ref.ref[ha.RSY] = -dyn2rad(actuator.current_position)
if (actuator.id == 11):
ref.ref[ha.REB] = -dyn2rad(actuator.current_position)
if (actuator.id == 12):
ref.ref[ha.RWY] = -dyn2rad(actuator.current_position) - .868583
if (actuator.id == 13):
ref.ref[ha.RWP] = dyn2rad(actuator.current_position)
if (actuator.id == 14):
ref.ref[ha.RWR] = dyn2rad(actuator.current_position)
if (actuator.id == 15):
if (dyn2rad(actuator.current_position) >= 0.18):
ref.ref[ha.RF1] = -1
ref.ref[ha.RF2] = -1
if (dyn2rad(actuator.current_position) <= 0.20):
ref.ref[ha.RF1] = 1
ref.ref[ha.RF2] = 1
#time.sleep(.0001)
# Write to the feed-forward channel
r.put(ref)
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
'''actuator.moving_speed = 80
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 400
actuator.max_torque = 400'''
actuator.moving_speed = 460
actuator.synchronized = True
#actuator._set_torque_limit(0)
#actuator.torque_enable = True
#
#stuff for haptic feedback
#myActuators[5].torque_enable = True
#myActuators[2]._set_torque_limit(0)
'''Oculus Rift stuff
ovr_Initialize()
hmd = ovrHmd_Create(0)
hmdDesc = ovrHmdDesc()
ovrHmd_GetDesc(hmd, byref(hmdDesc))
print hmdDesc.ProductName
ovrHmd_StartSensor( \
hmd,
ovrSensorCap_Orientation |
ovrSensorCap_YawCorrection,
0
)'''
#create initial data for averaging filter lists
while ((len(s1List) < window) and (len(s1List_r) < window)):
print("Initial Data")
actuator.read_all()
'''ss = ovrHmd_GetSensorState(hmd, ovr_GetTimeInSeconds())
pose = ss.Predicted.Pose
tiltlist.append(rad2dyn(pose.Orientation.x*np.pi));
panlist.append(rad2dyn(pose.Orientation.y*np.pi));
tilt = np.mean(tiltlist);
pan = np.mean(panlist); '''
for actuator in myActuators:
'''if ( actuator.id == 1):
s0List.append(dyn2rad(actuator.current_position));
s0 = np.mean(s0List);
if ( actuator.id == 2):
s1List.append(dyn2rad(actuator.current_position));
s1 = np.mean(s1List);
if ( actuator.id == 3):
e0List.append(dyn2rad(actuator.current_position));
e0 = np.mean(e0List);
if ( actuator.id == 4):
e1List.append(dyn2rad(actuator.current_position));
e1 = np.mean(e1List);
if ( actuator.id == 5):
w0List.append(dyn2rad(actuator.current_position));
w0 = np.mean(w0List);
if ( actuator.id == 6):
w1List.append(dyn2rad(actuator.current_position));
w1 = np.mean(w1List);
if ( actuator.id == 7):
w2List.append(dyn2rad(actuator.current_position));
w2 = np.mean(w2List);'''
if (actuator.id == 8):
lf2List.append(actuator.current_position)
lf2 = np.mean(lf2List)
if (actuator.id == 12):
s1List_r.append(dyn2rad(actuator.current_position))
s1_r = np.mean(s1List_r)
if (actuator.id == 13):
e0List_r.append(dyn2rad(actuator.current_position))
e0_r = np.mean(e0List_r)
if (actuator.id == 14):
e1List_r.append(dyn2rad(actuator.current_position))
e1_r = np.mean(e1List_r)
if (actuator.id == 15):
w0List_r.append(dyn2rad(actuator.current_position))
w0_r = np.mean(w0List_r)
if (actuator.id == 16):
w1List.append(dyn2rad(actuator.current_position))
w1_r = np.mean(w1List_r)
if (actuator.id == 17):
w2List_r.append(dyn2rad(actuator.current_position))
w2_r = np.mean(w2List_r) + 1.3
'''if ( actuator.id == 18):
rf2List.append(dyn2rad(actuator.current_position));
rf2 = np.mean(rf2List); '''
time.sleep(.01) #200 Hz refresh rate
#read data in real-time
timeout = time.time() + 60 * 180 #Terminate 180 minutes from now
while True:
actuator.read_all()
net.synchronize()
time.sleep(0.01)
'''ss = ovrHmd_GetSensorState(hmd, ovr_GetTimeInSeconds())
pose = ss.Predicted.Pose
tilt = rad2dyn(pose.Orientation.x*np.pi);
myActuators[8]._set_goal_position(tilt);
pan = rad2dyn(pose.Orientation.y*np.pi);
myActuators[9]._set_goal_position(pan);
tiltlist.append(rad2dyn(pose.Orientation.x*np.pi));
panlist.append(rad2dyn(pose.Orientation.y*np.pi));
panlist.pop(0);
tiltlist.pop(0);
tilt = np.mean(tiltlist);
pan = np.mean(panlist);
#myActuators[1]._set_goal_position(tilt);
myActuators[2]._set_goal_position(pan);
print "tilt %d" %tilt
print "pan %d" %pan '''
for actuator in myActuators:
#left arm
'''if ( actuator.id == 1):
s0List.append(dyn2rad(actuator.current_position));
s0List.pop(0);
s0 = np.mean(s0List);
ref.ref[ha.LSY] = (s0 + 0.8)*0.8;
if ( actuator.id == 2):
s1List.append(-dyn2rad(actuator.current_position));
s1List.pop(0);
s1 = np.mean(s1List);
ref.ref[ha.LSP] = -s1;
if ( actuator.id == 3):
e0List.append(dyn2rad(actuator.current_position));
e0List.pop(0);
e0 = np.mean(e0List);
ref.ref[ha.LSR] = e0;
if ( actuator.id == 4):
e1List.append(dyn2rad(actuator.current_position));
e1List.pop(0);
e1 = np.mean(e1List);
ref.ref[ha.LEB] = e1 - .35;
if ( actuator.id == 5):
w0List.append(dyn2rad(actuator.current_position));
w0List.pop(0);
w0 = np.mean(w0List);
ref.ref[ha.LHY] = w0;
if ( actuator.id == 6):
w1List.append(dyn2rad(actuator.current_position));
w1List.pop(0);
w1 = np.mean(w1List);
ref.ref[ha.LHP] = w1;
if ( actuator.id == 7):
w2List.append(dyn2rad(actuator.current_position));
w2List.pop(0);
w2 = np.mean(w2List);
ref.ref[ha.LWR] = w2 + 1.9; '''
#for debugging: print "[ID] radian value = %f" %dyn2rad(actuator.current_position)
#print "LSY, LSP, LSR, LWR values = %f %f %f %f" %(ref.ref[ha.LSY], ref.ref[ha.LSP], ref.ref[ha.LSR], ref.ref[ha.LWR])''
if (actuator.id == 8):
lf2List.append(actuator.current_position)
lf2List.pop(0)
lf2 = np.mean(lf2List)
ref.ref[ha.LF2] = -lf2 + 600
#print "LF2 value = %f" %(ref.ref[ha.LF2])
#right arm
if (actuator.id == 11):
s0List_r.append(dyn2rad(actuator.current_position))
s0List_r.pop(0)
s0_r = np.mean(s0List_r)
ref.ref[ha.RSY] = (s0_r - 0.8) * 0.8
if (actuator.id == 12):
s1List_r.append(dyn2rad(actuator.current_position))
s1List_r.pop(0)
s1_r = np.mean(s1List_r)
ref.ref[ha.RSP] = -s1_r
if (actuator.id == 13):
e0List_r.append(dyn2rad(actuator.current_position))
e0List_r.pop(0)
e0_r = np.mean(e0List_r)
ref.ref[ha.RSR] = e0_r
if (actuator.id == 14):
e1List_r.append(dyn2rad(actuator.current_position))
e1List_r.pop(0)
e1_r = np.mean(e1List_r)
ref.ref[ha.REB] = -e1_r + 1.6
if (actuator.id == 15):
w0List_r.append(dyn2rad(actuator.current_position))
w0List_r.pop(0)
w0_r = np.mean(w0List_r)
ref.ref[ha.RHY] = w0_r
if (actuator.id == 16):
w1List_r.append(dyn2rad(actuator.current_position))
w1List_r.pop(0)
w1_r = np.mean(w1List_r)
ref.ref[ha.RHP] = w1_r
if (actuator.id == 17):
w2List_r.append(dyn2rad(actuator.current_position))
w2List_r.pop(0)
w2_r = np.mean(w2List_r)
ref.ref[ha.RWR] = w2_r - 1.4
#for debugging: print "[ID] radian value = %f" %dyn2rad(actuator.current_position)
'''if ( actuator.id == 18):
rf2List.append(dyn2rad(actuator.current_position));
rf2List.pop(0);
rf2 = np.mean(rf2List);
ref.ref[ha.RF2] = rf2 + 600;
print "RF2 value = %f" %(ref.ref[ha.RF2]) '''
#print "RSY/RSP/RSR/REB/RHY/RHP/LF2 values = %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f" %(ref.ref[ha.RSY], ref.ref[ha.RSP], ref.ref[ha.RSR], ref.ref[ha.REB], ref.ref[ha.RHY],ref.ref[ha.RHP], ref.ref[ha.RWR], ref.ref[ha.LF2])
#print "LSY/LSP/LSR/LEB/LHY/LHP/LWR/LF2 values = %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f %0.2f" %(ref.ref[ha.LSY], ref.ref[ha.LSP], ref.ref[ha.LSR], ref.ref[ha.LEB], ref.ref[ha.LHY],ref.ref[ha.LHP], ref.ref[ha.LWR], ref.ref[ha.LF2])
r.put(ref)
if time.time() > timeout:
print "Time out, hubo-ach channels closing."
break
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName, baudrate=baudRate, timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 40
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 1500
actuator.max_torque = 1500
i = 0
while 1:
[statuss, framesizes] = r.get(ref, wait=False, last=True)
for actuator in myActuators:
if ( actuator.id == LHY):
actuator.goal_position = rad2dyn(ref.ref[ha.LHY])
if ( actuator.id == RHY):
actuator.goal_position = rad2dyn(ref.ref[ha.RHY])
if ( actuator.id == LHR):
actuator.goal_position = rad2dyn(ref.ref[ha.LHR])
if ( actuator.id == RHR):
actuator.goal_position = rad2dyn(ref.ref[ha.RHR])
if ( actuator.id == LHP):
actuator.goal_position = rad2dyn(-ref.ref[ha.LHP])#rad2dyn(-ref.ref[ha.LHP])
if ( actuator.id == RHP):
actuator.goal_position = rad2dyn(ref.ref[ha.RHP])
if ( actuator.id == LKN):
actuator.goal_position = rad2dyn(-ref.ref[ha.LKN])#rad2dyn(-ref.ref[ha.LKN])
if ( actuator.id == RKN):
actuator.goal_position = rad2dyn(ref.ref[ha.RKN])
if ( actuator.id == LAP):
actuator.goal_position = rad2dyn(-ref.ref[ha.LAP])#rad2dyn(-ref.ref[ha.LAP])
if ( actuator.id == RAP):
actuator.goal_position = rad2dyn(ref.ref[ha.RAP])
if ( actuator.id == LAR):
actuator.goal_position = rad2dyn(-ref.ref[ha.LAR])
if ( actuator.id == RAR):
actuator.goal_position = rad2dyn(-ref.ref[ha.RAR])
print actuator.goal_position
time.sleep(.02)
net.synchronize()
def main(settings):
portName = settings['port']
baudRate = settings['baudRate']
highestServoId = settings['highestServoId']
# Establish a serial connection to the dynamixel network.
# This usually requires a USB2Dynamixel
serial = serial_stream.SerialStream(port=portName,
baudrate=baudRate,
timeout=1)
net = dynamixel_network.DynamixelNetwork(serial)
# Ping the range of servos that are attached
print "Scanning for Dynamixels..."
net.scan(1, highestServoId)
myActuators = []
for dyn in net.get_dynamixels():
print dyn.id
myActuators.append(net[dyn.id])
if not myActuators:
print 'No Dynamixels Found!'
sys.exit(0)
else:
print "...Done"
for actuator in myActuators:
actuator.moving_speed = 80
actuator.synchronized = True
actuator.torque_enable = False
actuator.torque_limit = 0
actuator.max_torque = 0
#create initial data for averaging filter lists
while ((len(s0List) < window) and (len(s1List) < window)):
actuator.read_all()
for actuator in myActuators:
'''if ( actuator.id == 1):
s0List.append(dyn2rad(actuator.current_position));
s0 = np.mean(s0List);'''
if (actuator.id == 2):
s1List.append(dyn2rad(actuator.current_position) + 3.14)
s1 = np.mean(s1List)
if (actuator.id == 3):
e0List.append(dyn2rad(actuator.current_position))
e0 = np.mean(e0List)
if (actuator.id == 4):
e1List.append(dyn2rad(actuator.current_position))
e1 = np.mean(e1List)
if (actuator.id == 5):
w0List.append(dyn2rad(actuator.current_position))
w0 = np.mean(w0List)
if (actuator.id == 6):
w1List.append(dyn2rad(actuator.current_position))
w1 = np.mean(w1List)
'''if ( actuator.id == 7):
w2List.append(dyn2rad(actuator.current_position));
w2 = np.mean(w2List);
if ( actuator.id == 18):
lf2List.append(dyn2rad(actuator.current_position));
lf2 = np.mean(lf2List);'''
if (actuator.id == 12):
s1List_r.append(dyn2rad(actuator.current_position))
s1_r = np.mean(s1List_r)
if (actuator.id == 13):
e0List_r.append(dyn2rad(actuator.current_position))
e0_r = np.mean(e0List_r)
if (actuator.id == 14):
e1List_r.append(dyn2rad(actuator.current_position))
e1_r = np.mean(e1List_r)
if (actuator.id == 15):
w0List_r.append(dyn2rad(actuator.current_position))
w0_r = np.mean(w0List_r)
if (actuator.id == 16):
w1List.append(dyn2rad(actuator.current_position))
w1_r = np.mean(w1List_r)
'''if ( actuator.id == 17):
w2List_r.append(dyn2rad(actuator.current_position));
w2_r = np.mean(w2List_r);
if ( actuator.id == 18):
rf2List.append(dyn2rad(actuator.current_position));
rf2 = np.mean(rf2List); '''
time.sleep(.01) #200 Hz refresh rate
#read data in real-time
timeout = time.time() + 60 * 180 #Terminate 180 minutes from now
while True:
actuator.read_all()
for actuator in myActuators:
#left arm
'''if ( actuator.id == 1):
s0List.append(dyn2rad(actuator.current_position));
s0List.pop(0);
s0 = np.mean(s0List);
ref.ref[ha.LSY] = s0;'''
if (actuator.id == 2):
s1List.append(dyn2rad(actuator.current_position))
s1List.pop(0)
s1 = np.mean(s1List)
ref.ref[ha.LSP] = s1
if (actuator.id == 3):
e0List.append(dyn2rad(actuator.current_position))
e0List.pop(0)
e0 = np.mean(e0List)
ref.ref[ha.LSR] = e0
if (actuator.id == 4):
e1List.append(dyn2rad(actuator.current_position))
e1List.pop(0)
e1 = np.mean(e1List)
ref.ref[ha.LEB] = e1 - 1.73
#offset of 1.73
if (actuator.id == 5):
w0List.append(dyn2rad(actuator.current_position))
w0List.pop(0)
w0 = np.mean(w0List)
ref.ref[ha.LHY] = w0
if (actuator.id == 6):
w1List.append(dyn2rad(actuator.current_position))
w1List.pop(0)
w1 = np.mean(w1List)
ref.ref[ha.LHP] = w1
'''if ( actuator.id == 7):
w2List.append(dyn2rad(actuator.current_position));
w2List.pop(0);
w2 = np.mean(w2List);
ref.ref[ha.LWR] = w2;
#for debugging: print "[ID] radian value = %f" %dyn2rad(actuator.current_position)
print "LSY, LSP, LSR, LWR values = %f %f %f %f" %(ref.ref[ha.LSY], ref.ref[ha.LSP], ref.ref[ha.LSR], ref.ref[ha.LWR])
if ( actuator.id == 8):
lf2List.append(dyn2rad(actuator.current_position));
lf2List.pop(0);
lf2 = np.mean(lf2List);
ref.ref[ha.LF2] = lf2 - 470;
print "LF2 value = %f" %(ref.ref[ha.LF2])'''
#right arm
'''if ( actuator.id == 11):
s0List_r.append(dyn2rad(actuator.current_position));
s0List_r.pop(0);
s0_r = np.mean(s0List_r);
ref.ref[ha.RSY] = s0_r;'''
if (actuator.id == 12):
s1List_r.append(dyn2rad(actuator.current_position))
s1List_r.pop(0)
s1_r = np.mean(s1List_r)
ref.ref[ha.RSP] = s1_r
if (actuator.id == 13):
e0List_r.append(dyn2rad(actuator.current_position))
e0List_r.pop(0)
e0_r = np.mean(e0List_r)
ref.ref[ha.RSR] = e0_r
if (actuator.id == 14):
e1List_r.append(dyn2rad(actuator.current_position))
e1List_r.pop(0)
e1_r = np.mean(e1List_r)
ref.ref[ha.REB] = e1_r - 1.73
#offset of 1.73
if (actuator.id == 15):
w0List_r.append(dyn2rad(actuator.current_position))
w0List_r.pop(0)
w0_r = np.mean(w0List_r)
ref.ref[ha.RHY] = w0_r
if (actuator.id == 16):
w1List_r.append(dyn2rad(actuator.current_position))
w1List_r.pop(0)
w1_r = np.mean(w1List_r)
ref.ref[ha.RHP] = w1_r
'''if ( actuator.id == 17):
w2List_r.append(dyn2rad(actuator.current_position));
w2List_r.pop(0);
w2_r = np.mean(w2List_r);
ref.ref[ha.RWR] = w2_r;
#for debugging: print "[ID] radian value = %f" %dyn2rad(actuator.current_position)
print "RSY, RSP, RSR, RWR values = %f %f %f %f" %(ref.ref[ha.RSY], ref.ref[ha.RSP], ref.ref[ha.RSR], ref.ref[ha.RWR])
if ( actuator.id == 18):
rf2List.append(dyn2rad(actuator.current_position));
rf2List.pop(0);
rf2 = np.mean(rf2List);
ref.ref[ha.RF2] = rf2 - 470;
print "RF2 value = %f" %(ref.ref[ha.RF2])'''
r.put(ref)
time.sleep(.02) #200 Hz refresh rate
if time.time() > timeout:
print "Time out, hubo-ach channels closing."
break
