def think(self, perception):
if not self.is_animating():
ret, frame = self.cap.read()
frame = cv2.resize(frame, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA)
emotion, score = self.detector.top_emotion(frame)
print(emotion, score)
if self.isAngry == True and (emotion != 'angry' or score == None or score < 0.6):
print("not angry")
self.start_animation(handsDown())
self.isAngry = False
elif emotion == 'angry' and score > 0.6 and self.isAngry == False:
print("angry")
self.start_animation(handsUp())
self.isAngry = True
elif emotion == 'happy' and score > 0.8:
print("wave")
self.start_animation(hello())
elif emotion == 'sad' and score > 0.6:
print("sad")
self.start_animation(sad())
return super(EmoBotAgent, self).think(perception)
def __init__(self, simspark_ip='localhost',
simspark_port=3100,
teamname='DAInamite',
player_id=0,
sync_mode=True):
super(AngleInterpolationAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode)
self.keyframes = hello()
def execute_keyframes(self, keyframes):
'''excute keyframes, note this function is blocking call,
e.g. return until keyframes are executed
'''
# YOUR CODE HERE
#print keyframes
self.keyframes = hello()
return 0
def standing_up(self):
posture = self.posture
if (posture == 'Belly'):
self.keyframes = leftBellyToStand()
elif (posture == 'Back' or posture == 'Left'):
self.keyframes = leftBackToStand()
elif (posture == 'Right'):
self.keyframes = rightBackToStand()
elif (posture == 'Stand'):
self.keyframes = hello()
def __init__(self,
simspark_ip='localhost',
simspark_port=3100,
teamname='DAInamite',
player_id=0,
sync_mode=True):
super(AngleInterpolationAgent,
self).__init__(simspark_ip, simspark_port, teamname, player_id,
sync_mode)
self.keyframes = hello()
def standing_up(self, perception):
posture = self.posture
# YOUR CODE HERE
# The robot plays the animation, but can't actually stand up using that.
# I expect that the interpolation is at fault for that.
if posture == 'Belly':
self.keyframes = rightBellyToStand()
if posture == 'Back':
self.keyframes = rightBackToStand()
if posture == 'Standing':
self.keyframes = hello()
def standing_up(self):
posture = self.posture
# YOUR CODE HERE
if posture == 'Back' or posture == 'HeadBack':
self.keyframes = rightBackToStand()
return
elif posture == 'Belly' or posture == 'Frog:
self.keyframes = rightBellyToStand()
return
elif posture == 'Right' or posture is 'Left':
self.keyframes = rightBackToStand()
return
elif posture == 'Stand':
self.keyframes = hello()
return
# YOUR CODE HERE
temp = self.rpc_client.set_transform(effector_name, transform)
if __name__ == '__main__':
agent = ClientAgent()
print "start"
print 'first get the angle of LShoulderPitch'
print agent.get_angle('LShoulderPitch')
print 'now set the angle of RShoulderPitch to 1'
agent.set_angle('RShoulderPitch', 1)
print 'now get the transform of LHipYwaPitch'
print agent.get_transform('LHipYawPitch')
from numpy.matlib import identity
T = identity(4)
T[-1, 1] = 0.05
T[-1, 2] = 0.26
T = T.tolist()
print 'now execute the keyframe hello'
agent.execute_keyframes(hello())
print 'now set the transorm of LLeg to ', T
agent.set_transform('LLeg', T)
print 'now get the posture'
print agent.get_posture()
print 'now testing the Post Handler, the server side block the call so that the server only can do one task and ' \
'not both togethere'
agent.post.execute_keyframes(hello())
agent.post.set_transform('LLeg', T)
# TEST CODE HERE
#solution should be unique but due to error margin solutions actually marginally larger than 1 (or actually marginally smaller than 0) might get chosen
if len(candidates) > 1: # if thats the case, there must also exist a correct solution -> choose the one closer to 0.5
candidates = np.asarray([(x,np.abs(x-0.5)) for x in candidates],dtype = [("value", float),("distance", float)])
candidates = np.sort(candidates, order="distance")
t = candidates[0][0]
else: #if there's only one solution it must be the right one
t = candidates[0]
if t < 0.: #clip values marginally smaller than 0 to 0
t = 0.
if t > 1.: #clip values marginally larger than 1 to 1
t = 1.
#getting y values
coefficientsY = np.dot(bezierMatrix, y)
result = np.dot(np.array([1, t, t**2, t**3]),coefficientsY)
target_joints[name] = result
self.keyframeDone = done #when no joint gets an update, the keyframe was completely executed
return target_joints
if __name__ == '__main__':
agent = AngleInterpolationAgent()
agent.set_keyframes(hello()) # CHANGE DIFFERENT KEYFRAMES
agent.run()
return T
def forward_kinematics(self, joints):
'''forward kinematics
:param joints: {joint_name: joint_angle}
'''
for chain_joints in self.chains.values():
T = identity(4)
for joint in chain_joints:
angle = joints[joint]
Tl = self.local_trans(joint, angle)
T = T.dot(Tl)
self.transforms[joint] = T
for i in self.transforms:
if i == 'RElbowRoll':
x = array(self.transforms.get(i))[3][0]
y = array(self.transforms.get(i))[3][1]
z = array(self.transforms.get(i))[3][2]
print "x: " , x , " y: ", y , " z: " , z
if __name__ == '__main__':
agent = ForwardKinematicsAgent()
agent.keyframes = hello()
agent.run()
def recognize_posture(self, perception):
posture = 'unknown'
current_angle = np.zeros(10)
current_angle[0] = (perception.imu[0])
current_angle[1] = (perception.imu[1])
current_angle[2] = (perception.joint.get('LHipYawPitch'))
current_angle[3] = (perception.joint.get('LHipRoll'))
current_angle[4] = (perception.joint.get('LHipPitch'))
current_angle[5] = (perception.joint.get('LKneePitch'))
current_angle[6] = (perception.joint.get('RHipYawPitch'))
current_angle[7] = (perception.joint.get('RHipRoll'))
current_angle[8] = (perception.joint.get('RHipPitch'))
current_angle[9] = (perception.joint.get('RKneePitch'))
posture = self.posture_classifier.predict(current_angle.reshape(1,-1))[0]
print(posture)
return posture
if __name__ == '__main__':
agent = PostureRecognitionAgent()
agent.keyframes = hello() # CHANGE DIFFERENT KEYFRAMES
agent.run()
print(agent.transforms['RElbowRoll'].round(2))
print('LElbowRoll:')
print(agent.transforms['LElbowRoll'].round(2))
for chain_joints in agent.chains.values():
for joint in chain_joints:
vec = agent.transforms[joint] * [[1], [0], [0], [1]]
T_ = agent.transforms[joint].round(2)
xs.append(vec[0][0])
ys.append(vec[1][0])
zs.append(vec[2][0])
ax.scatter(xs, ys, zs)
# Set aspect ratio to 1:1:1
# https://stackoverflow.com/questions/8130823/set-matplotlib-3d-plot-aspect-ratio#comment40750814_19933125
scaling = np.array(
[getattr(ax, 'get_{}lim'.format(dim))() for dim in 'xyz'])
ax.auto_scale_xyz(*[[np.min(scaling), np.max(scaling)]] * 3)
plt.show()
if __name__ == '__main__':
agent = ForwardKinematicsAgent()
agent.keyframes = hello()
try:
agent.run()
except KeyboardInterrupt:
plot(agent)
keys = keyframes[2][i]
x = []
x.extend(times)
y = [key[0] for key in keys]
k = len(x) - 1 if len(x) <= 3 else 3
if rel_time > times[-1]:
continue
is_done = False
tck = scipy.interpolate.splrep(x=x, y=y, k=k)
r = scipy.interpolate.splev([rel_time + 0.025], tck)
target_joints[joint_name] = r[0]
if is_done:
self._start_time = None
self.keyframes = ([], [], [])
return target_joints
if __name__ == '__main__':
agent = AngleInterpolationAgent()
agent.keyframes = hello(
) # wipe_forehead(None) # CHANGE DIFFERENT KEYFRAMES
agent.run()
self.np_marshall.marshall(transform))
return
class NumpyMarshall(object):
def marshall(self, transform):
value_list = []
for i in range(4):
for j in range(4):
value_list.append(transform[i, j].item())
return value_list
def unmarshall(self, value_list):
transform = zeros([4, 4])
index = 0
for i in range(4):
for j in range(4):
transform[i, j] = value_list[index]
index += 1
return transform
if __name__ == '__main__':
agent = ClientAgent()
# TEST CODE HERE
print(agent.get_transform("RAnkleRoll"))
agent.execute_keyframes(keyframes=hello())
print("Done")
"jsonrpc": "2.0",
"id": 0,
}
response = requests.post(self.url,
data=json.dumps(payload),
headers=self.headers).json()
return response[u'result']
def get_transform(self, name):
'''get transform with given name
'''
# YOUR CODE HERE
return None
def set_transform(self, effector_name, transform):
'''solve the inverse kinematics and control joints use the results
'''
# YOUR CODE HERE
return None
if __name__ == '__main__':
agent = ClientAgent()
# TEST CODE HERE
agent.echo("Hello Server!")
print "Get angle of LElbowYaw: ", agent.get_angle("LElbowYaw")
print "Set angle of LElbowYaw to 30 - ", agent.set_angle(
"LElbowYaw", math.radians(-30))
print "Posture - ", agent.get_posture()
print "Keyframes (hello) - ", agent.execute_keyframes(hello())
if not self.in_motion:
self.motion_start = perception.time
self.initial_joints = perception.joint.copy()
self.in_motion = True
self.interpolators = SplineInterpolators(self.keyframes,
self.initial_joints)
result = self.interpolators.compute(
perception.time - self.motion_start,
speed_factor=self.speed_factor)
if not result:
with open('logs.pickle', 'wb') as fp:
pickle.dump(
(self.joints_log, self.target_joints_log,
self.actions_log), fp)
self.actions_log = []
self.joints_log = []
self.target_joints_log = []
self.still = True
self.in_motion = False
return result
if __name__ == '__main__':
agent = AngleInterpolationAgent()
agent.set_keyframes(keyframes.hello())
agent.run()
def execute_keyframes(self, keyframes):
'''excute keyframes, note this function is blocking call,
e.g. return until keyframes are executed
'''
# YOUR CODE HERE
self.proxy.execute_keyframes(keyframes)
def get_transform(self, name):
'''get transform with given name
'''
# YOUR CODE HERE
return self.proxy.get_transform(name)
def set_transform(self, effector_name, transform):
'''solve the inverse kinematics and control joints use the results
'''
# YOUR CODE HERE
self.proxy.set_transform(effector_name, transform)
if __name__ == '__main__':
proxy = rpc.ServerProxy("http://localhost:8000")
agent = ClientAgent(proxy)
# TEST CODE HERE
#print proxy.system.listMethods()
keyframes = hello()
agent.post_handler.execute_keyframes(keyframes)
#print agent.get_angle("LShoulderPitch")
#agent.post_handler.set_angle("LShoulderPitch", 5)
poly = np.poly1d(coffs)
splines.append([times[i], times[i + 1], poly]) # save the polynom with the times of the polynom
# Save the polynoms of the joint
name = joint_name[1]
saved_target_splines[name] = splines
return saved_target_splines
def get_latest_endTime(self):
latest_end_time = -1
for joint_name, spline_list in self.saved_target_splines.iteritems():
if latest_end_time < spline_list[-1][1]:
latest_end_time = spline_list[-1][1]
return latest_end_time
def createPostureKeyframes(self, keyframes, perception):
duration = 2
return_keyframe = (keyframes[0], [[0, duration]] * len(keyframes[0]), [])
for i in range(len(keyframes[0])):
if keyframes[0][i] in perception.joint.keys():
return_keyframe[2].insert(i, [[perception.joint[keyframes[0][i]], [3, 0, 0]], keyframes[2][i][0]])
else:
return_keyframe[2].insert(i, [[0, [3, 0, 0]], [0, [3, 0, 0]]])
return return_keyframe
if __name__ == '__main__':
agent = AngleInterpolationAgent()
agent.keyframes = hello() # CHANGE DIFFERENT KEYFRAMES
agent.run()
return self.server.get_posture()
def execute_keyframes(self, keyframes):
'''excute keyframes, note this function is blocking call,
e.g. return until keyframes are executed
'''
# YOUR CODE HERE
self.thread_id_kf += 1
self.server.execute_keyframes(keyframes)
def get_transform(self, name):
'''get transform with given name
'''
# YOUR CODE HERE
return self.server.get_transform(name)
def set_transform(self, effector_name, transform):
'''solve the inverse kinematics and control joints use the results
'''
# YOUR CODE HERE
self.server.set_transform(effector_name, transform)
if __name__ == '__main__':
agent = ClientAgent()
# TEST CODE HERE
print(agent.get_angle('HeadYaw'))
#agent.execute_keyframes(hello())
post_handler = PostHandler()
post_handler.execute_keyframes(hello())
