public double Get_polar_max(double pol_ang) { if (polar_Max == null) { return(0.001); } return(polar_Max.Get_y(pol_ang)); }
/// Set body-relative coord. and update auxiliary variables /// Also, current position becomes the 'resting position' coordinates /// for the current time. public void Impose_Rel_Coord(ChCoordsys m_coord) { ChQuaternion qtemp;// = new ChQuaternion(1, 0, 0, 0); // set the actual coordinates FrameMoving.SetCoord(m_coord); // set the resting position coordinates rest_coord.pos.x = m_coord.pos.x - motion_X.Get_y(ChTime); rest_coord.pos.y = m_coord.pos.y - motion_Y.Get_y(ChTime); rest_coord.pos.z = m_coord.pos.z - motion_Z.Get_y(ChTime); qtemp = ChQuaternion.Q_from_AngAxis2(-(motion_ang.Get_y(ChTime)), motion_axis); rest_coord.rot = ChQuaternion.Qcross(m_coord.rot, qtemp); // ***%%% check // set also the absolute positions, and other. UpdateState(); }
public double Get_Rcurrent(double x, double x_dt, double t) { double mR = 0; if (active) { double modulator; if (modul_R != null) { modulator = modul_R.Get_y(x); } else { modulator = 1; } mR = R * modulator; } return(mR); }
public double Get_Kcurrent(double x, double x_dt, double t) { double mK = 0; if (active) { double modulator; if (modul_K != null) { modulator = modul_K.Get_y(x); } else { modulator = 1; } mK = K * modulator; } return(mK); }
public double Get_iFcurrent(double x, double x_dt, double t) { double mforce = 0; if (active) { double modulator; // the internal force contribute = iforce if (modul_iforce != null) { modulator = modul_iforce.Get_y(t); } else { modulator = 1; } mforce = iforce * modulator; } return(mforce); }
/// Inherits, then also adds the spring custom forces to the C_force and C_torque. public override void UpdateForces(double mytime) { // Inherit force computation: // also base class can add its own forces. base.UpdateForces(mytime); spr_react = 0.0; ChVector m_force; double deform = Get_SpringDeform(); spr_react = spr_f * mod_f_time.Get_y(ChTime); spr_react -= (spr_k * mod_k_d.Get_y(deform) * mod_k_speed.Get_y(dist_dt)) * (deform); spr_react -= (spr_r * mod_r_d.Get_y(deform) * mod_r_speed.Get_y(dist_dt)) * (dist_dt); m_force = ChVector.Vmul(ChVector.Vnorm(relM.pos), spr_react); C_force = ChVector.Vadd(C_force, m_force); }
/// 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); } }
public double GetForce(double x, double x_dt) { double cush_coord; double cush_coord_norm; double force; double m_min, m_max; if (!penalty_only) { m_min = min; m_max = max; } else { m_min = -999999999; m_max = 999999999; } if ((x > m_min) && (x < (min + minCushion))) { cush_coord = (min + minCushion) - x; if (minCushion >= 0.0000001) { cush_coord_norm = cush_coord / minCushion; } else { cush_coord_norm = 1; } if (cush_coord_norm > 1) { cush_coord_norm = 1; // clip cushion forces at stopper limit } force = cush_coord * Kmin * modul_Kmin.Get_y(cush_coord_norm); force += (-x_dt) * Rmin * modul_Rmin.Get_y(cush_coord_norm); if (force < 0) { force = 0; } // damping could cause neg force while going away, // so -as the limit is not "sticky"- clip force sign. return(force); } if ((x < m_max) && (x > (max - maxCushion))) { cush_coord = x - (max - maxCushion); if (maxCushion >= 0.0000001) { cush_coord_norm = cush_coord / maxCushion; } else { cush_coord_norm = 1; } if (cush_coord_norm > 1) { cush_coord_norm = 1; // clip cushion forces at stopper limit } force = (-cush_coord) * Kmax * modul_Kmax.Get_y(cush_coord_norm); force += (-x_dt) * Rmax * modul_Rmax.Get_y(cush_coord_norm); if (force > 0) { force = 0; } // damping could cause pos force while going away, // so -as the limit is not "sticky"- clip force sign. return(force); } return(0); }
// Updates motion laws, marker positions, etc. public override void UpdateTime(double mytime) { // First, inherit to parent class base.UpdateTime(mytime); // If LEARN MODE, just record motion if (learn) { /* do not change deltas, in free mode maybe that 'limit on X' changed them * deltaC.pos = VNULL; * deltaC_dt.pos = VNULL; * deltaC_dtdt.pos = VNULL; * deltaC.rot = QUNIT; * deltaC_dt.rot = QNULL; * deltaC_dtdt.rot = QNULL; */ // if (dist_funct.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER) // dist_funct = new ChFunction_Recorder(); // record point double rec_dist = ChVector.Vlength(ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos)); rec_dist -= offset; // (ChFunction_Recorder)(dist_funct).AddPoint(mytime, rec_dist, 1); // (x,y,w) x=t } // Move (well, rotate...) marker 2 to align it in actuator direction // ! 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); // ChMatrix33<double> ma = new ChMatrix33<double>(0); ma.Set_A_quaternion(marker2.GetAbsCoord().rot); ChVector absdist = ChVector.Vsub(marker1.GetAbsCoord().pos, marker2.GetAbsCoord().pos); ChVector mx = ChVector.Vnorm(absdist); ChVector my = ma.Get_A_Yaxis(); if (ChVector.Vequal(mx, my)) { if (mx.x == 1.0) { my = ChVector.VECT_Y; } else { my = ChVector.VECT_X; } } ChVector mz = ChVector.Vnorm(ChVector.Vcross(mx, my)); my = ChVector.Vnorm(ChVector.Vcross(mz, mx)); ma.Set_A_axis(mx, my, mz); ChCoordsys newmarkpos; ChVector oldpos = marker2.FrameMoving.GetPos(); // backup to avoid numerical err.accumulation newmarkpos.pos = marker2.GetAbsCoord().pos; newmarkpos.rot = ma.Get_A_quaternion(); marker2.Impose_Abs_Coord(newmarkpos); // rotate "main" marker2 into tangent position (may add err.accumulation) marker2.FrameMoving.SetPos(oldpos); // backup to avoid numerical err.accumulation if (learn) { return; // no need to go on further...--.>>> } // imposed relative positions/speeds deltaC.pos = ChVector.VNULL; deltaC.pos.x = dist_funct.Get_y(ChTime) + offset; // distance is always on M2 'X' axis deltaC_dt.pos = ChVector.VNULL; deltaC_dt.pos.x = dist_funct.Get_y_dx(ChTime); // distance speed deltaC_dtdt.pos = ChVector.VNULL; deltaC_dtdt.pos.x = dist_funct.Get_y_dxdx(ChTime); // distance acceleration // add also the centripetal acceleration if distance vector's rotating, // as centripetal acc. of point sliding on a sphere surface. ChVector tang_speed = GetRelM_dt().pos; tang_speed.x = 0; // only z-y coords in relative tang speed vector double len_absdist = ChVector.Vlength(absdist); // don't divide by zero if (len_absdist > 1E-6) { deltaC_dtdt.pos.x -= Math.Pow(ChVector.Vlength(tang_speed), 2) / ChVector.Vlength(absdist); // An = Adelta -(Vt^2 / r) } deltaC.rot = ChQuaternion.QUNIT; // no relative rotations imposed! deltaC_dt.rot = ChQuaternion.QNULL; deltaC_dtdt.rot = ChQuaternion.QNULL; // Compute motor variables // double m_rotation; // double m_torque; mot_rerot = (deltaC.pos.x - offset) / mot_tau; mot_rerot_dt = deltaC_dt.pos.x / mot_tau; mot_rerot_dtdt = deltaC_dtdt.pos.x / mot_tau; mot_retorque = mot_rerot_dtdt * mot_inertia + (react_force.x * mot_tau) / mot_eta; // m_rotation = (deltaC.pos.x() - offset) / mot_tau; // m_torque = (deltaC_dtdt.pos.x() / mot_tau) * mot_inertia + (react_force.x() * mot_tau) / mot_eta; if (learn_torque_rotation) { // if (mot_torque.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER) // mot_torque = new ChFunction_Recorder(); // if (mot_rot.Get_Type() != ChFunction.FunctionType.FUNCT_RECORDER) // mot_rot = new ChFunction_Recorder(); // std::static_pointer_cast<ChFunction_Recorder>(mot_torque).AddPoint(mytime, mot_retorque, 1); // (x,y,w) x=t // std::static_pointer_cast<ChFunction_Recorder>(mot_rot).AddPoint(mytime, mot_rerot, 1); // (x,y,w) x=t } }