--- Auto generated by 'aubo_scope' v0.29.2-beta.30 2024-10-28 17:26:46
--- 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_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})
    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.3043597582803451,-0.615563426105025,0.1935808542565011,0.4048481862326474,0.06520242947157577,-2.888348238417729}
    setPlanContext(sched.current_thread_id(), 1, "初始变量")
    Base = {0,0,0,0,0,0}
    Tool = {0,0,0,0,0,0}
    u5e73u9762_0 = {0.304359758280345,-0.615563426105025,0.193580854256501,0.404848186232647,0.0652024294715758,-2.88834823841773}
    u8defu70b9_0_p = {0.3142889404538007,-0.6268658450694643,0.202902464029622,2.742285839965072,0.0942362094217168,0.6233042597798215}    
    u8defu70b9_0_q = {-0.9162386636104006,-0.5596375249432372,1.496931791117509,0.884130474083835,1.46563029493067,-0.01072898200979608}    
    u8defu70b9_1_p = {0.3154718511807706,-0.6296003451234818,0.1960148685134031,2.742331593947815,0.09426571833226245,0.6233143402675576}    
    u8defu70b9_1_q = {-0.9173439526209515,-0.5741746779130292,1.493566936207273,0.8953656439628112,1.466063122239099,-0.0118477305612449}    
    u8defu70b9_2_p = {0.2745308868090771,-0.6401898799468728,0.1933489667478195,2.742157468440869,0.09421229230117006,0.6232707618725841}    
    u8defu70b9_2_q = {-0.9786798383504894,-0.5614163972843174,1.524413991003114,0.9184595484871448,1.490162066445718,-0.06855177599353124}    
    
    setPlanContext(sched.current_thread_id(), 4, "机器人编程")
    setPlanContext(sched.current_thread_id(), 5, "关节运动")
    setPlanContext(sched.current_thread_id(), 6, "路点_0")
    moveJoint(inverseKinematics(u8defu70b9_0_q, u8defu70b9_0_p), 1.39626, 1.0472, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 7, "ForceControl_0")
    -- 前置函数
    -- 阻塞等待力控结束
    local function condFullfiled_0()
        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.2) and (force[3] < -1 + 0.2) ) 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, "路点_1")
    moveLine(u8defu70b9_1_p, 0.02, 0.003, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 10, "Successfully")
    function Successfully_0()
        setPlanContext(sched.current_thread_id(), 11, "等待：0")
    end
    
    setPlanContext(sched.current_thread_id(), 12, "Failure")
    function Failure_0()
        setPlanContext(sched.current_thread_id(), 13, "等待：0")
    end
    textmsg("等待力控结束")
    status = condFullfiled_0()
    textmsg("关闭力控")
    fcDisable()
    stopJoint(2)
    sleep(0)
    -- 关闭实时显示参考系下的力数据
    setBool("real_time_enable_flag", false)
    setBool("script_run_flag", false)
    if ( status >= 0 ) then
        textmsg("力控成功")
        Successfully_0()
    else
        textmsg("力控失败")
        Failure_0()
    end
    
    setPlanContext(sched.current_thread_id(), 14, "ForceControl_1")
    -- 前置函数
    -- 阻塞等待力控结束
    local function condFullfiled_1()
        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, 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(), 15, "直线运动")
    setPlanContext(sched.current_thread_id(), 16, "路点_2")
    moveLine(u8defu70b9_2_p, 0.06, 0.03, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 17, "Successfully")
    function Successfully_1()
        setPlanContext(sched.current_thread_id(), 18, "等待：0")
    end
    
    setPlanContext(sched.current_thread_id(), 19, "Failure")
    function Failure_1()
        setPlanContext(sched.current_thread_id(), 20, "等待：0")
    end
    textmsg("等待力控结束")
    status = condFullfiled_1()
    textmsg("关闭力控")
    fcDisable()
    stopJoint(2)
    sleep(0)
    -- 关闭实时显示参考系下的力数据
    setBool("real_time_enable_flag", false)
    setBool("script_run_flag", false)
    if ( status >= 0 ) then
        textmsg("力控成功")
        Successfully_1()
    else
        textmsg("力控失败")
        Failure_1()
    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