コード例 #1
0
        public override void IntLoadConstraint_C(int off_L,
                                                 ref ChVectorDynamic <double> Qc,
                                                 double c,
                                                 bool do_clamp,
                                                 double recovery_clamp)
        {
            int cnt = 0;

            for (int i = 0; i < mask.nconstr; i++)
            {
                if (mask.Constr_N(i).IsActive())
                {
                    if (do_clamp)
                    {
                        if (mask.Constr_N(i).IsUnilateral())
                        {
                            Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp);
                        }
                        else
                        {
                            Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp);
                        }
                    }
                    else
                    {
                        Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt);
                    }
                    cnt++;     // The Gremlin in the works was here!   I had accidently locked this variable in the above else, causing joint jittering
                }
            }
        }
コード例 #2
0
        public override void IntLoadConstraint_C(int off_L,
                                                 ref ChVectorDynamic <double> Qc,
                                                 double c,
                                                 bool do_clamp,
                                                 double recovery_clamp)
        {
            int cnt = 0;

            for (int i = 0; i < mask.nconstr; i++)
            {
                if (mask.Constr_N(i).IsActive())
                {
                    if (do_clamp)
                    {
                        if (mask.Constr_N(i).IsUnilateral())
                        {
                            Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp);
                        }
                        else
                        {
                            Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp);
                        }
                    }
                    else
                    {
                        Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt);
                    }
                    cnt++;
                }
            }
        }
コード例 #3
0
        public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
        {
            int cnt = 0;

            for (int i = 0; i < mask.nconstr; i++)
            {
                if (mask.Constr_N(i).IsActive())
                {
                    if (do_clamp)
                    {
                        if (mask.Constr_N(i).IsUnilateral())
                        {
                            mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
                                                     ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp));
                        }
                        else
                        {
                            mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
                                                     ChMaths.ChMin(ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp));
                        }
                    }
                    else
                    {
                        mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + factor * C.matrix.ElementN(cnt));
                    }

                    cnt++;
                }
            }
        }
コード例 #4
0
 /// Fill the matrix with random float numbers, falling within the
 /// "max"/"min" range.
 public void FillRandom(double max, double min)
 {
     for (int i = 0; i < rows * columns; ++i)
     {
         SetElementN(i, min + ChMaths.ChRandom() * (max - min));
     }
 }
コード例 #5
0
        public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
        {
            if (!IsActive())
            {
                return;
            }

            if (do_clamp)
            {
                Cx.Set_b_i(Cx.Get_b_i() + ChMaths.ChMin(ChMaths.ChMax(factor * (curr_dist - distance), -recovery_clamp), recovery_clamp));
            }
            else
            {
                Cx.Set_b_i(Cx.Get_b_i() + factor * (curr_dist - distance));
            }
        }
コード例 #6
0
        public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
        {
            if (!this.IsActive())
            {
                return;
            }

            //***TEST***

            /*
             *  GetLog()<< "cload: " ;
             *  if (this.c_x) GetLog()<< " x";
             *  if (this.c_y) GetLog()<< " y";
             *  if (this.c_z) GetLog()<< " z";
             *  if (this.c_rx) GetLog()<< " Rx";
             *  if (this.c_ry) GetLog()<< " Ry";
             *  if (this.c_rz) GetLog()<< " Rz";
             *  GetLog()<< *this.C << "\n";
             */
            int cnt = 0;

            for (int i = 0; i < mask.nconstr; i++)
            {
                if (mask.Constr_N(i).IsActive())
                {
                    if (do_clamp)
                    {
                        if (mask.Constr_N(i).IsUnilateral())
                        {
                            mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
                                                     ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp));
                        }
                        else
                        {
                            mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() +
                                                     ChMaths.ChMin(ChMaths.ChMax(factor * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp));
                        }
                    }
                    else
                    {
                        mask.Constr_N(i).Set_b_i(mask.Constr_N(i).Get_b_i() + factor * C.matrix.ElementN(cnt));
                    }

                    cnt++;
                }
            }
        }
コード例 #7
0
        public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
        {
            if (!IsActive())
            {
                return;
            }

            double cnstr_dist_violation = do_clamp
                                              ? ChMaths.ChMin(ChMaths.ChMax(factor * (m_cur_dist - m_dist), -recovery_clamp), recovery_clamp)
                                              : factor * (m_cur_dist - m_dist);

            double cnstr_dot_violation =
                do_clamp ? ChMaths.ChMin(ChMaths.ChMax(factor * m_cur_dot, -recovery_clamp), recovery_clamp) : factor * m_cur_dot;

            m_cnstr_dist.Set_b_i(m_cnstr_dist.Get_b_i() + cnstr_dist_violation);
            m_cnstr_dot.Set_b_i(m_cnstr_dot.Get_b_i() + cnstr_dot_violation);
        }
コード例 #8
0
        public override void IntLoadConstraint_C(int off_L,
                                                 ref ChVectorDynamic <double> Qc,
                                                 double c,
                                                 bool do_clamp,
                                                 double recovery_clamp)
        {
            double res = 0;  // no residual anyway! allow drifting...

            double cnstr_violation = c * res;

            if (do_clamp)
            {
                cnstr_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_violation, -recovery_clamp), recovery_clamp);
            }

            Qc.matrix[off_L] += cnstr_violation;
        }
コード例 #9
0
        public override void IntLoadConstraint_C(int off_L,
                                                 ref ChVectorDynamic <double> Qc,
                                                 double c,
                                                 bool do_clamp,
                                                 double recovery_clamp)
        {
            if (!IsActive())
            {
                return;
            }

            double cnstr_dist_violation =
                do_clamp ? ChMaths.ChMin(ChMaths.ChMax(c * (m_cur_dist - m_dist), -recovery_clamp), recovery_clamp) : c * (m_cur_dist - m_dist);

            double cnstr_dot_violation =
                do_clamp ? ChMaths.ChMin(ChMaths.ChMax(c * m_cur_dot, -recovery_clamp), recovery_clamp) : c * m_cur_dot;

            Qc.matrix[off_L + 0] += cnstr_dist_violation;
            Qc.matrix[off_L + 1] += cnstr_dot_violation;
        }
コード例 #10
0
        public override void IntLoadConstraint_C(int off_L,
                                                 ref ChVectorDynamic <double> Qc,
                                                 double c,
                                                 bool do_clamp,
                                                 double recovery_clamp)
        {
            if (!IsActive())
            {
                return;
            }

            if (do_clamp)
            {
                Qc.matrix[off_L] += ChMaths.ChMin(ChMaths.ChMax(c * (curr_dist - distance), -recovery_clamp), recovery_clamp);
            }
            else
            {
                Qc.matrix[off_L] += c * (curr_dist - distance);
            }
        }
コード例 #11
0
        public override void ConstraintsBiLoad_C(double factor = 1, double recovery_clamp = 0.1, bool do_clamp = false)
        {
            double C;

            if (this.avoid_angle_drift)
            {
                C = this.GetMotorRot() - aux_dt - this.rot_offset;
            }
            else
            {
                C = 0.0;
            }

            double res = factor * C;

            if (do_clamp)
            {
                res = ChMaths.ChMin(ChMaths.ChMax(res, -recovery_clamp), recovery_clamp);
            }

            constraint.Set_b_i(constraint.Get_b_i() + res);
        }
コード例 #12
0
        public override void IntLoadConstraint_C(int off_L, ref ChVectorDynamic <double> Qc, double c, bool do_clamp, double recovery_clamp)
        {
            // Add the time-dependent term in residual C as
            //   C = d_error - d_setpoint - d_offset
            // with d_error = x_pos_A-x_pos_B, and d_setpoint = x(t)
            double C;

            if (this.avoid_angle_drift)
            {
                C = this.GetMotorRot() - aux_dt - this.rot_offset;
            }
            else
            {
                C = 0.0;
            }

            double res = c * C;

            if (do_clamp)
            {
                res = ChMaths.ChMin(ChMaths.ChMax(res, -recovery_clamp), recovery_clamp);
            }
            Qc.matrix[off_L] += res;
        }
コード例 #13
0
        /// Performs the static analysis,
        /// doing a linear solve.

        public override void StaticAnalysis()
        {
            ChIntegrableIIorder mintegrable = (ChIntegrableIIorder)this.integrable;

            // setup main vectors
            mintegrable.StateSetup(ref X, ref V, ref A);

            ChState                  Xnew = new ChState();
            ChStateDelta             Dx   = new ChStateDelta();
            ChVectorDynamic <double> R    = new ChVectorDynamic <double>();
            ChVectorDynamic <double> Qc   = new ChVectorDynamic <double>();
            double T = 0;

            // setup auxiliary vectors
            Dx.Reset(mintegrable.GetNcoords_v(), GetIntegrable());
            Xnew.Reset(mintegrable.GetNcoords_x(), mintegrable);
            R.Reset(mintegrable.GetNcoords_v());
            Qc.Reset(mintegrable.GetNconstr());
            L.Reset(mintegrable.GetNconstr());

            mintegrable.StateGather(ref X, ref V, ref T);  // state <- system

            // Set speed to zero
            V.matrix.FillElem(0);

            // Extrapolate a prediction as warm start
            Xnew = X;

            // use Newton Raphson iteration to solve implicit Euler for v_new
            //
            // [ - dF/dx    Cq' ] [ Dx  ] = [ f ]
            // [ Cq         0   ] [ L   ] = [ C ]

            for (int i = 0; i < this.GetMaxiters(); ++i)
            {
                mintegrable.StateScatter(Xnew, V, T);  // state -> system
                R.Reset();
                Qc.Reset();
                mintegrable.LoadResidual_F(ref R, 1.0);
                mintegrable.LoadConstraint_C(ref Qc, 1.0);

                double cfactor = ChMaths.ChMin(1.0, ((double)(i + 2) / (double)(incremental_steps + 1)));
                R  *= cfactor;
                Qc *= cfactor;

                //	GetLog()<< "Non-linear statics iteration=" << i << "  |R|=" << R.NormInf() << "  |Qc|=" << Qc.NormInf()
                //<< "\n";
                if ((R.matrix.NormInf() < this.GetTolerance()) && (Qc.matrix.NormInf() < this.GetTolerance()))
                {
                    break;
                }

                mintegrable.StateSolveCorrection(
                    ref Dx, ref L, R, Qc,
                    0,           // factor for  M
                    0,           // factor for  dF/dv
                    -1.0,        // factor for  dF/dx (the stiffness matrix)
                    Xnew, V, T,  // not needed here
                    false,       // do not StateScatter update to Xnew Vnew T+dt before computing correction
                    true         // force a call to the solver's Setup() function
                    );

                Xnew += Dx;
            }

            X = Xnew;

            mintegrable.StateScatter(X, V, T);    // state -> system
            mintegrable.StateScatterReactions(L); // -> system auxiliary data
        }
コード例 #14
0
        // Updates motion laws, marker positions, etc.
        public override void UpdateTime(double mytime)
        {
            // First, inherit to parent class
            base.UpdateTime(mytime);

            // Move markers 1 and 2 to align them as gear teeth

            ChMatrix33 <double> ma1        = new ChMatrix33 <double>(0);
            ChMatrix33 <double> ma2        = new ChMatrix33 <double>(0);
            ChMatrix33 <double> mrotma     = new ChMatrix33 <double>(0);
            ChMatrix33 <double> marot_beta = new ChMatrix33 <double>(0);
            ChVector            mx;
            ChVector            my;
            ChVector            mz;
            ChVector            mr;
            ChVector            mmark1;
            ChVector            mmark2;
            ChVector            lastX;
            ChVector            vrota;
            ChCoordsys          newmarkpos = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));

            ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; // new ChFrame<double>();
            ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();

            ((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
            ((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);

            ChVector vbdist = ChVector.Vsub(Get_shaft_pos1(), Get_shaft_pos2());
            // ChVector Trad1 = ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir1(), vbdist))));
            // ChVector Trad2 = ChVector.Vnorm(ChVector.Vcross(ChVector.Vnorm(ChVector.Vcross(Get_shaft_dir2(), vbdist)), Get_shaft_dir2()));

            double dist = ChVector.Vlength(vbdist);

            // compute actual rotation of the two wheels (relative to truss).
            ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(-vbdist);

            md1.z = 0;
            md1   = ChVector.Vnorm(md1);
            ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(-vbdist);

            md2.z = 0;
            md2   = ChVector.Vnorm(md2);

            double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
            double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
            double old_a1      = a1;
            double old_a2      = a2;
            double turns_a1    = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
            double turns_a2    = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
            double a1U         = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
            double a1M         = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
            double a1L         = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;

            a1 = a1M;
            if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
            {
                a1 = a1U;
            }
            if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
            {
                a1 = a1L;
            }
            double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
            double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
            double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;

            a2 = a2M;
            if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
            {
                a2 = a2U;
            }
            if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
            {
                a2 = a2L;
            }

            // compute new markers coordsystem alignment
            my = ChVector.Vnorm(vbdist);
            mz = Get_shaft_dir1();
            mx = ChVector.Vnorm(ChVector.Vcross(my, mz));
            mr = ChVector.Vnorm(ChVector.Vcross(mz, mx));
            mz = ChVector.Vnorm(ChVector.Vcross(mx, my));
            ChVector mz2, mx2, mr2, my2;

            my2 = my;
            mz2 = Get_shaft_dir2();
            mx2 = ChVector.Vnorm(ChVector.Vcross(my2, mz2));
            mr2 = ChVector.Vnorm(ChVector.Vcross(mz2, mx2));

            ma1.Set_A_axis(mx, my, mz);

            // rotate csys because of beta
            vrota.x = 0.0;
            vrota.y = beta;
            vrota.z = 0.0;
            mrotma.Set_A_Rxyz(vrota);
            marot_beta.nm.matrix.MatrMultiply(ma1.nm.matrix, mrotma.nm.matrix);
            // rotate csys because of alpha
            vrota.x = 0.0;
            vrota.y = 0.0;
            vrota.z = alpha;
            if (react_force.x < 0)
            {
                vrota.z = alpha;
            }
            else
            {
                vrota.z = -alpha;
            }
            mrotma.Set_A_Rxyz(vrota);
            ma1.nm.matrix.MatrMultiply(marot_beta.nm.matrix, mrotma.nm.matrix);

            ma2.nm.matrix.CopyFromMatrix(ma1.nm.matrix);

            // is a bevel gear?
            double be       = Math.Acos(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2()));
            bool   is_bevel = true;

            if (Math.Abs(ChVector.Vdot(Get_shaft_dir1(), Get_shaft_dir2())) > 0.96)
            {
                is_bevel = false;
            }

            // compute wheel radii so that:
            //            w2 = - tau * w1
            if (!is_bevel)
            {
                double pardist = ChVector.Vdot(mr, vbdist);
                double inv_tau = 1.0 / tau;
                if (!epicyclic)
                {
                    r2 = pardist - pardist / (inv_tau + 1.0);
                }
                else
                {
                    r2 = pardist - (tau * pardist) / (tau - 1.0);
                }
                r1 = r2 * tau;
            }
            else
            {
                double gamma2;
                if (!epicyclic)
                {
                    gamma2 = be / (tau + 1.0);
                }
                else
                {
                    gamma2 = be / (-tau + 1.0);
                }
                double al = ChMaths.CH_C_PI - Math.Acos(ChVector.Vdot(Get_shaft_dir2(), my));
                double te = ChMaths.CH_C_PI - al - be;
                double fd = Math.Sin(te) * (dist / Math.Sin(be));
                r2 = fd * Math.Tan(gamma2);
                r1 = r2 * tau;
            }

            // compute markers positions, supposing they
            // stay on the ideal wheel contact point
            mmark1     = ChVector.Vadd(Get_shaft_pos2(), ChVector.Vmul(mr2, r2));
            mmark2     = mmark1;
            contact_pt = mmark1;

            // correct marker 1 position if phasing is not correct
            if (checkphase)
            {
                double realtau = tau;
                if (epicyclic)
                {
                    realtau = -tau;
                }
                double m_delta;
                m_delta = -(a2 / realtau) - a1 - phase;

                if (m_delta > ChMaths.CH_C_PI)
                {
                    m_delta -= (ChMaths.CH_C_2PI);  // range -180..+180 is better than 0...360
                }
                if (m_delta > (ChMaths.CH_C_PI / 4.0))
                {
                    m_delta = (ChMaths.CH_C_PI / 4.0);  // phase correction only in +/- 45°
                }
                if (m_delta < -(ChMaths.CH_C_PI / 4.0))
                {
                    m_delta = -(ChMaths.CH_C_PI / 4.0);
                }

                vrota.x = vrota.y = 0.0;
                vrota.z = -m_delta;
                mrotma.Set_A_Rxyz(vrota);  // rotate about Z of shaft to correct
                mmark1 = abs_shaft1.GetA().MatrT_x_Vect(ChVector.Vsub(mmark1, Get_shaft_pos1()));
                mmark1 = mrotma.Matr_x_Vect(mmark1);
                mmark1 = ChVector.Vadd(abs_shaft1.GetA().Matr_x_Vect(mmark1), Get_shaft_pos1());
            }
            // Move Shaft 1 along its direction if not aligned to wheel
            double   offset = ChVector.Vdot(Get_shaft_dir1(), (contact_pt - Get_shaft_pos1()));
            ChVector moff   = Get_shaft_dir1() * offset;

            if (Math.Abs(offset) > 0.0001)
            {
                local_shaft1.SetPos(local_shaft1.GetPos() + Body1.TransformDirectionParentToLocal(moff));
            }

            // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
            marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
            marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);

            // move marker1 in proper positions
            newmarkpos.pos = mmark1;
            newmarkpos.rot = ma1.Get_A_quaternion();
            marker1.Impose_Abs_Coord(newmarkpos);  // move marker1 into teeth position
                                                   // move marker2 in proper positions
            newmarkpos.pos = mmark2;
            newmarkpos.rot = ma2.Get_A_quaternion();
            marker2.Impose_Abs_Coord(newmarkpos);  // move marker2 into teeth position

            // imposed relative positions/speeds
            deltaC.pos      = ChVector.VNULL;
            deltaC_dt.pos   = ChVector.VNULL;
            deltaC_dtdt.pos = ChVector.VNULL;

            deltaC.rot      = ChQuaternion.QUNIT; // no relative rotations imposed!
            deltaC_dt.rot   = ChQuaternion.QNULL;
            deltaC_dtdt.rot = ChQuaternion.QNULL;
        }
コード例 #15
0
        public override void ContIntLoadConstraint_C(int off_L,
                                                     ref ChVectorDynamic <double> Qc,
                                                     double c,
                                                     bool do_clamp,
                                                     double recovery_clamp
                                                     )
        {
            bool bounced = false;

            // Elastic Restitution model (use simple Newton model with coefficient e=v(+)/v(-))
            // Note that this works only if the two connected items are two ChBody.

            if (this.objA != null && this.objB != null)
            {
                var oA = (ChBody)(object)this.objA;
                var oB = (ChBody)(object)this.objB;
                if (this.restitution != 0)
                {
                    // compute normal rebounce speed
                    ChVector V1_w        = oA.GetContactPointSpeed(this.p1);
                    ChVector V2_w        = oB.GetContactPointSpeed(this.p2);
                    ChVector Vrel_w      = V2_w - V1_w;
                    ChVector Vrel_cplane = this.contact_plane.MatrT_x_Vect(Vrel_w);

                    double h = this.container.GetSystem().GetStep();  // = 1.0 / c;  // not all steppers have c = 1/h

                    double neg_rebounce_speed = Vrel_cplane.x * this.restitution;
                    if (neg_rebounce_speed < -this.container.GetSystem().GetMinBounceSpeed())
                    {
                        if (this.norm_dist + neg_rebounce_speed * h < 0)
                        {
                            // CASE: BOUNCE
                            bounced           = true;
                            Qc.matrix[off_L] += neg_rebounce_speed;
                        }
                    }
                }
            }

            if (!bounced)
            {
                // CASE: SETTLE (most often, and also default if two colliding items are not two ChBody)

                if (this.compliance != 0)
                {
                    double h = 1.0 / c;                        // was: this->container->GetSystem()->GetStep(); note not all steppers have c = 1/h

                    double alpha    = this.dampingf;           // [R]=alpha*[K]
                    double inv_hpa  = 1.0 / (h + alpha);       // 1/(h+a)
                    double inv_hhpa = 1.0 / (h * (h + alpha)); // 1/(h*(h+a))

                    //***TODO*** move to KRMmatricesLoad() the following, and only for !bounced case
                    Nx.Set_cfm_i((inv_hhpa) * this.compliance);
                    Tu.Set_cfm_i((inv_hhpa) * this.complianceT);
                    Tv.Set_cfm_i((inv_hhpa) * this.complianceT);

                    double qc = inv_hpa * this.norm_dist;  //***TODO*** see how to move this in KRMmatricesLoad()

                    // Note: clamping of Qc in case of compliance is questionable: it does not limit only the outbound
                    // speed, but also the reaction, so it might allow longer 'sinking' not related to the real compliance.
                    // I.e. If clamping kicks in (when using large timesteps and low compliance), it acts as a numerical damping.
                    if (do_clamp)
                    {
                        qc = ChMaths.ChMax(qc, -recovery_clamp);
                    }

                    Qc.matrix[off_L] += qc;
                }
                else
                {
                    if (do_clamp)
                    {
                        if (this.Nx.Constraint2TuplesNall.GetCohesion() != 0)
                        {
                            Qc.matrix[off_L] += ChMaths.ChMin(0.0, ChMaths.ChMax(c * this.norm_dist, -recovery_clamp));
                        }
                        else
                        {
                            Qc.matrix[off_L] += ChMaths.ChMax(c * this.norm_dist, -recovery_clamp);
                        }
                    }
                    else
                    {
                        Qc.matrix[off_L] += c * this.norm_dist;
                    }
                }
            }
        }
コード例 #16
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);
        }
コード例 #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);
        }
コード例 #18
0
        /// Updates motion laws, marker positions, etc.
        public override void UpdateTime(double mytime)
        {
            // First, inherit to parent class
            base.UpdateTime(mytime);

            ChFrame <double> abs_shaft1 = ChFrame <double> .FNULL; //new ChFrame<double>();
            ChFrame <double> abs_shaft2 = ChFrame <double> .FNULL; //new ChFrame<double>();

            ((ChFrame <double>)Body1).TransformLocalToParent(local_shaft1, abs_shaft1);
            ((ChFrame <double>)Body2).TransformLocalToParent(local_shaft2, abs_shaft2);

            ChVector dcc_w = ChVector.Vsub(Get_shaft_pos2(), Get_shaft_pos1());

            // compute actual rotation of the two wheels (relative to truss).
            ChVector md1 = abs_shaft1.GetA().MatrT_x_Vect(dcc_w);

            md1.z = 0;
            md1   = ChVector.Vnorm(md1);
            ChVector md2 = abs_shaft2.GetA().MatrT_x_Vect(dcc_w);

            md2.z = 0;
            md2   = ChVector.Vnorm(md2);

            double periodic_a1 = ChMaths.ChAtan2(md1.x, md1.y);
            double periodic_a2 = ChMaths.ChAtan2(md2.x, md2.y);
            double old_a1      = a1;
            double old_a2      = a2;
            double turns_a1    = Math.Floor(old_a1 / ChMaths.CH_C_2PI);
            double turns_a2    = Math.Floor(old_a2 / ChMaths.CH_C_2PI);
            double a1U         = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 + ChMaths.CH_C_2PI;
            double a1M         = turns_a1 * ChMaths.CH_C_2PI + periodic_a1;
            double a1L         = turns_a1 * ChMaths.CH_C_2PI + periodic_a1 - ChMaths.CH_C_2PI;

            a1 = a1M;
            if (Math.Abs(a1U - old_a1) < Math.Abs(a1M - old_a1))
            {
                a1 = a1U;
            }
            if (Math.Abs(a1L - a1) < Math.Abs(a1M - a1))
            {
                a1 = a1L;
            }
            double a2U = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 + ChMaths.CH_C_2PI;
            double a2M = turns_a2 * ChMaths.CH_C_2PI + periodic_a2;
            double a2L = turns_a2 * ChMaths.CH_C_2PI + periodic_a2 - ChMaths.CH_C_2PI;

            a2 = a2M;
            if (Math.Abs(a2U - old_a2) < Math.Abs(a2M - old_a2))
            {
                a2 = a2U;
            }
            if (Math.Abs(a2L - a2) < Math.Abs(a2M - a2))
            {
                a2 = a2L;
            }

            // correct marker positions if phasing is not correct
            double m_delta = 0;

            if (checkphase)
            {
                double realtau = tau;

                m_delta = a1 - phase - (a2 / realtau);

                if (m_delta > ChMaths.CH_C_PI)
                {
                    m_delta -= (ChMaths.CH_C_2PI);  // range -180..+180 is better than 0...360
                }
                if (m_delta > (ChMaths.CH_C_PI / 4.0))
                {
                    m_delta = (ChMaths.CH_C_PI / 4.0);  // phase correction only in +/- 45°
                }
                if (m_delta < -(ChMaths.CH_C_PI / 4.0))
                {
                    m_delta = -(ChMaths.CH_C_PI / 4.0);
                }
                //***TODO***
            }

            // Move markers 1 and 2 to align them as pulley ends

            ChVector d21_w = dcc_w - Get_shaft_dir1() * ChVector.Vdot(Get_shaft_dir1(), dcc_w);
            ChVector D21_w = ChVector.Vnorm(d21_w);

            shaft_dist = d21_w.Length();

            ChVector U1_w = ChVector.Vcross(Get_shaft_dir1(), D21_w);

            double gamma1 = Math.Acos((r1 - r2) / shaft_dist);

            ChVector Ru_w = D21_w * Math.Cos(gamma1) + U1_w * Math.Sin(gamma1);
            ChVector Rl_w = D21_w * Math.Cos(gamma1) - U1_w * Math.Sin(gamma1);

            belt_up1  = Get_shaft_pos1() + Ru_w * r1;
            belt_low1 = Get_shaft_pos1() + Rl_w * r1;
            belt_up2  = Get_shaft_pos1() + d21_w + Ru_w * r2;
            belt_low2 = Get_shaft_pos1() + d21_w + Rl_w * r2;

            // marker alignment
            ChMatrix33 <double> maU = new ChMatrix33 <double>(0);
            ChMatrix33 <double> maL = new ChMatrix33 <double>(0);

            ChVector Dxu = ChVector.Vnorm(belt_up2 - belt_up1);
            ChVector Dyu = Ru_w;
            ChVector Dzu = ChVector.Vnorm(ChVector.Vcross(Dxu, Dyu));

            Dyu = ChVector.Vnorm(ChVector.Vcross(Dzu, Dxu));
            maU.Set_A_axis(Dxu, Dyu, Dzu);

            // ! Require that the BDF routine of marker won't handle speed and acc.calculus of the moved marker 2!
            marker2.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);
            marker1.SetMotionType(ChMarker.eChMarkerMotion.M_MOTION_EXTERNAL);

            ChCoordsys newmarkpos = new ChCoordsys();

            // move marker1 in proper positions
            newmarkpos.pos = this.belt_up1;
            newmarkpos.rot = maU.Get_A_quaternion();
            marker1.Impose_Abs_Coord(newmarkpos);  // move marker1 into teeth position
                                                   // move marker2 in proper positions
            newmarkpos.pos = this.belt_up2;
            newmarkpos.rot = maU.Get_A_quaternion();
            marker2.Impose_Abs_Coord(newmarkpos);  // move marker2 into teeth position

            double phase_correction_up  = m_delta * r1;
            double phase_correction_low = -phase_correction_up;
            double hU = ChVector.Vlength(belt_up2 - belt_up1) + phase_correction_up;
            double hL = ChVector.Vlength(belt_low2 - belt_low1) + phase_correction_low;

            // imposed relative positions/speeds
            deltaC.pos      = new ChVector(-hU, 0, 0);
            deltaC_dt.pos   = ChVector.VNULL;
            deltaC_dtdt.pos = ChVector.VNULL;

            deltaC.rot      = ChQuaternion.QUNIT; // no relative rotations imposed!
            deltaC_dt.rot   = ChQuaternion.QNULL;
            deltaC_dtdt.rot = ChQuaternion.QNULL;
        }
コード例 #19
0
 /// Set the user modulation of the torque (or brake, if you use it between
 /// a fixed shaft and a free shaft). The modulation must range from
 /// 0 (switched off) to 1 (max torque). Default is 1, when clutch is created.
 /// You can update this during integration loop to simulate the pedal pushing by the driver.
 public void SetModulation(double mm)
 {
     modulation = ChMaths.ChMax(ChMaths.ChMin(mm, 1.0), 0.0);
 }