[dunderMethods]

Here is my new code. It gives me the results I want! But it seems overly complicated.

I'm rotating the tool then modifying the TCP so that it aligns with the picture I want to show and the camera target.

I'm open to suggestions for a more elegant way to do this - or let me know if this is actually the right way.

Code:

def set_picture(self, new_pic_idx):
""" Rotate the end effector so that the given new_pic_idx is the picture that will be shown to the camera """
if 0 < new_pic_idx <= self.num_pic_idx and isinstance(new_pic_idx, int): # If the new picture index is valid
new_pic_angle = ((self.current_pic_idx - 1) * -self.deg_per_idx) - ((new_pic_idx - 1) * -self.deg_per_idx)
new_tcp_angle = (new_pic_idx - 1) * -self.deg_per_idx # I don't know why I have to calculate this differently but it works

# If the angle is 180 it confuses the robot so we divide the move into 2x -90deg moves (+90 causes problems)
if abs(new_pic_angle) == 180:
for _ in range(2):
half_angle = -90
curr_pose = self.robot.Pose() # Get the robot's current pose
new_pose = robomath.rotz(half_angle * self.d2r) # Generate the rotation matrix
self.robot.MoveJ(curr_pose * new_pose) # Move the robot to the new pose
else:
curr_pose = self.robot.Pose() # Get the robot's current pose
new_pose = robomath.rotz(new_pic_angle * self.d2r) # Generate the rotation matrix
self.robot.MoveJ(curr_pose * new_pose) # Move the robot to the new pose

tcp_pose = robomath.rotz(new_tcp_angle * self.d2r) # Generate a rotation matrix for the TCP
self.robot.setPoseTool(tcp_pose) # Set the new TCP
self.robot.WaitMove(timeout=5000)
self.current_pic_idx = new_pic_idx # Update the current picture index