力反馈 OpenHaptics ProgGuide-HDAPI

本节目的：学习HDAPI中的常用操作
原文来源于 OpenHaptics ProgGuide 中第5节，翻译理解水平有限，如有错误望指正。

HDAPI 程序典型流程

采用 PHANTOM mini

1: 触控设备操作

对设备的操作包含获取和设置状态，且只能在伺服循环中通过调度程序回调的方式使用。
1. 初始化
首先对设备初始化
HHD hHD = hdInitDevice(HD_DEFAULT_DEVICE);
然后启动力
hdEnable(HD_FORCE_OUTPUT)
当调度程序启动时才会真正启动力
2. 设置当前设备
hdMakeCurrentDevice(hHD);
当多个设备使用时需要轮流设置不同的设备作为当前设备，使其可以被操作。如果只有一个设备则无需调用
3. 设备能力（选项）
有些设备的特性是可以开关的，hdEnable() 或者 hdDisable()

2: 触控帧

1. 触控帧定义了一个范围（hdBeginFrame（）和hdEndFrame（）为界），在该范围内设备状态保证一致。在帧的开始，更新并存储设备状态以在该帧中使用，帧中的所有状态查询使用这些数据。帧结束时，将诸如力的新状态写出到设备。从上一帧获取最后信息，例如位置信息。
2. 大多数触控操作应该在触控帧内运行。在帧内调用操作可确保所使用数据的一致性，因为状态在帧内保持不变。获取帧外的状态通常会返回最后一帧的状态。在帧外设置状态通常会导致错误。

3: 调度程序操作

调度程序允许在调度程序线程中运行调用。由于设备需要以非常高的速率（通常1000Hz）发送强制更新，因此调度程序管理高优先级线程。如果开发人员需要进行查询或更改状态，他应该在此循环中执行此操作。否则，由于状态不断变化，应用程序查询或设置状态通常是不安全的。例如，用户不应该查询以调度程序执行速率更改的数据，变量不应该在两个线程之间共享。
用户应该仅通过使用调度程序回调来访问由触控线程修改的变量。
回调函数通常是下面这样
HDCallbackCode HDCALLBACK DeviceStateCallback(void *pUserData);
返回下面两个值之一
HD_CALLBACK_DONE or HD_CALLBACK_CONTINUE
根据返回值决定回调函数是否继续运行。
可以将回调设置为运行一次或者多次，具体取决于回调的返回值。如果返回值请求继续回调，则重新调度并在下一个回调时钟周期再次运行。否则将从调度程序中删除并被认为是完整的，并在同步操作的情况下将控制返回到调用线程。
例子

// 客户端数据声明
struct DeviceDisplayState{
	HDdouble position[3];
	HDdouble force[3];
}
// 客户端数据的使用
HDCallbackCode HDCALLBACK DeviceStateCallback(void *pUserData){
	DeviceDisplayState *pDisplayState = static_cast<DeviceDisplayState *>(pUserData);
	hdGetDoublev(HD_CURRENT_POSITION, pDisplayState->position);
	hdGetDoublev(HD_CURRENT_FORCE, pDisplayState->force);
	// 只调用一次
	return HD_CALLBACK_DONE;
}

同步/异步调用
同步调用：调用完成后返回，应用程序线程等待同步调用完成后继续
主要用来获取应用程序调度程序状态的快照，例如应用程序查询位置或者调度程序正在更改的变量或者状态，应该使用同步调用来执行（也就是说查询的值如果在更改，则线程等待更改完成后再返回）
例如

// 获取当前位置
DeviceDisplayState state; 
hdScheduleSynchronous(DeviceStateCallback, &state, HD_MIN_SCHEDULER_PRIORITY);

异步调用：立即返回（可以得到连续的值）
通常管理触觉循环，持久化表示触觉效果，每次迭代期间回调将效果应用于设备
例如

HDCallbackCode HDCALLBACK CoulombCallback(void *data)
{
	HHD hHD = hdGetCurrentDevice();
	hdBeginFrame(hHD);
	HDdouble pos[3];
	// 循环末端得到速度
	hdGetDoublev(HD_CURRENT_POSITION,pos);
	HDdouble force[3];
	// 根据位置计算力
	forceField(pos, force);
	// 将力写入设备
	hdSetDoublev(HD_CURRENT_FORCE, force);
	// 清除力
	hdEndFrame(hHD);
	// 每帧执行
	return HD_CALLBACK_CONTINUE;
}
hdScheduleAsynchronous(AForceSettingCallback, (void*)0, HD_DEFAULT_SCHEDULER_PRIORITY);

异步回调函数返回一个句柄，可以在将来用于对回调函数执行操作（取消调度回调，强行终止等）
例如

HDSchedulerHandle calibrationHandle = hdScheduleAsynchronous(aCallback, (void*)0, HD_MIN_SCHEDULER_PRIORITY);
hdStopScheduler();
hdUnschedule(calibrationHandle);

回调函数的优先级决定了在调度中的运行顺序。优先级高的先运行，相同的随机运行。
无论设备数量多少，都只运行一个调度程序线程。多个设备共享一个调度程序线程

4: 状态

1. 获取状态
可以通过使用hdGet系列函数检索设备状态和其它信息，例如hdGetDoublev(), hdGetIntegerv()，这些函数都需要有效的参数以及返回地址或者数组（保证数量一样大）
参数类型不是通用的，如果类型无效则返回HD_INVALID_INPUT_TYPE错误。
例如 HD_DEVICE_MODEL_TYPE 需要一个字符串而且只能被 hdGetString() 调用
CURRENT / LAST 状态指的是当前正在查询的帧或者上一帧的状态。如果在帧外面调用这两个状态则视为在前一帧内进行。（此时CURRENT返回上一帧，LAST返回上上帧）
例子

HDint buttonState;
HDstring vendor;
hduVector3Dd position;
HDfloat velocity[3];
HDdouble transform[16];

hdGetIntegerv(HD_CURRENT_BUTTONS,&buttonState);
hdGetString(HD_DEVICE_VENDOR,vendor);
hdGetDoublev(HD_CURRENT_POSITION,position);
hdGetFloatv(HD_CURRENT_VELOCITY,velocity);
hdGetDoublev(HD_LAST_ENDPOINT_TRANSFORM,transform);

通常应该在调度程序线程内，触觉帧中调用获取状态

2. 设置状态
设置状态应该在触控帧内完成，不支持笛卡尔力和 torques with motor DAC的混合
例如在HD_CURRENT_FORCE 和 HD_CURRENT_MOTOR_DAC_VALUES上调用 hdSetDoublev() 将导致错误
例如

HDdouble force[3] = {0.5, 0.0, 1.0};
hdSetDoublev(HD_CURRENT_FORCE,force);
HDfloat rampRate = .5;
hdSetFloatv(HD_FORCE_RAMPING_RATE,&rampRate);

注意

• 触觉帧结束前力不会发送到设备。
• 两次设置相同的状态将会用第二个替换第一个
• 如果累积力，可以使用一个私有变量或者重复使用 hdGet()/hdSet()

3. 状态同步
调度程序在线程之间提供状态同步功能。
考虑一个需要以伺服循环速率更新的状态，从另一个线程（图形线程）中访问或者修改。
一个实例是动态模拟位置的更新，根据伺服线程中运动的一些运动方程。
位置频繁发生变化，图形重绘函数随机地获取这些位置并绘制。
图形重绘期间，状态需要保持一致，因为图形线程速率(60Hz)比伺服线程速率(1000Hz)低得多。
下面的例子中，位置可能会在两次查询时发生变化

HDCallbackCode positionUpdateCallback(void *pUserData)
{
	hduVector3Dd *position = (hduVector3Dd *)pUserData;
	hdGetDoublev(HD_CURRENT_POSITION,*position);
	return HD_CALLBACK_CONTINUE;
}
void func()
{
	hduVector3Dd position;
	hdScheduleAsynchronous(positionUpdateCallback,
	position,
	HD_DEFAULT_SCHEDULER_PRIORITY);
	hduVector3Dd pos1 = position;
	hduVector3Dd pos2 = position;
	// 大概率下是不相等的
	assert(pos1 == pos2);
}

5: 校准接口

校准允许设备保持物理位置的准确概念。例如校准之前，设备可能认为机械臂位于工作空间中心，但实际上机械臂已经发生偏离。
1. 校准类型
编码器硬件校准
Inkwell 校准
自动校准
2. 查询校准
由于每种设备校准形式不同，可以通过 HD_CALIBRATION_STYLE 查询所支持的校准类型。
对于前两种校准，除了提示用户将设备放入合适的位置外，还应该调用 hdUpdateCalibration() 一次。
自动校准则通过 HD_CALIBRATION_NEEDS_UPDATE 返回值进行更新，使用 hdCheckCalibration() 定期检查并通过 hdUpdateCalibration() 进行更新。
注意 也可以通过 PHANToM Test 校准设备。
3. 校准时机
由于校准可能导致设备位置跳跃，因此可以通过一些力检查或者禁用力来执行校准。
否则校准可能导致设备进入力感很大的位置（例如物体内部）
4. 调用校准
首先，用户应该从支持的样式列表中选择校准样式，因为某些设备可能支持多种类型的校准。
例如

HDint supportedCalibrationStyles;
hdGetIntegerv(HD_CALIBRATION_STYLE, &supportedCalibrationStyles);
if (supportedCalibrationStyles & HD_CALIBRATION_ENCODER_RESET)
{
	calibrationStyleSupported = true;
}
if (supportedCalibrationStyles & HD_CALIBRATION_INKWELL)
{
	calibrationStyleInkwellSupported = true;
}
if (supportedCalibrationStyles & HD_CALIBRATION_AUTO)
{
	calibrationStyleAutoSupported = true;
}
// 接着定义回调函数用来检查校准
HDCallbackCode CalibrationStatusCallback (void *pUserData)
{
	HDenum *pStatus = (HDenum *) pUserData;
	hdBeginFrame(hdGetCurrentDevice());
	*pStatus = hdCheckCalibration();
	hdEndFrame(hdGetCurrentDevice());
	return HD_CALLBACK_DONE;
}
// 例子：回调函数更新校准
HDCallbackCode UpdateCalibrationCallback (void *pUserData)
{
	HDenum *calibrationStyle = (HDint *) pUserData;
	if (hdCheckCalibration() == HD_CALIBRATION_NEEDS_UPDATE)
	{
		hdUpdateCalibration(*calibrationStyle);
	}
	return HD_CALLBACK_DONE;
}

6: 力/扭矩控制

PHANTOM 笛卡尔空间
所有 PHANToM 设备工作区间都在笛卡尔坐标系中，默认时
X轴指向 PHANToM 的右侧
Y轴指向 PHANToM 上方
Z轴指向 “out”, 朝向用户

PHANTOM 关节空间
关节1，关节2和关节3是有助于PHANTOM的X，Y和Z力的基础关节。
也就是说这三个关节决定6自由度中沿x, y, z三个直角坐标轴方向的移动自由度

关节4，关节5和关节6是分别对应绕x, y, z这三个坐标轴的转动自由度。



迪卡尔基本的力由hdSetFloatv()或者hdSetDoublev()设定，并将第一个参数设为HD_CURRENT_FORCE

HDfloat baseForce[3];
baseForce[0] = force_x;
baseForce[1] = force_y;
baseForce[2] = force_z;
hdSetFloatv(HD_CURRENT_FORCE, baseForce);

下面说明了在6DOF设备上左右关节扭矩值的典型用法

HDdouble baseTorque[3] = {100, 250, 200}; //Base Torque in mNm
hdSetDoublev(HD_CURRENT_JOINT_TORQUE, baseTorque);
HDdouble gimbalTorque[3] = {30, 65, 0.0}; //Gimbal Torque in mNm
hdSetDoublev(HD_CURRENT_GIMBAL_TORQUE, gimbalTorque);

7: Error Reporting and Handling

// 未完待续

/* Check if an error occurred while attempting to render the force */
if (HD_DEVICE_ERROR(error = hdGetError()))
{
	if (hduIsForceError(&error))
	{
		bRenderForce = FALSE;
	}
	else if (hduIsSchedulerError(&error))
	{
		return HD_CALLBACK_DONE;
	}
}

8: Cleanup

在应用程序退出之前，应该停止调度程序并终止所有调度程序的操作。回调函数可以通过 hdUnschedule() 终止，也可以通过知道返回 HD_CALLBACK_DONE 的回调本身终止。最后禁用设备

hdStopScheduler();
hdUnschedule(scheduleCallbackHandle);
hdDisableDevice(hdGetCurrentDevice());