Simbody  3.5
ImpulseSolver.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_IMPULSE_SOLVER_H_
2 #define SimTK_SIMBODY_IMPULSE_SOLVER_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2014 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKmath.h"
29 
30 namespace SimTK {
31 
110 public:
111  struct UncondRT;
112  struct UniContactRT;
113  struct UniSpeedRT;
114  struct BoundedRT;
116  struct StateLtdFrictionRT;
117 
118  // How to treat a unilateral contact (input to solver).
119  enum ContactType {TypeNA=-1, Observing=0, Known=1, Participating=2};
120 
121  // These are the results after the solve is complete. Don't mess with the
122  // enum numbering here; we're counting on it.
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};
127 
128 
129  ImpulseSolver(Real roll2slipTransitionSpeed,
130  Real convergenceTol,
131  int maxIters)
132  : m_maxRollingTangVel(roll2slipTransitionSpeed),
133  m_convergenceTol(convergenceTol),
134  m_maxIters(maxIters)
135  {
136  clearStats();
137  }
138 
139  virtual ~ImpulseSolver() {}
140 
141  void setMaxRollingSpeed(Real roll2slipTransitionSpeed) {
142  assert(roll2slipTransitionSpeed >= 0);
143  m_maxRollingTangVel = roll2slipTransitionSpeed;
144  }
145  Real getMaxRollingSpeed() const {return m_maxRollingTangVel;}
146 
148  assert(tol >= 0);
149  m_convergenceTol = tol;
150  }
151  Real getConvergenceTol() const {return m_convergenceTol;}
152 
153  void setMaxIterations(int maxIts) {
154  assert(maxIts > 0);
155  m_maxIters = maxIts;
156  }
157  int getMaxIterations() const {return m_maxIters;}
158 
159  // We'll keep stats separately for different "phases". The meaning of a
160  // phase is up to the caller.
161  static const int MaxNumPhases = 3;
162 
163  void clearStats() const {
164  for (int i=0; i < MaxNumPhases; ++i)
165  clearStats(i);
166  m_nBilateralSolves = m_nBilateralIters = m_nBilateralFail = 0;
167  }
168 
169  void clearStats(int phase) const {
170  SimTK_ERRCHK2(0<=phase&&phase<MaxNumPhases,
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;
174  }
175 
177  virtual bool solve
178  (int phase,
179  const Array_<MultiplierIndex>& participating, // p<=m of these
180  const Matrix& A, // m X m, symmetric
181  const Vector& D, // m, diag>=0 added to A
182  const Array_<MultiplierIndex>& expanding, // nx<=m of these
183  Vector& piExpand, // m
184  Vector& verrStart, // m, RHS (in/out)
185  Vector& verrApplied, // m
186  Vector& pi, // m, known+unknown
187  Array_<UncondRT>& unconditional,
188  Array_<UniContactRT>& uniContact, // with friction
189  Array_<UniSpeedRT>& uniSpeed,
190  Array_<BoundedRT>& bounded,
191  Array_<ConstraintLtdFrictionRT>& consLtdFriction,
192  Array_<StateLtdFrictionRT>& stateLtdFriction
193  ) const = 0;
194 
195 
217  virtual bool solveBilateral
218  (const Array_<MultiplierIndex>& participating, // p<=m of these
219  const Matrix& A, // m X m, symmetric
220  const Vector& D, // m, diag>=0 added to A
221  const Vector& rhs, // m, RHS
222  Vector& pi // m, unknown result
223  ) const = 0;
224 
225  // Printable names for the enum values for debugging.
226  static const char* getContactTypeName(ContactType ct);
227  static const char* getUniCondName(UniCond uc);
228  static const char* getFricCondName(FricCond fc);
229  static const char* getBndCondName(BndCond bc);
230 
231  // Show details for each uni contact in the array.
232  static void dumpUniContacts(const String& msg,
233  const Array_<UniContactRT>& uniContacts);
234 
235 protected:
236  Real m_maxRollingTangVel; // Sliding above this speed if solver cares.
237  Real m_convergenceTol; // Meaning depends on concrete solver.
238  int m_maxIters; // Meaning depends on concrete solver.
239 
240  mutable long long m_nSolves[MaxNumPhases];
241  mutable long long m_nIters[MaxNumPhases];
242  mutable long long m_nFail[MaxNumPhases];
243  mutable long long m_nBilateralSolves;
244  mutable long long m_nBilateralIters;
245  mutable long long m_nBilateralFail;
246 };
247 
249  UncondRT() {}
250 
251  // Input to solver.
252  ConstraintIndex m_constraint; // Back pointer to Simbody element.
253  Array_<MultiplierIndex> m_mults; // Which constraint multipliers?
254 
255  // Set by solver on return.
256  Array_<Real> m_impulse; // Same size as m_mults.
257 };
258 
259 // A unilateral contact (possibly with friction), joint stop, rope.
260 // These are the only constraints that can undergo impacts. Note that the COR
261 // is here for the convenience of the time stepper; it doesn't affect the
262 // impulse solvers. "Known" here means the normal constraint does not
263 // participate (that is, the constraint equation cannot be active), but an
264 // expansion impulse has been supplied for it.
267  : m_sign(1), m_type(TypeNA), m_effCOR(NaN), m_effMu(NaN),
268  m_contactCond(UniNA), m_frictionCond(FricNA),
269  m_impulse(NaN)
270  {}
271 
272  bool hasFriction() const {return !m_Fk.empty();}
273 
274  // Input to solver.
275  UnilateralContactIndex m_ucx; // Back pointer to Simbody element.
276  MultiplierIndex m_Nk; // multiplier for the normal constraint
277  Real m_sign; // sign convention for normal multiplier
278 
279  Array_<MultiplierIndex> m_Fk; // optional friction multipliers
280 
281  // These solver inputs can change during a step.
282  ContactType m_type; // Observing, Known, Participating
283  Real m_effCOR; // velocity-dependent COR
284  Real m_effMu; // if there is friction, else NaN
285 
286  // Working values for use by the solver, with final values returned.
292 };
293 
294 // Ratchet.
297  : m_ix(ix), m_sign(sign), m_speedCond(UniNA), m_impulse(NaN)
298  { assert(sign==-1 || sign==1); }
299 
300  // Input to solver.
301  MultiplierIndex m_ix; // which constraint multiplier
302  Real m_sign; // allowable sign for non-zero multiplier
303 
304  // Set by solver on return.
307 };
308 
309 // Torque-limited motor.
312  : m_ix(ix), m_lb(lb), m_ub(ub), m_boundedCond(BndNA), m_impulse(NaN)
313  { assert(m_lb<=m_ub); }
314 
315  // Input to solver.
316  MultiplierIndex m_ix; // which constraint multiplier
317  Real m_lb, m_ub; // effective lower, upper bounds; lb <= ub
318 
319  // Set by solver on return.
322 };
323 
324 // Friction acting at a joint-like constraint, bead-on-a-wire.
327  (const Array_<MultiplierIndex>& frictionComponents,
328  const Array_<MultiplierIndex>& normalComponents,
329  Real effMu)
330  : m_Fk(frictionComponents), m_Nk(normalComponents),
331  m_effMu(effMu), m_frictionCond(FricNA),
332  m_Fimpulse(frictionComponents.size(), NaN)
333  { assert(m_Fk.size()<=3 && m_Nk.size()<=3);
334  assert(isNaN(m_effMu) || m_effMu>=0); }
335 
336  // Inputs to solver.
337  ConstraintLimitedFrictionIndex m_clfx; // Back pointer to Simbody element.
340 
341  // Set by solver on return.
343  Array_<Real> m_Fimpulse; // same size as m_Fk
344 };
345 
346 // Friction acting at a compliant contact.
348  StateLtdFrictionRT(const Array_<MultiplierIndex>& frictionComponents,
349  Real knownN, Real muEff)
350  : m_Fk(frictionComponents), m_knownN(knownN), m_effMu(muEff),
351  m_frictionCond(FricNA), m_Fimpulse(frictionComponents.size(), NaN)
352  { assert(m_Fk.size()<=3); assert(m_knownN >= 0);
353  assert(isNaN(m_effMu) || m_effMu>=0); }
354 
355  // Inputs to solver.
356  StateLimitedFrictionIndex m_slfx; // Back pointer to Simbody element.
360 
361  // Set by solver on return.
363  Array_<Real> m_Fimpulse; // same size as m_Fk
364 };
365 
366 } // namespace SimTK
367 
368 #endif // SimTK_SIMBODY_IMPULSE_SOLVER_H_
MultiplierIndex m_Nk
Definition: ImpulseSolver.h:276
UniSpeedRT(MultiplierIndex ix, int sign)
Definition: ImpulseSolver.h:296
Real getMaxRollingSpeed() const
Definition: ImpulseSolver.h:145
void setMaxIterations(int maxIts)
Definition: ImpulseSolver.h:153
BndCond
Definition: ImpulseSolver.h:125
virtual ~ImpulseSolver()
Definition: ImpulseSolver.h:139
void clearStats() const
Definition: ImpulseSolver.h:163
ConstraintIndex m_constraint
Definition: ImpulseSolver.h:252
Definition: ImpulseSolver.h:248
StateLimitedFrictionIndex m_slfx
Definition: ImpulseSolver.h:356
BndCond m_boundedCond
Definition: ImpulseSolver.h:320
Definition: ImpulseSolver.h:295
BoundedRT(MultiplierIndex ix, Real lb, Real ub)
Definition: ImpulseSolver.h:311
long long m_nBilateralSolves
Definition: ImpulseSolver.h:243
ConstraintLimitedFrictionIndex m_clfx
Definition: ImpulseSolver.h:337
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
UniCond
Definition: ImpulseSolver.h:123
Real m_knownN
Definition: ImpulseSolver.h:358
ImpulseSolver(Real roll2slipTransitionSpeed, Real convergenceTol, int maxIters)
Definition: ImpulseSolver.h:129
This is the abstract base class for impulse solvers, which solve an important subproblem of the conta...
Definition: ImpulseSolver.h:109
Array_< MultiplierIndex > m_Nk
Definition: ImpulseSolver.h:338
ContactType m_type
Definition: ImpulseSolver.h:282
int m_maxIters
Definition: ImpulseSolver.h:238
Every Simbody header and source file should include this header before any other Simbody header...
Array_< Real > m_impulse
Definition: ImpulseSolver.h:256
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2037
Definition: ImpulseSolver.h:325
UnilateralContactIndex m_ucx
Definition: ImpulseSolver.h:275
Definition: ImpulseSolver.h:347
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:593
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
Array_< MultiplierIndex > m_mults
Definition: ImpulseSolver.h:253
Vec3 m_impulse
Definition: ImpulseSolver.h:306
ContactType
Definition: ImpulseSolver.h:119
UniCond m_contactCond
Definition: ImpulseSolver.h:287
UniContactRT()
Definition: ImpulseSolver.h:266
Real m_sign
Definition: ImpulseSolver.h:302
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
Real m_effCOR
Definition: ImpulseSolver.h:283
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
int getMaxIterations() const
Definition: ImpulseSolver.h:157
Array_< Real > m_Fimpulse
Definition: ImpulseSolver.h:363
long long m_nBilateralIters
Definition: ImpulseSolver.h:244
Vec3 m_impulse
Definition: ImpulseSolver.h:291
FricCond m_frictionCond
Definition: ImpulseSolver.h:288
MultiplierIndex m_ix
Definition: ImpulseSolver.h:301
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
Array_< MultiplierIndex > m_Fk
Definition: ImpulseSolver.h:357
UncondRT()
Definition: ImpulseSolver.h:249
Real m_convergenceTol
Definition: ImpulseSolver.h:237
Real m_ub
Definition: ImpulseSolver.h:317
Real m_slipMag
Definition: ImpulseSolver.h:290
FricCond m_frictionCond
Definition: ImpulseSolver.h:342
Definition: ImpulseSolver.h:265
Vec2 m_slipVel
Definition: ImpulseSolver.h:289
FricCond m_frictionCond
Definition: ImpulseSolver.h:362
Definition: ImpulseSolver.h:310
bool isNaN(const negator< float > &x)
Definition: negator.h:273
UniCond m_speedCond
Definition: ImpulseSolver.h:305
Real m_impulse
Definition: ImpulseSolver.h:321
SimTK::String is a plug-compatible std::string replacement (plus some additional functionality) inten...
Definition: String.h:62
void setMaxRollingSpeed(Real roll2slipTransitionSpeed)
Definition: ImpulseSolver.h:141
void clearStats(int phase) const
Definition: ImpulseSolver.h:169
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
Real m_sign
Definition: ImpulseSolver.h:277
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
MultiplierIndex m_ix
Definition: ImpulseSolver.h:316
Real m_effMu
Definition: ImpulseSolver.h:339
Real getConvergenceTol() const
Definition: ImpulseSolver.h:151
StateLtdFrictionRT(const Array_< MultiplierIndex > &frictionComponents, Real knownN, Real muEff)
Definition: ImpulseSolver.h:348
Real m_effMu
Definition: ImpulseSolver.h:359
bool hasFriction() const
Definition: ImpulseSolver.h:272
long long m_nBilateralFail
Definition: ImpulseSolver.h:245
Array_< Real > m_Fimpulse
Definition: ImpulseSolver.h:343
Real m_effMu
Definition: ImpulseSolver.h:284
Unique integer type for Subsystem-local multiplier indexing.
void setConvergenceTol(Real tol)
Definition: ImpulseSolver.h:147
Real m_maxRollingTangVel
Definition: ImpulseSolver.h:236
Array_< MultiplierIndex > m_Fk
Definition: ImpulseSolver.h:279
FricCond
Definition: ImpulseSolver.h:124