Ejemplo n.º 1
0
        public void ShurBvectorCompute(ref ChSystemDescriptor sysd)
        {
            // ***TO DO*** move the following thirty lines in a short function ChSystemDescriptor::ShurBvectorCompute() ?

            // Compute the b_shur vector in the Shur complement equation N*l = b_shur
            // with
            //   N_shur  = D'* (M^-1) * D
            //   b_shur  = - c + D'*(M^-1)*k = b_i + D'*(M^-1)*k
            // but flipping the sign of lambdas,  b_shur = - b_i - D'*(M^-1)*k
            // Do this in three steps:

            // Put (M^-1)*k    in  q  sparse vector of each variable..
            for (int iv = 0; iv < sysd.GetVariablesList().Count; iv++)
            {
                if (sysd.GetVariablesList()[iv].IsActive())
                {
                    sysd.GetVariablesList()[iv].Compute_invMb_v(sysd.GetVariablesList()[iv].Get_qb().matrix,
                                                                sysd.GetVariablesList()[iv].Get_fb().matrix);   // q = [M]'*fb
                }
            }
            // ...and now do  b_shur = - D'*q = - D'*(M^-1)*k ..
            int s_i = 0;
            for (int ic = 0; ic < sysd.GetConstraintsList().Count; ic++)
            {
                if (sysd.GetConstraintsList()[ic].IsActive())
                {
                    r.matrix[s_i, 0] = sysd.GetConstraintsList()[ic].Compute_Cq_q();
                    ++s_i;
                }
            }

            // ..and finally do   b_shur = b_shur - c
            sysd.BuildBiVector(tmp.matrix);  // b_i   =   -c   = phi/h
            r.matrix.MatrInc(tmp.matrix);
        }
Ejemplo n.º 2
0
        // Override/implement system functions of ChPhysicsItem
        // (to assemble/manage data for system solver)

        public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
        {
            //  if (!IsActive())
            //	return;

            mdescriptor.InsertConstraint(constraint);
        }
Ejemplo n.º 3
0
 /// Inject Jacobian blocks into the system descriptor.
 /// Tell to a system descriptor that there are item(s) of type ChKblock in this object
 /// (for further passing it to a solver)
 public override void ContInjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
 {
     if (m_Jac != null)
     {
         mdescriptor.InsertKblock(m_Jac.m_KRM);
     }
 }
Ejemplo n.º 4
0
        // Use this for initialization
        public override void Start()
        {
            out_step = 2000 * step;

            base.Start();

            if (init_sys)
            {
                m_use_mat_props  = true;
                m_contact_model  = contactForceModel;
                m_adhesion_model = adhesionForceModel;
                m_tdispl_model   = tangentialDisplacementModel;
                m_stiff_contact  = false;

                descriptor = new ChSystemDescriptor();
                descriptor.SetNumThreads(parallel_thread_number);

                solver_speed = new ChSolverSMC();
                solver_stab  = new ChSolverSMC();

                collision_system = new collision.ChCollisionSystemBullet((uint)max_objects, scene_size);

                // For default SMC there is no need to create contacts 'in advance'
                // when models are closer than the safety envelope, so set default envelope to 0
                collision.ChCollisionModel.SetDefaultSuggestedEnvelope(0);

                contact_container = gameObject.AddComponent <ChContactContainerSMC>();
                contact_container.SetSystem(this);

                m_minSlipVelocity        = 1e-4;
                m_characteristicVelocity = 1;
            }
        }
Ejemplo n.º 5
0
        public override void Start()
        {
            base.Start();

            if (init_sys)
            {
                SetSolverWarmStarting(true);
                // Set default contact container
                contact_container = gameObject.AddComponent <ChContactContainerNSC>() as ChContactContainerNSC;//new ChContactContainerNSC();
                contact_container.SetSystem(this);

                // Set default collision engine
                collision_system = new collision.ChCollisionSystemBullet((uint)max_objects, scene_size);

                // Set the system descriptor
                descriptor = new ChSystemDescriptor();
                descriptor.SetNumThreads(parallel_thread_number);

                // Set default solver
                SetSolverType(solverType);

                // Set default collision envelope and margin.
                collision.ChCollisionModel.SetDefaultSuggestedEnvelope(0.03);
                // collision.ChCollisionModel.SetDefaultSuggestedMargin(0.01);
            }
        }
Ejemplo n.º 6
0
        //
        // SOLVER INTERFACE
        //

        public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
        {
            if (!IsActive())
            {
                return;
            }

            mdescriptor.InsertConstraint(Cx);
        }
Ejemplo n.º 7
0
        public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
        {
            // base behaviour too
            base.InjectConstraints(ref mdescriptor);

            mdescriptor.InsertConstraint(Rx);
            mdescriptor.InsertConstraint(Ru);
            mdescriptor.InsertConstraint(Rv);
        }
Ejemplo n.º 8
0
        //
        // SOLVER INTERFACE
        //

        public override void InjectConstraints(ref ChSystemDescriptor descriptor)
        {
            if (!IsActive())
            {
                return;
            }

            descriptor.InsertConstraint(m_cnstr_dist);
            descriptor.InsertConstraint(m_cnstr_dot);
        }
Ejemplo n.º 9
0
        public override void InjectVariables(ref ChSystemDescriptor mdescriptor)
        {
            // First, inherit to parent class
            base.InjectVariables(ref mdescriptor);

            if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
            {
                innershaft1.InjectVariables(ref mdescriptor);
                innershaft2.InjectVariables(ref mdescriptor);
            }
        }
Ejemplo n.º 10
0
        public void _InjectKRMmatrices <Tcont>(List <Tcont> contactlist, ref ChSystemDescriptor mdescriptor)
            where Tcont : ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> >
        {
            List <Tcont> .Enumerator itercontact = contactlist.GetEnumerator();

            //itercontact.MoveNext();
            while (itercontact.Current != contactlist.LastOrDefault())
            {
                itercontact.MoveNext();
                ((IEnumerator <ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> > >)itercontact).Current.ContInjectKRMmatrices(ref mdescriptor);
            }
        }
Ejemplo n.º 11
0
        public override void InjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
        {
            // _InjectKRMmatrices(contactlist_3_3, mdescriptor);
            // _InjectKRMmatrices(contactlist_6_3, mdescriptor);
            _InjectKRMmatrices(contactlist_6_6, ref mdescriptor);

            /*_InjectKRMmatrices(contactlist_333_3, mdescriptor);
             * _InjectKRMmatrices(contactlist_333_6, mdescriptor);
             * _InjectKRMmatrices(contactlist_333_333, mdescriptor);
             * _InjectKRMmatrices(contactlist_666_3, mdescriptor);
             * _InjectKRMmatrices(contactlist_666_6, mdescriptor);
             * _InjectKRMmatrices(contactlist_666_333, mdescriptor);
             * _InjectKRMmatrices(contactlist_666_666, mdescriptor);*/
        }
Ejemplo n.º 12
0
        //
        // SOLVER INTERFACE
        //

        public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
        {
            if (!this.IsActive())
            {
                return;
            }

            for (int i = 0; i < mask.nconstr; i++)
            {
                if (mask.Constr_N(i).IsActive())
                {
                    mdescriptor.InsertConstraint(mask.Constr_N(i));
                }
            }
        }
Ejemplo n.º 13
0
 public override void InjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
 {
     for (int i = 0; i < bodylist.Count; i++)
     {
         bodylist[i].InjectKRMmatrices(ref mdescriptor);
     }
     for (int i = 0; i < linklist.Count; i++)
     {
         linklist[i].InjectKRMmatrices(ref mdescriptor);
     }
     for (int i = 0; i < otherphysicslist.Count; i++)
     {
         otherphysicslist[i].InjectKRMmatrices(ref mdescriptor);
     }
 }
Ejemplo n.º 14
0
        public double Res4(ref ChSystemDescriptor sysd)
        {
            // Project the gradient (for rollback strategy)
            // g_proj = (l-project_orthogonal(l - gdiff*g, fric))/gdiff;
            double gdiff = 1.0 / Math.Pow(nc, 2.0);

            sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix);
            tmp.matrix.MatrInc(r.matrix);
            tmp.matrix.MatrScale(-gdiff);
            tmp.matrix.MatrInc(gammaNew.matrix);
            sysd.ConstraintsProject(tmp.matrix);
            tmp.matrix.MatrSub(gammaNew.matrix, tmp.matrix);
            tmp.matrix.MatrScale(1.0 / gdiff);

            return(tmp.matrix.NormTwo());
        }
Ejemplo n.º 15
0
 public override double Solve(ref ChSystemDescriptor sysd  //< system description with constraints and variables
                              )
 {
     return(0);
 }
Ejemplo n.º 16
0
        public override void InjectVariables(ref ChSystemDescriptor mdescriptor)
        {
            variable.SetDisabled(!IsActive());

            mdescriptor.InsertVariables(variable);
        }
Ejemplo n.º 17
0
        /// Performs the solution of the problem.
        /// \return  the maximum constraint violation after termination.
        public override double Solve(ref ChSystemDescriptor sysd  //< system description with constraints and variables
                                     )
        {
            List <ChConstraint> mconstraints = sysd.GetConstraintsList();
            List <ChVariables>  mvariables   = sysd.GetVariablesList();

            // 1)  Update auxiliary data in all constraints before starting,
            //     that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and  [Eq_i]=[invM_i]*[Cq_i]'
            for (int ic = 0; ic < mconstraints.Count; ic++)
            {
                mconstraints[ic].Update_auxiliary();
            }

            // 2)  Compute, for all items with variables, the initial guess for
            //     still unconstrained system:
            for (int iv = 0; iv < mvariables.Count; iv++)
            {
                if (mvariables[iv].IsActive())
                {
                    mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix);  // q = [M]'*fb
                }
            }

            // 3)  For all items with variables, add the effect of initial (guessed)
            //     lagrangian reactions of constraints, if a warm start is desired.
            //     Otherwise, if no warm start, simply resets initial lagrangians to zero.
            if (warm_start)
            {
                for (int ic = 0; ic < mconstraints.Count; ic++)
                {
                    if (mconstraints[ic].IsActive())
                    {
                        mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
                    }
                }
            }
            else
            {
                for (int ic = 0; ic < mconstraints.Count; ic++)
                {
                    mconstraints[ic].Set_l_i(0.0);
                }
            }

            // 4)  Perform the iteration loops (if there are any constraints)
            double maxviolation   = 0.0;
            double maxdeltalambda = 0.0;

            if (mconstraints.Count == 0)
            {
                return(maxviolation);
            }

            for (int iter = 0; iter < max_iterations; iter++)
            {
                maxviolation   = 0;
                maxdeltalambda = 0;

                // The iteration on all constraints
                for (int ic = 0; ic < mconstraints.Count; ic++)
                {
                    // skip computations if constraint not active.
                    if (mconstraints[ic].IsActive())
                    {
                        // compute residual  c_i = [Cq_i]*q + b_i
                        double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i();

                        // true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
                        double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual));

                        // compute:  delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i )
                        double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual);

                        // update:   lambda += delta_lambda;
                        double old_lambda = mconstraints[ic].Get_l_i();
                        mconstraints[ic].Set_l_i(old_lambda + deltal);

                        // If new lagrangian multiplier does not satisfy inequalities, project
                        // it into an admissible orthant (or, in general, onto an admissible set)
                        mconstraints[ic].Project();

                        // After projection, the lambda may have changed a bit..
                        double new_lambda = mconstraints[ic].Get_l_i();

                        // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
                        if (this.shlambda != 1.0)
                        {
                            new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda;
                            mconstraints[ic].Set_l_i(new_lambda);
                        }

                        double true_delta = new_lambda - old_lambda;

                        // For all items with variables, add the effect of incremented
                        // (and projected) lagrangian reactions:
                        mconstraints[ic].Increment_q(true_delta);

                        if (this.record_violation_history)
                        {
                            maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
                        }

                        maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
                    } // end IsActive()
                }     // end loop on constraints

                // For recording into violation history, if debugging
                if (this.record_violation_history)
                {
                    AtIterationEnd(maxviolation, maxdeltalambda, iter);
                }

                // Terminate the loop if violation in constraints has been successfully limited.
                if (maxviolation < tolerance)
                {
                    break;
                }
            }  // end iteration loop

            return(maxviolation);
        }
Ejemplo n.º 18
0
 /// Tell to a system descriptor that there are variables of type
 /// ChVariables in this object (for further passing it to a solver)
 /// Basically does nothing, but maybe that inherited classes may specialize this.
 public virtual void InjectVariables(ref ChSystemDescriptor mdescriptor)
 {
 }
Ejemplo n.º 19
0
        /// Performs the solution of the problem.
        public override double Solve(ref ChSystemDescriptor sysd)
        {
            bool verbose = false;
            List <ChConstraint> mconstraints = sysd.GetConstraintsList();

            // List<ChVariables> mvariables;// = sysd.GetVariablesList();
            //  if (verbose)
            //     Debug.Log("Number of constraints: " + mconstraints.Count + "nNumber of variables :" + mvariables.Count);

            // Update auxiliary data in all constraints before starting,
            // that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and  [Eq_i]=[invM_i]*[Cq_i]'
            for (int ic = 0; ic < mconstraints.Count; ic++)
            {
                mconstraints[ic].Update_auxiliary();
            }

            double L, t;
            double theta;
            double thetaNew;
            double Beta;
            double obj1, obj2;

            nc = sysd.CountActiveConstraints();
            gamma_hat.Resize(nc, 1);
            gammaNew.Resize(nc, 1);
            g.Resize(nc, 1);
            y.Resize(nc, 1);
            gamma.Resize(nc, 1);
            yNew.Resize(nc, 1);
            r.Resize(nc, 1);
            tmp.Resize(nc, 1);

            residual = 10e30;

            Beta = 0.0;
            obj1 = 0.0;
            obj2 = 0.0;

            // Compute the b_shur vector in the Shur complement equation N*l = b_shur
            ShurBvectorCompute(ref sysd);

            // Optimization: backup the  q  sparse data computed above,
            // because   (M^-1)*k   will be needed at the end when computing primals.
            ChMatrixDynamic <double> Minvk = new ChMatrixDynamic <double>();

            sysd.FromVariablesToVector(Minvk.matrix, true);

            // (1) gamma_0 = zeros(nc,1)
            if (warm_start)
            {
                for (int ic = 0; ic < mconstraints.Count; ic++)
                {
                    if (mconstraints[ic].IsActive())
                    {
                        mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
                    }
                }
            }
            else
            {
                for (int ic = 0; ic < mconstraints.Count; ic++)
                {
                    mconstraints[ic].Set_l_i(0.0);
                }
            }
            sysd.FromConstraintsToVector(gamma.matrix);

            // (2) gamma_hat_0 = ones(nc,1)
            gamma_hat.matrix.FillElem(1);

            // (3) y_0 = gamma_0
            y.matrix.CopyFromMatrix(gamma.matrix);

            // (4) theta_0 = 1
            theta = 1.0;

            // (5) L_k = norm(N * (gamma_0 - gamma_hat_0)) / norm(gamma_0 - gamma_hat_0)
            tmp.matrix.MatrSub(gamma.matrix, gamma_hat.matrix);
            L = tmp.matrix.NormTwo();
            sysd.ShurComplementProduct(yNew.matrix, tmp.matrix);
            L = yNew.matrix.NormTwo() / L;
            yNew.matrix.FillElem(0);  // reset yNew to be all zeros

            // (6) t_k = 1 / L_k
            t = 1.0 / L;

            // (7) for k := 0 to N_max
            for (tot_iterations = 0; tot_iterations < max_iterations; tot_iterations++)
            {
                // (8) g = N * y_k - r
                sysd.ShurComplementProduct(g.matrix, y.matrix);
                g.matrix.MatrInc(r.matrix);

                // (9) gamma_(k+1) = ProjectionOperator(y_k - t_k * g)
                gammaNew.matrix.CopyFromMatrix(g.matrix);
                gammaNew.matrix.MatrScale(-t);
                gammaNew.matrix.MatrInc(y.matrix);
                sysd.ConstraintsProject(gammaNew.matrix);

                // (10) while 0.5 * gamma_(k+1)' * N * gamma_(k+1) - gamma_(k+1)' * r >= 0.5 * y_k' * N * y_k - y_k' * r + g' *
                // (gamma_(k+1) - y_k) + 0.5 * L_k * norm(gamma_(k+1) - y_k)^2
                sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix);  // Here tmp is equal to N*gammaNew;
                tmp.matrix.MatrScale(0.5);
                tmp.matrix.MatrInc(r.matrix);
                obj1 = tmp.matrix.MatrDot(gammaNew.matrix, tmp.matrix);

                sysd.ShurComplementProduct(tmp.matrix, y.matrix);  // Here tmp is equal to N*y;
                tmp.matrix.MatrScale(0.5);
                tmp.matrix.MatrInc(r.matrix);
                obj2 = tmp.matrix.MatrDot(y.matrix, tmp.matrix);
                tmp.matrix.MatrSub(gammaNew.matrix, y.matrix);  // Here tmp is equal to gammaNew - y
                obj2 = obj2 + tmp.matrix.MatrDot(tmp.matrix, g.matrix) + 0.5 * L * tmp.matrix.MatrDot(tmp.matrix, tmp.matrix);

                while (obj1 >= obj2)
                {
                    // (11) L_k = 2 * L_k
                    L = 2.0 * L;

                    // (12) t_k = 1 / L_k
                    t = 1.0 / L;

                    // (13) gamma_(k+1) = ProjectionOperator(y_k - t_k * g)
                    gammaNew.matrix.CopyFromMatrix(g.matrix);
                    gammaNew.matrix.MatrScale(-t);
                    gammaNew.matrix.MatrInc(y.matrix);
                    sysd.ConstraintsProject(gammaNew.matrix);

                    // Update the components of the while condition
                    sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix);  // Here tmp is equal to N*gammaNew;
                    tmp.matrix.MatrScale(0.5);
                    tmp.matrix.MatrInc(r.matrix);
                    obj1 = tmp.matrix.MatrDot(gammaNew.matrix, tmp.matrix);

                    sysd.ShurComplementProduct(tmp.matrix, y.matrix);  // Here tmp is equal to N*y;
                    tmp.matrix.MatrScale(0.5);
                    tmp.matrix.MatrInc(r.matrix);
                    obj2 = tmp.matrix.MatrDot(y.matrix, tmp.matrix);
                    tmp.matrix.MatrSub(gammaNew.matrix, y.matrix);  // Here tmp is equal to gammaNew - y
                    obj2 = obj2 + tmp.matrix.MatrDot(tmp.matrix, g.matrix) + 0.5 * L * tmp.matrix.MatrDot(tmp.matrix, tmp.matrix);

                    // (14) endwhile
                }

                // (15) theta_(k+1) = (-theta_k^2 + theta_k * sqrt(theta_k^2 + 4)) / 2
                thetaNew = (-Math.Pow(theta, 2.0) + theta * Math.Sqrt(Math.Pow(theta, 2.0) + 4.0)) / 2.0;

                // (16) Beta_(k+1) = theta_k * (1 - theta_k) / (theta_k^2 + theta_(k+1))
                Beta = theta * (1.0 - theta) / (Math.Pow(theta, 2) + thetaNew);

                // (17) y_(k+1) = gamma_(k+1) + Beta_(k+1) * (gamma_(k+1) - gamma_k)
                tmp.matrix.MatrSub(gammaNew.matrix, gamma.matrix);  // Here tmp is equal to gammaNew - gamma;
                tmp.matrix.MatrScale(Beta);
                yNew.matrix.MatrAdd(gammaNew.matrix, tmp.matrix);

                // (18) r = r(gamma_(k+1))
                double res = Res4(ref sysd);

                // (19) if r < epsilon_min
                if (res < residual)
                {
                    // (20) r_min = r
                    residual = res;

                    // (21) gamma_hat = gamma_(k+1)
                    gamma_hat.matrix.CopyFromMatrix(gammaNew.matrix);

                    // (22) endif
                }

                // (23) if r < Tau
                if (residual < this.tolerance)
                {
                    // (24) break
                    break;

                    // (25) endif
                }

                // (26) if g' * (gamma_(k+1) - gamma_k) > 0
                tmp.matrix.MatrSub(gammaNew.matrix, gamma.matrix);
                if (tmp.matrix.MatrDot(tmp.matrix, g.matrix) > 0)
                {
                    // (27) y_(k+1) = gamma_(k+1)
                    yNew.matrix.CopyFromMatrix(gammaNew.matrix);

                    // (28) theta_(k+1) = 1
                    thetaNew = 1.0;

                    // (29) endif
                }

                // (30) L_k = 0.9 * L_k
                L = 0.9 * L;

                // (31) t_k = 1 / L_k
                t = 1.0 / L;

                // perform some tasks at the end of the iteration

                /* if (this.record_violation_history)
                 * {
                 *   tmp.MatrSub(gammaNew, gamma);
                 *   AtIterationEnd(residual, tmp.NormInf(), tot_iterations);
                 * }*/

                // Update iterates
                theta = thetaNew;
                gamma.matrix.CopyFromMatrix(gammaNew.matrix);
                y.matrix.CopyFromMatrix(yNew.matrix);

                // (32) endfor
            }

            //  if (verbose)
            //     Debug.Log("Residual: " + residual + ", Iter: " + tot_iterations);

            // (33) return Value at time step t_(l+1), gamma_(l+1) := gamma_hat
            sysd.FromVectorToConstraints(gamma_hat.matrix);

            // Resulting PRIMAL variables:
            // compute the primal variables as   v = (M^-1)(k + D*l)
            // v = (M^-1)*k  ...    (by rewinding to the backup vector computed at the beginning)
            sysd.FromVectorToVariables(Minvk.matrix); // PROBLEM slow!

            // ... + (M^-1)*D*l     (this increment and also stores 'qb' in the ChVariable items)
            for (int ic = 0; ic < mconstraints.Count; ic++)
            {
                if (mconstraints[ic].IsActive())
                {
                    mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
                }
            }

            return(residual);
        }
Ejemplo n.º 20
0
 /// Performs the solution of the problem.
 /// This function MUST be implemented in children classes, with specialized
 /// methods such as iterative or direct solvers.
 /// Returns true if it successfully solves the problem and false otherwise.
 public abstract double Solve(ref ChSystemDescriptor sysd  //< system description with constraints and variables
                  );
Ejemplo n.º 21
0
 public override void InjectVariables(ref ChSystemDescriptor mdescriptor)
 {
     mdescriptor.InsertVariables(variable);
 }
Ejemplo n.º 22
0
        // Old...

        public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
        {
            mdescriptor.InsertConstraint(constraint);
        }
Ejemplo n.º 23
0
 /// This function does the setup operations for the solver.
 /// The purpose of this function is to prepare the solver for subsequent calls to the
 /// solve function. This function is called only as frequently it is determined that
 /// it is appropriate to perform the setup phase.
 public virtual bool Setup(ref ChSystemDescriptor sysd  //< system description with constraints and variables
                    )
 {
     return true;
 }
Ejemplo n.º 24
0
        /// Performs the solution of the problem.
        /// \return  the maximum constraint violation after termination.
        public override double Solve(ref ChSystemDescriptor sysd  //< system description with constraints and variables
                                     )
        {
            List <ChConstraint> mconstraints = sysd.GetConstraintsList();
            List <ChVariables>  mvariables   = sysd.GetVariablesList();

            double maxviolation    = 0.0;
            double maxdeltalambda  = 0.0;
            int    i_friction_comp = 0;

            double[] old_lambda_friction = new double[3];
            int      nConstr             = mconstraints.Count;
            int      nVars = mvariables.Count;

            // 1)  Update auxiliary data in all constraints before starting,
            //     that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and  [Eq_i]=[invM_i]*[Cq_i]'
            for (int ic = 0; ic < nConstr; ic++)
            {
                mconstraints[ic].Update_auxiliary();
            }

            // Average all g_i for the triplet of contact constraints n,u,v.
            int j_friction_comp = 0;

            double[] gi_values = new double[3];
            for (int ic = 0; ic < nConstr; ic++)
            {
                if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC)
                {
                    gi_values[j_friction_comp] = mconstraints[ic].Get_g_i();
                    j_friction_comp++;
                    if (j_friction_comp == 3)
                    {
                        double average_g_i = (gi_values[0] + gi_values[1] + gi_values[2]) / 3.0;
                        mconstraints[ic - 2].Set_g_i(average_g_i);
                        mconstraints[ic - 1].Set_g_i(average_g_i);
                        mconstraints[ic - 0].Set_g_i(average_g_i);
                        j_friction_comp = 0;
                    }
                }
            }

            // 2)  Compute, for all items with variables, the initial guess for
            //     still unconstrained system:
            for (int iv = 0; iv < nVars; iv++)
            {
                if (mvariables[iv].IsActive())
                {
                    mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix);  // q = [M]'*fb
                }
            }
            // 3)  For all items with variables, add the effect of initial (guessed)
            //     lagrangian reactions of constraints, if a warm start is desired.
            //     Otherwise, if no warm start, simply resets initial lagrangians to zero.
            if (warm_start)
            {
                for (int ic = 0; ic < nConstr; ic++)
                {
                    if (mconstraints[ic].IsActive())
                    {
                        mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
                    }
                }
            }
            else
            {
                for (int ic = 0; ic < nConstr; ic++)
                {
                    mconstraints[ic].Set_l_i(0.0);
                }
            }

            // 4)  Perform the iteration loops
            for (int iter = 0; iter < max_iterations;)
            {
                //
                // Forward sweep, for symmetric SOR
                //
                maxviolation    = 0;
                maxdeltalambda  = 0;
                i_friction_comp = 0;
                int dummy = mconstraints.Count;
                for (int ic = 0; ic < dummy; ic++)
                {
                    // skip computations if constraint not active.
                    if (mconstraints[ic].IsActive())
                    {
                        // compute residual  c_i = [Cq_i]*q + b_i + cfm_i*l_i
                        double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i() +
                                           mconstraints[ic].Get_cfm_i() * mconstraints[ic].Get_l_i();

                        // true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
                        double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual));

                        // compute:  delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i )
                        double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual);

                        if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC)
                        {
                            candidate_violation = 0;
                            // update:   lambda += delta_lambda;
                            old_lambda_friction[i_friction_comp] = mconstraints[ic].Get_l_i();
                            mconstraints[ic].Set_l_i(old_lambda_friction[i_friction_comp] + deltal);
                            i_friction_comp++;

                            if (i_friction_comp == 1)
                            {
                                candidate_violation = Math.Abs(ChMaths.ChMin(0.0, mresidual));
                            }

                            if (i_friction_comp == 3)
                            {
                                mconstraints[ic - 2].Project();  // the N normal component will take care of N,U,V

                                double new_lambda_0 = mconstraints[ic - 2].Get_l_i();
                                double new_lambda_1 = mconstraints[ic - 1].Get_l_i();
                                double new_lambda_2 = mconstraints[ic - 0].Get_l_i();
                                // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
                                if (this.shlambda != 1.0)
                                {
                                    new_lambda_0 = shlambda * new_lambda_0 + (1.0 - shlambda) * old_lambda_friction[0];
                                    new_lambda_1 = shlambda * new_lambda_1 + (1.0 - shlambda) * old_lambda_friction[1];
                                    new_lambda_2 = shlambda * new_lambda_2 + (1.0 - shlambda) * old_lambda_friction[2];
                                    mconstraints[ic - 2].Set_l_i(new_lambda_0);
                                    mconstraints[ic - 1].Set_l_i(new_lambda_1);
                                    mconstraints[ic - 0].Set_l_i(new_lambda_2);
                                }
                                double true_delta_0 = new_lambda_0 - old_lambda_friction[0];
                                double true_delta_1 = new_lambda_1 - old_lambda_friction[1];
                                double true_delta_2 = new_lambda_2 - old_lambda_friction[2];
                                mconstraints[ic - 2].Increment_q(true_delta_0);
                                mconstraints[ic - 1].Increment_q(true_delta_1);
                                mconstraints[ic - 0].Increment_q(true_delta_2);

                                if (this.record_violation_history)
                                {
                                    maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0));
                                    maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1));
                                    maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_2));
                                }
                                i_friction_comp = 0;
                            }
                        }
                        else
                        {
                            // update:   lambda += delta_lambda;
                            double old_lambda = mconstraints[ic].Get_l_i();
                            mconstraints[ic].Set_l_i(old_lambda + deltal);

                            // If new lagrangian multiplier does not satisfy inequalities, project
                            // it into an admissible orthant (or, in general, onto an admissible set)
                            mconstraints[ic].Project();

                            // After projection, the lambda may have changed a bit..
                            double new_lambda = mconstraints[ic].Get_l_i();

                            // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
                            if (this.shlambda != 1.0)
                            {
                                new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda;
                                mconstraints[ic].Set_l_i(new_lambda);
                            }

                            double true_delta = new_lambda - old_lambda;

                            // For all items with variables, add the effect of incremented
                            // (and projected) lagrangian reactions:
                            mconstraints[ic].Increment_q(true_delta);

                            if (this.record_violation_history)
                            {
                                maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
                            }
                        }

                        maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
                    } // end IsActive()
                }     // end constraint loop

                // Terminate the loop if violation in constraints has been successfully limited.
                // if (maxviolation < tolerance)
                //	break;

                // For recording into violation history, if debugging
                if (this.record_violation_history)
                {
                    AtIterationEnd(maxviolation, maxdeltalambda, iter);
                }

                // Increment iter count (each sweep, either forward or backward, is considered
                // as a complete iteration, to be fair when comparing to the non-symmetric SOR :)
                iter++;

                //
                // Backward sweep, for symmetric SOR
                //
                maxviolation    = 0.0;
                maxdeltalambda  = 0.0;
                i_friction_comp = 0;

                for (int ic = (nConstr - 1); ic >= 0; ic--)
                {
                    // skip computations if constraint not active.
                    if (mconstraints[ic].IsActive())
                    {
                        // compute residual  c_i = [Cq_i]*q + b_i + cfm_i*l_i
                        double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i() +
                                           mconstraints[ic].Get_cfm_i() * mconstraints[ic].Get_l_i();

                        // true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
                        double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual));

                        // compute:  delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i )
                        double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual);

                        if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC)
                        {
                            candidate_violation = 0;
                            // update:   lambda += delta_lambda;
                            old_lambda_friction[i_friction_comp] = mconstraints[ic].Get_l_i();
                            mconstraints[ic].Set_l_i(old_lambda_friction[i_friction_comp] + deltal);
                            i_friction_comp++;
                            if (i_friction_comp == 3)
                            {
                                mconstraints[ic].Project();  // the N normal component will take care of N,U,V

                                double new_lambda_0 = mconstraints[ic + 2].Get_l_i();
                                double new_lambda_1 = mconstraints[ic + 1].Get_l_i();
                                double new_lambda_2 = mconstraints[ic + 0].Get_l_i();
                                // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
                                if (this.shlambda != 1.0)
                                {
                                    new_lambda_0 = shlambda * new_lambda_0 + (1.0 - shlambda) * old_lambda_friction[0];
                                    new_lambda_1 = shlambda * new_lambda_1 + (1.0 - shlambda) * old_lambda_friction[1];
                                    new_lambda_2 = shlambda * new_lambda_2 + (1.0 - shlambda) * old_lambda_friction[2];
                                    mconstraints[ic + 2].Set_l_i(new_lambda_0);
                                    mconstraints[ic + 1].Set_l_i(new_lambda_1);
                                    mconstraints[ic + 0].Set_l_i(new_lambda_2);
                                }
                                double true_delta_0 = new_lambda_0 - old_lambda_friction[0];
                                double true_delta_1 = new_lambda_1 - old_lambda_friction[1];
                                double true_delta_2 = new_lambda_2 - old_lambda_friction[2];
                                mconstraints[ic + 2].Increment_q(true_delta_0);
                                mconstraints[ic + 1].Increment_q(true_delta_1);
                                mconstraints[ic + 0].Increment_q(true_delta_2);

                                candidate_violation = Math.Abs(ChMaths.ChMin(0.0, mresidual));

                                if (this.record_violation_history)
                                {
                                    maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0));
                                    maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1));
                                    maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_2));
                                }
                                i_friction_comp = 0;
                            }
                        }
                        else
                        {
                            // update:   lambda += delta_lambda;
                            double old_lambda = mconstraints[ic].Get_l_i();
                            mconstraints[ic].Set_l_i(old_lambda + deltal);

                            // If new lagrangian multiplier does not satisfy inequalities, project
                            // it into an admissible orthant (or, in general, onto an admissible set)
                            mconstraints[ic].Project();

                            // After projection, the lambda may have changed a bit..
                            double new_lambda = mconstraints[ic].Get_l_i();

                            // Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
                            if (this.shlambda != 1.0)
                            {
                                new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda;
                                mconstraints[ic].Set_l_i(new_lambda);
                            }

                            double true_delta = new_lambda - old_lambda;

                            // For all items with variables, add the effect of incremented
                            // (and projected) lagrangian reactions:
                            mconstraints[ic].Increment_q(true_delta);

                            if (this.record_violation_history)
                            {
                                maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
                            }
                        }

                        maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
                    } // end IsActive()
                }     // end loop on constraints

                // For recording into violation history, if debugging
                if (this.record_violation_history)
                {
                    AtIterationEnd(maxviolation, maxdeltalambda, iter);
                }

                // Terminate the loop if violation in constraints has been successfully limited.
                if (maxviolation < tolerance)
                {
                    break;
                }

                iter++;
            }

            return(maxviolation);
        }
Ejemplo n.º 25
0
 public virtual void ContInjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
 {
 }
Ejemplo n.º 26
0
 public virtual void InjectConstraints(ref ChSystemDescriptor mdescriptor)
 {
 }