--- Auto generated by 'aubo_scope' v0.29.2-beta.20 2024-10-29 11:11:00
--- 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_multi_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.3908760102997187,0.230766789133336,0.08454221951626303,-3.14151545999883,1.361065740538089e-05,-1.829290285585734}    
    u8defu70b9_0_q = {2.879256474643207,-0.1285879731106847,2.162748918023181,0.7204634236128015,1.570782419643955,-0.003842219094016728}    
    u8defu70b9_1_p = {-0.3908798566233993,0.2307692758943663,0.06754786195057778,-3.14154918081645,1.643995365561657e-05,-1.829293649131409}    
    u8defu70b9_1_q = {2.879254461509089,-0.1629521776130982,2.182656260282533,0.7747673517535141,1.570779719989671,-0.003840869026402112}    
    u8defu70b9_2_p = {-0.3908796280810956,0.2307710033272382,0.06555460663974258,-3.14155746197983,1.78215898116922e-05,-1.829297000596291}    
    u8defu70b9_2_q = {2.879251110129859,-0.1671439733037274,2.184860757663858,0.7811732814701844,1.570778370170519,-0.003840869018574974}    
    u8defu70b9_5_p = {-0.3908761355386076,0.2307763115076273,0.06473332036678024,-3.141522911916209,2.848814997175168e-05,-1.829314102225136}    
    u8defu70b9_5_q = {2.879242107355514,-0.1688858633853058,2.185740054218754,0.7837599588360887,1.570767571551046,-0.003832768809492399}    
    u8defu70b9_3_p = {-0.3908715033930407,0.2307743323417741,0.07171853013290841,-3.14150360513031,2.70639513951282e-05,-1.82930902679711}    
    u8defu70b9_3_q = {2.879243132223197,-0.1542878527430039,2.177945396849766,0.7613479788330973,1.570768921380622,-0.003836818940608634}    
    
    setPlanContext(sched.current_thread_id(), 4, "机器人编程")
    setPlanContext(sched.current_thread_id(), 5, "直线运动")
    setPlanContext(sched.current_thread_id(), 6, "路点_0")
    moveLine(u8defu70b9_0_p, 1.2, 0.25, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 7, "路点_1")
    moveLine(u8defu70b9_1_p, 1.2, 0.25, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 8, "ForceControl_5")
    -- 前置函数
    -- 阻塞等待力控结束
    local function condFullfiled_5()
        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] > -10 - 1) and (force[3] < -10 + 1) ) then
                textmsg("Z方向达到目标力-10N")
                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, -10, 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_5()
        setPlanContext(sched.current_thread_id(), 12, "等待：0")
    end
    
    setPlanContext(sched.current_thread_id(), 13, "Failure")
    function Failure_5()
        setPlanContext(sched.current_thread_id(), 14, "等待：0")
    end
    textmsg("等待力控结束")
    status = condFullfiled_5()
    textmsg("关闭力控")
    fcDisable()
    stopJoint(2)
    sleep(0)
    -- 关闭实时显示参考系下的力数据
    setBool("real_time_enable_flag", false)
    setBool("script_run_flag", false)
    if ( status >= 0 ) then
        textmsg("力控成功")
        Successfully_5()
    else
        textmsg("力控失败")
        Failure_5()
    end
    
    setPlanContext(sched.current_thread_id(), 15, "直线运动")
    setPlanContext(sched.current_thread_id(), 16, "路点_5")
    moveLine(u8defu70b9_5_p, 0.2, 0.1, 0, 0)
    
    setPlanContext(sched.current_thread_id(), 17, "等待：0.05s")
    sleep(0.05)
    
    setPlanContext(sched.current_thread_id(), 18, "ForceControl_6")
    -- 前置函数
    -- 阻塞等待力控结束
    local function condFullfiled_6()
        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] > -10 - 1) and (force[3] < -10 + 1) ) then
                textmsg("Z方向达到目标力-10N")
                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, -10, 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(), 19, "Successfully")
    function Successfully_6()
        setPlanContext(sched.current_thread_id(), 20, "等待：0")
    end
    
    setPlanContext(sched.current_thread_id(), 21, "Failure")
    function Failure_6()
        setPlanContext(sched.current_thread_id(), 22, "等待：0")
    end
    textmsg("等待力控结束")
    status = condFullfiled_6()
    textmsg("关闭力控")
    fcDisable()
    stopJoint(2)
    sleep(0)
    -- 关闭实时显示参考系下的力数据
    setBool("real_time_enable_flag", false)
    setBool("script_run_flag", false)
    if ( status >= 0 ) then
        textmsg("力控成功")
        Successfully_6()
    else
        textmsg("力控失败")
        Failure_6()
    end
    
    setPlanContext(sched.current_thread_id(), 23, "直线运动")
    setPlanContext(sched.current_thread_id(), 24, "路点_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_multi_press()
end

function app:robot_error_handler(name, err)
  --
  print("An error hanppen to robot "..name)
end

-- return our app object
return app