def right_arm_ik_test(verbose=False):
angle_set = [[0] * 5, k.right_arm_initial_pose, k.right_arm_work_pose]
pos, ori = k.right_arm_get_position(angle_set[2])
a = angle_set[1]
delta = angle_set[2] - k.right_arm_set_position(a, pos, ori)
print np.sum(delta * delta)
def right_arm_ik_test(verbose=False):
angle_set = [
[0] * 5,
k.right_arm_initial_pose,
k.right_arm_work_pose]
pos, ori = k.right_arm_get_position(angle_set[2])
a = angle_set[1]
delta = angle_set[2] - k.right_arm_set_position(a, pos,ori)
print np.sum(delta*delta)
def right_arm_fk_test():
motion = naoqi.ALProxy("ALMotion", host, port)
angle_set = [
[0] * 5,
[1.57, 0, 0, 0, 0],
[0, 0, 1.57, 0, 0],
[1.57, 0, 0, 0, 0],
[0, 0, 0, 0, 1.57],
[0, 0, 0, 1.57, 0],
[0, 0, 1.57, 1.57, 0],
[1.57, -1.57, 0, 0, 0],
[1.57, -1.57, 0, 0, -1.57],
[1.57, -1.57, 0, 0, 1.57],
[1.57, 0, 0, 0, 0],
]
for a in angle_set:
motion.setAngles(k.right_arm_tags, a, 1.0)
time.sleep(2.0)
pos0 = motion.getPosition("RArm", 0, True)
angles = motion.getAngles(k.right_arm_tags, True)
pos_, ori_ = k.right_arm_get_position(angles)
pos = pos0[0:3]
pos[0] = pos[0] + 0.057
pos[2] = pos[2] - 0.08682
pos_error = [p - p_ for p, p_ in zip(pos, pos_)]
sum_pos_error = 0
for p in pos_error:
sum_pos_error = sum_pos_error + math.fabs(p)
ori = pos0[3:]
Tx = k.transX(ori[0], 0, 0, 0)
Ty = k.transY(-ori[1], 0, 0, 0)
Tz = k.transZ(ori[2], 0, 0, 0)
T = Tz.dot(Ty.dot(Tx))[0:3, 0:3]
T_ = ori_
ori_error = T - T_
sum_ori_error = 0
for o in ori_error:
for v in o:
sum_ori_error = sum_ori_error + math.fabs(v)
pass
print 'Error(a, pos ori), ', a, ', ', sum_pos_error, ', ', sum_ori_error
def right_arm_fk_test():
motion = naoqi.ALProxy("ALMotion", host, port)
angle_set = [ [0] * 5,
[1.57, 0, 0, 0, 0],
[0, 0, 1.57, 0, 0],
[1.57, 0, 0, 0, 0],
[0, 0, 0, 0, 1.57],
[0, 0, 0, 1.57, 0],
[0, 0, 1.57, 1.57, 0],
[1.57, -1.57, 0, 0, 0],
[1.57, -1.57, 0, 0, -1.57],
[1.57, -1.57, 0, 0, 1.57],
[1.57, 0, 0, 0, 0],
]
for a in angle_set:
motion.setAngles(k.right_arm_tags, a, 1.0)
time.sleep(2.0)
pos0 = motion.getPosition("RArm", 0, True)
angles = motion.getAngles(k.right_arm_tags, True)
pos_, ori_ = k.right_arm_get_position(angles)
pos = pos0[0:3]
pos[0] = pos[0] + 0.057
pos[2] = pos[2] - 0.08682
pos_error = [p - p_ for p, p_ in zip(pos, pos_)]
sum_pos_error = 0
for p in pos_error:
sum_pos_error = sum_pos_error + math.fabs(p)
ori = pos0[3:]
Tx = k.transX(ori[0],0,0,0)
Ty = k.transY(-ori[1],0,0,0)
Tz = k.transZ(ori[2],0,0,0)
T = Tz.dot(Ty.dot(Tx))[0:3,0:3]
T_ = ori_
ori_error = T - T_
sum_ori_error = 0
for o in ori_error:
for v in o:
sum_ori_error = sum_ori_error + math.fabs(v)
pass
print 'Error(a, pos ori), ', a, ', ', sum_pos_error, ', ', sum_ori_error
def get_actual_position(self,
shoulder_pitch,
shoulder_roll,
elbow_yaw,
elbow_roll,
right_arm=False):
"""
get actual position of Pepper's hands
returns a list [x, y, z]
"""
if right_arm:
actual_wrist, _ = pk.right_arm_get_position(
[shoulder_pitch, shoulder_roll, elbow_yaw, elbow_roll, 0])
else:
actual_wrist, _ = pk.left_arm_get_position(
[shoulder_pitch, shoulder_roll, elbow_yaw, elbow_roll, 0])
actual_wrist[
2] += self.pepper_head_to_base_link_z # pk outputs position with respect to head frame, not base_link frame
actual_wrist[
0] += self.pepper_head_to_base_link_x # pk outputs position with respect to head frame, not base_link frame
actual_wrist = actual_wrist[:
-1] # pepper_kinematics has an extra 1 at the end that's not needed
return list(actual_wrist)
