[dunderMethods]

Hi, I'm programming a UR10 to repeatedly move back and forth between targets A and B with a hexagon-shaped "tool" that has a picture on each of it's 6 sides. At target B there is a camera which will look at the picture. Every N cycles I want to rotate the tool to a different picture while it is at target A (out of the camera's view). After rotating the tool the robot repeats the cycle with the new picture.

The problem I am having is the tool always rotates to the desired image WHILE it moves to target B and rotates back to the first image WHILE it returns to target A.

Instead, I need it to:

1. Start at target A
   
2. Rotate tool to the desired image
   
3. Move to target B while maintaining orientation
   
4. Return to target A while maintaining orientation
   
5. Repeat steps 3 and 4 N times
   
6. Rotate tool to the next desired image (while at target A)
   
7. Repeat steps 3-6 until all images have been shown.
   

I have a simple program called "show_from_right" defined within the RoboDK GUI which executes the moves between A and B. I'm using Python to call this program and run the code that changes which image is shown in between program calls. The images below (next post) show the setup. Here is my code for changing which picture is shown (it runs in a wrapper around the API)

Code:

def set_picture(self, new_pic_idx):
""" Rotate the tool so that the given new_pic_idx is the picture that will be shown to the camera """
    new_pic_angle        = (new_pic_idx - 1) * -self.deg_per_idx      # Calculate the new picture angle
    new_pose            = robomath.rotz(new_pic_angle * self.d2r) # Calculate the new pose (d2r converts deg to radians)
    result              = self.robot.setPoseTool(new_pose)           # Move the robot to the new pose
    self.current_pic_idx = new_pic_idx                                # Update the current picture index
    self.robot.WaitMove(timeout=5000)

Hopefully someone can help me understand what I am doing wrong here!
   
   

[Albert]

setPoseTool is not a command that will provoke a robot movement. This function only updates the Tool (TCP) in RoboDK and the real robot if you are connected with the driver.

You should move the robot using a MoveJ or a MoveL command. Example:

Code:

...
robot.setPoseTool(new_tool) # Update the TCP
...
robot.MoveJ(new_pose) # Move the robot. Blocking by default

[dunderMethods]

Thanks for the reply! I've updated my code and here's where I am at now:

The tool instantly rotates to the selected image when I call set_picture(pic_idx) from python (jupyter), great!


Any time I use set_picture to rotate the tool 180deg in one move (moving from picture 1 -> 4 for example) the robot goes crazy and tries to twist itself into a pretzel. I can make incremental moves (1 -> 3 -> 5) that when summed exceed 180 just fine; or if I go 1 -> 6 it turns the opposite direction with no issues. I even tried adding some logic to add or subtract 1deg if the new angles is  abs(180) resulting in -179deg or 179deg and this still causes the robot to go crazy. What am I doing wrong here?


Here is the updated code:

Code:

self.deg_per_idx = 360/6           # Divide 360deg by the 6 sides of the hexagon
self.d2r = 0.017453292519943295    # Myltiply by this value to convert degrees to radians

def set_picture(self, new_pic_idx):
""" Rotate the tool so that the given new_pic_idx is the picture that will be shown to the camera """
   # The next line calculates how many degrees to rotate from the current pic_idx to the new_pic_idx
   new_pic_angle        = ((self.current_pic_idx - 1) * -self.deg_per_idx) - ((new_pic_idx - 1) * -self.deg_per_idx)
   curr_pose            = self.robot.Pose()                            # Get the robot's current pose
   new_pose             = robomath.rotz(new_pic_angle * self.d2r)      # Calculate the new pose (d2r converts deg to radians)
   result               = self.robot.MoveJ(curr_pose * new_pose)       # Move the robot to the new pose
   self.current_pic_idx = new_pic_idx                                  # Update the current picture index
   self.robot.WaitMove(timeout=5000)

[dunderMethods]

I decided to break the single 180deg rotation into two 90deg rotations which does work without the robot going off the rails. It's not ideal because it pauses half way through the rotation but that won't interfere with my testing so I'll just use it this way.

[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