我猜你的传感器就像触摸探头一样。我建议你在脚本上运行搜索算法，即使你的目标是创建一个插件。您可以在幕后触发Python脚本并收集结果。脚本会更好，因为你有更少的函数回调，你可以线性地执行你的机器人程序(这在插件上不容易做到，因为你不能执行阻塞机器人的运动)。
我附上了一些代码，展示如何使用Python代码运行二进制搜索算法。搜索是按1毫米的步长进行的，当发现一个接触点时，它开始将搜索分为两半，直到达到一定的公差。
我们在RoboDK API中也有一个搜索函数(SearchL)，您可以从Python脚本运行。另一方面，这种搜索算法需要在机器人控制器上进行额外的配置步骤。
代码:
定义posetoward (pose1, pose2, dist1_mm):
返回从pose1到pose2的距离为delta_mm的姿态"""
如果dist1_mm <= 0:
#越界/无效
Dist1_mm = 0
返回pose1
pose_delta = invH(pose1) * pose2
距离= norm3(pose_delta.Pos())
如果dist1_mm > distance:
#越界
Dist1_mm =距离
返回pose2
Factor = dist1_mm/distance
x,y,z,w,p,r = Pose_2_UR(pose_delta)
返回pose1 * UR_2_Pose([x*factor,y*factor,z*factor,w*factor,p*factor,r*factor])
def ProbePoint(xyzijk, r, poseref=eye(4)):
"""使用首选探测方法探测点""" "
如果这是真的:
返回ProbePointSteps(xyzijk, r, poseref)
其他:
返回ProbePointBinary(xyzijk, r, poseref)
def ProbePointBinary(pose_from, pose_to, r, search_step_max=1):
"""使用二进制搜索算法探测一个点，并返回具有碰撞点的关节。如果没有发现碰撞则返回None。
pose_from:来自姿态
pose_to:摆姿势
R:机器人物品
”“”
#稳定时间，以秒为单位
Pause_move = 0.25
探头的数字输入
Di_probe = 0
当探针激活时，DI ON设置为1，否则设置为0
Probe_on = 1
让机器人移动的最小增量(使其可重复性减半)
Min_increment = 0.005
#-------------------------------------------
Search_step = search_step_max
pose_distance =距离(pose_from.Pos()， pose_to.Pos())
#移动机器人到接近点
r.setSpeed (SpeedApproach SpeedApproach)
r.MoveL (pose_from)
#运行搜索
r.setSpeed (SpeedProbe SpeedProbe)
#----------------------------------
target关节=无
#定义第一个增量，shift_z包含相对于起点到第二个点的偏移量
Shift_z = search_step
Itcount = 0
Itcount_same = 0 #相同方向的计数器
shift_z_last =无
contact =无
contact_last =无
sense_last =无
而真正的:
#确保新点在可接受的范围内
Shift_z = min(max(Shift_z, 0)， pose_distance)
#计算要移动到的新姿势
pose_shift_z = PoseTowards(pose_from, pose_to, shift_z)
#移动机器人到新位置
r.MoveL (pose_shift_z)
#这应该引发一个异常，如果机器人有问题
#确认移动是可能的
#status, status_msg = robot.ConnectedState()
#if status != ROBOTCOM_READY:
# #如果连接不成功，停止
# printLog(“真正的机器人不可能移动”)
#继续
#强制延迟以获得更稳定的数据!!
暂停(PAUSE_MOVE)
#获取探针输入
contact_str = r.getDI(DI_PROBE)
contact_str中的contact = str(PROBE_ON)
#检测运动的感觉
如果联系else，感觉= 1.0
If sense != sense_last:
#重置在同一方向移动的计数器
Itcount_same = 0
Itcount = Itcount + 1
Itcount_same = Itcount_same + 1
#为步骤应用乘数因子
乘数= 1
如果contact_last不是None:
除了第一次迭代，我们总是会落在这里
乘数= 0.5
如果contact == contact_last:
乘数= 2
Sense_last = sense
Delta_move_abs = abs(search_step)
Shift_z = Shift_z + sense*delta_move_abs
Search_step = Search_step *乘数
检测迭代的结束:
print("%02i-Plane Z= %5.3f mm %s Step= %。3.f" % (itcount, shift_z, u'\u2191' if sense > 0 else u'\u2193', delta_move_abs))
如果contact and not contact_last and shift_z_last not None and delta_move_abs < 4*TOLERANCE_MM: # shift_z_last- t。< S.TOLERANCE_MM:
#终止标准，我们完成了，我们检测到一个有效点
target关节= r.关节().list()
# T。SHIFT_Z_START = T.shift_z
打印(“接触迭代:“)
print (str (targetJoints))
打破
如果itcount > 100:
print("已达到最大迭代次数!")
打破
Elif itcount_same > 10:
打印(“目标出界?”)
打破
shift_z_last = T.shift_z
Contact_last = contact
#确保我们没有迈出太小的一步
如果search_step < MIN_INCREMENT:
search_step = MIN_INCREMENT
Elif search_step > search_step_max
Search_step = search_step_max
#我们完成了。移动到接近点。
r.setSpeed (SpeedApproach SpeedApproach)
r.MoveL (pose_from)
返回targetJoints
def PoseSplit(pose1, pose2, delta_mm=1):
"""在给定delta_mm增量的2个姿势之间分割移动""" "
pose_delta = invH(pose1) * pose2
距离= norm(pose_delta.Pos())
如果distance <= delta_mm:
返回(pose2)
Pose_list = []
x,y,z,w,p,r = Pose_2_UR(pose_delta)
步骤= max(1,int(距离/delta_mm))
Xd = x/steps
Yd = y/步
Zd = z/步长
Wd = w/步长
Pd = p/步
Rd = r/steps
对于range(steps-1)中的I:
因子= i+1
pose_list。追加(pose1 * UR_2_Pose([xd*factor,yd*factor,zd*factor,wd*factor,pd*factor,rd*factor]))
返回pose_list
