関節のリミットをチェックする

[fkanehiro]

From [email protected] on September 09, 2013 13:07:58

http:https://code.google.com/p/rtm-ros-robotics/issues/detail?id=197 で議論されていますが，いまのhrpsysでは関節のリミットをチェックしていないので，
チェックする必要がありますね．

上のissueで議論されていますが，先ずはベースシステム（以下でUSE_COMMONでセットされている部分）でチェックして，ユーザ開発者がどのようなことをしても安全なようにSoftErrorLimit で追加しておこうと思いました． http:https://code.google.com/p/rtm-ros-robotics/source/browse/trunk/rtmros_common/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch

Original issue: http:https://code.google.com/p/hrpsys-base/issues/detail?id=133

[fkanehiro]

From [email protected] on September 08, 2013 21:46:09

添付のようなdiffでどうでしょうか？

Cc: [email protected] [email protected]

Attachment: error_limit.patch

[fkanehiro]

From [email protected] on September 08, 2013 22:43:48

パッチの41行めは

•      ((m_robot->link(i)->llimit > m_qRef.data[i]) &&
  

  ではなく

•      ((m_robot->link(i)->llimit > m_qRef.data[i]) ||
  

  ではないでしょうか。
  あと、関節上書きしているところはどこでしょうか。

[fkanehiro]

From nakaokat on September 08, 2013 22:58:14

24行目のbool joint_limit_error = false
はposition_limit_errorの間違いでしょうか。

また、38,39行目の
double limit = m_robot->m_servoErrorLimit[i];
double error = m_robot->link(i)->ulimit < m_qRef.data[i];
は使われていないようです。
そもそも、errorは比較ではなくて差をとりたくて入れたように思われます。

強制的に可動域内に動かす場合の懸念事項として、
実機の関節角可動域がモデルの可動域よりも広く設定されている場合には
サーボON時に実機がモデル可動域外にあると、急に動きだすことになってしまいます。

リセット時に可動域外にあった場合、強制的にリミットまで戻すのではなく、
可動域内に戻っていく方向のみ許可するような実装になっていると動かしやすいです。
一度可動域内に入ればそれ以降はリミットで打ち止めで良いと思います。

[fkanehiro]

From [email protected] on September 09, 2013 05:23:48

ありがとうございます

添付のものでどうでしょうか．

可動域内に戻っていく方向のみ許可するような実装になっていると動かしやすいです。

徐々に可動域内に動かすようにしましたが危険でしょうか？また動く速度はこれでどうでしょうか．

Attachment: error_limit.patch

[fkanehiro]

From [email protected] on September 09, 2013 23:35:29

可動範囲内への引き戻しは，

• if ( m_robot->link(i)->llimit > m_qRef.data[i] )
• m_qRef.data[i] += deg2rad(m_robot->lvlimit*0.5);
• if ( m_robot->link(i)->ulimit < m_qRef.data[i] )
• m_qRef.data[i] -= deg2rad(m_robot->uvlimit*0.5);

のように，lvlimit, uvlimitなどが使えそうです．
ただし，ロボットによってはlvlimit, uvlimitが設定されてないのもありますね．
0.5は適当ですが，マージンがあると安心そうです

[fkanehiro]

From [email protected] on September 10, 2013 09:54:37

lvlimitはハードウェア限界がかかれているだろうから，かなり全力で頑張る方向になっていると思うけど，ソッチの方がいいのかな？

徐々に可動域内に動かす
のであれば，定数で遅めの値にして，まさのときのためにdeg2rad(min(1, m_robot->lvlimit*0.5))
みたいにしてもいいとは思うけど．

そうでなくて，マージンをつけてでも，早く動くロボットは早くもどって欲しいんだろうか？

また，vlimitはdeg?HRP4のデータは以下のようになっているので，radっぽい．ライブラリを介したdegになる？
llimit [-0.785398163397448]
ulimit [0.785398163397448]
lvlimit [-8.756247044]
uvlimit [ 8.756247044]

Cc: nakaokat

[fkanehiro]

From [email protected] on September 10, 2013 16:18:57

lvlimitはハードウェア限界がかかれているだろうから，かなり全力で頑張る方向になっていると思うけど，ソッチの方がいいのかな？
すいません、lvlimitでなくて、確かに徐々に動いてくれる方が安心ですね。

vlimitはrad/sになると思います。 http:https://www.openrtp.jp/openhrp3/jp/create_model.html

[fkanehiro]

From nakaokat on September 11, 2013 23:23:51

vlimitを使うのはさすがに速すぎて危険なロボットも出てくるので反対です。
サーボオンしたときに勝手に動きはじめてしまうのも怖いですし、自動的に戻るようにする必要はないと思いますが、いかがでしょうか。
言葉が足らなかったかもしれませんが、
上での提案はlimitを越えてさらに離れる方向には動かないようにし、limit内部に戻る方向へ動く指令は素通りさせるということでした。

ところで、rtc内部でロボットを自動で動かすとなると、
rtcの周期はロボットによって異なると思いますので、周期を考慮する必要がありそうです。
rtcの中で周期は取れるのでしょうか。
confに書かれているexec_cxt.periodic.rateまたはdtに情報はありますが、どちらを読み込むのが正解でしょうか。
launchではperiodic_rateを設定しているようです。
（dtは必要なくなったということでしょうか）

[fkanehiro]

From [email protected] on September 13, 2013 03:04:03

periodic_rateはあくまでもRTCの周期で，制御プログラム的な周期はdtであるという認識でいいともいます．dtはXX.confにかかれているようです．

サーボオンしたときに勝手に動きはじめてしまうのも怖いですし、自動的に戻るようにする必要はないと思いますが、いかがでしょうか。
言葉が足らなかったかもしれませんが、
上での提案はlimitを越えてさらに離れる方向には動かないようにし、limit内部に戻る方向へ動く指令は素通りさせるということでした。

コード書いてくれると嬉しいです．いっこ前のを変数に覚えておいて，，というコードになるのかな．

[fkanehiro]

From nakaokat on September 13, 2013 03:18:40

そうですね、オーバーしている場合前回のと比べてerrorが小さくなる方向ならば許可するようにしてみます。

ただ、サーボオンするときに指令値を実機の値に合わせる処理に問題が出るかもしれません。
サーボオフ状態ではリミットを考慮せず素通りするようにしておけば大丈夫なような気がするので、そのようにしてみます。

[fkanehiro]

From nakaokat on September 15, 2013 23:49:54

添付のようなものでどうでしょうか。

以前のパッチにあったm_robot->link(i)とm_qRef.data[i]だと対応が取れていないようで、
変な場所でリミットにかかってしまいます。
linkではなくてjointとしてやるとうまく動いているように見えます。

今回の関節リミット以外の場所にも変更が必要になりそうですが、
とりあえずこのパッチは関節リミットの部分だけ含めています。

Attachment: jointanglelimit.patch

[fkanehiro]

From [email protected] on September 16, 2013 22:14:38

ありがとうございます． r847 でコミットしました．確認して下さい．ik0313さんもお願いします．

Cc: [email protected]

[fkanehiro]

From [email protected] on September 17, 2013 02:05:19

以前のパッチにあったm_robot->link(i)とm_qRef.data[i]だと対応が取れていないようで、
変な場所でリミットにかかってしまいます。
linkではなくてjointとしてやるとうまく動いているように見えます
jointは、idを考慮してかえしてくれるので、あっていると思います。
時々間違えそうになるので、注意が必要ですね。

hrplib/hrpModel/Body.hから。

/**
This function returns a link that has a given joint ID.
If there is no link that has a given joint ID,
the function returns a dummy link object whose ID is minus one.
The maximum id can be obtained by numJoints().
/
inline Link joint(int id) const {
return jointIdToLinkArray[id];
}

[fkanehiro]

From [email protected] on September 17, 2013 02:26:35

r848 でコメントに入れておきました．

[fkanehiro]

From [email protected] on September 20, 2013 07:34:12

Status: Fixed