/// Get the link coordinate system, expressed relative to Body2 (spherical side). /// This represents the 'main' reference of the link: reaction forces /// and reaction torques are reported in this coordinate system. public override ChCoordsys GetLinkRelativeCoords() { ChVector pos1 = Body2.TransformPointParentToLocal(Body1.TransformPointLocalToParent(m_pos1)); ChMatrix33 <double> A = new ChMatrix33 <double>(0); ChVector u = (m_pos2 - pos1).GetNormalized(); ChVector w = Body2.TransformDirectionParentToLocal(Body1.TransformDirectionLocalToParent(m_dir1)); ChVector v = ChVector.Vcross(w, u); A.Set_A_axis(u, v, w); return(new ChCoordsys(pos1, A.Get_A_quaternion())); }
/// Get the link coordinate system, expressed relative to Body2 (the 'master' /// body). This represents the 'main' reference of the link: reaction forces /// are expressed in this coordinate system. /// (It is the coordinate system of the contact plane relative to Body2) public override ChCoordsys GetLinkRelativeCoords() { //ChVector D2local; ChVector D2temp = (ChVector.Vnorm(Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2))); ChVector D2rel = Body2.TransformDirectionParentToLocal(D2temp); ChVector Vx = new ChVector(0, 0, 0), Vy = new ChVector(0, 0, 0), Vz = new ChVector(0, 0, 0); //ChMatrix33<double> rel_matrix = new ChMatrix33<double>(); ChVector.XdirToDxDyDz(D2rel, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz); rel_matrix.Set_A_axis(Vx, Vy, Vz); ChQuaternion Ql2 = rel_matrix.Get_A_quaternion(); return(new ChCoordsys(pos2, Ql2)); }
// ----------------------------------------------------------------------------- // Draw a spring in 3D space, with given color. // ----------------------------------------------------------------------------- public static void drawSpring(double radius, ChVector start, ChVector end, int mresolution, double turns) { ChMatrix33 <double> rel_matrix = new ChMatrix33 <double>(0); ChVector dist = end - start; ChVector Vx = new ChVector(0, 0, 0); ChVector Vy = new ChVector(0, 0, 0); ChVector Vz = new ChVector(0, 0, 0); double length = dist.Length(); ChVector dir = ChVector.Vnorm(dist); ChVector.XdirToDxDyDz(dir, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz); rel_matrix.Set_A_axis(Vx, Vy, Vz); ChQuaternion Q12 = rel_matrix.Get_A_quaternion(); ChCoordsys mpos = new ChCoordsys(start, Q12); double phaseA = 0; double phaseB = 0; double heightA = 0; double heightB = 0; for (int iu = 1; iu <= mresolution; iu++) { phaseB = turns * ChMaths.CH_C_2PI * (double)iu / (double)mresolution; heightB = length * ((double)iu / mresolution); ChVector V1 = new ChVector(heightA, radius * Math.Cos(phaseA), radius * Math.Sin(phaseA)); ChVector V2 = new ChVector(heightB, radius * Math.Cos(phaseB), radius * Math.Sin(phaseB)); Gizmos.color = new Color(255, 255, 0); Gizmos.DrawLine(new Vector3((float)mpos.TransformLocalToParent(V1).x, (float)mpos.TransformLocalToParent(V1).y, (float)mpos.TransformLocalToParent(V1).z), new Vector3((float)mpos.TransformLocalToParent(V2).x, (float)mpos.TransformLocalToParent(V2).y, (float)mpos.TransformLocalToParent(V2).z)); phaseA = phaseB; heightA = heightB; } }
/// 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; }
/// Construct from pos and rotation (as a 3x3 matrix) public ChFrame(ChVector mv, ChMatrix33 <Real> ma) { coord = new ChCoordsys(mv, ma.Get_A_quaternion()); Amatrix = new ChMatrix33 <Real>(ma); }
/// Impose the rotation as a 3x3 matrix. /// Note: the rotation matrix must be already orthogonal! public virtual void SetRot(ChMatrix33 <Real> mA) { coord.rot = mA.Get_A_quaternion(); Amatrix.nm.matrix.CopyFromMatrix(mA.nm.matrix); }
// 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 } }
// 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; }