--- Auto generated by 'aubo_scope' v0.29.1-rc.25 2024-07-09 15:14:14 --- 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_line() 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_3_p = {0.2491725917415579,0.400004234387154,0.3697305883973762,3.141569558552882,-5.400180437912918e-05,2.845210342512213} u8defu70b9_3_q = {1.274441528373243,0.04253066645089438,1.789510532885332,0.1762066361622099,1.570850327963835,2.751390313289298e-05} 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} setPlanContext(sched.current_thread_id(), 108, "线程_0") handler_u7ebfu7a0b_0=thread(function() while true do setPlanContext(sched.current_thread_id(), 109, "线程内计算力控运行时间") -- 线程内计算力控运行时间 setPlanContext(sched.current_thread_id(), 110, "循环_0 ") u5faau73af_0 = 1 while true do setPlanContext(sched.current_thread_id(), 111, "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(), 112, "如果 condition_timer_is_counting == true") setPlanContext(sched.current_thread_id(), 113, "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, "文件夹") -- Folder setPlanContext(sched.current_thread_id(), 27, "target_pose = calculate_point_to_move_towards(usercoord_in_base, {0.0, 0.0, 1.0}, 0.01)") --subscript context start target_pose = calculate_point_to_move_towards(usercoord_in_base, {0.0, 0.0, 1.0}, 0.01) --subscript context end setPlanContext(sched.current_thread_id(), 28, "moveLine(target_pose, 0.07, 0.002, 0, 0)") --subscript context start moveLine(target_pose, 0.07, 0.002, 0, 0) --subscript context end -- Folder setPlanContext(sched.current_thread_id(), 29, "力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力)") -- 力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力) setPlanContext(sched.current_thread_id(), 30, "循环_1 ") u5faau73af_1 = 1 while true do sched.sync_program_point() if ((isCondFullfiled() == false) and (time > 100)) then setPlanContext(sched.current_thread_id(), 31, "如果 (isCondFullfiled() == false) and (time > 100)") setPlanContext(sched.current_thread_id(), 32, "condition_timer_is_counting = false") --subscript context start condition_timer_is_counting = false --subscript context end setPlanContext(sched.current_thread_id(), 33, "textmsg(\" 条件等待超时 ! time = \" .. time)") --subscript context start textmsg(" 条件等待超时 ! time = " .. time) --subscript context end setPlanContext(sched.current_thread_id(), 34, "ret = -1") --subscript context start ret = -1 --subscript context end setPlanContext(sched.current_thread_id(), 35, "跳出循环") break end setPlanContext(sched.current_thread_id(), 36, "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(), 37, "如果 equal(target_pose, tcp_pose)") setPlanContext(sched.current_thread_id(), 38, "textmsg(\" circle机械臂已到位 !\")") --subscript context start textmsg(" circle机械臂已到位 !") --subscript context end setPlanContext(sched.current_thread_id(), 39, "ret = 0") --subscript context start ret = 0 --subscript context end setPlanContext(sched.current_thread_id(), 40, "跳出循环") break end setPlanContext(sched.current_thread_id(), 41, "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(), 42, "如果 (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(), 43, "如果 is_steady_state == false") setPlanContext(sched.current_thread_id(), 44, "before_sync_time = realtime.time()") --subscript context start before_sync_time = realtime.time() --subscript context end setPlanContext(sched.current_thread_id(), 45, "is_steady_state = true") --subscript context start is_steady_state = true --subscript context end else setPlanContext(sched.current_thread_id(), 46, "否则") setPlanContext(sched.current_thread_id(), 47, "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(), 48, "如果 time > 0.01") setPlanContext(sched.current_thread_id(), 49, "ret = 1") --subscript context start ret = 1 --subscript context end setPlanContext(sched.current_thread_id(), 50, "textmsg(\" 维持稳态退出！ \")") --subscript context start textmsg(" 维持稳态退出！ ") --subscript context end setPlanContext(sched.current_thread_id(), 51, "跳出循环") break end end else setPlanContext(sched.current_thread_id(), 52, "否则") setPlanContext(sched.current_thread_id(), 53, "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(), 54, "如果 isCondFullfiled()") setPlanContext(sched.current_thread_id(), 55, "textmsg(\" 力控终止条件满足 !\")") --subscript context start textmsg(" 力控终止条件满足 !") --subscript context end setPlanContext(sched.current_thread_id(), 56, "ret = 1") --subscript context start ret = 1 --subscript context end setPlanContext(sched.current_thread_id(), 57, "跳出循环") break end :: end_of_u5faau73af_1:: u5faau73af_1 = u5faau73af_1 + 1 sched.cancel_point() end -- 力控结束条件判断, 超时或者满足力控结束条件(达到力控保护性停止条件，即最大力) setPlanContext(sched.current_thread_id(), 58, "结束力控") -- 结束力控 setPlanContext(sched.current_thread_id(), 59, "stopForceControl()") --subscript context start stopForceControl() --subscript context end -- 结束力控 setPlanContext(sched.current_thread_id(), 60, "对力控返回结果进行日志输出和运动") -- 对力控返回结果进行日志输出和运动 sched.sync_program_point() if (ret < 0) then setPlanContext(sched.current_thread_id(), 61, "如果 ret < 0") setPlanContext(sched.current_thread_id(), 62, "textmsg(\" drag 超时,退回起点 \" .. ret)") --subscript context start textmsg(" drag 超时,退回起点 " .. ret) --subscript context end setPlanContext(sched.current_thread_id(), 63, "关节运动") else setPlanContext(sched.current_thread_id(), 65, "否则") setPlanContext(sched.current_thread_id(), 66, "textmsg(\" press finish ret= \" .. ret)") --subscript context start textmsg(" press finish ret= " .. ret) --subscript context end end -- 对力控返回结果进行日志输出和运动 setPlanContext(sched.current_thread_id(), 67, "设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable") -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable setPlanContext(sched.current_thread_id(), 68, "target_force = 0") --subscript context start target_force = 0 --subscript context end setPlanContext(sched.current_thread_id(), 69, "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(), 70, "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(), 71, "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(), 72, "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(), 73, "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(), 74, "init()") --subscript context start init() --subscript context end -- 设置力控参数MDK，目标力大小goal_wrench和开启力控方向enable setPlanContext(sched.current_thread_id(), 75, "开启力控和开始计时") -- 开启力控和开始计时 setPlanContext(sched.current_thread_id(), 76, "condition_timer_is_counting = true") --subscript context start condition_timer_is_counting = true --subscript context end setPlanContext(sched.current_thread_id(), 77, "t0 = os.time()") --subscript context start t0 = os.time() --subscript context end setPlanContext(sched.current_thread_id(), 78, "startForceControl()") --subscript context start startForceControl() --subscript context end -- 开启力控和开始计时 setPlanContext(sched.current_thread_id(), 79, "文件夹") -- Folder setPlanContext(sched.current_thread_id(), 80, "towardsPos2 = poseTrans(usercoord_in_base, {0.1, 0, 0.0015, 0, 0.0, 0})") --subscript context start towardsPos2 = poseTrans(usercoord_in_base, {0.1, 0, 0.0015, 0, 0.0, 0}) --subscript context end setPlanContext(sched.current_thread_id(), 81, "moveLine(towardsPos2, 0.07, 0.03, 0, 0)") --subscript context start moveLine(towardsPos2, 0.07, 0.03, 0, 0) --subscript context end -- 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 setPlanContext(sched.current_thread_id(), 104, "关节运动") else setPlanContext(sched.current_thread_id(), 106, "否则") setPlanContext(sched.current_thread_id(), 107, "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_line() end function app:robot_error_handler(name, err) -- print("An error hanppen to robot "..name) end -- return our app object return app