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 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():
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():
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)
r = ach.Channel(ha.MDS_CHAN_REF_NAME)
## s.flush()
## r.flush()
#open ach channel for coodinates being sent from file
### k = ach.Channel(ike.CONTROLLER_REF_NAME)
coordinates = ike.CONTROLLER_REF()
coordinates.x = -0.25
coordinates.y = -0.20
coordinates.z = 0.20
# feed-forward will now be refered to as "state"
### state = ha.HUBO_STATE()
# feed-back will now be refered to as "ref"
ref = ha.MDS_REF()
#bend elbow to start to force jacobian to solve in righ direction
#avoid singularity
ref.joint[ha.REB].ref = -math.pi / 6
ref.joint[ha.LEB].ref = -math.pi / 6
### r.put(ref)
### time.sleep(3)
def FKE_arm(t1, t2, t3):
x = l3 * (math.sin(t3) * math.sin(t1) - math.cos(t1) * math.cos(t2) *
math.cos(t3)) - l2 * math.cos(t1) * math.cos(t2)
y = -l3 * (math.sin(t1) * math.cos(t2) * math.cos(t3) +
math.cos(t1) * math.sin(t3)) - l2 * math.sin(t1) * math.cos(t2)
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
eff_end = np.array([0.3, 0.2, 0.0, 0.0, 0.0, 0.0])
# Define Arm
arm = 'left'
# Get current joint space pose of arm
jntn = mds.getAddress('LSP',state)
j0 = state.joint[jntn].ref
jntn = mds.getAddress('LSR',state)
j1 = state.joint[jntn].ref
jntn = mds.getAddress('LSY',state)
j2 = state.joint[jntn].ref
jntn = mds.getAddress('LEP',state)
j3 = state.joint[jntn].ref
jntn = mds.getAddress('LWY',state)
j4 = state.joint[jntn].ref
jntn = mds.getAddress('LWR',state)
j5 = state.joint[jntn].ref
eff_joint_space_current = [j0, j1, j2, j3, j4, j5]
# set the dof and the order (dof = 3)
dof = 3
order = ['p_x','p_y','p_z']
# set the allowable error
err = np.array([0.01,0.01, 0.01])
# Set solving step number max
stepNum = 1000
# Deisred position for only the dof we want
eff_end = eff_end[:dof]
# Solve IK
jnt_return = ik.getIK(eff_joint_space_current, eff_end, order, arm, err, stepNum)
# exits if no solution within setNum itirations
if (jnt_return[1] == -1):
print 'no IK found within ', stepNum, ' steps'
quit()
# else set the new joint space
else:
# returns in the followin order
# Joint space return = [LSP, LSR, LRY, LEP, LWY, LWR]
eff_joint_space_current = jnt_return[0]
# Set to joint space
jntn = mds.getAddress('LSP',state)
ref.joint[jntn].ref = eff_joint_space_current[0]
jntn = mds.getAddress('LSR',state)
ref.joint[jntn].ref = eff_joint_space_current[1]
jntn = mds.getAddress('LSY',state)
ref.joint[jntn].ref = eff_joint_space_current[2]
jntn = mds.getAddress('LEP',state)
ref.joint[jntn].ref = eff_joint_space_current[3]
jntn = mds.getAddress('LWY',state)
ref.joint[jntn].ref = eff_joint_space_current[4]
jntn = mds.getAddress('LWR',state)
ref.joint[jntn].ref = eff_joint_space_current[5]
# Send to the robot
r.put(ref)
# get FK of arm
A = ik.getFkArm(eff_joint_space_current,arm)
eff_end_ret = ik.getPosCurrentFromOrder(A,order)
# find different in desired vs actuial pos
eff_end_dif = eff_end - eff_end_ret
# Print
print 'N-3 DOF IK Solution - for ', arm, ' arm'
print 'des:\tx = ', round(eff_end[0],5) , '\ty = ' , round(eff_end[1],5) , '\tz = ', round(eff_end[2],5)
print 'ret:\tx = ', round(eff_end_ret[0],5) , '\ty = ' , round(eff_end_ret[1],5) , '\tz = ', round(eff_end_ret[2],5)
print 'dif:\tx = ', round(eff_end_dif[0],5) , '\ty = ' , round(eff_end_dif[1],5) , '\tz = ', round(eff_end_dif[2],5)
import ach import time import signal import mds_ik as ik import sys 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()
l3 = 0.3127375 # elbow to wrist roll l4 = 0.0635 # distance to middle of hand alpha = .5 dT1 = .001 dT2 = .001 dT3 = .001 #open ach channel for coodinates being sent from file ### k = ach.Channel(ike.CONTROLLER_REF_NAME) coordinates = ike.CONTROLLER_REF() coordinates.x = -0.25 coordinates.y = -0.20 coordinates.z = 0.20 # feed-forward will now be refered to as "state" state = ha.MDS_REF() # feed-back will now be refered to as "ref" ref = ha.MDS_REF() #bend elbow to start to force jacobian to solve in righ direction #avoid singularity ref.joint[ha.REB].ref = -math.pi / 6 ref.joint[ha.LEB].ref = -math.pi / 6 ### r.put(ref) ### time.sleep(3) # rotation matrix definition (abotu x, y, or z) def_x = 1 def_y = 2
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... "
import ach
import time
import signal
import mds_ik as ik
import sys
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()
#initial feed-back
initial_ref = mds.MDS_REF()
initial_state = mds.MDS_STATE()
# feed-back will now be refered to as "state"
ref = mds.MDS_REF()
state = mds.MDS_STATE()
# Command line for console
ikc = mds.MDS_IK()
#define arm constant
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)