10-23-2021, 02:43 PM(This post was last modified: 10-23-2021, 02:57 PM byHofbauec.)
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.
我已经试图移动机器人一点点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++).
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?
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""" 如果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
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
# 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
# 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
# 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()
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 如果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]) )
10-25-2021, 04:15 PM(This post was last modified: 10-25-2021, 04:16 PM byHofbauec.)
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)
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.
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?
10-27-2021, 01:27 PM(This post was last modified: 10-27-2021, 01:27 PM byAlbert.)
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.
