01 力反饋 OpenHaptics ConsoleExamples-FrictionlessPlane

HDAPI Overview

本文目的: 分析 HDAPI demo，如何產生一個觸碰平面的力

Haptic Device（觸覺設備）API 包含兩個主要的部分：設備以及調度程序。設備抽象化就可以讓任何支持3維觸覺的機器在 HDAPI 下使用。調度程序回調允許程序員輸入在伺服循環線程內執行的命令。
HDAPI 典型用途：

• 初始化設備
• 初始化調度程序
• 啓動調度程序
• 使用調度程序執行一些觸覺命令
• 完成時退出

開始

HDAPI 需要一個安裝驅動的 3D 觸覺設備，以及安裝的 HDAPI
項目應該包含 HDAPI 的頭文件以及 HDAPI 庫文件以及實用工具(utility)的庫文件

由於3維觸覺設備不易表示出來，下面採用語言描述
ConsoleExamples 中的 FrictionlessPlane：
創建一個高度爲0的平面，觸覺設備從上或者從下碰到平面時產生推力，當觸覺設備的力大於閾值時會發生穿透。

力的判定

如圖（左側），當觸覺設備位置在平面法向量（綠色部分）上方，此時投影爲正，表示沒有發生穿透現象
接着設備朝着平面運動（圖右側），當投影爲負時表示發生穿透。
現在用穿透的位移（點到平面的距離）當作彈簧形變，根據胡可定律產生力，力的方向和當前平面的法向量一致（所以產生排斥力）。
當力大於閾值時設置力爲0，同時改變朝向(directionFlag)如下圖

這樣當設備朝着上方移動時（重複上述過程）仍然可以感受到平面，符合實際情況。

demo源碼解析

int main(int argc, char* argv[])
{
    HDErrorInfo error;
    
    // 初始化默認的 haptic device.
    HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to initialize haptic device");
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        return -1;
    }

    // 創建一個隨動調度程序並施夾力
    hdEnable(HD_FORCE_OUTPUT);
    hdStartScheduler();
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Failed to start the scheduler");
        fprintf(stderr, "\nPress any key to quit.\n");
        getch();
        return -1;
    }

    // 安排一個無摩擦平面的回調, 會運行在servoloop 中，當刺破平面時施加力
    HDCallbackCode hPlaneCallback = hdScheduleAsynchronous(
        FrictionlessPlaneCallback, 0, HD_DEFAULT_SCHEDULER_PRIORITY);

    printf("Plane example.\n");
    printf("Move the device up and down to feel a plane along Y=0.\n");
    printf("Push hard against the plane to popthrough to the other side.\n");
    printf("Press any key to quit.\n\n");

    while (!_kbhit())
    {       
        if (!hdWaitForCompletion(hPlaneCallback, HD_WAIT_CHECK_STATUS))
        {
            fprintf(stderr, "\nThe main scheduler callback has exited\n");
            fprintf(stderr, "\nPress any key to quit.\n");
            getch();
            break;
        }
    }

    // 清理並關閉 haptic device, 清除所有的回調函數.
    hdStopScheduler();
    hdUnschedule(hPlaneCallback);
    hdDisableDevice(hHD);

    return 0;
}

// 平面在y=0處，當穿破物體時提供一個排斥力
HDCallbackCode HDCALLBACK FrictionlessPlaneCallback(void *data)
{
    // 硬度，值越大平面越硬
    const double planeStiffness = 2.85;

    // 穿透平面時最大的力
    const double popthroughForceThreshold = 30.0;
    
    // 朝向：穿破平面時改變，1和-1表示面對平面的正面和背面
    // 這個例子中，1表示位於平面上方，-1表示位於平面下方
    static int directionFlag = 1;

    hdBeginFrame(hdGetCurrentDevice());

    // device 的位置.
    hduVector3Dd position;
    hdGetDoublev(HD_CURRENT_POSITION, position);

    // 如果用戶穿透平面, 設置一個沿着平面法向量上排斥的力 
	// 當平面法向量+y但位置爲負時，發生穿透

    if ((position[1] <= 0 && directionFlag > 0) || 
        (position[1] > 0) && (directionFlag < 0))
    {
        double penetrationDistance = fabs(position[1]);
        hduVector3Dd forceDirection(0,directionFlag,0);

        // 胡克定律
        double k = planeStiffness;
        hduVector3Dd x = penetrationDistance*forceDirection;
        hduVector3Dd f = k*x;

        // 發生穿刺時力歸0，更改朝向
        if (f.magnitude() > popthroughForceThreshold)
        {
            f.set(0.0,0.0,0.0);
            directionFlag = -directionFlag;
        }
        hdSetDoublev(HD_CURRENT_FORCE, f);
    }

    hdEndFrame(hdGetCurrentDevice());

    // 發生錯誤時結束回調函數
    HDErrorInfo error;
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        hduPrintError(stderr, &error, "Error detected during main scheduler callback\n");
        if (hduIsSchedulerError(&error))
        {
            return HD_CALLBACK_DONE;  
        }
    }
    return HD_CALLBACK_CONTINUE;
}

效果

可以看到整個過程中間有明顯的阻力，當繼續施加力之後會直接穿透平面

bug

• 速度太快時無法感受到力，此時會直接刺破平面，感覺是碰撞檢測出了問題