Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_PLUS_IMPULSE_SOLVER_H_ 00002 #define SimTK_SIMBODY_PLUS_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: Thomas Uchida, 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 "simbody/internal/ImpulseSolver.h" 00028 00029 namespace SimTK { 00030 00034 class SimTK_SIMBODY_EXPORT PLUSImpulseSolver : public ImpulseSolver { 00035 public: 00036 explicit PLUSImpulseSolver(Real roll2slipTransitionSpeed) 00037 : ImpulseSolver(roll2slipTransitionSpeed, 00038 1e-10, // default PLUS convergence tol 00039 20), // default Newton iteration limit 00040 m_minSmoothness(SqrtEps), // sharpness of smoothed discontinuities 00041 m_cosMaxSlidingDirChange(std::cos(Pi/6)) // 30 degrees 00042 {} 00043 00045 bool solve 00046 (int phase, 00047 const Array_<MultiplierIndex>& participating, 00048 const Matrix& A, 00049 const Vector& D, 00050 const Array_<MultiplierIndex>& expanding, 00051 Vector& piExpand, // in/out 00052 Vector& verrStart, // in/out 00053 Vector& verrApplied, 00054 Vector& pi, 00055 Array_<UncondRT>& unconditional, 00056 Array_<UniContactRT>& uniContact, 00057 Array_<UniSpeedRT>& uniSpeed, 00058 Array_<BoundedRT>& bounded, 00059 Array_<ConstraintLtdFrictionRT>& consLtdFriction, 00060 Array_<StateLtdFrictionRT>& stateLtdFriction 00061 ) const OVERRIDE_11; 00062 00064 bool solveBilateral 00065 (const Array_<MultiplierIndex>& participating, // p<=m of these 00066 const Matrix& A, // m X m, symmetric 00067 const Vector& D, // m, diag>=0 added to A 00068 const Vector& rhs, // m, RHS 00069 Vector& pi // m, unknown result 00070 ) const OVERRIDE_11; 00071 00072 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(PLUSImpulseSolver, ActiveIndex); 00073 00074 private: 00075 00076 // Given point P and line segment AB, find the point closest to P that lies 00077 // on AB, which we call Q. Returns stepLength, the ratio AQ:AB. In our case, 00078 // P is the origin and AB is the line segment connecting the initial and 00079 // final tangential velocity vectors. 00080 // @author Thomas Uchida 00081 Real calcSlidingStepLengthToOrigin(const Vec2& A, const Vec2& B, Vec2& Q) 00082 const; 00083 Real calcSlidingStepLengthToOrigin(const Vec3& A, const Vec3& B, Vec3& Q) 00084 const; 00085 00086 // Given vectors A and B, find step length alpha such that the angle between 00087 // A and A+alpha*(B-A) is MaxSlidingDirChange. The solutions were generated 00088 // in Maple using the law of cosines, then exported as optimized code. 00089 // @author Thomas Uchida 00090 Real calcSlidingStepLengthToMaxChange(const Vec2& A, const Vec2& B) const; 00091 Real calcSlidingStepLengthToMaxChange(const Vec3& A, const Vec3& B) const; 00092 00093 void classifyFrictionals(Array_<UniContactRT>& uniContacts) const; 00094 00095 // Go through the given set of active constraints and build a reverse map 00096 // from the multipliers to the active index. 00097 void fillMult2Active(const Array_<MultiplierIndex,ActiveIndex>& active, 00098 Array_<ActiveIndex,MultiplierIndex>& mult2active) const; 00099 00100 // Copy the active rows and columns of A into the Jacobian. These will 00101 // be the right values for the linear equations, but rows for nonlinear 00102 // equations (sliding, impending) will get overwritten. Initialize piActive 00103 // from pi. 00104 void initializeNewton(const Matrix& A, 00105 const Vector& piGuess, 00106 const Vector& verrApplied, 00107 const Array_<UniContactRT>& bounded) const; 00108 00109 // Given a new piActive, update the impending slip directions and calculate 00110 // the new err(piActive). 00111 void updateDirectionsAndCalcCurrentError 00112 (const Matrix& A, Array_<UniContactRT>& uniContact, 00113 const Vector& piELeft, const Vector& verrAppliedLeft, 00114 const Vector& piActive, 00115 Vector& errActive) const; 00116 00117 // Replace rows of Jacobian for constraints corresponding to sliding or 00118 // impending slip frictional elements. This is the partial derivative of the 00119 // constraint error w.r.t. pi. Also set rhs m_verrActive. 00120 void updateJacobianForSliding(const Matrix& A, 00121 const Array_<UniContactRT>& uniContact, 00122 const Vector& piELeft, 00123 const Vector& verrAppliedLeft) const; 00124 00125 // These are set on construction. 00126 Real m_minSmoothness; 00127 Real m_cosMaxSlidingDirChange; 00128 00129 // This starts out as verr and is then reduced during each interval. 00130 mutable Vector m_verrLeft; // m of these 00131 mutable Vector m_verrExpand; // -A*piExpand for not-yet-applied piE 00132 00133 // This is a subset of the given participating constraints that are 00134 // presently active. Only the rows and columns of A that are listed here 00135 // can be used (and we'll replace some of those rows). Note that a 00136 // "known" unilateral contact (typically one undergoing Poisson expansion) 00137 // is *not* active, although its friction constraints are. 00138 mutable Array_<MultiplierIndex,ActiveIndex> m_active; // na of these 00139 00140 // This is the inverse mapping from m_active. Given an index into the full 00141 // A matrix (all proximal constraint equations, each with a Simbody-assigned 00142 // multiplier), this returns either the corresponding index into the 00143 // m_active array, or an invalid index if this proximal constraint is not 00144 // active. 00145 mutable Array_<ActiveIndex,MultiplierIndex> m_mult2active; // m of these 00146 00147 // Each of these is indexed by ActiveIndex; they have dimension na. 00148 mutable Matrix m_JacActive; // Jacobian for Newton iteration 00149 mutable Vector m_rhsActive; // per-interval RHS for Newton iteration 00150 mutable Vector m_piActive; // Current impulse during Newton. 00151 mutable Vector m_errActive; // Error(piActive) 00152 00153 mutable Matrix m_bilateralActive; // temp for use by solveBilateral() 00154 }; 00155 00156 } // namespace SimTK 00157 00158 #endif // SimTK_SIMBODY_PLUS_IMPULSE_SOLVER_H_