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 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())