def getPosition(self, name, space, useSensors): try: pos = self.proxy.getPosition(name, space, useSensors) print 'getPosition:', pos return ssr.FloatArray(pos) except: traceback.print_exc() return ssr.FloatArray([])
def getStiffness(self, name): # raise CORBA.NO_IMPLEMENT(0, CORBA.COMPLETED_NO) # *** Implement me # Must return: result try: d = self.proxy.getStiffnesses(name.data) return ssr.FloatArray(d) except: traceback.print_exc() return ssr.FloatArray([])
def getAngles(self, name, useSensors): # raise CORBA.NO_IMPLEMENT(0, CORBA.COMPLETED_NO) # *** Implement me # Must return: result # print 'getAngles', name, ',', useSensors try: d = self.proxy.getAngles(name.data, useSensors) print '==>', d return ssr.FloatArray(d) except: traceback.print_exc() return ssr.FloatArray([])
def movePTPCartesianRel(self, carPoint): pos = self._motion._ptr().getPosition(self._handTag, 0, True) x = carPoint.carPos[0][3] + pos.data[0] y = carPoint.carPos[1][3] + pos.data[1] z = carPoint.carPos[2][3] + pos.data[2] wx = 0 wy = 0 wz = 0 axisMask = 7 tag = '' if self._right_left == 'right': tag = 'RArm' else: tag = 'LArm' self._motion._ptr().setPosition(tag, 0, ssr.FloatArray([x, y, z, wx, wy, wz]), 1.0, axisMask) #self._angles = self._motion._ptr().getAngles(ssr.StringArray(self._tags), True) #angle_sets = self._angles.data #pos, ori = fk.calc_fk_and_jacob(angle_sets, False, self._right_left == 'right') #target_pos = np.array([pos[0] + carPoint.carPos[0][3], pos[1] + carPoint.carPos[1][3], pos[2] + carPoint.carPos[2][3], 1]) #target_ori = None #epsilon = 0.1 #target_angles = ik.calc_inv_pos(angle_sets, target_pos, target_ori, epsilon, right=self._right_left == 'right') #angles = [a for a in target_angles] #self._motion._ptr().setAngles(ssr.StringArray(self._tags), ssr.FloatArray(angles), 1.0) result = JARA_ARM.RETURN_ID(JARA_ARM.OK, "") return result
def movePTPJointRel(self, jointPoints): self._angles = self._motion._ptr().getAngles(ssr.StringArray(self._tags), True) pos_ = [0] * 5 for i in range(0, 5): pos_[i] = self._angles[i] + jointPoints.pos[i] self._motion._ptr().setAngles(ssr.StringArray(self._tags), ssr.FloatArray(pos_), 1.0) result = JARA_ARM.RETURN_ID(JARA_ARM.OK, "") return result
def onExecute(self, ec_id): try: c = raw_input("# Input Command: (help for help)") cmds = c.split() if cmds[0] == "help": self.show_help() elif cmds[0] == "say": text = "" for c in cmds[1:]: text = text + " " + c self._textToSpeech._ptr().say(text) #elif cmds[0] == "behave": # self._behaviorManager._ptr().runBehavior(cmds[1]) elif cmds[0] == "stop": self._motion._ptr().stopMove() elif cmds[0] == "moveToward": if len(cmds) == (1 + 3): vx = float(cmds[1]) vy = float(cmds[2]) vz = float(cmds[3]) self._motion._ptr().moveToward(vx, vy, vz) elif cmds[0] == "moveTo": if len(cmds) == (1 + 3): x = float(cmds[1]) y = float(cmds[2]) z = float(cmds[3]) self._motion._ptr().moveTo(x, y, z) elif cmds[0] == "shakeHead": self._motion._ptr().setStiffness( ssr.StringArray(["HeadYaw", "HeadPitch"]), ssr.FloatArray([0.8, 0.7])) self._motion._ptr().setAngles( ssr.StringArray(["HeadYaw", "HeadPitch"]), ssr.FloatArray([0.2, -0.2]), 0.8) time.sleep(1.0) self._motion._ptr().setAngles(ssr.StringArray(["HeadYaw"]), ssr.FloatArray([-0.2]), 0.8) #self._motion._ptr().setAngles(ssr.StringArray(["RElbowYaw"]), ssr.FloatArray([-0.2]), 0.8) elif cmds[0] == "openHand": self._motion._ptr().openHand("RHand") elif cmds[0] == "closeHand": self._motion._ptr().closeHand("RHand") except Exception, e: print e
def movePTPJointAbs(self, jointPoints): self._motion._ptr().setAngles(ssr.StringArray(self._tags), ssr.FloatArray(jointPoints.pos), 1.0) result = JARA_ARM.RETURN_ID(JARA_ARM.OK, "") return result