/usr/include/simbody/simbody/internal/PGSImpulseSolver.h is in libsimbody-dev 3.5.4+dfsg-1ubuntu2.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 | #ifndef SimTK_SIMBODY_PGS_IMPULSE_SOLVER_H_
#define SimTK_SIMBODY_PGS_IMPULSE_SOLVER_H_
/* -------------------------------------------------------------------------- *
* Simbody(tm) *
* -------------------------------------------------------------------------- *
* This is part of the SimTK biosimulation toolkit originating from *
* Simbios, the NIH National Center for Physics-Based Simulation of *
* Biological Structures at Stanford, funded under the NIH Roadmap for *
* Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
* *
* Portions copyright (c) 2014 Stanford University and the Authors. *
* Authors: Michael Sherman *
* Contributors: *
* *
* Licensed under the Apache License, Version 2.0 (the "License"); you may *
* not use this file except in compliance with the License. You may obtain a *
* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
* *
* Unless required by applicable law or agreed to in writing, software *
* distributed under the License is distributed on an "AS IS" BASIS, *
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
* See the License for the specific language governing permissions and *
* limitations under the License. *
* -------------------------------------------------------------------------- */
#include "simbody/internal/ImpulseSolver.h"
namespace SimTK {
/** Projected Gauss Seidel impulse solver.
Finds a solution to
<pre>
[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
</pre>
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.
**/
class SimTK_SIMBODY_EXPORT PGSImpulseSolver : public ImpulseSolver {
public:
explicit PGSImpulseSolver(Real roll2slipTransitionSpeed)
: ImpulseSolver(roll2slipTransitionSpeed,
1e-6, // default PGS convergence tolerance
100), // default PGS max number iterations
m_SOR(1.2) {}
/** 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. **/
bool solve
(int phase,
const Array_<MultiplierIndex>& participating,
const Matrix& A,
const Vector& D,
const Array_<MultiplierIndex>& expanding, // nx<=m of these
Vector& piExpand,
Vector& verrStart, // in/out
Vector& verrApplied, // in/out
Vector& pi,
Array_<UncondRT>& unconditional,
Array_<UniContactRT>& uniContact,
Array_<UniSpeedRT>& uniSpeed,
Array_<BoundedRT>& bounded,
Array_<ConstraintLtdFrictionRT>& consLtdFriction,
Array_<StateLtdFrictionRT>& stateLtdFriction
) const OVERRIDE_11;
/** 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. **/
bool solveBilateral
(const Array_<MultiplierIndex>& participating, // p<=m of these
const Matrix& A, // m X m, symmetric
const Vector& D, // m, diag>=0 added to A
const Vector& rhs, // m, RHS
Vector& pi // m, unknown result
) const OVERRIDE_11;
private:
Real m_SOR;
};
} // namespace SimTK
#endif // SimTK_SIMBODY_PGS_IMPULSE_SOLVER_H_
|