Simbody
3.5
|
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_