def test_shoulders(arm, torso):
# Shoulder pan joint
# Could be useful to see the individual joints. Note that using
# `arm.move_to_joints()` doesn't require MoveIt. If we adjust the 0-th
# indexed joint angle, that will rotate the arm like if we were rotating our
# arm going across at eye level, with the hand, wrist, elbow, etc., FIXED.
print("shoulder pan joint")
pose0 = [0, 0, 0, 0, 0, 0, 0]
pose0[0] = DEGS_TO_RADS * -92
arm.move_to_joints(ArmJoints.from_list(pose0))
pose0[0] = DEGS_TO_RADS * 92
arm.move_to_joints(ArmJoints.from_list(pose0))
# Shoulder LIFT joint, now imagine we have a straight arm (actually, it just
# has to be fixed, not straight...) but we move it up and down. :-)
print("shoulder LIFT joint")
pose1 = [0, 0, 0, 0, 0, 0, 0]
pose1[1] = DEGS_TO_RADS * -70
arm.move_to_joints(ArmJoints.from_list(pose1))
pose1[1] = DEGS_TO_RADS * 87 # ha, this is amusing
arm.move_to_joints(ArmJoints.from_list(pose1))
def test_reader(arm, reader):
names = ArmJoints.names()
arm_vals = reader.get_joints(names)
for k, v in zip(names, arm_vals):
print '{}\t{:.4f}'.format(k, v)
print("")
# Move and then read the joints again to be clear
pose = [0, 0, 0, 0, 0, 0, 0]
pose[1] = DEGS_TO_RADS * -70
arm.move_to_joints(ArmJoints.from_list(pose))
arm_vals = reader.get_joints(names)
for k, v in zip(names, arm_vals):
print '{}\t{:.4f}'.format(k, v)
print("")