/// Someone (ex. an ChExternalObject() ) may send this message to /// the marker to tell that time has changed (even if simulation is /// not running! - so it is different from the usual UpdateTime() -) public void UpdatedExternalTime(double prevtime, double mtime) { double mstep = mtime - prevtime; ChCoordsys m_rel_pos_dt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); ChCoordsys m_rel_pos_dtdt = new ChCoordsys(new ChVector(0, 0, 0), new ChQuaternion(1, 0, 0, 0)); // do not try to switch on the M_MOTION_KEYFRAMED mode if // we are already in the M_MOTION_EXTERNAL mode, maybe because // a link point-surface is already moving the marker and // it will handle the accelerations by itself if (this.motion_type == eChMarkerMotion.M_MOTION_EXTERNAL) { return; } // otherwise see if a BDF is needed, cause an external 3rd party is moving the marker this.motion_type = eChMarkerMotion.M_MOTION_FUNCTIONS; // if POSITION or ROTATION ("rel_pos") has been changed in acceptable time step... if ((!(ChVector.Vequal(FrameMoving.coord.pos, last_rel_coord.pos)) || !(ChQuaternion.Qequal(FrameMoving.coord.rot, last_rel_coord.rot))) && (Math.Abs(mstep) < 0.1) && (mstep != 0)) { // ... and if motion wasn't caused by motion laws, then it was a keyframed movement! if ((motion_X.Get_y(mtime) == 0) && (motion_Y.Get_y(mtime) == 0) && (motion_Z.Get_y(mtime) == 0) && (motion_ang.Get_y(mtime) == 0) && (motion_X.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_Y.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_Z.Get_Type() == ChFunction.FunctionType.FUNCT_CONST) && (motion_ang.Get_Type() == ChFunction.FunctionType.FUNCT_CONST)) { // compute the relative speed by BDF ! m_rel_pos_dt.pos = ChVector.Vmul(ChVector.Vsub(FrameMoving.coord.pos, last_rel_coord.pos), 1 / mstep); m_rel_pos_dt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(FrameMoving.coord.rot, last_rel_coord.rot), 1 / mstep); // compute the relative acceleration by BDF ! m_rel_pos_dtdt.pos = ChVector.Vmul(ChVector.Vsub(m_rel_pos_dt.pos, last_rel_coord_dt.pos), 1 / mstep); m_rel_pos_dtdt.rot = ChQuaternion.Qscale(ChQuaternion.Qsub(m_rel_pos_dt.rot, last_rel_coord_dt.rot), 1 / mstep); // Set the position, speed and acceleration in relative space, // automatically getting also the absolute values, FrameMoving.SetCoord_dt(m_rel_pos_dt); FrameMoving.SetCoord_dtdt(m_rel_pos_dtdt); // update the remaining state variables this.UpdateState(); // remember that the movement of this guy won't need further update // of speed and acc. via motion laws! this.motion_type = eChMarkerMotion.M_MOTION_KEYFRAMED; } } // restore state buffers and that's all. last_time = ChTime; last_rel_coord = (ChCoordsys)FrameMoving.coord; last_rel_coord_dt = (ChCoordsys)FrameMoving.coord_dt; }
// 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 } }