コードレビュー/2011年3月23日

開発者向け情報

拘束処理の流れ

StepPart1

• ギア
• 接触判定

StepPart2

• UpdateCacheLCP（剛体のローカル系での加速度を計算？）
• UpdateState(ContactPointとJointについて)
  • ヤコビアンを計算したり

• （関節の場合）PHBallJoint等のSetupLCP
  • SetConstrainedIndex(どの軸を拘束するかの決定:軸IDを設定)
    • numCondition(拘束する軸ID)とtargetAxis(拘束軸の数)を決める
  • ModifyJacobian
    • PathJointが使っているらしい

• PHBallJointLimitのSetupLCP
  • joint.SetupLCP（関節を作るためのLCPの準備）
  • limit.SetupLCP（可動域のためのLCPの準備）
    • （PHBallJoint has_a PHBallJointLimit）
    • PHBallJointLimit等のCompResponseMatrix
    • PHBallJoint等別に定義がある

• 1Dの場合
  • fMaxDt = FLT_MAX, fMinDt = 0,
  • axisIndexは自由度をもっている軸ID
  • 1軸の関節は可動軸，拘束軸が同じになる
  • A, dAなどはrangeSpring, rangeDamplerから計算される
  • diffは拘束を違反している量（侵入量）
  • joint1DのiterateLCPはconstraintのiterateLCPに統合できているので，呼ばない

• BallJointの場合
  • まず，Joint作成時に可動域の指定SetConstLineをしておく
  • SetConstLineについて
    • ファイルをロードして，関節のスイング角が0の時のツイスト角の可動域を読む
    • 次に，スイング方位角に対してスイング角が並ぶ，スプラインの傾き，ツイスト角の範囲
    • 最初と最後はスイング方位角とスイング角が一致するように
    • ロードした後，最初と最後のスイング方位角とスイング角が一致してなかったら，ポイントを一個増やして一致させる
    • ここで拘束がかかっていると，発振してしまうので，この時点では拘束はかけない

• SetupLCP
  • 拘束は可動軸と同じ向きに設定できないので，constNumConditionとtargetaxisを変更しないといけない
• CheckLimit(関節が可動域に入ってるかどうか)
  • 剛体の姿勢をオイラー角に変換
  • 可動域制限にかかっているかの判定のところ
  • SwingTwistでLimitするかSplineでLimitかで分岐

• SwingTwistの場合
  • それぞれの軸に対して，最大，最小をチェックして，違反していたら，numConditionに軸IDを入れて拘束をかける

• Splineの場合
  • 前回のswing方位角と今回のを比較して，差分の小さい方へ回るようにしている
  • 可動域との交点を確認する
    • Line[0] 前回から今回のswing方位角とswing角の差分ベクトル
    • Line[0]とLine[1]は直線の傾きと切片
    • 媒介変数で定式化したスプラインと直線との交点をもとめる
    • 交点でのスプラインの法線をもとめ，拘束の向きをきめる
    • 侵入量は繰り返し計算でやっている(内積でやったほうがいいことになった)
  • twist角の違反のチェック
    • スプライン指定しているポイントに設定されているtwist角の可動域を取ってきて，ポイント間内で重みを計算し，可動域を求める

• motor.SetupLCP（バネダンパのためのLCPの準備）
• PHBallJointのIterateLCP
  • joint.IterateLCP
  • motor.IterateLCP
    • limitより先に行う必要がある（でないとバネダンパによる運動が可動域を逸脱する） が，現在は6x3のヤコビアンを用いることでjoint.IterateLCPの中でまとめて計算している（これが可能になるようjoint.SetupLCPもそのための準備をしている）．
  • limit.IterateLCP

• pointPool : 謎

• IterateLCP
  • numCondition : 何番目の軸に拘束をかけるかのidが収まった配列
    • 変数名は直したほうがいい
    • VMatrixで試したら遅かったのでintの固定長配列になっているが，vectorにはしてみてもいいかもしれない
  • numConditionの要素ごとにfの更新式を走らせる．→fnewへ．
  • fnewに対して
    • 摩擦の場合は静止摩擦・動摩擦を考慮したプロジェクション
    • それ以外の場合は軸ごとにmax, minでクリッピング (バネダンパや，関節可動域を実現するためのクリッピングがこれで実現される)
      
• ヤコビアンの計算
  • movableAxesは拘束されることもある。可動域、モータ、バネダンパ
    

TO DO:

• constrを消す
• jointLimit1DのiterateLCPは消す
• motor全部についてiterateLCPは消す
• numCondition→constrainedAxes の型をvectorにする
• targetAxis→nConstrainedAxes いらなくなる
• axisIndex→movableAxes
• 拘束がない場合にすぐにループを抜けるようにする
• treejointで対応しているか　 要検討