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