--- Auto generated by 'aubo_scope' v0.29.2-beta.30 2024-10-29 09:04:26
--- 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_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})
    INF = 9999
    setHomePosition({0,-0.2617993877991494,1.74532925199433,0.4363323129985824,1.570796326794897,0}    )
    modbusDeleteAllSignals()
    setDigitalInputActionDefault()
    setDigitalOutputRunstateDefault()
    setStandardDigitalInputAction(0, StandardInputAction.StartProgram)
    setPayload(0.34, {0,0,0.1}, {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)
    u5e73u9762_0= {0.3109200116682875,-0.6222135022922457,0.195214947566921,0.4448384792596049,0.009950297105082185,-2.737779535753387}
    setPlanContext(sched.current_thread_id(), 1, "初始变量")
    Base = {0,0,0,0,0,0}
    Tool = {0,0,0,0,0,0}
    u5e73u9762_0 = {0.310920011668287,-0.622213502292246,0.195214947566921,0.444838479259605,0.00995029710508218,-2.73777953575339}
    u8defu70b9_0_p = {0.2780019238256224,-0.6309866400658882,0.1943817423736986,2.70811316037783,0.1040328609654174,0.6453122847017078}    
    u8defu70b9_0_q = {-0.9663961277955598,-0.5462270931808761,1.561865224180557,0.9761356213493786,1.487931905398896,-0.08433896866709101}    
    u8defu70b9_1_p = {0.2403735873569713,-0.6468141648329371,0.1944081784262161,2.708130179405125,0.1039931969404005,0.6454485909804498}    
    u8defu70b9_1_q = {-1.030398791165512,-0.5483182632479292,1.559327039721425,0.9792571132093227,1.515332074839298,-0.1424735450340169}    
    u8defu70b9_2_p = {0.252419689130268,-0.6753683798229281,0.2091665252418558,2.708247695837891,0.1040347749844944,0.6454032317610069}    
    u8defu70b9_2_q = {-1.037002969547113,-0.5990115323551005,1.429894941017762,0.9006806165892024,1.517624592362758,-0.1482690293333255}    
    u8defu70b9_3_p = {0.2771984239636606,-0.629232856094987,0.1983981525417194,2.708178303996996,0.1039922651754528,0.6453337111917625}    
    u8defu70b9_3_q = {-0.9658756315856051,-0.5370755451129089,1.564510569624092,0.9695295028537743,1.487733831884868,-0.08381810868576074}    
    u8defu70b9_4_p = {0.2780033088876714,-0.6309842890781607,0.1944011394082612,2.70810089979771,0.1040388414433115,0.6452945063000868}    
    u8defu70b9_4_q = {-0.9663900043107368,-0.5461995374991727,1.561856038953323,0.9761099451531159,1.487917233286745,-0.08432062852690335}    
    
    setPlanContext(sched.current_thread_id(), 4, "机器人编程")
    setPlanContext(sched.current_thread_id(), 5, "直线运动")
    setPlanContext(sched.current_thread_id(), 6, "路点_3")
    moveLine(u8defu70b9_3_p, 1.2, 0.25, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 7, "ForceControl_3")
    -- 前置函数
    -- 阻塞等待力控结束
    local function condFullfiled_3()
        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] > -1 - 0.1) and (force[3] < -1 + 0.1) ) then
                textmsg("Z方向达到目标力-1N")
                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, -1, 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")
    textmsg("开启力控")
    sleep(0)
    fcEnable()
    textmsg("激活终止条件")
    setCondActive()
    -- 添加子节点
    setPlanContext(sched.current_thread_id(), 8, "直线运动")
    setPlanContext(sched.current_thread_id(), 9, "路点_4")
    moveLine(u8defu70b9_4_p, 0.02, 0.003, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 10, "Successfully")
    function Successfully_3()
        setPlanContext(sched.current_thread_id(), 11, "等待：0")
    end
    
    setPlanContext(sched.current_thread_id(), 12, "Failure")
    function Failure_3()
        setPlanContext(sched.current_thread_id(), 13, "等待：0")
    end
    textmsg("等待力控结束")
    status = condFullfiled_3()
    textmsg("关闭力控")
    fcDisable()
    stopJoint(2)
    sleep(0)
    -- 关闭实时显示参考系下的力数据
    setBool("real_time_enable_flag", false)
    setBool("script_run_flag", false)
    if ( status >= 0 ) then
        textmsg("力控成功")
        Successfully_3()
    else
        textmsg("力控失败")
        Failure_3()
    end
    
    setPlanContext(sched.current_thread_id(), 14, "等待：0.50s")
    sleep(0.5)
    
    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 = 0
        -- 循环判断终止条件
        while not (isCondFullfiled()) do
            -- 参考轨迹运行完成并且力控完成调整
            if (getQueueSize() > 0) or (isSteady() == false) then 
                
            else
                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 = {0, 0, 3000, 0, 0, 0}
    K = {0, 0, 0, 0, 0, 0}
    enable = {false, false, true, false, false, false}
    goal_wrench = {0, 0, -1, 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")
    textmsg("开启力控")
    sleep(0)
    fcEnable()
    textmsg("激活终止条件")
    setCondActive()
    -- 添加子节点
    setPlanContext(sched.current_thread_id(), 16, "直线运动")
    setPlanContext(sched.current_thread_id(), 17, "路点_0")
    moveLine(u8defu70b9_0_p, 0.06, 0.03, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 18, "圆")
    local circle_wp_18 = getTargetTcpPose()
    setCirclePathMode(0)
    setPlanContext(sched.current_thread_id(), 19, "路点_1")
    
    setPlanContext(sched.current_thread_id(), 20, "路点_2")
    local circle_wp_20 = calculateCircleFourthPoint(circle_wp_18,poseTrans(Base,u8defu70b9_1_p),poseTrans(Base,u8defu70b9_2_p),1)
    for i = 1, 1 do
        moveCircle(u8defu70b9_1_p, u8defu70b9_2_p, 0.06, 0.03, 1e-05, 0)
        moveCircle(circle_wp_20, circle_wp_18, 0.06, 0.03, 1e-05, 0)
    end
    
    setPlanContext(sched.current_thread_id(), 21, "Successfully")
    function Successfully_2()
        setPlanContext(sched.current_thread_id(), 22, "等待：0")
    end
    
    setPlanContext(sched.current_thread_id(), 23, "Failure")
    function Failure_2()
        setPlanContext(sched.current_thread_id(), 24, "等待：0")
    end
    textmsg("等待力控结束")
    status = condFullfiled_2()
    textmsg("关闭力控")
    fcDisable()
    stopJoint(2)
    sleep(0)
    -- 关闭实时显示参考系下的力数据
    setBool("real_time_enable_flag", false)
    setBool("script_run_flag", false)
    if ( status >= 0 ) then
        textmsg("力控成功")
        Successfully_2()
    else
        textmsg("力控失败")
        Failure_2()
    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