def execute_keyframes(self, keyframes):
'''excute keyframes, note this function is blocking call,
e.g. return until keyframes are executed
'''
# for i in range(len(keyframes)):
return self.client.execute_keyframes(keyframes)
def get_transform(self, name):
'''get transform with given name
'''
return self.client.get_transform(name)
def set_transform(self, effector_name, transform):
'''solve the inverse kinematics and control joints use the results 'blocking'
'''
self.client.set_transform(effector_name, transform)
if __name__ == '__main__':
agent = ClientAgent()
# TEST CODE HERE
# agent.execute_keyframes(hello())
# time.sleep(6)
agent.execute_keyframes(leftBackToStand())
while (True):
print('Current posture is ' '"' + agent.get_posture() + '"')
# time.sleep(8)
# print (agent.get_transform('RAnkleRoll'))
# print('Current posture is ''"' + agent.get_posture() + '"')
np.cos(joint_angle), 0], [0, 0, 0, 1]]))
for i in self.chains.keys():
if joint_name in self.chains[i]:
t[0,
3] = self.joint_len[i][self.chains[i].index(joint_name)][0]
t[1,
3] = self.joint_len[i][self.chains[i].index(joint_name)][1]
t[2,
3] = self.joint_len[i][self.chains[i].index(joint_name)][2]
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 = np.dot(t, tl)
self.transforms[joint] = t
if __name__ == '__main__':
agent = ForwardKinematicsAgent()
agent.keyframes = leftBackToStand()
agent.run()
def standing_up(self):
posture = self.posture
if posture == "Back":
self.keyframes = leftBackToStand()
if posture == "Belly":
self.keyframes = leftBellyToStand()
# if we are in the right window then get right time and then use bezier
if joint_times[0] < elapsed_time < joint_times[-1]:
frame_n = np.argmax(np.array(joint_times) > elapsed_time)
max_t = joint_times[frame_n]
min_t = joint_times[frame_n - 1] if frame_n != 0 else 0
time = (elapsed_time - min_t) / (max_t - min_t)
target_joints[joint_name] = self.bezier(
joint_keys, frame_n, time)
# compensate for missing RHipYawPitch
if "LHipYawPitch" in target_joints:
target_joints["RHipYawPitch"] = target_joints["LHipYawPitch"]
return target_joints
def bezier(self, keys, frame_n, t):
p0 = keys[frame_n - 1][0]
p3 = keys[frame_n][0]
p1 = p0 + keys[frame_n - 1][2][
2] # check with someone on this, according to docs the 2nd handle value is angle and third is time, but if I use the 2nd it does not work very well
p2 = p3 + keys[frame_n][1][2]
return np.power(1 - t, 3) * p0 + 3 * t * np.power(
1 - t, 2) * p1 + 3 * np.power(t, 2) * (1 - t) * p2 + np.power(
t, 3) * p3
if __name__ == '__main__':
agent = AngleInterpolationAgent()
agent.keyframes = leftBackToStand() # CHANGE DIFFERENT KEYFRAMES
agent.run()