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()
Ejemplo n.º 3
0
 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()
Ejemplo n.º 4
0
    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()
Ejemplo n.º 6
0
 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()
Ejemplo n.º 9
0
    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())
Ejemplo n.º 11
0
 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())
Ejemplo n.º 14
0
 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()
Ejemplo n.º 20
0
 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=([],[],[])
Ejemplo n.º 21
0
 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
Ejemplo n.º 23
0
    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
Ejemplo n.º 26
0
    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
Ejemplo n.º 27
0
        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()