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(): 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 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)
import os import math from ctypes import * import copy from numpy.linalg import inv import mds_ik_include as ike from mds_ach import * #define global variables # feed-forward will now be refered to as "state" state = mds.MDS_REF() # feed-back will now be refered to as "state" ref = mds.MDS_REF() state = mds.MDS_STATE() ikc = mds.MDS_IK() def mainLoop(): # Open Ach Channels r = ach.Channel(mds.MDS_CHAN_REF_FILTER_NAME) s = ach.Channel(mds.MDS_CHAN_STATE_NAME) # Make Structs state = mds.MDS_STATE() ref = mds.MDS_REF() # Get the latest on the channels [status, framesize] = s.get(state, wait=False, last=True) [status, framesize] = r.get(ref, wait=False, last=True) # Desired position
def mainLoop(): # Start curses s = ach.Channel(mds.MDS_CHAN_STATE_NAME) rf = ach.Channel(mds.MDS_CHAN_REF_FILTER_NAME) r = ach.Channel(mds.MDS_CHAN_REF_NAME) k = ach.Channel(mds.MDS_CHAN_IK_NAME) state = mds.MDS_STATE() ref = mds.MDS_REF() ref_filt = mds.MDS_REF() ikc = mds.MDS_IK() doloop = True while (doloop): # Block until input is received var = raw_input(mdsIntro) c = var.split(" ") # Get latest states [status, framesize] = s.get(state, wait=False, last=True) [status, framesize] = r.get(ref, wait=False, last=True) [status, framesize] = rf.get(ref_filt, wait=False, last=True) [status, framesize] = k.get(ikc, wait=False, last=True) # get command cmd = c[0] if cmd == 'get': # try: if c[1] == 'fk': arm = c[2] eff_joint_space_current = [0.0, 0.0, 0.0, -0.5, 0.0, 0.0] if arm == 'left': jntn = getAddress('LSP', state) j0 = state.joint[jntn].ref jntn = getAddress('LSR', state) j1 = state.joint[jntn].ref jntn = getAddress('LSY', state) j2 = state.joint[jntn].ref jntn = getAddress('LEP', state) j3 = state.joint[jntn].ref jntn = getAddress('LWY', state) j4 = state.joint[jntn].ref jntn = getAddress('LWR', state) j5 = state.joint[jntn].ref eff_joint_space_current = [j0, j1, j2, j3, j4, j5] elif arm == 'right': jntn = getAddress('RSP', state) j0 = state.joint[jntn].ref jntn = getAddress('RSR', state) j1 = state.joint[jntn].ref jntn = getAddress('RSY', state) j2 = state.joint[jntn].ref jntn = getAddress('REP', state) j3 = state.joint[jntn].ref jntn = getAddress('RWY', state) j4 = state.joint[jntn].ref jntn = getAddress('RWR', state) j5 = state.joint[jntn].ref eff_joint_space_current = [j0, j1, j2, j3, j4, j5] A = ik.getFkArm(eff_joint_space_current, arm) order = ['p_x', 'p_y', 'p_z', 't_x', 't_y', 't_z'] eff_end_ret = ik.getPosCurrentFromOrder(A, order) print '6DOF FK - for ', arm, ' arm' print 'pos:\tx = ', round(eff_end_ret[0], 5), '\ty = ', round( eff_end_ret[1], 5), '\tz = ', round(eff_end_ret[2], 5) print 'rot:\tx = ', round(eff_end_ret[3], 5), '\ty = ', round( eff_end_ret[4], 5), '\tz = ', round(eff_end_ret[5], 5) else: jnt = c[1] jntn = getAddress(jnt, state) pos = state.joint[jntn].pos pos_r = state.joint[jntn].ref jntName = (mds.ubytes2str(state.joint[jntn].name)).replace( '\0', '') print jntName, ' State = ', str(format(pos, '.5f')), ' rad' print jntName, ' Ref = ', str(format(pos_r, '.5f')), ' rad' # except: # print " Invalid input... " elif cmd == 'exit': s.close() r.close() quit() doloop = False elif cmd == 'goto': try: if c[1] == 'filter': jnt = c[2] jntn = getAddress(jnt, state) pos = float(c[3]) ref_filt.joint[jntn].ref = pos rf.put(ref_filt) else: jnt = c[1] jntn = getAddress(jnt, state) pos = float(c[2]) pos0 = state.joint[jntn].ref doLoop = True while (doLoop): pd = pos - pos0 psign = np.sign(pd) if np.abs(pd) > radT: pos0 = pos0 + psign * radT else: pos0 = pos doLoop = False ref.joint[jntn].ref = pos0 rf.put(ref) # r.put(ref) # Changed to filter ref put 2015-07-13 time.sleep(T) sys.stdout.write(".") sys.stdout.flush() print "done" except: print " Invalid input... " elif cmd == 'zeroall': for i in range(mds.MDS_JOINT_COUNT): ref.joint[i].ref = 0.0 r.put(ref) elif cmd == 'ik': arm = c[1] armi = -1 dof = int(c[2]) eff = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] for i in range(0, dof): eff[i] = float(c[i + 3]) 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] k.put(ikc) elif cmd == 'ik3': try: arm = c[1] dof = 3 jointSet = c[len(c) - 1] eff_end = np.array([float(c[2]), float(c[3]), float(c[4])]) ref = doIK(state, ref, eff_end, dof, jointSet, arm) if jointSet == 'set': r.put(ref) except: print " Invalid input... " elif cmd == 'ik5': try: arm = c[1] dof = 5 jointSet = c[len(c) - 1] jointSetFilter = c[len(c) - 2] eff_end = np.array([ float(c[2]), float(c[3]), float(c[4]), float(c[5]), float(c[5]) ]) ref = doIK(state, ref, eff_end, dof, jointSet, arm) if jointSet == 'set': if jointSetFilter == 'filter': rf.put(ref) else: r.put(ref) except: print " Invalid input... "