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,