Beispiel #1
0
        //
        // UPDATING
        //

        /// Updates the time.dependant variables (ex: ChFunction objects
        /// which impose the body-relative motion, etc.)
        public void UpdateTime(double mytime)
        {
            ChCoordsys   csys      = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
            ChCoordsys   csys_dt   = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
            ChCoordsys   csys_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0));
            ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0);
            double       ang, ang_dt, ang_dtdt;

            ChTime = mytime;

            // if a imposed motion (keyframed movement) affects the marker position (example,from R3D animation system),
            // compute the speed and acceleration values by BDF (example,see the UpdatedExternalTime() function, later)
            // so the updating via motion laws can be skipped!
            if (motion_type == eChMarkerMotion.M_MOTION_KEYFRAMED)
            {
                return;
            }

            // skip relative-position-functions evaluation also if
            // someone is already handling this from outside..
            if (motion_type == eChMarkerMotion.M_MOTION_EXTERNAL)
            {
                return;
            }

            // positions:
            // update positions:    rel_pos
            csys.pos.x = motion_X.Get_y(mytime);
            csys.pos.y = motion_Y.Get_y(mytime);
            csys.pos.z = motion_Z.Get_y(mytime);
            if (motion_X.Get_Type() != ChFunction.FunctionType.FUNCT_MOCAP)
            {
                csys.pos += rest_coord.pos;
            }

            // update speeds:		rel_pos_dt
            csys_dt.pos.x = motion_X.Get_y_dx(mytime);
            csys_dt.pos.y = motion_Y.Get_y_dx(mytime);
            csys_dt.pos.z = motion_Z.Get_y_dx(mytime);

            // update accelerations
            csys_dtdt.pos.x = motion_X.Get_y_dxdx(mytime);
            csys_dtdt.pos.y = motion_Y.Get_y_dxdx(mytime);
            csys_dtdt.pos.z = motion_Z.Get_y_dxdx(mytime);

            // rotations:

            ang      = motion_ang.Get_y(mytime);
            ang_dt   = motion_ang.Get_y_dx(mytime);
            ang_dtdt = motion_ang.Get_y_dxdx(mytime);

            if ((ang != 0) || (ang_dt != 0) || (ang_dtdt != 0))
            {
                // update q
                ChVector motion_axis_versor = ChVector.Vnorm(motion_axis);
                qtemp    = ChQuaternion.Q_from_AngAxis2(ang, motion_axis_versor);
                csys.rot = ChQuaternion.Qcross(qtemp, rest_coord.rot);
                // update q_dt
                csys_dt.rot = ChQuaternion.Qdt_from_AngAxis(csys.rot, ang_dt, motion_axis_versor);
                // update q_dtdt
                csys_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(ang_dtdt, motion_axis_versor, csys.rot, csys_dt.rot);
            }
            else
            {
                csys.rot      = FrameMoving.coord.rot; // rel_pos.rot;
                csys_dt.rot   = ChQuaternion.QNULL;
                csys_dtdt.rot = ChQuaternion.QNULL;
            }

            // Set the position, speed and acceleration in relative space,
            // automatically getting also the absolute values,
            if (!(csys == this.FrameMoving.coord))
            {
                FrameMoving.SetCoord(csys);
            }

            if (!(csys_dt == this.FrameMoving.coord_dt) || !(csys_dt.rot == new ChQuaternion(0, 0, 0, 0)))
            {
                FrameMoving.SetCoord_dt(csys_dt);
            }

            if (!(csys_dtdt == this.FrameMoving.coord_dtdt) || !(csys_dtdt.rot == new ChQuaternion(0, 0, 0, 0)))
            {
                FrameMoving.SetCoord_dtdt(csys_dtdt);
            }
        }
Beispiel #2
0
        /// Updates motion laws, etc. for the impose rotation / impose speed modes
        public override void UpdateTime(double mytime)
        {
            // First, inherit to parent class
            base.UpdateTime(mytime);

            if (!IsActive())
            {
                return;
            }

            // DEFAULTS compute rotation vars...
            // by default for torque control..

            motion_axis    = ChVector.VECT_Z; // motion axis is always the marker2 Z axis (in m2 relative coords)
            mot_rot        = relAngle;
            mot_rot_dt     = ChVector.Vdot(relWvel, motion_axis);
            mot_rot_dtdt   = ChVector.Vdot(relWacc, motion_axis);
            mot_rerot      = mot_rot / mot_tau;
            mot_rerot_dt   = mot_rot_dt / mot_tau;
            mot_rerot_dtdt = mot_rot_dtdt / mot_tau;

            // nothing more to do here for torque control
            if (eng_mode == eCh_eng_mode.ENG_MODE_TORQUE)
            {
                return;
            }

            // If LEARN MODE, just record motion
            if (learn)
            {
                deltaC.pos      = ChVector.VNULL;
                deltaC_dt.pos   = ChVector.VNULL;
                deltaC_dtdt.pos = ChVector.VNULL;
                if (!(limit_Rx.Get_active() || limit_Ry.Get_active() || limit_Rz.Get_active()))
                {
                    deltaC.rot      = ChQuaternion.QUNIT;
                    deltaC_dt.rot   = ChQuaternion.QNULL;
                    deltaC_dtdt.rot = ChQuaternion.QNULL;
                }

                if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION)
                {
                    if (rot_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
                    {
                        rot_funct = new ChFunction_Recorder();
                    }

                    // record point
                    double rec_rot = relAngle;  // ***TO DO*** compute also rotations with cardano mode?
                    if (impose_reducer)
                    {
                        rec_rot = rec_rot / mot_tau;
                    }
                    ChFunction_Recorder rec = (ChFunction_Recorder)rot_funct;
                    rec.AddPoint(mytime, rec_rot, 1);  // x=t
                }

                if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED)
                {
                    if (spe_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER)
                    {
                        spe_funct = new ChFunction_Recorder();
                    }

                    // record point
                    double rec_spe = ChVector.Vlength(relWvel);  // ***TO DO*** compute also with cardano mode?
                    if (impose_reducer)
                    {
                        rec_spe = rec_spe / mot_tau;
                    }
                    ChFunction_Recorder rec = (ChFunction_Recorder)spe_funct;
                    rec.AddPoint(mytime, rec_spe, 1);  //  x=t
                }
            }

            if (learn)
            {
                return;  // no need to go on further...--.>>>
            }
            // Impose relative positions/speeds

            deltaC.pos      = ChVector.VNULL;
            deltaC_dt.pos   = ChVector.VNULL;
            deltaC_dtdt.pos = ChVector.VNULL;

            if (eng_mode == eCh_eng_mode.ENG_MODE_ROTATION)
            {
                if (impose_reducer)
                {
                    mot_rerot      = rot_funct.Get_y(ChTime);
                    mot_rerot_dt   = rot_funct.Get_y_dx(ChTime);
                    mot_rerot_dtdt = rot_funct.Get_y_dxdx(ChTime);
                    mot_rot        = mot_rerot * mot_tau;
                    mot_rot_dt     = mot_rerot_dt * mot_tau;
                    mot_rot_dtdt   = mot_rerot_dtdt * mot_tau;
                }
                else
                {
                    mot_rot        = rot_funct.Get_y(ChTime);
                    mot_rot_dt     = rot_funct.Get_y_dx(ChTime);
                    mot_rot_dtdt   = rot_funct.Get_y_dxdx(ChTime);
                    mot_rerot      = mot_rot / mot_tau;
                    mot_rerot_dt   = mot_rot_dt / mot_tau;
                    mot_rerot_dtdt = mot_rot_dtdt / mot_tau;
                }
                deltaC.rot      = ChQuaternion.Q_from_AngAxis2(mot_rot, motion_axis);
                deltaC_dt.rot   = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis);
                deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot);
            }

            if (eng_mode == eCh_eng_mode.ENG_MODE_SPEED)
            {
                if (impose_reducer)
                {
                    mot_rerot_dt   = spe_funct.Get_y(ChTime);
                    mot_rerot_dtdt = spe_funct.Get_y_dx(ChTime);
                    mot_rot_dt     = mot_rerot_dt * mot_tau;
                    mot_rot_dtdt   = mot_rerot_dtdt * mot_tau;
                }
                else
                {
                    mot_rot_dt     = spe_funct.Get_y(ChTime);
                    mot_rot_dtdt   = spe_funct.Get_y_dx(ChTime);
                    mot_rerot_dt   = mot_rot_dt / mot_tau;
                    mot_rerot_dtdt = mot_rot_dtdt / mot_tau;
                }
                deltaC.rot = ChQuaternion.Qnorm(GetRelM().rot); // just keep current position, -assume always good after integration-.
                ChMatrix33 <double> relA = new ChMatrix33 <double>(0);
                relA.Set_A_quaternion(GetRelM().rot);           // ..but adjust to keep Z axis aligned to shaft, anyway!
                ChVector displaced_z_axis = relA.Get_A_Zaxis();
                ChVector adjustment       = ChVector.Vcross(displaced_z_axis, ChVector.VECT_Z);
                deltaC.rot      = ChQuaternion.Q_from_AngAxis2(ChVector.Vlength(adjustment), ChVector.Vnorm(adjustment)) % deltaC.rot;
                deltaC_dt.rot   = ChQuaternion.Qdt_from_AngAxis(deltaC.rot, mot_rot_dt, motion_axis);
                deltaC_dtdt.rot = ChQuaternion.Qdtdt_from_AngAxis(mot_rot_dtdt, motion_axis, deltaC.rot, deltaC_dt.rot);
            }
        }