--- Auto generated by 'aubo_scope' v0.29.3-rc.7 2024-12-17 09:31:02 --- DO NOT MODIFY IT! local app = {} local aubo = require('aubo') local sched = sched or aubo.sched local math = aubo.math or math local realtime = aubo.realtime or realtime local sleep = sched.sleep local thread = sched.thread local sync = sched.sync local run = sched.run local kill = sched.kill local halt = sched.halt app.PRIORITY = 1000 -- set the app priority, which determines app execution order app.VERSION = "0.1" app.VENDOR = "Aubo Robotics" function p_search_hole() local _ENV = sched.select_robot(1) setCollisionStopType(1) setCollisionLevel(6) setFreedriveDamp({0.26,0.28,0.29,0.29,0.29,0.3}) INF = 9999 gripper_service = sched.jsonrpc.proxy('http://127.0.0.1:30500/jsonrpc') setHomePosition({0,-0.2617993877991494,1.74532925199433,0.4363323129985824,1.570796326794897,0} ) modbusDeleteAllSignals() setDigitalInputActionDefault() setDigitalOutputRunstateDefault() setPayload(0.33, {0,0,0.06}, {0,0,0}, {0,0,0,0,0,0,0,0,0}) setTcpOffset({0,0,0.12,0,0,0}) setToolVoltageOutputDomain(24) setToolIoInput(0, true) setToolIoInput(1, true) setToolIoInput(2, true) setToolIoInput(3, true) u5e73u9762_0= {-0.08588358002836446,-0.5674440021646662,-0.1081947151074858,3.128722971588715,0.0309080693545616,1.572465810756998} hikvision_feature= {0,0,0,0,0,0} hikvision_feature_0= {0,0,0,0,0,0} hikvision_feature_1= {0,0,0,0,0,0} hikvision_feature_2= {0,0,0,0,0,0} hikvision_feature_3= {0,0,0,0,0,0} hikvision_feature_4= {0,0,0,0,0,0} hikvision_feature_5= {0,0,0,0,0,0} hikvision_feature_6= {0,0,0,0,0,0} hikvision_feature_7= {0,0,0,0,0,0} hikvision_feature_8= {0,0,0,0,0,0} hikvision_feature_9= {0,0,0,0,0,0} hikvision_feature_10= {0,0,0,0,0,0} hikvision_feature_11= {0,0,0,0,0,0} hikvision_feature_12= {0,0,0,0,0,0} u5e73u9762_1= {} Point_judgment = false setPlanContext(sched.current_thread_id(), 1, "初始变量") Base = {0,0,0,0,0,0} Tool = {0,0,0,0,0,0} u5e73u9762_0 = {-0.0858835800283645,-0.567444002164666,-0.108194715107486,3.12872297158871,0.0309080693545616,1.572465810757} hikvision_feature = {0,0,0,0,0,0} hikvision_feature_0 = {0,0,0,0,0,0} hikvision_feature_1 = {0,0,0,0,0,0} hikvision_feature_2 = {0,0,0,0,0,0} hikvision_feature_3 = {0,0,0,0,0,0} hikvision_feature_4 = {0,0,0,0,0,0} hikvision_feature_5 = {0,0,0,0,0,0} hikvision_feature_6 = {0,0,0,0,0,0} hikvision_feature_7 = {0,0,0,0,0,0} hikvision_feature_8 = {0,0,0,0,0,0} hikvision_feature_9 = {0,0,0,0,0,0} hikvision_feature_10 = {0,0,0,0,0,0} hikvision_feature_11 = {0,0,0,0,0,0} hikvision_feature_12 = {0,0,0,0,0,0} u8defu70b9_0_p = {0.4561790667937841,-0.07790020236364917,0.4110597392268388,-3.141588292520342,2.883452277897832e-05,1.67754119865599} u8defu70b9_0_q = {0.09653453202184273,0.04809159798242754,1.36199761542306,-0.2568943757912348,1.570767449247892,-0.01021033971766238} u8defu70b9_1_p = {-0.3870576579615208,-0.05249922349025658,0.07269517148020611,-3.141093904392234,-0.0007874893020196061,2.311484110215758} u8defu70b9_1_q = {-0.1814474104253216,-0.04513467842756082,-2.318572414306411,-0.7035702584239384,-1.57087461483238,2.219457227004868} setPlanContext(sched.current_thread_id(), 2, "在开始之前") setPlanContext(sched.current_thread_id(), 3, "PRE_HEAD_FUNCTION.lua") --subscript context start M_PI = 3.1415926 --[[ ------------------------------ 计算两点间距离 ------------------------------ --]] Point_judgment = false --计算两个位姿的位置距离 function distance_cal(p1, p2) local distance = poseDistance(p1, p2); return distance end --获取当前位姿，并与传进来的位姿做求distance，返回distance function get_distance_from_start(pre_p) sleep(0.1) local now_pose = getTcpPose() local result = distance_cal(now_pose, pre_p) return result end --END计算两点间距离 --[[ ------------------------------ 运动相关 ------------------------------ --]] --输入用户坐标系相对于tcp的偏移、用户坐标系下的坐标；输出base下的坐标 function targetPoseinUsercoordToBase(usercoord_in_tcp,target_pose_in_usercoord) local tcp_pose = getTcpPose() local usercoord_in_base = poseTrans(tcp_pose, usercoord_in_tcp) local target_pose_in_base = poseTrans(usercoord_in_base, target_pose_in_usercoord) return target_pose_in_base end --在用户坐标系(相对与tcp)的Y轴上进行左右来回摇摆移动，distance为摆幅，search_nums为一共摇摆几次 function searchY_move(usercoord_in_tcp,distance,search_nums,a,v) for i=1 ,search_nums do local target_pose_in_usercoord1 = {0.0, -distance, 0.0, 0.0, 0.0, 0.0} local target_pose_in_base1 = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord1) moveLine(target_pose_in_base1, a, v, 0, 0) local target_pose_in_usercoord2 = {0.0, distance, 0.0, 0.0, 0.0, 0.0} local target_pose_in_base2 = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord2) moveLine(target_pose_in_base2, a, v, 0, 0) end end --在用户坐标系(相对与tcp)的X轴上进行左右来回摇摆移动，distance为摆幅，search_nums为一共摇摆几次 function searchX_move(usercoord_in_tcp,distance,search_nums,a,v) for i=1 ,search_nums do local target_pose_in_usercoord1 = {-distance, 0.0, 0.0, 0.0, 0.0, 0.0} local target_pose_in_base1 = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord1) moveLine(target_pose_in_base1, a, v, 0, 0) sleep(0) local target_pose_in_usercoord2 = {distance, 0.0, 0.0, 0.0, 0.0, 0.0} local target_pose_in_base2 = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord2) moveLine(target_pose_in_base2, a, v, 0, 0) sleep(0) end end --在用户坐标系(相对与tcp)的X轴上移动distance function usercoord_X_move(usercoord_in_tcp,distance,a,v) local target_pose_in_usercoord = {distance, 0.0, 0.0, 0.0, 0.0, 0.0} local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord) moveLine(target_pose_in_base, a, v, 0, 0) end --在用户坐标系(相对与tcp)的Y轴上移动distance function usercoord_Y_move(usercoord_in_tcp,distance,a,v) local target_pose_in_usercoord = {0.0, distance, 0.0, 0.0, 0.0, 0.0} local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord) moveLine(target_pose_in_base, a, v, 0, 0) end --在用户坐标系(相对与tcp)的Z轴上移动distance function usercoord_Z_move(usercoord_in_tcp,distance,a,v) local target_pose_in_usercoord = {0.0, 0.0, distance, 0.0, 0.0, 0.0} local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord) moveLine(target_pose_in_base, a, v, 0, 0) end --在用户坐标系(相对与tcp)的Z轴上移动distance function usercoord_XYZ_move(usercoord_in_tcp,dis_x,dis_y,dis_z,a,v) local target_pose_in_usercoord = {dis_x, dis_x, dis_z, 0.0, 0.0, 0.0} local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp, target_pose_in_usercoord) moveLine(target_pose_in_base, a, v, 0, 0) end function move_Spiral() -- 螺旋线第一圈半径 SpiralParameters = { spiral = 0.01, -- 螺旋线步长 helix = 0.0, -- 平面螺旋 angle = 2 * 2 * M_PI, -- 旋转2圈 plane = 0, frame = getTcpPose() } --SpiralParameters.frame[3] = SpiralParameters.frame[3] - 0.001 local a = 0.005 local v = 0.005 radius = 0.02 -- 螺旋线第一圈半径 target_pose = getTcpPose() --计算螺旋轨迹起点 -- target_pose[1] = math.sqrt(2) * radius / 2 + target_pose[1] --target_pose[2] = math.sqrt(2) * radius / 2 + target_pose[2] --target_pose[3] = target_pose[3] + 0.004 target_pose[1] = target_pose[1] + 0.001 --target_pose[2] = target_pose[2] + 0.005 --target_pose[3] = target_pose[3] + 0.004 v = 0.005 a = 0.1 moveLine(target_pose, a, v, 0, 0) textmsg(a1) sleep(0) --textmsg(a2) -- 螺旋运动 moveSpiral(SpiralParameters, 0, 0.01, 0.1, 0) end --[[ --示例 -- 用户坐标系相对于tcp的偏移 local usercoord_in_tcp = { 0, 0, 0.0, 0, 0, 0 } local distance = 0.001 local search_nums = 5 local a = 0.005 local v = 0.005 searchY_move(usercoord_in_tcp, distance, search_nums, a, v) --]] --END运动相关 --subscript context end sleep(0) setPlanContext(sched.current_thread_id(), 6, "机器人编程") setPlanContext(sched.current_thread_id(), 7, "直线运动") setPlanContext(sched.current_thread_id(), 8, "路点_0") moveLine(u8defu70b9_0_p, 0.08, 0.02, 0, 0) setPlanContext(sched.current_thread_id(), 10, "ForceControl_1") -- 前置函数 -- 阻塞等待力控结束 local function condFullfiled_1() local ret = 0 local realtime = require "aubo.realtime" local before_sync_time = realtime.time() local is_steady_state = true local start_tcp_pose = getTcpPose() -- 循环判断终止条件 while not (isCondFullfiled()) do -- 判断是否达到目标力 local pose_in_base = {0.463247, -0.0820509, 0.404793, 3.14159, 3.0332e-05, 1.67753} local base_force = getTcpForce() local force = forceTrans(poseInverse(pose_in_base), base_force) if ( (force[3] > -2 - 0.5) and (force[3] < -2 + 0.5) ) then textmsg("Z方向达到目标力-2N") ret = 0 break end sync() end if (isCondFullfiled()) then textmsg("触发防护停止") ret = -1 end return ret end textmsg("力控初始化") M = {20.0, 20.0, 20.0, 10.0, 10.0, 10.0} D = {0, 0, 1000, 0, 0, 0} K = {0, 0, 0, 0, 0, 0} enable = {false, false, true, false, false, false} goal_wrench = {0, 0, -2, 0, 0, 0} frame_type = 4 feature = {0, 0, 0, 0, -0, 0} speed_limits = {0, 0, 0, 0, 0, 0} setDynamicModel(M, D, K) setTargetForce(feature, enable, goal_wrench, speed_limits, frame_type) textmsg("终止条件初始化") -- 设置实时显示参考系下的力数据 setBoolOutput(64, true) setBoolOutput(65, true) setString("reference_system_name", "TCP") textmsg("开启力控") sleep(0) fcEnable() textmsg("激活终止条件") setCondActive() -- 添加子节点 setPlanContext(sched.current_thread_id(), 11, "Successfully") function Successfully_1() setPlanContext(sched.current_thread_id(), 12, "等待：1.00s") sleep(1) end setPlanContext(sched.current_thread_id(), 13, "Failure") function Failure_1() setPlanContext(sched.current_thread_id(), 14, "等待：1.00s") sleep(1) end textmsg("等待力控结束") status = condFullfiled_1() textmsg("关闭力控") fcDisable() stopJoint(2) sleep(0) -- 关闭实时显示参考系下的力数据 setBoolOutput(65, false) setBoolOutput(64, false) if ( status >= 0 ) then textmsg("力控成功") Successfully_1() else textmsg("力控失败") Failure_1() end setPlanContext(sched.current_thread_id(), 15, "ForceControl_2") -- 前置函数 -- 阻塞等待力控结束 local function condFullfiled_2() local ret = 0 local realtime = require "aubo.realtime" local before_sync_time = realtime.time() local is_steady_state = true local start_tcp_pose = getTcpPose() -- 循环判断终止条件 while not (isCondFullfiled()) do -- 参考轨迹运行完成并且力控完成调整 if (is_steady_state == true) then is_steady_state = isSteady() end if (is_steady_state == false) and (getQueueSize() <= 0) and (isSteady() == true) then ret = 0 textmsg(" 机械臂到位 !") break end sync() end if (isCondFullfiled()) then textmsg("触发防护停止") ret = -1 end return ret end textmsg("力控初始化") M = {20.0, 20.0, 20.0, 10.0, 10.0, 10.0} D = {1000, 1000, 2000, 0, 0, 0} K = {0, 0, 0, 0, 0, 0} enable = {true, true, true, false, false, false} goal_wrench = {0, 0, -4, 0, 0, 0} frame_type = 4 feature = {0, 0, 0, 0, -0, 0} speed_limits = {0, 0, 0, 0, 0, 0} setDynamicModel(M, D, K) setTargetForce(feature, enable, goal_wrench, speed_limits, frame_type) textmsg("终止条件初始化") min_force = -2 max_force = 2 select = {0.0, 0.0, 1.0, 0.0, 0.0, 0.0} type1 = 4.0 feature1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0} timeout = -1 count = 1.0 outside = 0 args = {} table.insert(args, min_force) table.insert(args, max_force) for i = 1, 6 do table.insert(args, select[i]) end for i = 1, 6 do table.insert(args, feature1[i]) end table.insert(args, type1) table.insert(args, count) table.insert(args, outside) setCondAdvanced('ConditionContactForceOnUsercoord', args, timeout) -- 设置实时显示参考系下的力数据 setBoolOutput(64, true) setBoolOutput(65, true) setString("reference_system_name", "TCP") textmsg("开启力控") sleep(0) fcEnable() textmsg("激活终止条件") setCondActive() -- 添加子节点 setPlanContext(sched.current_thread_id(), 16, "move_Spiral()") --subscript context start move_Spiral() --subscript context end setPlanContext(sched.current_thread_id(), 17, "Successfully") function Successfully_2() setPlanContext(sched.current_thread_id(), 18, "等待：1.00s") sleep(1) end setPlanContext(sched.current_thread_id(), 19, "Failure") function Failure_2() setPlanContext(sched.current_thread_id(), 20, "等待：1.00s") sleep(1) end textmsg("等待力控结束") status = condFullfiled_2() textmsg("关闭力控") fcDisable() stopJoint(2) sleep(0) -- 关闭实时显示参考系下的力数据 setBoolOutput(65, false) setBoolOutput(64, false) if ( status >= 0 ) then textmsg("力控成功") Successfully_2() else textmsg("力控失败") Failure_2() end setPlanContext(sched.current_thread_id(), 21, "textmsg(\"aaaa\")") --subscript context start textmsg("aaaa") --subscript context end setPlanContext(sched.current_thread_id(), 22, "ForceControl_3") -- 前置函数 -- 阻塞等待力控结束 local function condFullfiled_3() local ret = 0 local realtime = require "aubo.realtime" local before_sync_time = realtime.time() local is_steady_state = true local start_tcp_pose = getTcpPose() -- 循环判断终止条件 while not (isCondFullfiled()) do -- 参考轨迹运行完成并且力控完成调整 if (is_steady_state == true) then is_steady_state = isSteady() end if (is_steady_state == false) and (getQueueSize() <= 0) and (isSteady() == true) then ret = 0 textmsg(" 机械臂到位 !") break end sync() end if (isCondFullfiled()) then textmsg("触发防护停止") ret = -1 end return ret end textmsg("力控初始化") M = {20.0, 20.0, 20.0, 10.0, 10.0, 10.0} D = {1000, 1000, 2000, 0, 0, 0} K = {0, 0, 0, 0, 0, 0} enable = {true, true, true, false, false, false} goal_wrench = {0, 0, -6, 0, 0, 0} frame_type = 4 feature = {0, 0, 0, 0, -0, 0} speed_limits = {0, 0, 0, 0, 0, 0} setDynamicModel(M, D, K) setTargetForce(feature, enable, goal_wrench, speed_limits, frame_type) textmsg("终止条件初始化") -- 设置实时显示参考系下的力数据 setBoolOutput(64, true) setBoolOutput(65, true) setString("reference_system_name", "TCP") textmsg("开启力控") sleep(0) fcEnable() textmsg("激活终止条件") setCondActive() -- 添加子节点 setPlanContext(sched.current_thread_id(), 23, "Successfully") function Successfully_3() setPlanContext(sched.current_thread_id(), 24, "弹窗 消息：success") sched.sync_program_point() clearNamedVariable("_popup_continue_or_stop") do local _popup_t = getControlSystemTime() popup(TraceLevel.INFO, "Info", "success", 1) while not variableUpdated("_popup_continue_or_stop", _popup_t) do sync() end if not getBool("_popup_continue_or_stop", true) then halt() end end end setPlanContext(sched.current_thread_id(), 25, "Failure") function Failure_3() setPlanContext(sched.current_thread_id(), 26, "弹窗 消息：failure") sched.sync_program_point() clearNamedVariable("_popup_continue_or_stop") do local _popup_t = getControlSystemTime() popup(TraceLevel.INFO, "Info", "failure", 1) while not variableUpdated("_popup_continue_or_stop", _popup_t) do sync() end if not getBool("_popup_continue_or_stop", true) then halt() end end end textmsg("等待力控结束") status = condFullfiled_3() textmsg("关闭力控") fcDisable() stopJoint(2) sleep(0) -- 关闭实时显示参考系下的力数据 setBoolOutput(65, false) setBoolOutput(64, false) if ( status >= 0 ) then textmsg("力控成功") Successfully_3() else textmsg("力控失败") Failure_3() end end function app:call_after_stop(name) if name == "rob1" then setPlanContext(sched.current_thread_id(), 5, "fcDisable()") --subscript context start fcDisable() --subscript context end end end function app:start(api) -- self.api = api print("start---") p_search_hole() end function app:robot_error_handler(name, err) -- print("An error hanppen to robot "..name) end -- return our app object return app