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