[[開発者向け情報>devel]]
~
**拘束処理の流れ [#h22031e9]
***StepPart1 [#j060c879]
-ギア
-接触判定
***StepPart2 [#bae41515]
-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で対応しているか 要検討
ページ一覧