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 mainLoop():
# Start curses
s = ach.Channel(mds.MDS_CHAN_STATE_NAME)
r = ach.Channel(mds.MDS_CHAN_REF_FILTER_NAME)
state = mds.MDS_STATE()
ref = mds.MDS_REF()
doloop = True
ii = 0
while(doloop):
# Get latest states
[status, framesize] = s.get(state, wait=False, last=True)
[status, framesize] = r.get(ref, wait=False, last=True)
# Get address of LSP
jntn = mds.getAddress('LSP',state)
print 'Address = ', jntn
# Set LSP reference to -0.123 using the filter controller (smooth)
ref.joint[jntn].ref = -0.123
# Command motors to desired references (post to ach channel)
r.put(ref)
if ii == 1:
doloop = False
ii = 1
# Wait 0.5 sec
time.sleep(0.5)
# Close channels
s.close()
r.close()
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):
# how long do we wait for a message from the servers
self.waitTime = [
1.0 / 30.0, 1.0 / 40.0, 1.0 / 50.0, 1.0 / 60.0, 1.0 / 70.0,
1.0 / 80.0, 1.0 / 90.0, 1.0 / 100.0
] ## 25 hz
self.currentWaitIndex = 0
self.currentExp = 0
self.exprNames = ["topNY", "all", "allNY", "allTor", "topTOR"]
f = open('ipaddresses.txt', 'r')
self.servers = f.readlines()
f.close()
## build list of channels for sending and receiving
self.taskSendChannels = {}
self.taskRecvChannels = {}
self.taskSendChannels['all'] = {}
self.taskRecvChannels['all'] = {}
for server in self.servers:
imgTaskChanName = server.replace(".", "").replace("\n",
"") + "VSTaskImg"
self.taskSendChannels['all'][server] = ach.Channel(
imgTaskChanName) # sending on
respChan = server.replace(".", "").replace("\n", "") + "VSResp"
self.taskRecvChannels['all'][server] = ach.Channel(
respChan) # receiving from
for expName in self.exprNames:
if expName != 'all':
f = open(expName + ".txt", 'r')
self.tempServ = f.readlines()
f.close()
self.taskSendChannels[expName] = {}
self.taskRecvChannels[expName] = {}
for server in self.tempServ:
self.taskSendChannels[expName][
server] = self.taskSendChannels['all'][server]
self.taskRecvChannels[expName][
server] = self.taskRecvChannels['all'][server]
print("done setting up now just waiting to get an image...")
self.id = 1.0
self.prevTime = time.time()
self.latency = []
self.pub = rospy.Publisher('/RosAria/cmd_vel', Twist, queue_size=10)
self.subscriber = rospy.Subscriber("/camera/image_raw",
Image,
self.callback,
queue_size=1)
rospy.on_shutdown(self.shutdown)
def mainLoop():
k = ach.Channel(mds.MDS_CHAN_IK_NAME)
r = ach.Channel(mds.MDS_CHAN_REF_FILTER_NAME)
s = ach.Channel(mds.MDS_CHAN_STATE_NAME)
k.flush()
state = mds.MDS_STATE()
ref = mds.MDS_REF()
ikc = mds.MDS_IK()
#take user input
while(1):
[status, framesize] = k.get(ikc, wait=True, last=True)
[status, framesize] = s.get(state, wait=False, last=True)
[status, framesize] = r.get(ref, wait=False, last=True)
at_x = 0.0
at_y = 0.0
at_z = 0.0
ar_x = 0.0
ar_y = 0.0
ar_z = 0.0
arm = 'none'
dof = 0
jointSet = 'no'
eff_end = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
runIK = False
armi = -1
if ikc.move == mds.LEFT:
runIK = True
arm = 'left'
if ikc.move == mds.RIGHT:
runIK = True
arm = 'right'
if runIK:
armi = ikc.move
dof = ikc.arm[armi].ik_method
at_x = ikc.arm[armi].t_x
at_y = ikc.arm[armi].t_y
at_z = ikc.arm[armi].t_z
ar_x = ikc.arm[armi].r_x
ar_y = ikc.arm[armi].r_y
ar_z = ikc.arm[armi].r_z
eff_end[0] = at_x
eff_end[1] = at_y
eff_end[2] = at_z
eff_end[3] = ar_x
eff_end[4] = ar_y
eff_end[5] = ar_z
print ikc.arm[armi].t_x
print eff_end
jointSet = 'set'
ref = doIK(state, ref, eff_end, dof, jointSet, arm)
r.put(ref)
def __init__(self):
# Open Hubo-Ach feed-forward and feed-back (reference and state) channels
self.s = ach.Channel(HUBO_CHAN_STATE_NAME)
self.r = ach.Channel(HUBO_CHAN_REF_NAME)
self.s.flush()
self.r.flush()
# feed-forward will now be refered to as "state"
self.state = HUBO_STATE()
# feed-back will now be refered to as "ref"
self.ref = HUBO_REF()
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):
rp.init_node("baxter_ach_from", anonymous=True)
# robot = ROBOT()
leftArm = bi.Limb("left")
rightArm = bi.Limb("right")
s = ach.Channel("baxterState")
# angles_right = {'right_s0': 0.0, 'right_s1': 0.0, 'right_w0': 0.0, 'right_w1': 0.0, 'right_w2': 0.0, 'right_e0': 0.0, 'right_e1': 0.0}
# angles_left = {'left_s0': 0.0, 'left_s1': 0.0, 'left_w0': 0.0, 'left_w1': 0.0, 'left_w2': 0.0, 'left_e0': 0.0, 'left_e1': 0.0}
while True:
print 'waiting'
# [statuss , frameSizes] = s.get(robot, wait = True , last = True)
robot = self._fromBaxterDictionary(leftArm, rightArm)
s.put(robot)
# try:
# leftArm.move_to_joint_positions(leftAngle)
# rightArm.move_to_joint_positions(rightAngle)
# except:
# print "nothing came through"
rp.Rate(1).sleep()
def main():
print("init node")
baxterDataRef = ROBOT()
r = ach.Channel("baxterRef")
#s = ach.Channel("baxterState")
for i in range(BAXTER_NUM_ARM_JOINTS):
baxterDataRef.arm[LEFT_ARM].joint[i].ref = 0.0
baxterDataRef.arm[RIGHT_ARM].joint[i].ref = 0.0
baxterDataRef.currTime = time.time()
r.put(baxterDataRef)
print "Sending new data at {}".format(baxterDataRef.currTime)
while True:
a = int(raw_input("0) LeftArm \n1) RightArm\n>>"))
j = int(raw_input("0) S0\n1) S1\n2) E0\n3) E1\n4) W0\n5) W1\n6) W2\n>>"))
v = float(raw_input("Enter position in degrees >>"))
baxterDataRef.arm[a].joint[j].ref = math.radians(v)
print baxterDataRef.arm[a].joint[j].ref
baxterDataRef.currTime = time.time()
r.put(baxterDataRef)
print "Sending new data at {}".format(baxterDataRef.currTime)
time.sleep(1)
def __init__(self):
rp.init_node("baxter_ach", anonymous=True)
robot = ROBOT()
leftArm = bi.Limb("left")
rightArm = bi.Limb("right")
r = ach.Channel("baxterRef")
# angles_right = {'right_s0': 0.0, 'right_s1': 0.0, 'right_w0': 0.0, 'right_w1': 0.0, 'right_w2': 0.0, 'right_e0': 0.0, 'right_e1': 0.0}
# angles_left = {'left_s0': 0.0, 'left_s1': 0.0, 'left_w0': 0.0, 'left_w1': 0.0, 'left_w2': 0.0, 'left_e0': 0.0, 'left_e1': 0.0}
while True:
print 'waiting'
[statuss, frameSizes] = r.get(robot, wait=True, last=True)
leftAngle, rightAngle = self._toBaxterDictionary(
robot, leftArm, rightArm)
try:
leftArm.move_to_joint_positions(leftAngle)
rightArm.move_to_joint_positions(rightAngle)
except:
print "nothing came through"
rp.Rate(1).sleep()
def mainLoop():
# Open ACH Channel for IK
k = ach.Channel(mds.MDS_CHAN_IK_NAME)
# Make new IK structure
ikc = mds.MDS_IK()
# Set the amount of DOF you want to control
dof = 3
# Set values for work-space in [x, y, z, rx, ry, rz] order
eff = [0.3, 0.2, 0.0, 0.0, 0.0, 0.0]
# set arm
armi = mds.LEFT
# Put setting into ik structure
ikc.move = armi
ikc.arm[armi].ik_method = dof
ikc.arm[armi].t_x = eff[0]
ikc.arm[armi].t_y = eff[1]
ikc.arm[armi].t_z = eff[2]
ikc.arm[armi].r_x = eff[3]
ikc.arm[armi].r_y = eff[4]
ikc.arm[armi].r_z = eff[5]
print eff
# put on to ACH channel
k.put(ikc)
def mainLoop():
s = ach.Channel(mds.MDS_CHAN_STATE_NAME)
r = ach.Channel(mds.MDS_CHAN_REF_NAME)
state = mds.MDS_STATE()
ref = mds.MDS_REF()
j = 0
[status, framesize] = s.get(state, wait=False, last=True)
for i in range(bs):
st[i] = 0.0
t[i] = state.time
re[i] = 0.0
t[bs - 1] = t[0] + Fs
while (1):
[status, framesize] = s.get(state, wait=False, last=True)
[status, framesize] = r.get(ref, wait=False, last=True)
st[j] = state.joint[jnt].pos
st_r[j] = state.joint[jnt].ref_r
st_c[j] = state.joint[jnt].ref_c
re[j] = ref.joint[jnt].ref
t[j] = state.time
plt.clf()
plt.axis([np.amin(t), np.amax(t), -3, 3])
# plt.scatter(t, y)
# plt.plot(t, y,'.r-')
# plt.plot(t, re,'.b-')
# plt.plot(t, st,'.g-')
pa = plt.plot(t, re, '.b', label='reference')
pb = plt.plot(t, st_r, '.k', label='cammanded via CAN')
pc = plt.plot(t, st_c, '.r', label='commanded by MCB')
pd = plt.plot(t, st, '.g', label='actual pos')
plt.legend([
'reference', 'cammanded via CAN', 'commanded by MCB', 'actual pos'
])
plt.grid(b=True, which='major', color='b', linestyle='-')
# plt.grid(b=True, which='minor', color='r', linestyle='--')
plt.draw()
# pylab.plot(t,y)
#pylab.show()
j = j + 1
if j >= bs:
j = 0
time.sleep(Fs)
def mainLoop(enabled):
# Start curses
stdscr = curses.initscr()
s = ach.Channel(mds.MDS_CHAN_STATE_NAME)
state = mds.MDS_STATE()
curses.resizeterm(101, 100)
while (1):
[status, framesize] = s.get(state, wait=False, last=True)
# print "joint = ", state.joint[0x004c].pos, " time = ",state.time
stdscr.addstr(heady, namex, "Joint", curses.A_BOLD)
stdscr.addstr(heady, addressx, "address", curses.A_BOLD)
stdscr.addstr(heady, enabledx, "Enabled", curses.A_BOLD)
stdscr.addstr(heady, posrefx, "Commanded Pos", curses.A_BOLD)
stdscr.addstr(heady, posstatex, "Actual Pos", curses.A_BOLD)
i = 0
stdscr.addstr(timey, timex, str(state.time))
# for j in range(rMin,rMax):
for j in range(39, mds.MDS_JOINT_COUNT):
jnt = state.joint[j]
if ((jnt.enabled == 1) | (enabled == 0)):
jntName = (mds.ubytes2str(jnt.name)).replace('\0', '')
jntNameShort = (mds.ubytes2str(jnt.nameshort)).replace(
'\0', '')
# if (False == math.isnan(jnt.pos)):
if ('' != jntName.strip()):
y = i + heady + 1
# Name
# print jnt.name
stdscr.addstr(y, shortnamex, jntNameShort)
stdscr.addstr(y, namex, jntName)
# Enabled
stdscr.addstr(y, enabledx, str(jnt.enabled))
# Addresses
outAdd = ''.join(format(jnt.address, '04x'))
outAdd = '0x' + outAdd
stdscr.addstr(y, addressx, outAdd)
# cmd pos
stdscr.addstr(y, posrefx, str(format(jnt.ref, '.5f')))
#stdscr.addstr(y,posrefx, str(jnt.ref))
# actuial pos
stdscr.addstr(y, posstatex, str(format(jnt.pos, '.5f')))
#stdscr.addstr(y,posstatex, str(jnt.pos))
i += 1
stdscr.refresh()
time.sleep(0.05)
# if status == ach.ACH_OK or status == ach.ACH_MISSED_FRAME:
# print b.ref[0]
# else:
# raise ach.AchException( c.result_string(status) )
s.close()
def main():
print("init node")
robot = ROBOT()
s = ach.Channel("baxterState")
thetas = np.zeros((6, 1))
while True:
[statuss, frameSizes] = s.get(robot, wait=True, last=True)
for i in range(BAXTER_NUM_ARM_JOINTS - 1):
thetas[i] = robot.arm[LEFT_ARM].joint[i].ref
spatial = getFK(thetas)
print robot.currTime, spatial[0], spatial[1], spatial[2]
def __init__(self):
f = open('ipaddresses.txt', 'r')
self.servers = f.readlines()
f.close()
## build list of channels for sending and receiving
self.taskSendChannels = []
self.taskRecvChannels = []
for server in self.servers:
imgTaskChanName = server.replace(".", "").replace("\n","") + "VSTaskImg"
self.taskSendChannels.append(ach.Channel(imgTaskChanName)) # sending on
respChan = server.replace(".", "").replace("\n", "") + "VSResp"
self.taskRecvChannels.append(ach.Channel(respChan)) # receiving from
print("done setting up now testing latency...")
for testID in range(0, len(self.servers)):
self.latency = []
for i in range(0,10000):
recvDat = VelDat()
self.beginSend = time.time()
self.taskSendChannels[testID].put('reducedTask')
self.taskRecvChannels[testID].get(recvDat, wait=True, last=True)
self.recvDatTime = time.time()
self.latency.append(self.recvDatTime - self.beginSend)
print("latency for {} count is {} is {}".format(self.servers[testID], i, self.recvDatTime - self.beginSend))
f = open("latency"+self.servers[testID].replace(".", "").replace("\n",""), "w")
f.write("\n".join(str(x) for x in self.latency))
f.close()
print("finished latency test and have written out to "+"latency"+self.servers[testID])
def mainLoop():
s = ach.Channel(mds.MDS_CHAN_STATE_NAME)
r = ach.Channel(mds.MDS_CHAN_REF_NAME)
state = mds.MDS_STATE()
ref = mds.MDS_REF()
[status, framesize] = r.get(ref, wait=False, last=True)
ri = ref.joint[jnt].ref
# bring joint to zero slowly
imax = int(np.ceil(tt0/Fs))
print "Sign Wave for the following joint:"
outAdd = ''.join(format(jnt,'04x'))
outAdd = '0x'+outAdd
print " Address: \t", outAdd
print " Freq (hz)\t", f
print " Amp (rad)\t", A
for i in range(imax):
ref.joint[jnt].ref = ri - (ri * float(i)/float(imax))
r.put(ref)
time.sleep(Fs)
tm = 0.0;
tmFlag = True
while(1):
[status, framesize] = s.get(state, wait=False, last=True)
t = state.time
if tmFlag:
tm = float(t)
tmFlag = False
ref.joint[jnt].ref = A * np.sin(2.0*np.pi*f*(t-tm))
# ref.joint[jnt].ref = A * np.sin(2*np.pi*f*t) - A * np.sin(2*np.pi*f*tm)
r.put(ref)
time.sleep(Fs)
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([])
def main():
print("init node")
baxterDataRef = ROBOT()
r = ach.Channel("baxterRef")
#s = ach.Channel("baxterState")
for i in range(BAXTER_NUM_ARM_JOINTS):
baxterDataRef.arm[LEFT_ARM].joint[i].ref = 0.0
baxterDataRef.arm[RIGHT_ARM].joint[i].ref = 0.0
while True:
baxterDataRef.currTime = time.time()
r.put(baxterDataRef)
time.sleep(1)
def main():
print("init node")
baxterDataRef = baxterStructure.ROBOT()
r = ach.Channel("baxterRef")
#s = ach.Channel("baxterState")
rospy.init_node("baxter_joint_pos_set")
left = bi.Limb('left')
right = bi.Limb('right')
rate = rospy.Rate(1000)
while not rospy.is_shutdown():
[statuss, frameSizes] = r.get(baxterDataRef, wait = True, last = True)
def mainLoop():
k = ach.Channel(mds.MDS_CHAN_IK_NAME)
ikc = mds.MDS_IK()
ii = 0
while (1):
c = [0.3, 0.2, 0.0]
if ii == 0:
c = [0.3, 0.2, 0.0]
ii = 1
elif ii == 1:
c = [0.4, 0.45, 0.25]
ii = ii + 1
elif ii == 2:
c = [0.4, -0.05, 0.25]
ii = ii + 1
elif ii == 3:
c = [0.4, -0.05, -0.25]
ii = ii + 1
elif ii == 4:
c = [0.4, 0.45, -0.25]
ii = 1
if True:
arm = 'left'
armi = -1
dof = 3
eff = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
for i in range(0, dof):
eff[i] = c[i]
if arm == 'left':
armi = mds.LEFT
if arm == 'right':
armi = mds.RIGHT
if armi >= 0:
ikc.move = armi
ikc.arm[armi].ik_method = dof
ikc.arm[armi].t_x = eff[0]
ikc.arm[armi].t_y = eff[1]
ikc.arm[armi].t_z = eff[2]
ikc.arm[armi].r_x = eff[3]
ikc.arm[armi].r_y = eff[4]
ikc.arm[armi].r_z = eff[5]
print c
k.put(ikc)
time.sleep(5.0)
def mainLoop():
# Start curses
s = ach.Channel(mds.MDS_CHAN_STATE_NAME)
state = mds.MDS_STATE()
while (doloop):
# Get latest states
[status, framesize] = s.get(state, wait=False, last=True)
# Get address of LSP
jntn = mds.getAddress('LSP', state)
print 'Address = ', jntn
# Get state (encoder angle) of LSP
j_pos = state.joint[jntn].pos
print 'State = ', j_pos
# Wait 0.1 sec
time.sleep(0.1)
# Close channels
s.close()
def main():
rp.init_node("fromBaxterInterface", anonymous=True)
# robot = ROBOT()
leftArm = bi.Limb("left")
rightArm = bi.Limb("right")
s = ach.Channel("baxterState")
# angles_right = {'right_s0': 0.0, 'right_s1': 0.0, 'right_w0': 0.0, 'right_w1': 0.0, 'right_w2': 0.0, 'right_e0': 0.0, 'right_e1': 0.0}
# angles_left = {'left_s0': 0.0, 'left_s1': 0.0, 'left_w0': 0.0, 'left_w1': 0.0, 'left_w2': 0.0, 'left_e0': 0.0, 'left_e1': 0.0}
while True:
print 'waiting'
robot = _fromBaxterDictionary(leftArm, rightArm)
robot.currTime = time.time()
print robot.currTime
s.put(robot)
rp.Rate(1).sleep()
def main():
rp.init_node("baxterInterface" , anonymous = True)
robot = ROBOT()
leftArm = bi.Limb("left")
rightArm = bi.Limb("right")
r = ach.Channel("baxterRef")
r.flush()
while True:
[statuss , frameSizes] = r.get(robot, wait = True , last = True)
print "Received new Data at {}".format(robot.currTime)
leftAngle , rightAngle = toBaxterDictionary( robot , leftArm , rightArm )
try:
leftArm.move_to_joint_positions(leftAngle)
rightArm.move_to_joint_positions(rightAngle)
except:
print "nothing came through"
rp.Rate(1).sleep()
def __init__(self, theta, thetaDot, torque, id):
s = ach.Channel('dynProcess')
p = '/dev/ttyUSB0'
b = 1000000
idByte = dyn._convertToSingleByte(id)
tickLow, tickHigh = dyn._convertToLowHighByte(
dyn._convertToTick(theta))
address = [0x1e]
velLow, velHigh = dyn._convertToLowHighByte(thetaDot)
torqueLow, torqueHigh = dyn._convertToLowHighByte(torque)
params = tickLow + tickHigh + velLow + velHigh + torqueLow + torqueHigh
self._openSerial(p, b)
self._transmitData(idByte, address, params, [0x03])
curData, curDataInt = self._readData(idByte, [0x02])
self.radian = dyn._convertToRadian(
dyn._convertLowHighByteToInteger(curDataInt[5], curDataInt[6]))
self.velocity = dyn._convertLowHighByteToInteger(
curDataInt[7], curDataInt[8])
self.torque = dyn._convertLowHighByteToInteger(curDataInt[9],
curDataInt[10])
while True:
processData.id = id
processData.velocity = self.velocity
processData.position = self.radian
processData.torque = self.torque
s.put(processData)
def printParams(jnt):
p = ach.Channel(mds.MDS_CHAN_PARAM_NAME)
param = mds.MDS_JOINT_PARAM()
[status, framesize] = p.get(param, wait=False, last=True)
j = param.joint[jnt]
print 'Name \t:\t', ubytes2str(j.name)
print 'Address \t:\t', j.address
print 'Rom \t:\t', j.rom
print 'Enc Rez \t:\t', j.encoderresolution
print 'Gear Ratio \t:\t', j.gearratio
print 'Home Acc \t:\t', j.HomeAcceleration
print 'Home Velos \t:\t', j.HomeVelocity
print 'Home Buff \t:\t', j.HomeBuffer
print 'home Timeout \t:\t', j.HomeTimeout
print 'MBC KP \t:\t', j.MCB_KP
print 'MBC KI \t:\t', j.MCB_KI
print 'MBC KD \t:\t', j.MCB_KD
print 'MBC KM \t:\t', j.MCB_KM
print 'Safty Margin \t:\t', j.safetymargin
print 'Enc Tic/Cnt \t:\t', j.encoderconfig_tickspercount
print 'Rom Margin \t:\t', j.rom_margin
print 'Coord Sys \t:\t', ubytes2str(j.coordsys)
print 'Home Type \t:\t', ubytes2str(j.HomeType)
print 'Home Target \t:\t', ubytes2str(j.HomeTarget)
ROBOT_CHAN_VIEW = 'robot-vid-chan'
ROBOT_TIME_CHAN = 'robot-time'
# CV setup
cv.NamedWindow("wctrl", cv.CV_WINDOW_AUTOSIZE)
#capture = cv.CaptureFromCAM(0)
#capture = cv2.VideoCapture(0)
# added
##sock.connect((MCAST_GRP, MCAST_PORT))
newx = 320
newy = 240
nx = 640
ny = 480
r = ach.Channel(ROBOT_DIFF_DRIVE_CHAN)
r.flush()
v = ach.Channel(ROBOT_CHAN_VIEW)
v.flush()
t = ach.Channel(ROBOT_TIME_CHAN)
t.flush()
i=0
print '======================================'
print '============= Robot-View ============='
print '========== Daniel M. Lofaro =========='
print '========= [email protected] =========='
print '======================================'
while True:
# Add imports here from robot_command import createCommandPacket #----------------------------------------------------- #--------[ Do not edit below ]------------------------ #----------------------------------------------------- dd = diff_drive ref = dd.H_REF() tim = dd.H_TIME() ROBOT_DIFF_DRIVE_CHAN = 'robot-diff-drive' ROBOT_CHAN_VIEW = 'robot-vid-chan' ROBOT_TIME_CHAN = 'robot-time' # CV setup r = ach.Channel(ROBOT_DIFF_DRIVE_CHAN) r.flush() t = ach.Channel(ROBOT_TIME_CHAN) t.flush() i = 0 print '======================================' print '============= Robot-View =============' print '========== Daniel M. Lofaro ==========' print '========= [email protected] ==========' print '======================================' ref.ref[0] = 0 ref.ref[1] = 0 while True: [status, framesize] = t.get(tim, wait=False, last=True)
import cgposition_include as pi import localization_include as li import localization_include2 as li2 import slam_include as si import os import math import time #CONSTANTS TAGSIZE = 0.1778 #FUNCTIONS r = ach.Channel(ri.RECOGNITION_CHANNEL_NAME) recognition = ri.RECOGNITION_TYPE() r.put(recognition) s = ach.Channel(si.SLAM_CHANNEL_NAME) slam = si.SLAM_TYPE() s.put(slam) #read in collected array collectedArray = li.readInCollectedArray() #read in localTagPose localTagPose = li.readInLocalTagPose()
import ach
import time
import argparse
import sys
import rospy
import baxter_interface as bi
import baxterStructure as bs
from ctypes import *
data_out = ach.Channel('state_channel')
data_in = ach.Channel('ref_channel')
state = bs.STATE()
ref = bs.STATE()
data_in.flush()
print("init node")
rospy.init_node("baxter_joint_pos_set")
right = bi.Limb('right')
left = bi.Limb('left')
rate = rospy.Rate(1000)
def moveArm(ref2, arm, limb):
ref = bs.A2B(ref2)
if arm == bs.RIGHT:
angles = {
'right_s0': ref.arm[bs.RIGHT].joint[bs.SY].ref,
'right_s1': ref.arm[bs.RIGHT].joint[bs.SP].ref,
'right_w0': ref.arm[bs.RIGHT].joint[bs.WY].ref,
'right_w1': ref.arm[bs.RIGHT].joint[bs.WP].ref,
