import optparse import yaml import dynamixel_network import numpy as np #hubo-ach stuff import hubo_ach as ha import ach from ctypes import * #define joints # feed-forward will now be refered to as "state" state = ha.HUBO_STATE() # encoder channel will be refered to as "encoder" encoder = ha.HUBO_ENC() # feed-back will now be refered to as "ref" ref = ha.HUBO_REF() # Get the current feed-forward (state) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) [statuss, framesizes] = r.get(ref, wait=False, last=True) def rad2dyn(rad): return np.int(np.floor((rad + np.pi) / (2.0 * np.pi) * 4096)) def dyn2rad(en): return ((en * 2.0 * np.pi) / 1024) - np.pi
def main(settings): # Open Hubo-Ach feed-forward and feed-back (reference and state) channels s = ach.Channel(ha.HUBO_CHAN_STATE_NAME) e = ach.Channel(ha.HUBO_CHAN_ENC_NAME) r = ach.Channel(ha.HUBO_CHAN_REF_NAME) #s.flush() #r.flush() # feed-forward will now be refered to as "state" state = ha.HUBO_STATE() # encoder channel will be refered to as "encoder" encoder = ha.HUBO_ENC() # feed-back will now be refered to as "ref" ref = ha.HUBO_REF() # Get the current feed-forward (state) [statuss, framesizes] = s.get(state, wait=False, last=True) [statuss, framesizes] = e.get(encoder, wait=False, last=True) portName = settings['port'] baudRate = settings['baudRate'] highestServoId = settings['highestServoId'] # Establish a serial connection to the dynamixel network. # This usually requires a USB2Dynamixel serial = dynamixel.SerialStream(port=portName, baudrate=baudRate, timeout=1) net = dynamixel.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 = 50 actuator.synchronized = True actuator.torque_enable = True actuator.torque_limit = 800 actuator.max_torque = 800 # Randomly vary servo position within a small range print myActuators print "Hubo-Ach neck server active" # print "Servo \tPosition" while True: # Get the current feed-forward (state) [statuss, framesizes] = s.get(state, wait=False, last=True) for actuator in myActuators: if (actuator.id == 1): actuator.goal_position = rad2dyn(state.joint[ha.NKY].ref) if (actuator.id == 2): actuator.goal_position = rad2dyn(state.joint[ha.NK1].ref) if (actuator.id == 3): actuator.goal_position = rad2dyn(state.joint[ha.NK2].ref) net.synchronize() for actuator in myActuators: actuator.read_all() time.sleep(0.01) if (actuator.id == 1): encoder.enc[ha.NKY] = dyn2rad(actuator.current_position) if (actuator.id == 2): encoder.enc[ha.NK1] = dyn2rad(actuator.current_position) if (actuator.id == 3): encoder.enc[ha.NK2] = dyn2rad(actuator.current_position) # print encoder.enc[ha.NKY], " : ", encoder.enc[ha.NK1], " : ", encoder.enc[ha.NK2] e.put(encoder) time.sleep(0.02)