class Smartie(object):
''' Class to connect to and publish the Korg NanoKontrol midi device
as a ros joystick
'''
def __init__(self):
''' connect to midi device and set up ros
'''
self.sp = SmartPAD('SmartPAD')
self.axis = Axis()
self.axis.init()
self.web = pages.Pages()
rospy.loginfo("Using input device %s" % self.sp.device)
os.environ['PORT'] = '8081'
def finish(self):
self.sp.finish()
self.axis.fini()
self.web.finish()
#del self.sp
#del self.axis
def run(self):
try:
thread.start_new_thread(self.web.run, ())
while not rospy.is_shutdown():
done, data = self.sp.readin()
if done:
rospy.signal_shutdown("Smartie Done")
self.axis.process_keys(data)
finally:
rospy.loginfo("Smartie Died")
self.finish()
class Kinemate(object):
''' Class to connect to and publish the Korg NanoKontrol midi device
as a ros joystick
'''
#ROS joint names
leftandright = ['leftShoulderPitch', 'leftShoulderRoll', 'leftShoulderYaw',
'leftElbowPitch', 'rightShoulderPitch', 'rightShoulderRoll',
'rightShoulderYaw', 'rightElbowPitch' ]
#Axis joint names
lalr = ['la0','la1','la2','la3','ra0','ra1','ra2','ra3']
def __init__(self):
self.axis = Axis(verbose=0)
self.axis.init()
self.poser = ArmPos()
self.csvf = open('kin.csv', 'wb')
self.csv = csv.writer(self.csvf)
#complicated reset just for fun with Python comprehensions
self.angles = [0.2, 1.4, 0.2, 1.5]
self.rtinvert = [ 1.0, 1.0, 1.0, 1.0]
self.lfinvert = [ 1.0,-1.0, 1.0,-1.0]
self.lch = ['la0', 'la1', 'la2', 'la3']
self.rch = ['ra0', 'ra1', 'ra2', 'ra3']
data = [ Datum(j[0],j[1]*j[2]) for j in zip(self.rch,self.angles,self.rtinvert)]
self.axis.process_keys(data)
rospy.sleep(0.5)
data = [ Datum(j[0],j[1]*j[2]) for j in zip(self.lch,self.angles,self.lfinvert)]
self.axis.process_keys(data)
self.pose = [0.2, 1.4, 0.2, 1.5, 0.2,-1.4, 0.2,-1.5]
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
self.poser.subscribe(Kinemate.leftandright)
def finish(self):
self.axis.fini()
self.csvf.close()
def setPose(self,joint,val):
self.pose[Kinemate.lalr.index(joint)] = val
self.axis.process_keys([Datum(joint,val)])
rospy.sleep(0.05)
pass
def delta(self,Euc):
lpalmtf = self.tfBuffer.lookup_transform('torso', 'leftPalm', rospy.Time())
lp = lpalmtf.transform.translation
rpalmtf = self.tfBuffer.lookup_transform('torso', 'rightPalm', rospy.Time())
rp = rpalmtf.transform.translation
pos = [lp.x,lp.y,lp.z,rp.x,rp.y,rp.z]
return [ pos[i]-Euc[i] for i in range(len(Euc)) ]
def derivative(self):
palm = self.delta([0,0,0,0,0,0])
pose = self.pose
for j in range(4):
ljoint = Kinemate.lalr[j]
lcurval = pose[j]
self.axis.process_keys([Datum(ljoint,lcurval+0.1)])
rospy.sleep(0.05)
rjoint = Kinemate.lalr[j+4]
rcurval = pose[j+4]
self.axis.process_keys([Datum(rjoint,rcurval+0.1)])
print "Derivative %d:" % j
self.savePose(start=palm)
self.axis.process_keys([Datum(ljoint,lcurval)])
rospy.sleep(0.05)
self.axis.process_keys([Datum(rjoint,rcurval)])
def savePose(self,start=[0,0,0,0,0,0]):
rospy.sleep(1.0)
palm = self.delta(start)
requested = [ "%.3f" % v for v in self.pose]
actual = [ "%.3f" % v for v in self.poser.val()]
xyz = [ "%.3f" % v for v in palm]
print "Req: %s" % requested
print "Act: %s" % actual
#print "(%.3f,%.3f,%.3f)" % (palm.x,palm.y,palm.z)
print "Result: %s" % xyz
self.writePose(rospy.Time.now().to_sec(),self.pose,self.poser.val(),palm)
def writePose(self,ts,req,act,xyz):
row = [ts]
row.extend(req)
row.extend(act)
row.extend(xyz)
ans = [ round(v,4) for v in row]
self.csv.writerow(ans)
print ans
def run(self):
rate = rospy.Rate(10) # 10hz
time.sleep(1)
rtShoulderPitch = [-0.2,-0.7]
rtShoulderRoll = [ 1.4, 0.9]
rtShoulderYaw = [ 0.2, 0.0]
rtElbowPitch = [ 1.5, 1.0]
try:
while not rospy.is_shutdown():
for j0 in rtShoulderPitch:
self.setPose('ra0',j0)
self.setPose('la0',j0)
for j1 in rtShoulderRoll:
self.setPose('ra1',j1)
self.setPose('la1',-j1)
for j2 in rtShoulderYaw:
self.setPose('ra2',j2)
self.setPose('la2',-0.5-j2)
for j3 in rtElbowPitch:
self.setPose('ra3',j3)
self.setPose('la3',-j3)
print " "
print "===Test Position==="
self.savePose()
self.derivative()
rospy.signal_shutdown("Kinemate Done")
finally:
rospy.loginfo("Kinemate Died")
self.finish()
