Simbody  3.5
ImpulseSolver.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_IMPULSE_SOLVER_H_
00002 #define SimTK_SIMBODY_IMPULSE_SOLVER_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                               Simbody(tm)                                  *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2014 Stanford University and the Authors.           *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00027 #include "SimTKmath.h"
00028 #include "simbody/internal/common.h"
00029 
00030 namespace SimTK {
00031 
00109 class SimTK_SIMBODY_EXPORT ImpulseSolver {
00110 public:
00111     struct UncondRT;
00112     struct UniContactRT;
00113     struct UniSpeedRT;
00114     struct BoundedRT;
00115     struct ConstraintLtdFrictionRT;
00116     struct StateLtdFrictionRT;
00117 
00118     // How to treat a unilateral contact (input to solver).
00119     enum ContactType {TypeNA=-1, Observing=0, Known=1, Participating=2};
00120 
00121     // These are the results after the solve is complete. Don't mess with the
00122     // enum numbering here; we're counting on it.
00123     enum UniCond  {UniNA=-1, UniOff=0, UniActive=1, UniKnown=2};
00124     enum FricCond {FricNA=-1, FricOff=0, Sliding=1, Impending=2, Rolling=3};
00125     enum BndCond  {BndNA=-1, SlipLow=0, ImpendLow=1, Engaged=2, 
00126                    ImpendHigh=3, SlipHigh=4};
00127 
00128 
00129     ImpulseSolver(Real roll2slipTransitionSpeed,
00130                   Real convergenceTol,
00131                   int maxIters) 
00132     :   m_maxRollingTangVel(roll2slipTransitionSpeed),
00133         m_convergenceTol(convergenceTol),
00134         m_maxIters(maxIters)
00135     {
00136         clearStats();
00137     }
00138 
00139     virtual ~ImpulseSolver() {}
00140 
00141     void setMaxRollingSpeed(Real roll2slipTransitionSpeed) {
00142         assert(roll2slipTransitionSpeed >= 0);
00143         m_maxRollingTangVel = roll2slipTransitionSpeed; 
00144     }
00145     Real getMaxRollingSpeed() const {return m_maxRollingTangVel;}
00146 
00147     void setConvergenceTol(Real tol) {
00148         assert(tol >= 0);
00149         m_convergenceTol = tol;
00150     }
00151     Real getConvergenceTol() const {return m_convergenceTol;}
00152 
00153     void setMaxIterations(int maxIts) {
00154         assert(maxIts > 0);
00155         m_maxIters = maxIts;
00156     }
00157     int getMaxIterations() const {return m_maxIters;}
00158 
00159     // We'll keep stats separately for different "phases". The meaning of a
00160     // phase is up to the caller.
00161     static const int MaxNumPhases = 3;
00162 
00163     void clearStats() const {
00164         for (int i=0; i < MaxNumPhases; ++i)
00165             clearStats(i);
00166         m_nBilateralSolves = m_nBilateralIters = m_nBilateralFail = 0;
00167     }
00168 
00169     void clearStats(int phase) const {
00170         SimTK_ERRCHK2(0<=phase&&phase<MaxNumPhases,
00171             "ImpulseSolver::clearStats(phase)",
00172             "Phase must be 0..%d but was %d\n", MaxNumPhases-1, phase);
00173         m_nSolves[phase] = m_nIters[phase] = m_nFail[phase] = 0;
00174     }
00175 
00177     virtual bool solve
00178        (int                                 phase,
00179         const Array_<MultiplierIndex>&      participating, // p<=m of these 
00180         const Matrix&                       A,     // m X m, symmetric
00181         const Vector&                       D,     // m, diag>=0 added to A
00182         const Array_<MultiplierIndex>&      expanding, // nx<=m of these 
00183         Vector&                             piExpand, // m
00184         Vector&                             verrStart,   // m, RHS (in/out)
00185         Vector&                             verrApplied, // m
00186         Vector&                             pi,       // m, known+unknown
00187         Array_<UncondRT>&                   unconditional,
00188         Array_<UniContactRT>&               uniContact, // with friction
00189         Array_<UniSpeedRT>&                 uniSpeed,
00190         Array_<BoundedRT>&                  bounded,
00191         Array_<ConstraintLtdFrictionRT>&    consLtdFriction,
00192         Array_<StateLtdFrictionRT>&         stateLtdFriction
00193         ) const = 0;
00194 
00195 
00217     virtual bool solveBilateral
00218        (const Array_<MultiplierIndex>&      participating, // p<=m of these 
00219         const Matrix&                       A,     // m X m, symmetric
00220         const Vector&                       D,     // m, diag>=0 added to A
00221         const Vector&                       rhs,   // m, RHS
00222         Vector&                             pi     // m, unknown result
00223         ) const = 0;
00224 
00225     // Printable names for the enum values for debugging.
00226     static const char* getContactTypeName(ContactType ct);
00227     static const char* getUniCondName(UniCond uc);
00228     static const char* getFricCondName(FricCond fc);
00229     static const char* getBndCondName(BndCond bc);
00230 
00231     // Show details for each uni contact in the array.
00232     static void dumpUniContacts(const String& msg,
00233                                 const Array_<UniContactRT>& uniContacts);
00234 
00235 protected:
00236     Real m_maxRollingTangVel; // Sliding above this speed if solver cares.
00237     Real m_convergenceTol;    // Meaning depends on concrete solver.
00238     int  m_maxIters;          // Meaning depends on concrete solver.
00239 
00240     mutable long long m_nSolves[MaxNumPhases];
00241     mutable long long m_nIters[MaxNumPhases];
00242     mutable long long m_nFail[MaxNumPhases];
00243     mutable long long m_nBilateralSolves;
00244     mutable long long m_nBilateralIters;
00245     mutable long long m_nBilateralFail;
00246 };
00247 
00248 struct ImpulseSolver::UncondRT {
00249     UncondRT() {}
00250 
00251     // Input to solver.
00252     ConstraintIndex m_constraint;    // Back pointer to Simbody element.
00253     Array_<MultiplierIndex> m_mults; // Which constraint multipliers?
00254 
00255     // Set by solver on return.
00256     Array_<Real>            m_impulse; // Same size as m_mults.
00257 };
00258 
00259 // A unilateral contact (possibly with friction), joint stop, rope.
00260 // These are the only constraints that can undergo impacts. Note that the COR
00261 // is here for the convenience of the time stepper; it doesn't affect the
00262 // impulse solvers. "Known" here means the normal constraint does not 
00263 // participate (that is, the constraint equation cannot be active), but an 
00264 // expansion impulse has been supplied for it.
00265 struct ImpulseSolver::UniContactRT {
00266     UniContactRT() 
00267     :   m_sign(1), m_type(TypeNA), m_effCOR(NaN), m_effMu(NaN),
00268         m_contactCond(UniNA), m_frictionCond(FricNA), 
00269         m_impulse(NaN)
00270     {}
00271 
00272     bool hasFriction() const {return !m_Fk.empty();}
00273 
00274     // Input to solver.
00275     UnilateralContactIndex m_ucx;   // Back pointer to Simbody element.
00276     MultiplierIndex        m_Nk;    // multiplier for the normal constraint
00277     Real                   m_sign;  // sign convention for normal multiplier
00278 
00279     Array_<MultiplierIndex> m_Fk;   // optional friction multipliers
00280 
00281     // These solver inputs can change during a step.
00282     ContactType     m_type;       // Observing, Known, Participating
00283     Real            m_effCOR;     // velocity-dependent COR
00284     Real            m_effMu;      // if there is friction, else NaN
00285 
00286     // Working values for use by the solver, with final values returned.
00287     UniCond         m_contactCond;
00288     FricCond        m_frictionCond;
00289     Vec2            m_slipVel;
00290     Real            m_slipMag;
00291     Vec3            m_impulse;
00292 };
00293 
00294 // Ratchet.
00295 struct ImpulseSolver::UniSpeedRT {
00296     UniSpeedRT(MultiplierIndex ix, int sign) 
00297     :   m_ix(ix), m_sign(sign), m_speedCond(UniNA), m_impulse(NaN)
00298     {   assert(sign==-1 || sign==1); }
00299 
00300     // Input to solver.
00301     MultiplierIndex m_ix;         // which constraint multiplier
00302     Real            m_sign;       // allowable sign for non-zero multiplier
00303 
00304     // Set by solver on return.
00305     UniCond         m_speedCond;
00306     Vec3            m_impulse;
00307 };
00308 
00309 // Torque-limited motor.
00310 struct ImpulseSolver::BoundedRT {
00311     BoundedRT(MultiplierIndex ix, Real lb, Real ub) 
00312     :   m_ix(ix), m_lb(lb), m_ub(ub), m_boundedCond(BndNA), m_impulse(NaN) 
00313     {   assert(m_lb<=m_ub); }
00314 
00315     // Input to solver.
00316     MultiplierIndex m_ix;         // which constraint multiplier
00317     Real            m_lb, m_ub;   // effective lower, upper bounds; lb <= ub
00318 
00319     // Set by solver on return.
00320     BndCond         m_boundedCond;
00321     Real            m_impulse;
00322 };
00323 
00324 // Friction acting at a joint-like constraint, bead-on-a-wire.
00325 struct ImpulseSolver::ConstraintLtdFrictionRT {
00326     ConstraintLtdFrictionRT
00327        (const Array_<MultiplierIndex>& frictionComponents, 
00328         const Array_<MultiplierIndex>& normalComponents,
00329         Real                           effMu)
00330     :   m_Fk(frictionComponents), m_Nk(normalComponents), 
00331         m_effMu(effMu), m_frictionCond(FricNA), 
00332         m_Fimpulse(frictionComponents.size(), NaN) 
00333     {   assert(m_Fk.size()<=3 && m_Nk.size()<=3); 
00334         assert(isNaN(m_effMu) || m_effMu>=0); }
00335 
00336     // Inputs to solver.
00337     ConstraintLimitedFrictionIndex m_clfx;   // Back pointer to Simbody element.
00338     Array_<MultiplierIndex> m_Fk, m_Nk;
00339     Real                    m_effMu;
00340 
00341     // Set by solver on return.
00342     FricCond                m_frictionCond;
00343     Array_<Real>            m_Fimpulse; // same size as m_Fk
00344 };
00345 
00346 // Friction acting at a compliant contact.
00347 struct ImpulseSolver::StateLtdFrictionRT {
00348     StateLtdFrictionRT(const Array_<MultiplierIndex>& frictionComponents, 
00349                        Real knownN, Real muEff)
00350     :   m_Fk(frictionComponents), m_knownN(knownN), m_effMu(muEff), 
00351         m_frictionCond(FricNA), m_Fimpulse(frictionComponents.size(), NaN) 
00352     {   assert(m_Fk.size()<=3); assert(m_knownN >= 0);
00353         assert(isNaN(m_effMu) || m_effMu>=0); }
00354 
00355     // Inputs to solver.
00356     StateLimitedFrictionIndex m_slfx;   // Back pointer to Simbody element.
00357     Array_<MultiplierIndex> m_Fk;
00358     Real                    m_knownN;
00359     Real                    m_effMu;
00360 
00361     // Set by solver on return.
00362     FricCond                m_frictionCond;
00363     Array_<Real>            m_Fimpulse; // same size as m_Fk
00364 };
00365 
00366 } // namespace SimTK
00367 
00368 #endif // SimTK_SIMBODY_IMPULSE_SOLVER_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines