Executing multiple robot motions during a function call - Printable Version

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_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