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)