1 #ifndef SimTK_SIMBODY_IMPULSE_SOLVER_H_
2 #define SimTK_SIMBODY_IMPULSE_SOLVER_H_
119 enum ContactType {TypeNA=-1, Observing=0, Known=1, Participating=2};
123 enum UniCond {UniNA=-1, UniOff=0, UniActive=1, UniKnown=2};
124 enum FricCond {FricNA=-1, FricOff=0, Sliding=1, Impending=2, Rolling=3};
125 enum BndCond {BndNA=-1, SlipLow=0, ImpendLow=1, Engaged=2,
126 ImpendHigh=3, SlipHigh=4};
132 : m_maxRollingTangVel(roll2slipTransitionSpeed),
133 m_convergenceTol(convergenceTol),
142 assert(roll2slipTransitionSpeed >= 0);
143 m_maxRollingTangVel = roll2slipTransitionSpeed;
149 m_convergenceTol = tol;
161 static const int MaxNumPhases = 3;
164 for (
int i=0; i < MaxNumPhases; ++i)
166 m_nBilateralSolves = m_nBilateralIters = m_nBilateralFail = 0;
171 "ImpulseSolver::clearStats(phase)",
172 "Phase must be 0..%d but was %d\n", MaxNumPhases-1, phase);
173 m_nSolves[phase] = m_nIters[phase] = m_nFail[phase] = 0;
240 mutable long long m_nSolves[MaxNumPhases];
241 mutable long long m_nIters[MaxNumPhases];
242 mutable long long m_nFail[MaxNumPhases];
330 :
m_Fk(frictionComponents),
m_Nk(normalComponents),
333 { assert(
m_Fk.size()<=3 &&
m_Nk.size()<=3);
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
Every Simbody header and source file should include this header before any other Simbody header.
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
This is the abstract base class for impulse solvers, which solve an important subproblem of the conta...
Definition: ImpulseSolver.h:109
static const char * getContactTypeName(ContactType ct)
Real m_maxRollingTangVel
Definition: ImpulseSolver.h:236
BndCond
Definition: ImpulseSolver.h:125
@ BndNA
Definition: ImpulseSolver.h:125
UniCond
Definition: ImpulseSolver.h:123
@ UniNA
Definition: ImpulseSolver.h:123
int getMaxIterations() const
Definition: ImpulseSolver.h:157
long long m_nBilateralIters
Definition: ImpulseSolver.h:244
long long m_nBilateralFail
Definition: ImpulseSolver.h:245
ContactType
Definition: ImpulseSolver.h:119
@ TypeNA
Definition: ImpulseSolver.h:119
virtual bool solveBilateral(const Array_< MultiplierIndex > &participating, const Matrix &A, const Vector &D, const Vector &rhs, Vector &pi) const =0
Solve a set of bilateral (unconditional) constraints for the impulse necessary to enforce them.
virtual ~ImpulseSolver()
Definition: ImpulseSolver.h:139
static const char * getUniCondName(UniCond uc)
long long m_nBilateralSolves
Definition: ImpulseSolver.h:243
Real m_convergenceTol
Definition: ImpulseSolver.h:237
void clearStats(int phase) const
Definition: ImpulseSolver.h:169
static void dumpUniContacts(const String &msg, const Array_< UniContactRT > &uniContacts)
static const char * getBndCondName(BndCond bc)
FricCond
Definition: ImpulseSolver.h:124
@ FricNA
Definition: ImpulseSolver.h:124
int m_maxIters
Definition: ImpulseSolver.h:238
void setMaxIterations(int maxIts)
Definition: ImpulseSolver.h:153
void setMaxRollingSpeed(Real roll2slipTransitionSpeed)
Definition: ImpulseSolver.h:141
Real getMaxRollingSpeed() const
Definition: ImpulseSolver.h:145
ImpulseSolver(Real roll2slipTransitionSpeed, Real convergenceTol, int maxIters)
Definition: ImpulseSolver.h:129
void setConvergenceTol(Real tol)
Definition: ImpulseSolver.h:147
Real getConvergenceTol() const
Definition: ImpulseSolver.h:151
void clearStats() const
Definition: ImpulseSolver.h:163
static const char * getFricCondName(FricCond fc)
virtual 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 =0
Solve.
Unique integer type for Subsystem-local multiplier indexing.
SimTK::String is a plug-compatible std::string replacement (plus some additional functionality) inten...
Definition: String.h:65
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
bool isNaN(const negator< float > &x)
Definition: negator.h:272
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
Definition: ImpulseSolver.h:310
Real m_ub
Definition: ImpulseSolver.h:317
Real m_lb
Definition: ImpulseSolver.h:317
Real m_impulse
Definition: ImpulseSolver.h:321
BoundedRT(MultiplierIndex ix, Real lb, Real ub)
Definition: ImpulseSolver.h:311
MultiplierIndex m_ix
Definition: ImpulseSolver.h:316
BndCond m_boundedCond
Definition: ImpulseSolver.h:320
Definition: ImpulseSolver.h:325
ConstraintLimitedFrictionIndex m_clfx
Definition: ImpulseSolver.h:337
Real m_effMu
Definition: ImpulseSolver.h:339
ConstraintLtdFrictionRT(const Array_< MultiplierIndex > &frictionComponents, const Array_< MultiplierIndex > &normalComponents, Real effMu)
Definition: ImpulseSolver.h:327
FricCond m_frictionCond
Definition: ImpulseSolver.h:342
Array_< Real > m_Fimpulse
Definition: ImpulseSolver.h:343
Array_< MultiplierIndex > m_Nk
Definition: ImpulseSolver.h:338
Array_< MultiplierIndex > m_Fk
Definition: ImpulseSolver.h:338
Definition: ImpulseSolver.h:347
StateLtdFrictionRT(const Array_< MultiplierIndex > &frictionComponents, Real knownN, Real muEff)
Definition: ImpulseSolver.h:348
Array_< MultiplierIndex > m_Fk
Definition: ImpulseSolver.h:357
Real m_effMu
Definition: ImpulseSolver.h:359
FricCond m_frictionCond
Definition: ImpulseSolver.h:362
Real m_knownN
Definition: ImpulseSolver.h:358
StateLimitedFrictionIndex m_slfx
Definition: ImpulseSolver.h:356
Array_< Real > m_Fimpulse
Definition: ImpulseSolver.h:363
Definition: ImpulseSolver.h:248
ConstraintIndex m_constraint
Definition: ImpulseSolver.h:252
UncondRT()
Definition: ImpulseSolver.h:249
Array_< MultiplierIndex > m_mults
Definition: ImpulseSolver.h:253
Array_< Real > m_impulse
Definition: ImpulseSolver.h:256
Definition: ImpulseSolver.h:295
UniSpeedRT(MultiplierIndex ix, int sign)
Definition: ImpulseSolver.h:296
Vec3 m_impulse
Definition: ImpulseSolver.h:306
MultiplierIndex m_ix
Definition: ImpulseSolver.h:301
Real m_sign
Definition: ImpulseSolver.h:302
UniCond m_speedCond
Definition: ImpulseSolver.h:305