開発者向け情報


拘束処理の流れ

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で対応しているか  要検討