def check_motors(self): """Test each motor status""" motors = [None]*len(self.AXIS_LIMITS) for i in xrange(1, len(self.AXIS_LIMITS)): LOG.debug('checking motor %i', i) enc = KNI.getEncoder(i) motors[i] = KNI.moveMot(i, enc, self._speed, self._accel) != -1 return motors
def set_neck_orientation(self, xyz, wait=True): """Absolute orientation: Our own version since the IK is useless (ERROR: No solution found)""" encs = [ (4+i, SpineHW.rad2enc(4+i, v)) for i,v in enumerate(xyz) ] for axis, enc in encs: LOG.debug('moving axis %i to encoder %i', axis, enc) if KNI.moveMot(axis, enc, self._speed, self._accel) == -1: raise SpineError('failed to reach rotation (axis %i)' % axis) if not wait: return for axis, enc in encs: if KNI.waitForMot(axis, enc, self._tolerance) != 1: raise SpineError('failed to wait for motor %i' % (axis))
def set_neck_rot_pos(self, rot_xyz=None, pos_xyz=None): """Absolute orentation and position using KNI IK""" if not self._motors_on or (rot_xyz == pos_xyz == None): LOG.info('motors are %s [rot: %s\t pos: %s]', self._motors_on, rot_xyz, pos_xyz) return False tp = KNI.TPos() KNI.getPosition(tp) if rot_xyz: tp.phy, tp.theta, tp.psi = rot_xyz if pos_xyz: tp.X, tp.Y, tp.Z = pos_xyz ret = KNI.moveToPos(tp, self._speed, self._accel) KNI.getPosition(tp) if ret == -1: raise SpineError('failed to reach rotation/position') return True
def reach_pose(self, pose): """This pose is defined so that the hardware is safe to switch-off. pose: a *list* of encoder values for each spine axis """ # no wait if KNI.moveToPosEnc(*pose+[self._speed, self._accel, self._tolerance]+ [True]) == -1: raise SpineError('failed to reach pose')
def init_arm(): # Just initializes the arm import os.path import conf KatHD400s_6m = __path__[0]+os.path.sep+"katana6M90T.cfg" LOG.info('trying to connect to Katana400s-6m on %s', conf.spine_hardware) if KNI.initKatana(KatHD400s_6m, conf.spine_hardware) == -1: raise SpineError('configuration file not found or' ' failed to connect to hardware', KatHD400s_6m) else: print 'loaded config file', KatHD400s_6m, 'and now connected'
def get_neck_info(self): """Returns NeckInfo instance""" # XXX: we are not using the same reference: create a mapping function # tp = KNI.TPos() # KNI.getPosition(tp) # self._neck_info.pos= self.round([tp.X, tp.Y, tp.Z]) # self._neck_info.rot= self.round([tp.phi, tp.theta, tp.psi]) self._neck_info.rot = self.round( \ [ SpineHW.enc2rad(i, KNI.getEncoder(i)) for i in \ xrange(4, len(self.AXIS_LIMITS)) ]) return self._neck_info
# Copyright (C) 2008 Neuronics AG # PKE/JHA, 2008 ############################################################################################ """ This Skript demonstrates the use of the KNI library through the Python wrapper For API doc, read the kni_wrapper doc or the kni_wrapper.h file. """ ############################################################################################# import sys sys.path.append("../") import KNI from KNI import TMovement ############################################################################################# import KNI KNI.initKatana("../../configfiles450/katana6M90T.cfg", "192.168.1.1") KNI.calibrate(0) home = TMovement() KNI.getPosition(home.pos) home.transition = KNI.PTP home.velocity = 50 home.acceleration = 2 raw_input("Press [ENTER] to release robot.") KNI.allMotorsOff() stackname = "test" raw_input("Press [ENTER] to store first point (p2p movement).") m = TMovement()
def get_torso_info(self): """Returns TorsoInfo instance""" self._torso_info.rot= self.round([SpineHW.enc2rad(2,KNI.getEncoder(2)), 0.0, SpineHW.enc2rad(1,KNI.getEncoder(1))]) return self._torso_info
def calibrate(self): """Mandatory call upon hardware switch on. Also can be use to test calibration procedure.""" # TODO: use another sequence so that the arm does not collide itself if KNI.calibrate(0) == -1: raise SpineError('failed to calibrate hardware')
def switch_auto(self): """Resumes normal (driven-mode) operation of the robot.""" if not self._motors_on and KNI.allMotorsOn() == -1: raise SpineError('failed to switch motors on') self._motors_on = True
def switch_manual(self): """WARNING: Allow free manual handling of the robot. ALL MOTORS OFF!""" if KNI.allMotorsOff() == -1: raise SpineError('failed to switch motors off') self._motors_on = False
def sendMsg(self, msg) : if self.isConnected : (axis, action) = msg if axis == 0 : if action == 'CALIB' : KNI.calibrate(0) elif action == 'SETHOME' : from KNI import TMovement self.home = TMovement() KNI.getPosition(self.home.pos) self.home.transition = KNI.PTP self.home.velocity = 50 self.home.acceleration = 2 elif action == 'MOVEHOME' : if self.home != None : KNI.executeMovement(self.home) else : if action == 'STOP' : KNI.moveMot(axis, KNI.getEncoder(axis), 50, 2) elif action == 'RIGHT' : if axis in self.minusMotors : KNI.moveMot(axis, 0, 50, 2) else : KNI.moveMot(axis, 31000, 50, 2) elif action == 'LEFT' : if axis in self.minusMotors : KNI.moveMot(axis, -31000, 50, 2) else : KNI.moveMot(axis, 0, 50, 2) else : print "Port is not connected"