Franka BilateralController.exe

Touchのチェックボックスをチェック

// TouchRelative選択の場合
OnBnClickedCkTouch()
	RunTouch()
		staticTouchRelativeThread()
			TouchRelativeThread()

TouchRelativeThread()

// TouchXの現状の状態を取得
hdScheduleSynchronous(copyDeviceDataCallback, &currentData, HD_MIN_SCHEDULER_PRIORITY);

// ボタン開放からボタン押下の変化でフィルター初期化
lowpass[i].Initialize(1.0, 0.001, last.val[i]);

// 前回からの変化を計算
Matrix16 vec = pos - last;

// 変化をロボット用軸に合わせて送信データとする
out.val[12] = m_ckXaxis ? -vec.val[14] * scale : 0.0;
out.val[13] = m_ckYaxis ? -vec.val[12] * scale : 0.0;
out.val[14] = m_ckZaxis ? vec.val[13] * scale : 0.0;

// 送信データをリングバッファに書いてトリガ発行
m_txring.Write((const unsigned char*)&rc_index, sizeof(rc_index));
m_txring.Write((const unsigned char*)&out, sizeof(out));
::PostMessage(this->m_hWnd, WM_UpdateDeviceData, 0, 0);

OnUpdateDeviceData()

// リングバッファのデータを読み
m_txring.Read(ptr, sizeof(double))

// ロボットに送信
const int tx = m_robottx.SendTo(txbuf, txbuflen, (const SOCKADDR*)&m_robottxaddr, txaddrlen, 0);

OnRobotReceive(int nErrorCode)

// ロボットの状態を受信する
const int rx = m_robotrx.ReceiveFrom(rxbuf, rxbuflen, (SOCKADDR*)&m_robotrxaddr, &rxaddrlen, 0);

// 最新状態を保持する
case rs_O_T_EE:memcpy(&m_O_T_EE, ptr, databyte);break;
case rs_O_T_EE_d:memcpy(&m_O_T_EE_d, ptr, databyte);break;
case rs_tau_J:memcpy(&m_tau_J, ptr, databyte);break;
case rs_q:memcpy(&m_q, ptr, databyte);break;
case rs_q_d:memcpy(&m_q_d, ptr, databyte);break;
case rs_dq:memcpy(&m_dq, ptr, databyte);break;
case rs_dq_d:memcpy(&m_dq_d, ptr, databyte);break;
case rs_joint_contact:memcpy(&m_joint_contact, ptr, databyte);break;
case rs_cartesian_contact:memcpy(&m_cartesian_contact, ptr, databyte);break;
case rs_tau_ext_hat_filtered:memcpy(&m_tau_ext_hat_filtered, ptr, databyte);break;
case rs_theta:memcpy(&m_theta, ptr, databyte);break;
case rs_dtheta:memcpy(&m_dtheta, ptr, databyte);break;

OnBnClickedBuTouchConnect()

// コールバック関数を設定
m_hAsyncCallback = hdScheduleAsynchronous(GeneralCallback, NULL, HD_MAX_SCHEDULER_PRIORITY);

GeneralCallback(void* ptr)

// ロボットへの外力を表現
CMFCApplication15Dlg::c_pInstance->RobotTauCallback();

RobotTauCallback()

// ロボットへの外力を計算
const Double7& tau = m_tau_ext_hat_filtered;
for (i = 0; i < sizeof(tau.val) / sizeof(tau.val[0]); i++) {
	Amplitude += fabs(tau.val[i]);
}

// 係数を掛ける
Amplitude *= m_EdTauAmp;

// スタイラスに力を加える
hdSetDoublev(HD_CURRENT_FORCE, force);