def standing_up(self): posture = self.posture # YOUR CODE HERE if self.keyframes == ([], [], []): if posture == "Back": if self.keyframes != rightBackToStand(): self.reset_time() self.keyframes = rightBackToStand() print( "Posture is \"Back\" --> Changing keyframe to rightBackToStand" ) elif posture == "Belly": if self.keyframes != rightBellyToStand(): self.reset_time() self.keyframes = rightBellyToStand() print( "Posture is \"Belly\" --> Changing keyframe to rightBellyToStand" ) elif posture == "Left": if self.keyframes != leftBackToStand(): self.reset_time() self.keyframes = leftBackToStand() print( "Posture is \"Left\" --> Changing keyframe to leftBackToStand" ) elif posture == "Right": if self.keyframes != rightBellyToStand(): self.reset_time() self.keyframes = rightBellyToStand() print( "Posture is \"Right\" --> Changing keyframe to rightBellyToStand" )
def standing_up(self): posture = self.posture print posture if posture == "Belly": self.keyframes = leftBellyToStand() if posture == "Back": self.keyframes = leftBackToStand() if posture == "Left": self.keyframes = leftBackToStand() if posture == "Right": self.keyframes = rightBackToStand()
def standing_up(self): posture = self.posture if posture == "Belly": self.keyframes = leftBellyToStand() if posture == "Left": self.keyframes = leftBackToStand() if posture == "Back": self.keyframes = leftBackToStand() if posture == "Stand": self.keyframes = self.keyframes else: self.keyframes = rightBellyToStand()
def standing_up(self): posture = self.posture # YOUR CODE HERE ROBOT_POSE_DATA_DIR = 'robot_pose_data' classes = listdir(ROBOT_POSE_DATA_DIR) if posture == 'Back': self.keyframes = leftBackToStand() if posture == 'Belly': self.keyframes = leftBellyToStand() self.keyframes = leftBackToStand()
def test(self): from keyframes import hello, leftBackToStand agent = AngleInterpolationAgent() keyframes = leftBackToStand() keyframes_time = np.max([t[-1] for t in keyframes[1]]) perception = agent.perception trajectory_t = [] trajectory = [] while perception.time < keyframes_time + start_time: agent.perception = perception target_joints = agent.angle_interpolation(keyframes, perception) trajectory.append(target_joints) trajectory_t.append(perception.time - start_time) perception.time += 0.01 keyframes = leftBackToStand() plot_joint = 'RKneePitch' plot_joint_id = None # check error in keyframe point error_at_key_frame = [] for i in range(len(keyframes[0])): n = keyframes[0][i] if n in ['LWristYaw', 'RWristYaw', 'LHand', 'RHand']: # ignore some joints continue if n == plot_joint: plot_joint_id = i for j in range(len(keyframes[1][i])): t = keyframes[1][i][j] for ti in range(len(trajectory_t)): tt = trajectory_t[ti] if abs(t - tt) < 0.005: e = trajectory[ti][n] - keyframes[2][i][j][0] error_at_key_frame.append(e) with open("test_angle_interpolation.txt", 'w') as f: f.write(str(np.mean(np.abs(error_at_key_frame)))) traj = [v.get(plot_joint, float('NaN')) for v in trajectory] k_t = keyframes[1][plot_joint_id] k_a = [v[0] for v in keyframes[2][plot_joint_id]] plt.plot(k_t, k_a, label='input', marker='x') plt.plot(trajectory_t, traj, label='output') plt.legend() plt.savefig('test_angle_interpolation.png') plt.show()
def standing_up(self): posture = self.posture # YOUR CODE HERE if (posture == 'Back'): agent.keyframes = leftBackToStand() elif (posture == 'Belly'): agent.keyframes = leftBellyToStand()
def standing_up(self): posture = self.posture # YOUR CODE HERE if posture == 'Belly': self.keyframes = keyframes.leftBellyToStand() elif posture == 'Back': self.keyframes = keyframes.leftBackToStand()
def standing_up(self): posture = self.posture if posture == 'Back': self.keyframes = leftBackToStand() elif posture == 'Belly': self.keyframes = leftBellyToStand()
def standing_up(self): posture = self.posture actions = {'Back': leftBackToStand(), 'Belly': leftBellyToStand()} if posture in actions: self.keyframes = actions[posture]
def standing_up(self): posture = self.posture # YOUR CODE HERE if posture == 'Back': self.set_keyframes(leftBackToStand()) elif posture == 'Belly': self.set_keyframes(leftBellyToStand())
def standing_up(self): posture = self.posture # YOUR CODE HERE if posture == 'Back' or posture =='Left': self.keyframes = leftBackToStand() elif posture == 'Belly' or posture == 'Right': self.keyframes = rightBellyToStand()
def standing_up(self): posture = self.recognize_posture(self.perception) # YOUR CODE HERE if posture == "Back" and self.lock == 0: self.keyframes = leftBackToStand() self.lock = 1 if posture == "Belly" and self.lock == 0: self.keyframes = rightBellyToStand() self.lock =1
def standing_up(self): posture = self.posture # YOUR CODE HERE if posture == 'Belly' and not self.is_animating(): self.start_animation(rightBellyToStand()) if posture == 'Back' and not self.is_animating(): self.start_animation(leftBackToStand())
def standing_up(self): posture = self.posture # YOUR CODE HERE if posture == 'Left': self.keyframe = leftBackToStand() if posture in ['Back', 'Headback', 'Sit', 'Right']: self.keyframe = rightBackToStand() if posture in ['Crouch', 'Belly', 'Frog']: self.keyframe = rightBellyToStand()
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): posture = self.posture # YOUR CODE HERE self.posture = self.recognize_posture(self.perception) # print(self.posture) if posture == "Belly": # print("belly to stand") self.keyframes = rightBellyToStand() elif posture == "Back": # print("back to stand") self.keyframes = leftBackToStand()
def standing_up(self): posture = self.posture # YOUR CODE HERE # Keyframe based movement is started by pressing k in SimSpark # Unfixed Problem, when the robot falls on its belly it makes a headstand when stiffness is set to 1 # I did not make Keyframes for standing up from that position if (posture == 'Back'): self.keyframes = leftBackToStand() if (posture == 'Belly'): self.keyframes = leftBellyToStand()
def standing_up(self): posture = self.posture # YOUR CODE HERE if posture == "Left": self.keyframes = keyframes.leftBackToStand() elif posture == "Right": self.keyframes = keyframes.rightBackToStand() elif posture == "Belly": self.keyframes = keyframes.leftBellyToStand() elif posture == "Back": self.keyframes = keyframes.rightBackToStand()
def standing_up(self): posture = self.posture # YOUR CODE HERE if posture == "Back": self.keyframes = leftBackToStand() elif posture == "Belly": self.keyframes = leftBellyToStand() elif posture == "Stand": self.keyframes = leftBellyToStand() elif posture == "Left": self.keyframes = rightBackToStand()
def standing_up(self): posture = self.posture # only update keyframe if the agent is in on mode and no other keyframe is being executed if self.is_on and self.t0==-1 : self.posture = self.recognize_posture(self.perception) # regonize posture # select keyframe based on the posture if posture=='Belly': self.keyframes=leftBellyToStand() elif posture=='Back' or posture=='Left' or posture=='HeadBack': self.keyframes=leftBackToStand() elif posture=='Right' or posture=='Sit': self.keyframes=rightBackToStand() elif posture=='Stand' or posture =='StandInit': self.keyframes=([],[],[])
def standing_up(self): posture = self.posture # YOUR CODE HERE print posture if self.started: if posture == 'Belly': print 'start belly' self.keyframes = leftBellyToStand() self.set_time() self.started = 0 elif posture == 'Back': print 'start back' self.keyframes = leftBackToStand() self.set_time() self.started = 0
def standing_up(self, perception): posture = self.posture if posture == "Back": new_keyframes = leftBackToStand() elif posture == "Belly": new_keyframes = leftBellyToStand() elif posture == "Left": new_keyframes = leftBellyToStand() elif posture == "Right": new_keyframes = rightBellyToStand() else: #do nothing return if new_keyframes != self.keyframes: self.init_time = perception.time self.keyframes = new_keyframes
def standing_up(self): posture = self.posture # YOUR CODE HERE leftBackToStand_classes = ['Back', 'HeadBack', 'Left', 'Sit'] leftBellyToStand_classes = ['Belly'] rightBackToStand_classes = ['Right'] if (self.perception.time - self.stiffness_on_off_time) > self.stiffness_off_cycle: if posture in leftBackToStand_classes: self.keyframes = leftBackToStand() elif posture in leftBellyToStand_classes: self.keyframes = leftBellyToStand() elif posture in rightBackToStand_classes: self.keyframes = rightBackToStand() else: self.keyframes = ([], [], [])
def standing_up(self): posture = self.posture # YOUR CODE HERE print self.startTime, self.stiffness if(self.startTime == -1 and self.stiffness == 1): # postures: ['Left', 'HeadBack', 'Belly', 'Stand', 'Frog', 'StandInit', 'Back', 'Right', 'Knee', 'Sit', 'Crouch'] if(posture == "Left"): self.keyframes = kf.leftBackToStand() print "### recognized :" + posture + ", executing leftBackToStand()" elif(posture == "Right"): self.keyframes = kf.rightBackToStand() print "### recognized :" + posture + ", executing rightBackToStand()" elif(posture == "Belly"): self.keyframes = kf.rightBellyToStand() print "### recognized :" + posture + ", executing leftBellyToStand()" elif(posture == "Back" or posture == "Frog" or posture == "Sit"): self.keyframes = kf.rightBackToStand() print "### recognized :" + posture + ", executing rightBackToStand()"
def standing_up(self): posture = self.posture # YOUR CODE HERE # print(f"{posture}") if self.old_posture != "unknown" and self.old_posture != posture and posture in ["Belly", "Back", "Left", "Right"] : logging.info(f"{self.old_posture} != {posture}!!!") self.start_time = -1 if self.start_time == -1: if posture == "Left": self.keyframes = kf.leftBackToStand() elif posture == "Right": self.keyframes = kf.rightBackToStand() elif posture == "Belly": self.keyframes = kf.leftBellyToStand() elif posture == "Back": self.keyframes = kf.rightBackToStand() self.old_posture = posture
def standing_up(self): posture = self.posture #print(posture) # YOUR CODE HERE leftBack = ['HeadBack', 'Left'] rightBack = ['Back', 'Right'] leftBelly = ['Belly', 'knee'] rightBelly = ['Crouch', 'Frog', 'Sit'] standing = ['Stand', 'StandInit'] #if (self.stiffness_on_off_time + self.stiffness_off_cycle - self.perception.time <= 0.1): if not self.keyframes[0]: if posture in leftBack: self.keyframes = keyframes.leftBackToStand() elif posture in rightBack: self.keyframes = keyframes.rightBackToStand() elif posture in leftBelly: self.keyframes = keyframes.leftBellyToStand() elif posture in rightBelly: self.keyframes = keyframes.rightBellyToStand() elif posture in standing: pass
super(PostureRecognitionAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode) self.posture = 'unknown' self.posture_classifier = pickle.load( open(path.join(pardir, 'joint_control', 'robot_pose.pkl'))) # LOAD YOUR CLASSIFIER def think(self, perception): self.posture = self.recognize_posture(perception) return super(PostureRecognitionAgent, self).think(perception) def recognize_posture(self, perception): posture = 'unknown' # YOUR CODE HERE feature_list = [[perception.joint['LHipYawPitch'], perception.joint['LHipRoll'], perception.joint['LHipPitch'], perception.joint['LKneePitch'], perception.joint['RHipYawPitch'], perception.joint['RHipRoll'], perception.joint['RHipPitch'], perception.joint['RKneePitch'], perception.imu[0], perception.imu[1]]] classes = listdir(path.join(pardir, 'joint_control', 'robot_pose_data')) posture = classes[self.posture_classifier.predict(feature_list[-1:])[0]] # print(posture) return posture if __name__ == '__main__': agent = PostureRecognitionAgent() agent.keyframes = leftBackToStand() # CHANGE DIFFERENT KEYFRAMES agent.run()
def standing_up(self): posture = self.posture self.keyframes = leftBackToStand() # YOUR CODE HERE
player_id=0, sync_mode=True): super(PostureRecognitionAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode) self.posture = 'unknown' self.posture_classifier = pickle.load(open(ROBOT_POSE_CLF)) # LOAD YOUR CLASSIFIER def think(self, perception): self.posture = self.recognize_posture(perception) return super(PostureRecognitionAgent, self).think(perception) def recognize_posture(self, perception): posture = 'unknown' # YOUR CODE HERE # read data data = np.empty(len(featureNames)) data[-2] = perception.imu[0] data[-1] = perception.imu[1] for i, feature in enumerate(featureNames[:-2]): data[i] = perception.joint[feature] #predict posture = self.posture_classifier.predict(data) #print posture return posture if __name__ == '__main__': agent = PostureRecognitionAgent() agent.set_keyframes(leftBackToStand()) # CHANGE DIFFERENT KEYFRAMES agent.run()
t_3 = times[j_index][t_index + 1] # Control times t_1 = keys[j_index][t_index][1][1] + t_0 t_2 = keys[j_index][t_index][2][1] + t_3 # Angles a_0 = keys[j_index][t_index][0] a_3 = keys[j_index][t_index + 1][0] # Control angles a_1 = keys[j_index][t_index][1][2] + a_0 a_2 = keys[j_index][t_index][2][2] + a_3 # dt = (start_time - t_0) / (t_3 - t_0) return self.calculate_bezier_interpolation(a_0, a_1, a_2, a_3, dt) @staticmethod def calculate_bezier_interpolation(a_0, a_1, a_2, a_3, dt): c_0 = (1 - dt)**3 c_1 = 3 * (1 - dt)**2 c_2 = 3 * (1 - dt) return c_0 * a_0 + c_1 * a_1 * dt + c_2 * a_2 * dt**2 + a_3 * dt**3 if __name__ == '__main__': agent = AngleInterpolationAgent() agent.keyframes = leftBackToStand() #hello, leftBackToStand, leftBellyToStand, rightBackToStand, rightBellyToStand, wipe_forehead agent.run()
def __init__(self, simspark_ip='localhost', simspark_port=3100, teamname='DAInamite', player_id=0, sync_mode=True): super(TestStandingUpAgent, self).__init__(simspark_ip, simspark_port, teamname, player_id, sync_mode) self.stiffness_on_off_time = 0 self.stiffness_on_cycle = 10 # in seconds self.stiffness_off_cycle = 3 # in seconds self.stop = 1 def think(self, perception): action = super(TestStandingUpAgent, self).think(perception) time_now = perception.time if self.stop: if time_now - self.stiffness_on_off_time < self.stiffness_off_cycle: action.stiffness = {j: 0 for j in self.joint_names} # turn off joints else: action.stiffness = {j: 1 for j in self.joint_names} # turn on joints self.stop = 0 if time_now - self.stiffness_on_off_time > self.stiffness_on_cycle + self.stiffness_off_cycle: self.stiffness_on_off_time = time_now return action if __name__ == '__main__': agent = TestStandingUpAgent() agent.keyframes= leftBackToStand() agent.run()
-t0 + 3 * t1 - 3 * t2 + t3, 3 * t0 - 6 * t1 + 3 * t2, -3 * t0 + 3 * t1, t0 - ti ] null_points = np.roots(coeff_t) i = False for root in null_points: if np.isreal(root): if 0 <= root <= 1: #lets say its the only one #print"Yeagh!!! found root: %f"%np.real(root) i = root #TODO see if roots make sense TODO remove name #print "roots of Polynoms: %s"%str(null_points) a0 = left_handle[1][0] a1 = left_handle[1][2][2] + a0 a3 = right_handle[1][0] a2 = right_handle[1][1][2] + a3 coeff_a = [ -a0 + 3 * a1 - 3 * a2 + a3, 3 * a0 - 6 * a1 + 3 * a2, -3 * a0 + 3 * a1, a0 ] #print("a0=%f, a1=%f, a2=%f, a3=%f")%(a0, a1, a2, a3) return np.polyval(coeff_a, i) if __name__ == '__main__': agent = AngleInterpolationAgent() agent.set_keyframes(leftBackToStand()) # CHANGE DIFFERENT KEYFRAMES #agent.keyframes = leftBackToStand() agent.run()
) # same here; classes = ['Left', 'Knee', 'Back', 'Right', 'Crouch', 'Sit', 'Frog', 'Stand', 'HeadBack', 'Belly', 'StandInit']; len: 11 #angles = np.zeros(len(features)) angles = np.array([]) for feature in features: angles = np.append(angles, perception.joint[feature]) #angles[-2] = perception.imu[0] #angles[-1] = perception.imu[1] angles = np.append(angles, perception.imu[0]) # AngleX angles = np.append(angles, perception.imu[1]) # AngleY #predicted = self.posture_classifier.predict(angles) # does not work because angles is 1d (shape (10,)), not 2d angles = np.array([angles]) # shape (1,10) #angles = np.reshape(angles, (-1,1)) # automatically set number of rows, but only one column -> shape (10,1) predicted = self.posture_classifier.predict( angles ) # gives number (as str) from 0 to 10 -> index of classes-array, 7 means: Stand, Robot is standing # print(predicted) # DEBUG, can get removed posture = classes[int(predicted)] # print(posture) # DEBUG, can get removed return posture if __name__ == '__main__': agent = PostureRecognitionAgent() #agent.keyframes = hello() # CHANGE DIFFERENT KEYFRAMES agent.keyframes = leftBackToStand() agent.run()
) point2 = (timesForkeyframeName[i + 1], keysForkeyframeName[i + 1][0]) # point3 = ( timesForkeyframeName[i + 1] + keysForkeyframeName[i + 1][1][1], keysForkeyframeName[i + 1][0] + keysForkeyframeName[i + 1][1][2], ) t = (timeInKeyframes - timesForkeyframeName[i]) / ( timesForkeyframeName[i + 1] - timesForkeyframeName[i] ) k = 1 # check float errors if t > 1: t = 1.0 target_joints[keyframeName] = ( ((k - t) ** 3) * point0[1] + 3 * ((k - t) ** 2) * t * point1[1] + 3 * (k - t) * (t ** 2) * point2[1] + (t ** 3) * point3[1] ) return target_joints if __name__ == "__main__": agent = AngleInterpolationAgent() agent.keyframes = leftBackToStand() # CHANGE DIFFERENT KEYFRAMES agent.run()