Simbody
3.5
|
Projected Gauss Seidel impulse solver. More...
Public Member Functions | |
PGSImpulseSolver (Real roll2slipTransitionSpeed) | |
bool | solve (int phase, const Array_< MultiplierIndex > &participating, const Matrix &A, const Vector &D, const Array_< MultiplierIndex > &expanding, Vector &piExpand, Vector &verrStart, Vector &verrApplied, Vector &pi, Array_< UncondRT > &unconditional, Array_< UniContactRT > &uniContact, Array_< UniSpeedRT > &uniSpeed, Array_< BoundedRT > &bounded, Array_< ConstraintLtdFrictionRT > &consLtdFriction, Array_< StateLtdFrictionRT > &stateLtdFriction) const override |
Solve with conditional constraints. | |
bool | solveBilateral (const Array_< MultiplierIndex > &participating, const Matrix &A, const Vector &D, const Vector &rhs, Vector &pi) const override |
Solve with only unconditional constraints. |
Projected Gauss Seidel impulse solver.
Finds a solution to
[A+D] (piExpand+piUnknown) = v subject to piExpand <= 0 (given)
piUnknown_z <= 0 unilateral contact must push, not pull ||(piUnknown_x,piUnknown+y)|| <= -mu*pi_z unilateral friction cone where pi=piExpand+piUnknown.
piUnknown_speed <= 0 one-way ratchets lb <= piUnknown_bounded <= ub torque-limited motor
||piUnknown_F|| <= mu*||piUnknown_N|| friction in bilateral constraint ||piUnknown_F|| <= mu*N friction with known normal force N
When piUnknown_z[k] hits its upper limit of 0, we must have v_z[k] >= 0 (contact surfaces separating). We don't explicitly enforce that here; it depends on all diag(A)[z[k]] > 0. That means that if v_z[k]<0 we could improve the solution by making piUnknown_z[k] negative, so it wouldn't have hit the limit.
SimTK::PGSImpulseSolver::PGSImpulseSolver | ( | Real | roll2slipTransitionSpeed | ) | [inline, explicit] |
bool SimTK::PGSImpulseSolver::solve | ( | int | phase, |
const Array_< MultiplierIndex > & | participating, | ||
const Matrix & | A, | ||
const Vector & | D, | ||
const Array_< MultiplierIndex > & | expanding, | ||
Vector & | piExpand, | ||
Vector & | verrStart, | ||
Vector & | verrApplied, | ||
Vector & | pi, | ||
Array_< UncondRT > & | unconditional, | ||
Array_< UniContactRT > & | uniContact, | ||
Array_< UniSpeedRT > & | uniSpeed, | ||
Array_< BoundedRT > & | bounded, | ||
Array_< ConstraintLtdFrictionRT > & | consLtdFriction, | ||
Array_< StateLtdFrictionRT > & | stateLtdFriction | ||
) | const [override, virtual] |
Solve with conditional constraints.
In the common underdetermined case (redundant contact) we will return the first solution encountered but it is unlikely to be the best possible solution.
Implements SimTK::ImpulseSolver.
bool SimTK::PGSImpulseSolver::solveBilateral | ( | const Array_< MultiplierIndex > & | participating, |
const Matrix & | A, | ||
const Vector & | D, | ||
const Vector & | rhs, | ||
Vector & | pi | ||
) | const [override, virtual] |
Solve with only unconditional constraints.
In the underdetermined case we return one of the possible solutions but it will not in general be the least squares one. In the overdetermined, inconsistent case we will iterate for a long time and may converge on the least-error solution but cannot guarantee that.
Implements SimTK::ImpulseSolver.