Simbody  3.5
PLUSImpulseSolver.h
Go to the documentation of this file.
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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines