C++にてPTP制御のループを抜け出す方法を教えてください

Question

直交多関節型マニピュレータのシミュレータを作成しているのですがその中で

speed[0]～[7]の速度[rad/s]の値によって移動させるPTP動作を、ptp_mymotion関数の計算式で行っている。
PTP 動作で定めた目標位置は、
目標位置=現在の座標位置+移動量
である。
座標位置を求める計算式は、
次に移動する座標位置=速度/フレームレートの値
である。
これらの計算式でΘ₁からΘ₈までそれぞれの移動する座標の計算を行う。次に移動する座標位置と目標位置が同じ値になった時ループをbreakで脱出し、PTP動作を停止する。また、各軸の座標の位置が ptp_mymotion 関数で定義した各軸の座標の制限値に到達すると、各軸に座標位置=座標制限値として PTP 動作を停止し、更にすべての軸が停止した時、ループを break で脱出する。

といった仕様にて移動させ目標値に到達した際にループを脱することを目標としていましたが

PTP 制御をやっている時に目標値に到着したことを正しく判定しないとループが終了しないし、そうすると制御自体も失敗するのが問題。ループが終了しないことを前提としていることがそもそも間違っている。
書いてある仕様では、目標値の到着の判定が正しくできないことが問題。

と指摘を受けました

radとrad/sを使用しているためdouble型を使用しておりそのため誤差が生じ正しく判定できないのではないかと考えています。

どのように改善すれば解決できるでしょうか。
回答よろしくお願いします。

Comments

[fana]

> 指摘を受けました

指摘してくれる人に訊いてください

[ha161073]

ごもっともなご意見です
しかしそれができないためこうして質問しているのです。
申し訳ないです。

何か改善の案がありましたらよろしくお願いいたします。

[y_waiwai]

コードも提示しないでおまえはなにを言ってるんだ、としかいいようがないですけど。
コードは、質問を編集し、<code>ボタンを押し、出てくる’’’の枠の中にコードを貼り付けてください

[fana]

正直，話の意味がよくわかりませんが……

あなたの実装した制御で「目標値に到着」するのですか？　すなわち，問題点とは
・到着する（実際には到着しているのに，判定処理がそのように判定結果を出さない，という問題
・到着しない（現在の制御では目標値に到達できない）
のどちらなのか？（あるいはその他か？）

前者なら判定処理の実装がダメなだけなのだろうし，後者なら判定以前の話な気がするし…

[fana]

> ループが終了しないことを前提としていることがそもそも間違っている

とはどういう話？

Answer 1

計算結果などの実数値の比較で誤差が問題になっている場合は、許容誤差を決めてその範囲内であれば等しいと判定することが多いと思います。

double allow_diff = 0.0001;

if (fabs(x - x0) < allow_diff) {
  // 目標値と一致したた場合の処理
}

2次元なら

if (hypot(x - x0, y - y0) < allow_diff) {
  // 目標値と一致したた場合の処理
}