def handle_quit(w, objects):
print(" ==> Inside the quit button rectangle")
hide(objects)
# TODO: Fade the window background from black to white.
return "quit"
def makeGrid():
w = GraphWin("Test of makeGrid()", 500, 500)
grid = utils.makeGrid()
utils.show(grid, w)
n = 0
print
print "Press any key or click anywhere in the window to hide/show the grid."
print
while True:
point = w.checkMouse()
key = w.checkKey()
if point or key:
n = n + 1
if n % 2 == 1:
utils.hide(grid)
else:
utils.show(grid, w)
def showAndHide():
w = GraphWin("Test of show() and hide", 500, 500)
c = Circle(Point(150, 150), 100)
c.setWidth(10)
c.setFill("yellow")
r = Rectangle(Point(250, 350), Point(350, 450))
r.setWidth(10)
r.setFill("red")
l = Line(c.getCenter(), r.getCenter())
l.setWidth(4)
l.setArrow("both")
l.setOutline("blue")
objects = [c, r, l]
n = 0
print
print "Click anywhere in the window."
print
print "Press any key or click anywhere in the window to show/hide all objects."
print
while True:
point = w.checkMouse()
key = w.checkKey()
if point or key:
n = n + 1
if n % 2 == 1:
utils.show(objects, w)
else:
utils.hide(objects)
def game(w):
"""
Argument(s):
w :: Graphical window.
Side effects:
Let the user play the game by moving the hero around on the grid.
"""
objects = init(w)
(grid, hero, food) = objects
play = True
while play:
key = w.getKey()
print(key)
move_hero(hero, key)
# TODO: Make the game end.
print("Game over!")
hide(objects)
def update(self, tf, time_to_wait):
"""
Update the joint valued of human in the simualted environment
on the basis of the position of the reference systems coming from
the kinect
@param tf tf
@param time_to_wait determine the frequency of the update. Expressed in seconds.
"""
time_since_last_update = rospy.get_rostime().secs - self.last_updated
if self.enabled and time_since_last_update > time_to_wait:
self.enabled = utils.hide(self.env, self.body)
elif not self.enabled and time_since_last_update <= time_to_wait:
self.enabled = utils.show(self.env, self.body)
#Chest
self.last_updated, person_transform = utils.getSkeletonTransformation(self.id, tf, 'torso', self.base_frame, self.last_updated)
if person_transform is not None:
#human face to kinect
person_orient_change = numpy.array([[ -1. , 0. , 0. , 0.],
[ 0. , 1. , 0. , 0.],
[ 0. , 0. , -1. , 0.],
[ 0. , 0. , 0. , 1. ]])
person_transform = numpy.dot(person_transform, person_orient_change)
#requied to have human standing
#angle/axis rotation for the alignment of human y with world z
th = numpy.arccos(numpy.asscalar(numpy.dot(numpy.array([0.,0.,1.]).T, person_transform[0:3,1])))
nn_axis = numpy.cross(person_transform[0:3,1], [0.,0.,1.])
axis = nn_axis/numpy.linalg.norm(nn_axis)
a11 = numpy.cos(th) + numpy.square(axis[0])*(1 - numpy.cos(th))
a12 = axis[0]*axis[1]*(1 - numpy.cos(th)) - axis[2]*numpy.sin(th)
a13 = axis[0]*axis[2]*(1 - numpy.cos(th)) + axis[1]*numpy.sin(th)
a21 = axis[1]*axis[0]*(1 - numpy.cos(th)) + axis[2]*numpy.sin(th)
a22 = numpy.cos(th) + numpy.square(axis[1])*(1 - numpy.cos(th))
a23 = axis[1]*axis[2]*(1 - numpy.cos(th)) - axis[0]*numpy.sin(th)
a31 = axis[2]*axis[0]*(1 - numpy.cos(th)) - axis[1]*numpy.sin(th)
a32 = axis[2]*axis[1]*(1 - numpy.cos(th)) + axis[0]*numpy.sin(th)
a33 = numpy.cos(th) + numpy.square(axis[2])*(1 - numpy.cos(th))
matrix_rot = numpy.array([[a11, a12 ,a13],
[a21, a22, a23],
[a31, a32, a33]])
person_position_transform_rot = numpy.dot(matrix_rot, person_transform[0:3,0:3])
person_position_transform = numpy.zeros((4, 4))
person_position_transform[0:3,0:3] = person_position_transform_rot
person_position_transform[0:4,3] = person_transform[0:4,3]
person_position_transform[2,3] = 0.85
self.body.SetTransform(person_position_transform)
#Left arm
ul_angles = self.getUpperLimbAngles(tf, 'L')
self.checkLimits(self.body.GetJoint('JLShoulder'), ul_angles, 0, self.collshl)
self.checkLimits(self.body.GetJoint('JLShoulder2'), ul_angles, 1, self.collsh2l)
self.checkLimits(self.body.GetJoint('JLShoulder3'), ul_angles, 2, self.collsh3l)
self.checkLimits(self.body.GetJoint('JLForearm'), ul_angles, 3, self.collell)
#Right arm
ul_angles = self.getUpperLimbAngles(tf, 'R')
self.checkLimits(self.body.GetJoint('JRShoulder'), ul_angles, 0, self.collshr)
self.checkLimits(self.body.GetJoint('JRShoulder2'), ul_angles, 1, self.collsh2r)
self.checkLimits(self.body.GetJoint('JRShoulder3'), ul_angles, 2, self.collsh3r)
self.checkLimits(self.body.GetJoint('JRForearm' ), ul_angles, 3, self.collelr)
#Left leg
ul_angles = self.getLowerLimbAngles(tf, 'L')
self.checkLimits(self.body.GetJoint('JLThigh'), ul_angles, 0, self.collthl)
self.checkLimits(self.body.GetJoint('JLCalf'), ul_angles, 1, self.collcal)
#Right leg
ul_angles = self.getLowerLimbAngles(tf, 'R')
self.checkLimits(self.body.GetJoint('JRThigh'), ul_angles, 0, self.collthr)
self.checkLimits(self.body.GetJoint('JRCalf'), ul_angles, 1, self.collcar)