Thread Rating:
  • 0 Vote(s) - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5

Executing multiple robot motions during a function call

#1
Hello Everyone,

I am currently working on a bed leveling system for a robotic 3D-printing station during the course of my master degree. I have already implemented every function necessary to level the actual print bed (creating probe points on irregular print surface, creating a mesh, interpolation of mesh, applying new z-values to G-Code). Therefore I used a button which calls a function with specific parameters declared above the button in the UI.

The last thing i need to implement seems to be quite a problem for my understanding of RoboDK. I need to move the robot down to the print bed (no problem) and as soon as a sensor sends a signal (via serial, input already works), the movement of the robot should stop and the next point should be probed.

I already tried to move the robot only a little bit at a time and check if the sensor is actve afterwards, but the main problem that prevents this implementation is, that the function which is called from the button only applies the motion, when the function is complete / ended. But i need to do a lot of probe points in this function inside a loop. I can't find a solution to move the robot without ending the function itself.

I am using ->runProgram(); where a lot of MoveL commands are implemented. But as I said, the actual "runProgram" only occurs at the end of the button call.

Is there a possibility to force the Robot to move inside of a button-function before the function reaches its end?

I hope someone can help me with my problem.

I am using the PluginExample as a base for my plugin (C++).
#2
I forgot to add:
A solution to my problem would be a script, which can run all the time in a constant loop even if the robot is performing a task.
Is something like that possible?
#3
I assume your sensor works like a touch probe. I recommend you to run the search algorithm on a script, even if your goal is to create a plugin. You can trigger a Python scripts behind the scenes and collect the result. A script is better as you'll have less function callbacks and you can execute your robot program linearly (which is not so easy to do on a plugin as you can't execute blocking robot movements).

I attached some code that shows how to run a binary search algorithm using Python code. The search is done by steps of 1 mm and when a contact is found it starts splitting the search by half until you meet a certain tolerance.

We also have a search function (SearchL) in the RoboDK API which you can run from a Python script. On the other hand, this search algorithm requires additional configuration steps on the robot controller.

Code:
def PoseTowards(pose1, pose2, dist1_mm):
   """Return the pose that is delta_mm far from pose1 towards pose2"""
   if dist1_mm <= 0:
       # out of bounds/invalid
       dist1_mm = 0
       return pose1

   pose_delta = invH(pose1) * pose2
   distance = norm3(pose_delta.Pos())
   if dist1_mm > distance:
       # out of bounds
       dist1_mm = distance
       return pose2

   factor = dist1_mm/distance
   x,y,z,w,p,r = Pose_2_UR(pose_delta)
   return pose1 * UR_2_Pose([x*factor,y*factor,z*factor,w*factor,p*factor,r*factor])
   
def ProbePoint(xyzijk, r, poseref=eye(4)):
   """Probe a point using the preferred probing method"""
   if True:
       return ProbePointSteps(xyzijk, r, poseref)
   else:
       return ProbePointBinary(xyzijk, r, poseref)


def ProbePointBinary(pose_from, pose_to, r, search_step_max=1):
   """Probe one point using a binary search algorithm and return the joints with the collision point. Returns None if no collision was found.
   pose_from: From pose
   pose_to: To pose
   r: robot item
   """

   # Stabilization time in seconds
   PAUSE_MOVE = 0.25

   # Digital input for the probe
   DI_PROBE = 0

   # Set to 1 for DI ON when probe activated or 0 otherwise
   PROBE_ON = 1

   # Minimum increment to make a robot move (make it half the repeatability of a robot)
   MIN_INCREMENT = 0.005

   #-------------------------------------------
   search_step = search_step_max
   pose_distance = distance(pose_from.Pos(), pose_to.Pos())

   # Move the robot to the approach point
   r.setSpeed(SpeedApproach, SpeedApproach)
   r.MoveL(pose_from)

   # Run the search
   r.setSpeed(SpeedProbe, SpeedProbe)
   


   #----------------------------------
   targetJoints = None

   # Define the first increment, shift_z contains the offset with respect to the start point towards the second point
   shift_z = search_step
   
   itcount = 0
   itcount_same = 0  # counter with the same direction
   
   shift_z_last = None
   contact = None
   contact_last = None
   sense_last = None
   while True:
       # Make sure the new point is within accepted boundaries
       shift_z = min(max(shift_z, 0), pose_distance)
       
       # Calculate the new pose to move to
       pose_shift_z = PoseTowards(pose_from, pose_to, shift_z)

       # Move the robot to the new position
       r.MoveL(pose_shift_z)
       
       # This should raise an exception if the robot had issues
       #Verify that the movement was possible
       #status, status_msg = robot.ConnectedState()
       #if status != ROBOTCOM_READY:
       #    # Stop if the connection did not succeed
       #    printLog("Move not possible for real robot")
       #    continue
       
       # Force delay to have more stable data!!
       pause(PAUSE_MOVE)

       # Get the probe input
       contact_str = r.getDI(DI_PROBE)
       contact = str(PROBE_ON) in contact_str

       # Detect the sense of motion
       sense = 1.0 if contact else -1.0    
       if sense != sense_last:
           # reset counter for movements in the same direction
           itcount_same = 0

       itcount = itcount + 1
       itcount_same = itcount_same + 1

       # Apply a multiplier factor for the step
       multiplier = 1
       if contact_last is not None:
           # We'll always fall here except for the first iteration
           multiplier = 0.5
           if contact == contact_last:
               multiplier = 2

       sense_last = sense
       delta_move_abs = abs(search_step)
       shift_z = shift_z + sense*delta_move_abs
           
       search_step = search_step * multiplier

       # Detect the end of the iteration:
       print("%02i-Plane Z= %5.3f mm %s Step= %.3f" % (itcount, shift_z, u'\u2191' if sense > 0 else u'\u2193', delta_move_abs))
       if contact and not contact_last and shift_z_last is not None and delta_move_abs < 4*TOLERANCE_MM: # shift_z_last-T.shift_z < S.TOLERANCE_MM:
           # Termination criteria, we are done, we detected a valid point
           targetJoints = r.Joints().list()
               
           #T.SHIFT_Z_START = T.shift_z
           print("Contact iteration:")
           print(str(targetJoints))
           break

       if itcount > 100:
           print("Reached maximum number of iteractions!")
           break

       elif itcount_same > 10:
           print("Target out of bounds?")                            
           break
       
       shift_z_last = T.shift_z
       contact_last = contact

       # Make sure we do not make a step too small
       if search_step < MIN_INCREMENT:
           search_step = MIN_INCREMENT
       elif search_step > search_step_max:
           search_step = search_step_max

   # We are done. Move to the approach point.
   r.setSpeed(SpeedApproach, SpeedApproach)
   r.MoveL(pose_from)

   return targetJoints
   
   
def PoseSplit(pose1, pose2, delta_mm=1):
   """Split the move between 2 poses given delta_mm increments"""
   pose_delta = invH(pose1) * pose2
   distance = norm(pose_delta.Pos())    
   if distance <= delta_mm:
       return [pose2]

   pose_list = []
   
   x,y,z,w,p,r = Pose_2_UR(pose_delta)        
   
   steps = max(1,int(distance/delta_mm))  

   xd = x/steps
   yd = y/steps
   zd = z/steps
   wd = w/steps
   pd = p/steps
   rd = r/steps
   for i in range(steps-1):
       factor = i+1
       pose_list.append( pose1 * UR_2_Pose([xd*factor,yd*factor,zd*factor,wd*factor,pd*factor,rd*factor]) )
       
   return pose_list
#4
I already imlemented this method in my program. Thank you very much you saved my day it works perfectly.

Is there a method to jump to the next instruction in a already running program in RoboDK via python? That would make my application failsafe proof. (I have put a picture of what I want to achive in the attachement)


Attached Files Image(s)
   
#5
It is great to hear you were able to implement this with your project.

Currently there is no API to start a program at a specific instruction but we'll implement it so you can start at an instruction given its id. Example:

Code:
program.setParam("Start", id)
The update should be available in less than 1 week.
#6
Thank you very much, that would be a great addition!

But will it be possible to jump directly to a new line with this addition during the active program, or do I need to stop the program, set the start ID and then restart it?
#7
Yes, you should be able to start the program on a new instruction even if it already running. However, the robot may complete running the current instruction before jumping to the new one.
#8
Thank you again for the update.

I already tried it and works as expected. The robot now moves to the next probe point as soon as the probing sensor activates.
  




Users browsing this thread:
1 Guest(s)