Springhead Users Manual: 関節制御

関節制御

$1$ 自由度関節の場合

`PHJoint1DDesc`
`double`	`spring`	可動範囲下限
`double`	`damper`	可動範囲上限
`double`	`targetPosition`	可動範囲制限用バネ係数
`double`	`targetVelocity`	可動範囲制限用ダンパ係数
`double`	`offsetForce`
`double`	`fMax`

`PHJoint1DIf`
`double`	`GetPosition()`	関節変位を取得
`double`	`GetVelocity()`	関節速度を取得
`void`	`SetSpring(double)`
`double`	`GetSpring()`
`void`	`SetDamper(double)`
`double`	`GetDamper()`
`void`	`SetTargetPosition(double)`
`double`	`GetTargetPosition()`
`void`	`SetTargetVelocity(double)`
`double`	`GetTargetVelocity()`
`void`	`SetOffsetForce(double)`
`double`	`GetOffsetForce()`
`void`	`SetTorqueMax(double)`	最大関節トルクを設定
`double`	`GetTorqueMax()`	最大関節トルクを取得

関節を駆動する力 $f$ は次式で与えられます．

$\displaystyle f = K(p_0 - p) + D(v_0 - v) + f_0$

ここで $p$ ， $v$ はそれぞれ関節変位と関節速度でGetPosition，GetVelocityで取得できます．その他の記号とディスクリプタ変数との対応は以下の通りです．

$K$	`spring`
$D$	`damper`
$p_0$	`targetPosition`
$v_0$	`targetVelocity`
$f_0$	`offsetForce`

上の式はバネ・ダンパモデルとPD制御則の二通りの解釈ができます．前者としてとらえるなら $K$ はバネ係数， $D$ はダンパ係数， $p_0$ はバネの自然長， $v_0$ は基準速度となります．後者としてとらえる場合は $K$ はPゲイン， $D$ はDゲイン， $p_0$ は目標変位， $v_0$ は目標速度となります．また， $f_0$ は関節トルクのオフセット項です．上の式で得られた関節トルクは最後に $\pm$ fMaxの範囲に収まるようにクランプされます．

ボールジョイントの場合

ヒンジと同様に，バネダンパモデル・PD制御を実現します．ボールジョイントの変位はクォータニオンで表されるため，目標変位targetPositionはクォータニオンで，目標速度targetVelocityは回転ベクトルで与えます．

`PHBallJointDesc`
`double spring` & バネ係数
`double damper` & ダンパ係数
`Quaterniond targetPosition` & 目標変位
`Vec3d targetVelocity` & 目標速度
`Vec3d offsetForce` & モータートルク
`double fMax` & 関節トルクの限度