INF = 9999
M_PI = 3.1415926

start_record = false

function init()
    timeout = -1
    ret = 0

    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 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}

    -- 力控坐标系选择 用户坐标系
    feature = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
    frame_type = 4
    -- 力控速度限制
    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, 1.0, 0.0}
    feature1 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
    type1 = 4
    timeout = -1
    count = 1.0
    outside = 1.0

    args = {}
    table.insert(args, min_force)
    table.insert(args, 10)
    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 condFullfiled(timeout)
    local ret = 0
    realtime = require "aubo.realtime"
    before = realtime.time()
    condition_timer = 0
    
    while not (isCondFullfiled()) do
        condition_timer = realtime.time() - before
        if ((timeout > 0) and (condition_timer > timeout)) then
            textmsg(" 条件等待超时 !  timer = " .. condition_timer)
            ret = -1
            break
        end
        sync()
    end
    return ret
end

function startForceControl()
    start_record = true
    sleep(0)
    fcEnable()
    -- 激活终止条件
    setCondActive()
end

function stopForceControl()
    condFullfiled(15)
    stopJoint(2)
    sleep(0)
    fcDisable()
end