--- Auto generated by 'aubo_scope' v0.29.1-rc.25 2024-07-09 15:15:32 --- DO NOT MODIFY IT! local app = {} local aubo = require('aubo') local sched = sched or aubo.sched local math = aubo.math or math 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_circle() 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}) setHomePosition({0,-0.2617993877991494,1.74532925199433,0.4363323129985824,1.570796326794897,0} ) modbusDeleteAllSignals() setDigitalInputActionDefault() setDigitalOutputRunstateDefault() setPayload(0, {0,0,0}, {0,0,0}, {0,0,0,0,0,0,0,0,0}) setTcpOffset({0,0,0,0,0,0}) setToolVoltageOutputDomain(0) setToolIoInput(0, true) setToolIoInput(1, true) setToolIoInput(2, true) setToolIoInput(3, true) local function add(num1,num2) return num1 + num2 end local function subtract(num1,num2) return num1 - num2 end local function multiply(num1,num2) return num1 * num2 end local function divide(num1,num2) return num1 / num2 end u5faau73af_1 = nil u5faau73af_2 = nil u5faau73af_3 = nil u5faau73af_4 = nil u5faau73af_5 = nil u5faau73af_0 = nil setPlanContext(sched.current_thread_id(), 1, "初始变量") u57fau5ea7 = {0,0,0,0,0,0} u5de5u5177 = {0,0,0,0,0,0} u8defu70b9_2_p = {-0.06226759754458359,-0.572573838235227,0.4282968131634991,-3.141464710923259,-7.33294465231698e-05,0.1075397968236509} u8defu70b9_2_q = {-1.466593814783879,-0.2492322959743556,1.027680920698606,-0.2940112967971265,1.570869228852143,-0.003337294176152524} u8defu70b9_0_p = {-0.06226277063844807,-0.5725850054608637,0.3903647815630276,3.141484142894835,-6.849092344526838e-05,0.1075409516795247} u8defu70b9_0_q = {-1.46658592654475,-0.2243989923029977,1.16296872933686,-0.1833203231737763,1.570865178737851,-0.003330543985666779} u8defu70b9_1_p = {-0.06226277313033934,-0.572585005338206,0.3903647816057852,3.141484142894704,-6.849092346828348e-05,0.1075409473054598} u8defu70b9_1_q = {-1.46657321206923,-0.2130259073847872,1.454794238429169,0.09713210187616188,1.570865177360647,-0.003317825136126215} u8defu70b9_4_p = {-0.04635556316346735,-0.570867703534344,0.3903658774358511,3.141484142894702,-6.849092346682807e-05,0.1075409473054574} u8defu70b9_4_q = {-1.438071684210293,-0.2049551772881162,1.464688270709085,0.09895732216193465,1.570862063588568,0.0251837026172741} u8defu70b9_5_p = {-0.04463887782615986,-0.5867692043297736,0.3903676129447177,3.141484142894701,-6.849092346556091e-05,0.1075409473054551} u8defu70b9_5_q = {-1.438771214339771,-0.2445176269526063,1.41551249699572,0.08934395210123447,1.570862140661368,0.02448417249053058} target_pose_p = {-0.06226261123347823,-0.572585206509827,0.3883648366609129,3.141484142894818,-6.849092345072349e-05,0.1075409515739362} target_pose_q = {-1.359446866284224,0.1369508260992485,2.446839645326057,0.7392075161629097,1.570853204899843,0.1038085159239009} setPlanContext(sched.current_thread_id(), 106, "线程_0") handler_u7ebfu7a0b_0=thread(function() while true do setPlanContext(sched.current_thread_id(), 107, "线程内计算力控运行时间") -- 线程内计算力控运行时间 setPlanContext(sched.current_thread_id(), 108, "循环_0 ") u5faau73af_0 = 1 while true do setPlanContext(sched.current_thread_id(), 109, "t1 = os.time()") --subscript context start t1 = os.time() --subscript context end sched.sync_program_point() if (condition_timer_is_counting == true) then setPlanContext(sched.current_thread_id(), 110, "如果 condition_timer_is_counting == true") setPlanContext(sched.current_thread_id(), 111, "time = t1 - t0") --subscript context start time = t1 - t0 --subscript context end end :: end_of_u5faau73af_0:: u5faau73af_0 = u5faau73af_0 + 1 sched.cancel_point() end -- 线程内计算力控运行时间 sched.cancel_point() end end, "u7ebfu7a0b_0") run(handler_u7ebfu7a0b_0) setPlanContext(sched.current_thread_id(), 4, "机器人编程") setPlanContext(sched.current_thread_id(), 5, "设置工具负载偏置和初始化参数") -- 设置工具负载偏置和初始化参数 setPlanContext(sched.current_thread_id(), 6, "force_offset={10.78, 0.36, 3.45, 0.01, -0.11, 0.01}") --subscript context start force_offset={10.78, 0.36, 3.45, 0.01, -0.11, 0.01} --subscript context end setPlanContext(sched.current_thread_id(), 7, "move_init.lua") --subscript context start INF = 9999 M_PI = 3.1415926 start_record = false function equal(q1, q2) eps = 0.001 local q1_size = 0 for k, v in pairs(q1) do q1_size = q1_size + 1 end local q2_size = 0 for k, v in pairs(q2) do q2_size = q2_size + 1 end if (not (q1_size == 0) and (q1_size == q2_size)) then for i = 1, 6 do if (math.sqrt((q1[i] - q2[i]) ^ 2) > eps) then return false end end return true end return false end function init() timeout = -1 ret = 0 realtime = require "aubo.realtime" sleep(0) -- 等待前面动作全部执行完成 setCollisionLevel(0) -- 选择传感器 selectTcpForceSensor("embedded") --selectTcpForceSensor("xinjingcheng_ftsensor") -- 传感器安装位姿 sensor_pose = {0, 0, 0, 0, 0, 0} --sensor_pose = {0, 0, 0.028, 0, 0, 0} setTcpForceSensorPose(sensor_pose) -- 传感器偏置 setTcpForceOffset(force_offset) usercoord_in_base = getTcpPose() -- 力控坐标系选择 用户坐标系 feature = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0} frame_type = 2 -- 力控速度限制 speed_limits = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0} -- 设置力控参数 setDynamicModel(M, D, K) -- 设置目标力 setTargetForce(feature, enable, goal_wrench, speed_limits, frame_type) min_force = -INF select = {0.0, 0.0, 1.0, 0.0, 0.0, 0.0} feature1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0} type1 = 2 timeout = -1 count = 1.0 outside = 1.0 args = {} table.insert(args, min_force) table.insert(args, 8) 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) end function startForceControl() start_record = true sleep(0) fcEnable() -- 激活终止条件 setCondActive() end function stopForceControl() stopJoint(2) sleep(0) fcDisable() end -- CSV 文件路径 local currentTime = os.date("%Y-%m-%d %H.%M.%S") filename = "test_data.csv" local data = { {"Program start time", currentTime}, { "pos_x", "pos_y", "pos_z", "pos_rx", "pos_ry", "pos_rz", "vel_x", "vel_y", "vel_z", "vel_rx", "vel_ry", "vel_rz", "force_x", "force_y", "force_z", "force_rx", "force_ry", "force_rz" } } -- 写入数据到 CSV 文件 file = io.open(filename, "w") for _, row in ipairs(data) do file:write(table.concat(row, ",") .. "\n") end file:flush() textmsg("机器人数据写入到 " .. filename) handler_record = thread( function() while true do if (start_record) then local tcp_pose = getTcpPose() local tcp_speed = getTcpSpeed() local tcp_force = getTcpForce() local frame_force=forceTrans(poseInverse(tcp_pose), tcp_force); file:write(table.concat(tcp_pose, ", ").. ", ") file:write(table.concat(tcp_speed, ", ").. ", ") file:write(table.concat(tcp_force, ", ") .. "\n") file:flush() end sync() end end, "handler_record" ) run(handler_record) --subscript context end -- 设置工具负载偏置和初始化参数 setPlanContext(sched.current_thread_id(), 8, "运动到初始点") -- 运动到初始点 setPlanContext(sched.current_thread_id(), 9, "直线运动") setPlanContext(sched.current_thread_id(), 10, "路点_2") moveLine(u8defu70b9_2_p, 0.5, 0.01, 0, 0) setPlanContext(sched.current_thread_id(), 11, "路点_0") moveLine(u8defu70b9_0_p, 0.5, 0.01, 0, 0) -- 运动到初始点 setPlanContext(sched.current_thread_id(), 12, "设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable") -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable setPlanContext(sched.current_thread_id(), 13, "target_force = 1") --subscript context start target_force = 1 --subscript context end setPlanContext(sched.current_thread_id(), 14, "M = {20.0, 20.0, 30.0, 10.0, 10.0, 10.0}") --subscript context start M = {20.0, 20.0, 30.0, 10.0, 10.0, 10.0} --subscript context end setPlanContext(sched.current_thread_id(), 15, "D = {300.0, 300.0, 1500.0, 100.0, 100.0, 100.0}") --subscript context start D = {300.0, 300.0, 1500.0, 100.0, 100.0, 100.0} --subscript context end setPlanContext(sched.current_thread_id(), 16, "K = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}") --subscript context start K = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0} --subscript context end setPlanContext(sched.current_thread_id(), 17, "goal_wrench = {0, 0, target_force, 0, 0, 0}") --subscript context start goal_wrench = {0, 0, target_force, 0, 0, 0} --subscript context end setPlanContext(sched.current_thread_id(), 18, "enable ={false, false, true, false, false, false}") --subscript context start enable ={false, false, true, false, false, false} --subscript context end setPlanContext(sched.current_thread_id(), 19, "init()") --subscript context start init() --subscript context end -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable setPlanContext(sched.current_thread_id(), 20, "开启力控和开始计时") -- 开启力控和开始计时 setPlanContext(sched.current_thread_id(), 21, "t0 = os.time()") --subscript context start t0 = os.time() --subscript context end setPlanContext(sched.current_thread_id(), 22, "condition_timer_is_counting = true") --subscript context start condition_timer_is_counting = true --subscript context end setPlanContext(sched.current_thread_id(), 23, "startForceControl()") --subscript context start startForceControl() --subscript context end setPlanContext(sched.current_thread_id(), 24, "before_sync_time = realtime.time()") --subscript context start before_sync_time = realtime.time() --subscript context end setPlanContext(sched.current_thread_id(), 25, "is_steady_state = false") --subscript context start is_steady_state = false --subscript context end -- 开启力控和开始计时 setPlanContext(sched.current_thread_id(), 26, "关节运动") setPlanContext(sched.current_thread_id(), 27, "target_pose") moveJoint(inverseKinematics(target_pose_q, target_pose_p), 1.39626, 1.0472, 0, 0) setPlanContext(sched.current_thread_id(), 28, "力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力)") -- 力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力) setPlanContext(sched.current_thread_id(), 29, "循环_1 ") u5faau73af_1 = 1 while true do sched.sync_program_point() if ((isCondFullfiled() == false) and (time > 100)) then setPlanContext(sched.current_thread_id(), 30, "如果 (isCondFullfiled() == false) and (time > 100)") setPlanContext(sched.current_thread_id(), 31, "condition_timer_is_counting = false") --subscript context start condition_timer_is_counting = false --subscript context end setPlanContext(sched.current_thread_id(), 32, "textmsg(\" 条件等待超时 ! time = \" .. time)") --subscript context start textmsg(" 条件等待超时 ! time = " .. time) --subscript context end setPlanContext(sched.current_thread_id(), 33, "ret = -1") --subscript context start ret = -1 --subscript context end setPlanContext(sched.current_thread_id(), 34, "跳出循环") break end setPlanContext(sched.current_thread_id(), 35, "tcp_pose = getTcpPose()") --subscript context start tcp_pose = getTcpPose() --subscript context end sched.sync_program_point() if (equal(target_pose, tcp_pose)) then setPlanContext(sched.current_thread_id(), 36, "如果 equal(target_pose, tcp_pose)") setPlanContext(sched.current_thread_id(), 37, "textmsg(\" circle机械臂已到位 !\")") --subscript context start textmsg(" circle机械臂已到位 !") --subscript context end setPlanContext(sched.current_thread_id(), 38, "ret = 0") --subscript context start ret = 0 --subscript context end setPlanContext(sched.current_thread_id(), 39, "跳出循环") break end setPlanContext(sched.current_thread_id(), 40, "tcp_force = getTcpForce()") --subscript context start tcp_force = getTcpForce() --subscript context end sched.sync_program_point() if ((tcp_force[3] > target_force - 0.5) and (tcp_force[3] < target_force + 0.5)) then setPlanContext(sched.current_thread_id(), 41, "如果 (tcp_force[3] > target_force - 0.5) and (tcp_force[3] < target_force + 0.5)") sched.sync_program_point() if (is_steady_state == false) then setPlanContext(sched.current_thread_id(), 42, "如果 is_steady_state == false") setPlanContext(sched.current_thread_id(), 43, "before_sync_time = realtime.time()") --subscript context start before_sync_time = realtime.time() --subscript context end setPlanContext(sched.current_thread_id(), 44, "is_steady_state = true") --subscript context start is_steady_state = true --subscript context end else setPlanContext(sched.current_thread_id(), 45, "否则") setPlanContext(sched.current_thread_id(), 46, "time = realtime.time() - before_sync_time") --subscript context start time = realtime.time() - before_sync_time --subscript context end sched.sync_program_point() if (time > 0.01) then setPlanContext(sched.current_thread_id(), 47, "如果 time > 0.01") setPlanContext(sched.current_thread_id(), 48, "ret = 1") --subscript context start ret = 1 --subscript context end setPlanContext(sched.current_thread_id(), 49, "textmsg(\" 维持稳态退出！ \")") --subscript context start textmsg(" 维持稳态退出！ ") --subscript context end setPlanContext(sched.current_thread_id(), 50, "跳出循环") break end end else setPlanContext(sched.current_thread_id(), 51, "否则") setPlanContext(sched.current_thread_id(), 52, "is_steady_state = false") --subscript context start is_steady_state = false --subscript context end end sched.sync_program_point() if (isCondFullfiled()) then setPlanContext(sched.current_thread_id(), 53, "如果 isCondFullfiled()") setPlanContext(sched.current_thread_id(), 54, "textmsg(\" 力控终止条件满足 !\")") --subscript context start textmsg(" 力控终止条件满足 !") --subscript context end setPlanContext(sched.current_thread_id(), 55, "ret = 1") --subscript context start ret = 1 --subscript context end setPlanContext(sched.current_thread_id(), 56, "跳出循环") break end :: end_of_u5faau73af_1:: u5faau73af_1 = u5faau73af_1 + 1 sched.cancel_point() end -- 力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力) setPlanContext(sched.current_thread_id(), 57, "结束力控") -- 结束力控 setPlanContext(sched.current_thread_id(), 58, "stopForceControl()") --subscript context start stopForceControl() --subscript context end -- 结束力控 setPlanContext(sched.current_thread_id(), 59, "对力控返回结果进行日志输出和运动") -- 对力控返回结果进行日志输出和运动 sched.sync_program_point() if (ret < 0) then setPlanContext(sched.current_thread_id(), 60, "如果 ret < 0") setPlanContext(sched.current_thread_id(), 61, "textmsg(\" drag 超时,退回起点 \" .. ret)") --subscript context start textmsg(" drag 超时,退回起点 " .. ret) --subscript context end else setPlanContext(sched.current_thread_id(), 62, "否则") setPlanContext(sched.current_thread_id(), 63, "textmsg(\" press finish ret= \" .. ret)") --subscript context start textmsg(" press finish ret= " .. ret) --subscript context end end -- 对力控返回结果进行日志输出和运动 setPlanContext(sched.current_thread_id(), 64, "设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable") -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable setPlanContext(sched.current_thread_id(), 65, "target_force = 0") --subscript context start target_force = 0 --subscript context end setPlanContext(sched.current_thread_id(), 66, "M = {30.0, 30.0, 30.0, 10.0, 10.0, 10.0}") --subscript context start M = {30.0, 30.0, 30.0, 10.0, 10.0, 10.0} --subscript context end setPlanContext(sched.current_thread_id(), 67, "D = {1000.0, 1000.0, 1000.0, 100.0, 100.0, 100.0}") --subscript context start D = {1000.0, 1000.0, 1000.0, 100.0, 100.0, 100.0} --subscript context end setPlanContext(sched.current_thread_id(), 68, "K = {1000.0, 1000.0, 1000.0, 100.0, 100.0, 100.0}") --subscript context start K = {1000.0, 1000.0, 1000.0, 100.0, 100.0, 100.0} --subscript context end setPlanContext(sched.current_thread_id(), 69, "goal_wrench = {0, 0, target_force, 0, 0, 0}") --subscript context start goal_wrench = {0, 0, target_force, 0, 0, 0} --subscript context end setPlanContext(sched.current_thread_id(), 70, "enable ={false, false, true, false, false, false}") --subscript context start enable ={false, false, true, false, false, false} --subscript context end setPlanContext(sched.current_thread_id(), 71, "init()") --subscript context start init() --subscript context end -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable setPlanContext(sched.current_thread_id(), 72, "开启力控和开始计时") -- 开启力控和开始计时 setPlanContext(sched.current_thread_id(), 73, "condition_timer_is_counting = true") --subscript context start condition_timer_is_counting = true --subscript context end setPlanContext(sched.current_thread_id(), 74, "t0 = os.time()") --subscript context start t0 = os.time() --subscript context end setPlanContext(sched.current_thread_id(), 75, "startForceControl()") --subscript context start startForceControl() --subscript context end -- 开启力控和开始计时 setPlanContext(sched.current_thread_id(), 76, "文件夹") -- Folder setPlanContext(sched.current_thread_id(), 77, "直线运动") setPlanContext(sched.current_thread_id(), 78, "路点_1") moveLine(u8defu70b9_1_p, 0.07, 0.03, 0, 0) setPlanContext(sched.current_thread_id(), 79, "圆") setCirclePathMode(0) setPlanContext(sched.current_thread_id(), 80, "路点_4") setPlanContext(sched.current_thread_id(), 81, "路点_5") moveCircle(u8defu70b9_4_p, u8defu70b9_5_p, 0.07, 0.03, 0, 0) -- Folder setPlanContext(sched.current_thread_id(), 82, "力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力)") -- 力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力) setPlanContext(sched.current_thread_id(), 83, "循环_1 ") u5faau73af_1 = 1 while true do sched.sync_program_point() if ((isCondFullfiled() == false) and (time > 100)) then setPlanContext(sched.current_thread_id(), 84, "如果 (isCondFullfiled() == false) and (time > 100)") setPlanContext(sched.current_thread_id(), 85, "condition_timer_is_counting = false") --subscript context start condition_timer_is_counting = false --subscript context end setPlanContext(sched.current_thread_id(), 86, "textmsg(\" 条件等待超时 ! time = \" .. time)") --subscript context start textmsg(" 条件等待超时 ! time = " .. time) --subscript context end setPlanContext(sched.current_thread_id(), 87, "ret = -1") --subscript context start ret = -1 --subscript context end setPlanContext(sched.current_thread_id(), 88, "跳出循环") break end setPlanContext(sched.current_thread_id(), 89, "tcp_pose = getTcpPose()") --subscript context start tcp_pose = getTcpPose() --subscript context end sched.sync_program_point() if (equal(towardsPos2, tcp_pose)) then setPlanContext(sched.current_thread_id(), 90, "如果 equal(towardsPos2, tcp_pose)") setPlanContext(sched.current_thread_id(), 91, "textmsg(\" circle机械臂已到位 !\")") --subscript context start textmsg(" circle机械臂已到位 !") --subscript context end setPlanContext(sched.current_thread_id(), 92, "ret = 0") --subscript context start ret = 0 --subscript context end setPlanContext(sched.current_thread_id(), 93, "跳出循环") break end setPlanContext(sched.current_thread_id(), 94, "tcp_force = getTcpForce()") --subscript context start tcp_force = getTcpForce() --subscript context end sched.sync_program_point() if (isCondFullfiled()) then setPlanContext(sched.current_thread_id(), 95, "如果 isCondFullfiled()") setPlanContext(sched.current_thread_id(), 96, "textmsg(\" 力控终止条件满足 !\")") --subscript context start textmsg(" 力控终止条件满足 !") --subscript context end setPlanContext(sched.current_thread_id(), 97, "ret = 1") --subscript context start ret = 1 --subscript context end setPlanContext(sched.current_thread_id(), 98, "跳出循环") break end :: end_of_u5faau73af_1:: u5faau73af_1 = u5faau73af_1 + 1 sched.cancel_point() end -- 力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力) setPlanContext(sched.current_thread_id(), 99, "结束力控") -- 结束力控 setPlanContext(sched.current_thread_id(), 100, "stopForceControl()") --subscript context start stopForceControl() --subscript context end -- 结束力控 setPlanContext(sched.current_thread_id(), 101, "对力控返回结果进行日志输出和运动") -- 对力控返回结果进行日志输出和运动 sched.sync_program_point() if (ret < 0) then setPlanContext(sched.current_thread_id(), 102, "如果 ret < 0") setPlanContext(sched.current_thread_id(), 103, "textmsg(\" circle 超时,退回起点 \" .. ret)") --subscript context start textmsg(" circle 超时,退回起点 " .. ret) --subscript context end else setPlanContext(sched.current_thread_id(), 104, "否则") setPlanContext(sched.current_thread_id(), 105, "textmsg(\" circle finish ret= \" .. ret)") --subscript context start textmsg(" circle finish ret= " .. ret) --subscript context end end -- 对力控返回结果进行日志输出和运动 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_circle() end function app:robot_error_handler(name, err) -- print("An error hanppen to robot "..name) end -- return our app object return app