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... "