--- Auto generated by 'aubo_scope' v0.29.2-beta.20 2024-10-29 10:43:20 --- 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_press() local _ENV = sched.select_robot(1) setCollisionStopType(1) setCollisionLevel(6) setHardwareCustomParameters("[over_force_config] \n is_enable =true") setFreedriveDamp({0.5,0.5,0.5,0.5,0.5,0.5}) INF = 9999 gripper_service = sched.jsonrpc.proxy('http://127.0.0.1:30500/jsonrpc') local api = sched.jsonrpc.proxy('http://127.0.0.1:8985/jsonrpc') setHomePosition({0,-0.2617993877991494,1.74532925199433,0.4363323129985824,1.570796326794897,0} ) modbusDeleteAllSignals() setDigitalInputActionDefault() setDigitalOutputRunstateDefault() setPayload(0.33, {-0.00119,-0.00146,0.0281}, {0,0,0}, {0,0,0,0,0,0,0,0,0}) setTcpOffset({0,0,0.1,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} 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} u8defu70b9_0_p = {-0.3903750166245932,0.2306297345403848,-0.01194580032789513,-3.141560720049292,5.685393622819493e-06,-1.829282920040596} u8defu70b9_0_q = {2.879259790982805,-0.1206321755245463,2.15977142333173,0.7095753606245877,1.570790518618636,-0.003846269177837802} u8defu70b9_1_p = {-0.3908722843562427,0.2307648895752932,0.07108534024286553,-3.141517635219312,9.56929057949904e-06,-1.829287396768598} u8defu70b9_1_q = {2.879256663714914,-0.1555832676553076,2.178684025198648,0.7633959850361512,1.570786469136163,-0.003844919172672944} u8defu70b9_3_p = {-0.3908721965624852,0.2307646268560812,0.07108320340050436,-3.141525041038945,1.094760923826745e-05,-1.829286269888038} u8defu70b9_3_q = {2.879257790583345,-0.1555855206605108,2.178691910514334,0.7634135344214434,1.570785119302402,-0.003844919164563415} u8defu70b9_2_p = {-0.3908713872643865,0.2307659156627358,0.06732044575455962,-3.141535410392009,1.098754594304124e-05,-1.829290946017775} u8defu70b9_2_q = {2.879254464613964,-0.1634121628215262,2.182924238209348,0.7754828736923115,1.57078511931245,-0.003843569117933659} setPlanContext(sched.current_thread_id(), 4, "机器人编程") setPlanContext(sched.current_thread_id(), 5, "直线运动") setPlanContext(sched.current_thread_id(), 7, "路点_1") moveLine(u8defu70b9_1_p, 0.02, 0.002, 0, 0) setPlanContext(sched.current_thread_id(), 8, "ForceControl_4") -- 前置函数 -- 阻塞等待力控结束 local function condFullfiled_4() local ret = 0 local realtime = require "aubo.realtime" local before_sync_time = realtime.time() local is_steady_state = 0 -- 循环判断终止条件 while not (isCondFullfiled()) do -- 判断是否达到目标力 local pose_in_base = getTcpPose() local base_force = getTcpForce() local force = forceTrans(poseInverse(pose_in_base), base_force) if ( (force[3] > -6 - 1) and (force[3] < -6 + 1) ) then textmsg("Z方向达到目标力-6N") 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, -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("终止条件初始化") -- 设置实时显示参考系下的力数据 setBool("script_run_flag", true) setBool("real_time_enable_flag", true) setString("reference_system_name", "TCP_1") textmsg("开启力控") sleep(0) fcEnable() textmsg("激活终止条件") setCondActive() -- 添加子节点 setPlanContext(sched.current_thread_id(), 11, "Successfully") function Successfully_4() setPlanContext(sched.current_thread_id(), 12, "等待：0") end setPlanContext(sched.current_thread_id(), 13, "Failure") function Failure_4() setPlanContext(sched.current_thread_id(), 14, "等待：0") end textmsg("等待力控结束") status = condFullfiled_4() textmsg("关闭力控") fcDisable() stopJoint(2) sleep(0) -- 关闭实时显示参考系下的力数据 setBool("real_time_enable_flag", false) setBool("script_run_flag", false) if ( status >= 0 ) then textmsg("力控成功") Successfully_4() else textmsg("力控失败") Failure_4() end setPlanContext(sched.current_thread_id(), 15, "直线运动") setPlanContext(sched.current_thread_id(), 16, "路点_3") moveLine(u8defu70b9_3_p, 0.2, 0.1, 0, 0) end function app:call_after_stop(name) if name == "rob1" then setPlanContext(sched.current_thread_id(), 3, "fcDisable()") --subscript context start fcDisable() --subscript context end end end function app:start(api) -- self.api = api print("start---") p_press() end function app:robot_error_handler(name, err) -- print("An error hanppen to robot "..name) end -- return our app object return app