def __init__(self, worldfn):
GLRealtimeProgram.__init__(self, "Hubo-ach virtual robot server")
# Create simulator
self.simulator = MakeSimulator(worldfn)
# Hubo-Ach Start and setup:
self.stateChannel = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
self.stateChannel.flush()
self.state = ha.HUBO_STATE()
self.state.time = 0
self.refChannel = ach.Channel(ha.HUBO_CHAN_REF_NAME)
self.refChannel.flush()
self.ref = ha.HUBO_REF()
self.prevRef = ha.HUBO_REF()
#self.timeChannel = ach.Channel(ha.HUBO_CHAN_VIRTUAL_TO_SIM_NAME)
#self.timeChannel.flush()
self.sim = ha.HUBO_VIRTUAL()
self.feedbackChannel = ach.Channel(ha.HUBO_CHAN_VIRTUAL_FROM_SIM_NAME)
self.feedbackChannel.flush()
def __init__(self):
self.state = ha.HUBO_STATE()
self.ref = ha.HUBO_REF()
self.stateChan = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
self.refChan = ach.Channel(ha.HUBO_CHAN_REF_NAME)
self.lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=1")
self.stateChan.flush()
self.refChan.flush()
self.subscription = self.lc.subscribe("HuboInput",
self.command_handler)
def __init__(self , constraintsAngles , numberOfSteps , accuracy = 0.01):
self.initTime = 0.0
bendKneesNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR'])
bendKneesValues = np.array([1.3 , 1.3 , -.83, -.83 , -0.6 , -0.6 , 0. , 0. , 0. , 0.],dtype=float)
straightKneesNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR'])
straightKneesValues = np.array([0. , 0. , 0., 0. , 0. , 0. ],dtype=float)
shiftWeightNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' ,'RHR' , 'LHR' , 'RAR' , 'LAR'])
shiftWeightLeftValues = np.array([1.3 , 1.3 , -.83, -.83 , -0.6 , -0.6 ,-0.28 , -0.28 , 0.28 , 0.28],dtype=float)
shiftWeightCenterValues = np.array([0.,0.,0.,0.],dtype=float)
shiftWeightRightValues = np.array([1.3 , 1.3 , -.83, -.83 , -0.6 , -0.6 ,.28 , 0.28 , -0.28 , -0.28],dtype=float)
kneePunchNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR'])
kneePunchRightValues = np.array([1.4 , 1.3 , -1. , -.83 , -0.6 , -0.6 , -.28 , -.28 , .28 , .28],dtype=float)
kneePunchLeftValues = np.array([1.3 , 1.4 , -.83 , -.9 , -0.6 , -0.6 , .28 , .28 , -.28 , -.28],dtype=float)
landLegNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR'])
landRightLegValues = np.array([1.3017 , 1.3 , -.269 , -0.83 ,0.,-0.6,-.28,-.28,0,0.])
landLeftLegValues = np.array([1.3 , 1.3017 , -0.83 , -.269 , -0.6 , 0.,.28,.28, 0.,.0])
shiftWeightAgainNames = np.array(['RKN' , 'LKN' , 'RHP' , 'LHP' , 'RAP' , 'LAP' , 'RHR' , 'LHR' , 'RAR' , 'LAR'])
shiftWeightLeftToRightValues = np.array([1.3017 , 1.899 , -.269 , -.6225 , 0. , 0. , 0. , 0. , 0. , 0. ],dtype=float)
shiftWeightRightToLeftValues = np.array([1.3017 , 1.899 , -.269 , -.6225 , 0. , 0. , 0. , 0. , 0. , 0. ],dtype=float)
s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
r = ach.Channel(ha.HUBO_CHAN_REF_NAME)
state = ha.HUBO_STATE()
ref = ha.HUBO_REF()
sim = ha.HUBO_VIRTUAL()
[status , framesize] = s.get(state,wait=False,last=True)
current = np.zeros([np.size(shiftWeightNames),1])
gain = 0
print 'bend'
current = self._sendCommand(r , ref , bendKneesNames , bendKneesValues,current , gain , correct=False)
print 'shift'
current = self._sendCommand(r , ref , shiftWeightNames , shiftWeightLeftValues,current , gain , correct=True)
print 'kneePunch'
current = self._sendCommand(r , ref , kneePunchNames , kneePunchRightValues,current , gain , correct=True)
current = self._sendCommand(r, ref, landLegNames, landRightLegValues, current, gain, correct=True)
current = self._sendCommand(r , ref, shiftWeightAgainNames, shiftWeightLeftToRightValues, current, gain, correct=True)
simTime = ha.HUBO_LOOP_PERIOD
def __init__(self):
low = .5
high = .78285
increment = 15
lowerLeg = .30038
upperLeg = .30003
torso = .18247
out = ik2DOF(low, 0., lowerLeg, upperLeg)
theta1 = out.theta1
theta2 = out.theta2
theta3 = 0. - theta1 - theta2
# print theta1
# print theta2
s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
r = ach.Channel(ha.HUBO_CHAN_REF_NAME)
state = ha.HUBO_STATE()
ref = ha.HUBO_REF()
# sim = ha.HUBO_VIRTUAL()
[status, framesize] = s.get(state, wait=False, last=True)
currentValues = self._returnDOFValues(state, s)
OGTheta1 = currentValues[17]
OGTheta2 = currentValues[16]
OGTheta3 = currentValues[15]
stillGoing = True
workingInc = 0
while stillGoing:
self._sendCommand(s, r, ref, state, theta1, theta2, theta3,
increment, 'DOWN')
self.simulationDelay(1, state, s)
self._sendCommand(s, r, ref, state, OGTheta1, OGTheta2, OGTheta3,
increment, 'UP')
self.simulationDelay(2, state, s)
workingInc += 1
if workingInc > 4:
stillGoing = False
def __init__(self):
### Standard Intro ###
s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
r = ach.Channel(ha.HUBO_CHAN_REF_NAME)
state = ha.HUBO_STATE()
ref = ha.HUBO_REF()
[status , framesize] = s.get(state,wait=False,last=True)
self.rightCoordinateNames = np.array(['RSP','RSR' ,'RSY','REB','RWR','RWP'])
self.leftCoordinateNames = np.array(['LSP','LSR' ,'LSY','LEB','LWR','LWP'])
sizes = np.array([])
deltaTheta = .1
error = 5
step = 10
leftGoal = np.array([])
rightGoal = np.array([])
GripperCommandAction,
GripperCommandGoal,
)
import baxter_interface
from baxter_interface import CHECK_VERSION
# Hubo-ach stuff
import hubo_ach as ha
import ach
from ctypes import *
import time
# feed-forward will now be refered to as "state"
state = ha.HUBO_STATE()
# 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)
class GripperClient(object):
def __init__(self, gripper):
ns = 'robot/end_effector/' + gripper + '_gripper/'
self._client = actionlib.SimpleActionClient(
ns + "gripper_action",
GripperCommandAction,
)
self._goal = GripperCommandGoal()
def __init__(self):
### Standard Intro ###
s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
r = ach.Channel(ha.HUBO_CHAN_REF_NAME)
state = ha.HUBO_STATE()
ref = ha.HUBO_REF()
[status, framesize] = s.get(state, wait=False, last=True)
self.rightCoordinateNames = np.array(
['RSP', 'RSR', 'RSY', 'REB', 'RWY', 'RWP'])
self.leftCoordinateNames = np.array(
['LSP', 'LSR', 'LSY', 'LEB', 'LWY', 'LWP'])
self.sizes = np.array([[300.38 + 300.03 + 303.87 - 181.87], [179.14],
[181.59]])
deltaTheta = .1
error = 5
step = 30
print "\n\n\nWELCOME TO INVERSE KINEMATICS"
print "CENTERING ARMS AND CALCULATION BOX BASED ON THIS POSITION.\n"
print "THIS SIMULATION RUNS SLOW BECAUSE IT IS DOING THE PLANNING ONLINE"
print "INSTEAD OF DOING IT A PRIORI.\n"
print "THIS MEANS IT IS TREATING THE NATURAL PHYSICS AS A DISTURBANCE"
print "WHICH MAKES THE CONTROL SLOWER."
print "IF THE CONTROL IS NOT SET CORRECTLY THEN SINGULARITIES ARE FOUND"
print "AND THINGS LIKE ARMS GO BACKWARDS AROUND IT TO TRY AND SOLVE THE STEP."
print "\n\nENJOY..."
self._raiseArmsToCenter(state, s, r, ref)
self._simulationDelay(1, state, s)
left = self._getFK(0, False, 'L', state, s)
right = self._getFK(0, False, 'R', state, s)
leftGoal = np.array([[left[0] + 50., left[1], left[2] + 60.],
[left[0] + 50., left[1], left[2] - 60.],
[left[0] - 50., left[1], left[2] - 60.],
[left[0] - 50., left[1], left[2] + 60.]])
rightGoal = np.array([[right[0] + 50., right[1], right[2] + 50.],
[right[0] + 50., right[1], right[2] - 50.],
[right[0] - 50., right[1], right[2] - 50.],
[right[0] - 50., right[1], right[2] + 50.]])
run = True
runningCount = 0
goal = 0
print "RUNNING ALGORITHM FOR 4 GOALS"
while run:
leftError = self._error(leftGoal[goal, ], 'L', state, s)
rightError = self._error(rightGoal[goal, ], 'R', state, s)
print "\n\nGOAL NUMBER {} \n\n".format(goal)
print "LEFT HAND GOAL POSITION IS: {}".format(leftGoal[goal, ])
print "RIGHT HAND GOAL POSITION IS: {}".format(rightGoal[goal, ])
print "\n\n"
print '\tLeft Error = {} , Right Error = {}'.format(
round(leftError, 3), round(rightError, 3))
while leftError > error or rightError > error:
if runningCount == 50:
print '\tLeft Error = {} , Right Error = {}'.format(
round(leftError, 3), round(rightError, 3))
runningCount = 0
lSideJacobian = self._getJacobian('L', deltaTheta, state, s)
rSideJacobian = self._getJacobian('R', deltaTheta, state, s)
lSideJacobian = np.linalg.pinv(lSideJacobian)
rSideJacobian = np.linalg.pinv(rSideJacobian)
leftNextStep = self._nextStep(step, leftError, state, s, 'L',
leftGoal[goal, ])
rightNextStep = self._nextStep(step, rightError, state, s, 'R',
rightGoal[goal, ])
lThetaUpdate = np.dot(lSideJacobian, leftNextStep)
rThetaUpdate = np.dot(rSideJacobian, rightNextStep)
self._adjustArms(state, s, r, ref, lThetaUpdate, rThetaUpdate)
leftError = self._error(leftGoal[goal, ], 'L', state, s)
rightError = self._error(rightGoal[goal, ], 'R', state, s)
# self._simulationDelay(.005,state,s)
runningCount += 1
goal += 1
if goal == 4:
run = False
print "\n\n\tFIN\n\n"
def __init__(self):
self.rightCoordinateNames = np.array(['RSP', 'RSR'
]) #,'RSY','REB','RWR','RWP'])
self.leftCoordinateNames = np.array(['LSP', 'LSR'
]) #,'LSY','LEB','LWR','LWP'])
self.rightGoal = np.array([[0., 0., -360.73],
[-138.05, -127.54, -307.90],
[138.05, 127.54, -307.90],
[127.54, 52.83, -333.27],
[-37.35, -270.55, -235.66]])
self.leftGoal = np.array([[0., 0., -360.73],
[-138.05, -127.54, -307.90],
[138.05, 127.54, -307.90],
[127.54, 52.83, -333.27],
[-37.35, -270.55, -235.66]])
self.dhArrayRight = np.array([
[0, -np.pi / 2., 0., 0.], # RSP
[0., 0., 179.14 + 181.59, 0.]
]) # RSR
self.dhArrayLeft = np.array([
[0., -np.pi / 2, 0., 0.], # LSP
[0., 0., 179.14 + 181.59, 0.]
]) # LSR
deltaTheta = .1
deltaError = np.array([.1, .1, .1])
s = ach.Channel(ha.HUBO_CHAN_STATE_NAME)
r = ach.Channel(ha.HUBO_CHAN_REF_NAME)
state = ha.HUBO_STATE()
ref = ha.HUBO_REF()
[status, framesize] = s.get(state, wait=False, last=True)
# rightCoordinates = np.zeros([1,np.size(self.rightCoordinateNames)])
# leftCoordinates = np.zeros([1,np.size(self.leftCoordinateNames)])
errorOk = .01
##### MAIN ALGORITHM HERE #####
running = True
goal = 0
while running:
self._getCurrentTheta(state, s) # Get Current Location
rightError = self._getMet('R', self.rightGoal, goal)
leftError = self._getMet('L', self.leftGoal, goal)
while rightError > errorOk and leftError > errorOk:
rightJacobian = getJacobian(self.dhArrayRight,
deltaTheta).jacobian
leftJacobian = getJacobian(self.dhArrayLeft,
deltaTheta).jacobian
rightJacobianInverse = np.linalg.pinv(rightJacobian)
leftJacobianInverse = np.linalg.pinv(leftJacobian)
rightCurrent = self._getFK(self.dhArrayRight)
leftCurrent = self._getFK(self.dhArrayLeft)
rightDeltaError = self._getNext(rightCurrent, self.rightGoal,
deltaError, 'R', goal)
leftDeltaError = self._getNext(leftCurrent, self.leftGoal,
deltaError, 'L', goal)
# print np.shape(rightDeltaError) , np.shape(rightJacobianInverse)
# print rightJacobianInverse
rightDeltaTheta = np.squeeze(
np.dot(rightDeltaError, rightJacobianInverse))
leftDeltaTheta = np.squeeze(
np.dot(leftDeltaError, leftJacobianInverse))
# for i in range(np.shape(self.dhArrayRight)[0]):
# self.dhArrayRight[i,0] += rightDeltaTheta[i]
#
# for i in range(np.shape(self.dhArrayLeft)[0]):
# self.dhArrayLeft[i,0] += leftDeltaTheta[i]
print rightDeltaTheta
print self.dhArrayRight
self._sendMoveCommand(
s, r, ref, state,
rightDeltaTheta + self.dhArrayRight[:, 0],
leftDeltaTheta + self.dhArrayLeft[:, 0])
goal += 1
if goal > np.shape(self.rightGoal)[0]:
goal = 1
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)
