//FOR ALL SEGMENTS public static void Gen_QL(Segment S_self, Segment S_parent) { Segment Sc = S_self; Segment Sp = S_parent; if (Sc.ID == Sp.ID)//ID=11 { Sc.QL = Sc.QW; } else { CQuaternion tmp = new CQuaternion(Sp.QW.m_q0, Sp.QW.m_q1, Sp.QW.m_q2, Sp.QW.m_q3); tmp.Conjugate(); Sc.QL = tmp * Sc.QW; } if (Sc.isFirstTime) { Sc.PrevRot = new Euler(); Sc.isFirstTime = false; } else { Sc.PrevRot = new Euler(Sc.Eul_Out.Eul_1, Sc.Eul_Out.Eul_2, Sc.Eul_Out.Eul_3); } Euler ang = CQuaternion.Quat2angle(Sc.QL.m_q0, Sc.QL.m_q1, Sc.QL.m_q2, Sc.QL.m_q3, 3); double y = ang.Eul_1; double x = ang.Eul_2; double z = ang.Eul_3; Sc.Eul_Out.Eul_1 = y * 180 / System.Math.PI; Sc.Eul_Out.Eul_2 = x * 180 / System.Math.PI; Sc.Eul_Out.Eul_3 = z * 180 / System.Math.PI; #region DeQUIVER Euler diff = new Euler((Sc.PrevRot.Eul_1 - Sc.Eul_Out.Eul_1), (Sc.PrevRot.Eul_2 - Sc.Eul_Out.Eul_2), (Sc.PrevRot.Eul_3 - Sc.Eul_Out.Eul_3)); //DEBUG #region DEBUG FOR DE-QUIVER /*if (Sc.ID == 11) { // Console.WriteLine(" Prev:" + Sc.PrevRot.ToString()); Console.WriteLine(" Diff:" + diff.ToString()); // Console.WriteLine(" Post:" + Sc.Eul_Out.ToString()); }*/ #endregion #region De_GIMBOL_LOCK if (System.Math.Abs(diff.Eul_1) < 100) { #region GIMBAL LOCK REMEDY PART2 //if (PrevRot.Eul_3 > 90) //{ // PrevRot.Eul_3 = PrevRot.Eul_3 - 180; // Sc.Eul_Out.Eul_3 = Sc.Eul_Out.Eul_3 - 180; //} //if (PrevRot.Eul_3 < -90) //{ // PrevRot.Eul_3 = PrevRot.Eul_3 + 180; // Sc.Eul_Out.Eul_3 = Sc.Eul_Out.Eul_3 + 180; //} #endregion Sc.Eul_Out.Eul_1 = (3 * Sc.PrevRot.Eul_1 + Sc.Eul_Out.Eul_1) / 4; } else { Console.WriteLine("Y rot GimbolLock!"); #region GIMBAL LOCK REMEDY PART1 //if (PrevRot.Eul_1 > 0 && Sc.Eul_Out.Eul_1<0) //{ // Console.WriteLine("Y rot GimbolLock --Prev: " + PrevRot.Eul_1.ToString()+ " --Post: " +Sc.Eul_Out.Eul_1); // Sc.Eul_Out.Eul_1 = Sc.Eul_Out.Eul_1 + 360; // Sc.Eul_Out.Eul_1 = (3 * PrevRot.Eul_1 + Sc.Eul_Out.Eul_1) / 4; //} //else if (PrevRot.Eul_1 < 0 && Sc.Eul_Out.Eul_1 > 0) //{ // Console.WriteLine("Y rot GimbolLock --Prev: " + PrevRot.Eul_1.ToString() + " --Post: " + Sc.Eul_Out.Eul_1); // Sc.Eul_Out.Eul_1 = Sc.Eul_Out.Eul_1 - 360; // Sc.Eul_Out.Eul_1 = (3 * PrevRot.Eul_1 + Sc.Eul_Out.Eul_1) / 4; //} #endregion } if (System.Math.Abs(diff.Eul_2) < 100) { Sc.Eul_Out.Eul_2 = (3 * Sc.PrevRot.Eul_2 + Sc.Eul_Out.Eul_2) / 4; } else { Console.WriteLine("X rot GimbolLock!"); } if (System.Math.Abs(diff.Eul_3) < 85) { Sc.Eul_Out.Eul_3 = (3 * Sc.PrevRot.Eul_3 + Sc.Eul_Out.Eul_3) / 4; } else { Console.WriteLine("Z rot GimbolLock!"); } #endregion #endregion if (Sc.ID == Sp.ID) { Sc.Qlocal = CQuaternion.angle2Quat(y, x, z, 3); Sc.Qworld = Sc.Qlocal; } else { Sc.Qlocal = CQuaternion.angle2Quat(y, x, z, 3); Sc.Qworld = Sp.Qworld * Sc.Qlocal; } }
//FOR ARMS public static void Gen_QW_horizontal(Segment S_self) { Segment S1 = S_self; CQuaternion q_current = S1.raw.Q; CQuaternion tmp0 = q_current * S1.Q_init; CQuaternion tmp1 = tmp0 * Global.Qtc; CQuaternion tmp2 = Global.invQtc * tmp1; S1.QW = new CQuaternion(tmp2.m_q0, -tmp2.m_q1, tmp2.m_q3, tmp2.m_q2); #region DisableShoulderJoints if (S1.ID == 4 || S1.ID == 5) { S1.QW = new CQuaternion(SegmentCollection.arraySegment[10].QW.m_q0, SegmentCollection.arraySegment[10].QW.m_q1, SegmentCollection.arraySegment[10].QW.m_q2, SegmentCollection.arraySegment[10].QW.m_q3); } #endregion #region DEBUG_AZIMUTH if (Global.DEBUG_AZIMUTH) { if (S1.ID == 11) { CQuaternion Zref = new CQuaternion(0, 0, 0, 1); CQuaternion ZW0 = Zref * S1.Q_init; CQuaternion ZW = S1.raw.Q * ZW0; CQuaternion YW = new CQuaternion(0, ZW.m_q1, ZW.m_q2, 0); double sqrtYW = 1 / System.Math.Sqrt(ZW.m_q1 * ZW.m_q1 + ZW.m_q2 * ZW.m_q2); CQuaternion YWnorm = new CQuaternion(0, YW.m_q1 * sqrtYW, YW.m_q2 * sqrtYW, 0); V3 SS = new V3(YWnorm.m_q1 * 0.5, (YWnorm.m_q2 + 1) * 0.5, YWnorm.m_q3 * 0.5); double w = System.Math.Sqrt(SS.x * SS.x + SS.y * SS.y + SS.z * SS.z); V3 normSS = new V3(SS.x / w, SS.y / w, SS.z / w); V3 Yref = new V3(0, 1, 0); V3 z = V3.cross(Yref, normSS); //Global.Qtc = new CQuaternion(w, z.x, z.y, z.z); Global.QAZIMUTH = new CQuaternion(w, z.x, z.y, z.z); CQuaternion DeltaQ = Global.QAZIMUTH * Global.invQtc; Euler EAZIMUTH = CQuaternion.Quat2angle(DeltaQ.m_q0, DeltaQ.m_q1, DeltaQ.m_q2, DeltaQ.m_q3, 1); double Azimuth = EAZIMUTH.Eul_1 * 57.3; if (Global.DEBUG_AZIMUTH) Console.WriteLine("AZIMUTH = " + Global.QAZIMUTH.ToString()); } } #endregion }
//FOR ARMS public static void Calibration_horizontal(Segment S) { Global.PrevRot = new Euler(0, 0, 0); CQuaternion tmp = new CQuaternion(S.raw.Q.m_q0, S.raw.Q.m_q1, S.raw.Q.m_q2, S.raw.Q.m_q3); S.Q_init = tmp.Conjugate(); if (S.ID == 11)//S.ID == 10|| { CQuaternion Zref = new CQuaternion(0, 0, 0, 1); CQuaternion ZW0 = Zref * S.Q_init; CQuaternion ZW = S.raw.Q * ZW0; CQuaternion YW = new CQuaternion(0, ZW.m_q1, ZW.m_q2, 0); double sqrtYW = 1/System.Math.Sqrt(ZW.m_q1*ZW.m_q1 + ZW.m_q2*ZW.m_q2); CQuaternion YWnorm = new CQuaternion(0, YW.m_q1*sqrtYW, YW.m_q2*sqrtYW, 0); V3 SS = new V3(YWnorm.m_q1*0.5, (YWnorm.m_q2+1)*0.5, YWnorm.m_q3*0.5); double w = System.Math.Sqrt(SS.x*SS.x + SS.y*SS.y + SS.z*SS.z); V3 normSS = new V3(SS.x/w, SS.y/w, SS.z/w); V3 Yref = new V3(0, 1, 0); V3 z = V3.cross(Yref, normSS); Global.Qtc = new CQuaternion(w, z.x, z.y, z.z); Global.invQtc = new CQuaternion(Global.Qtc.m_q0, -Global.Qtc.m_q1, -Global.Qtc.m_q2, -Global.Qtc.m_q3); if (Global.DEBUG_TPose) { Console.WriteLine("Qtc = " + Global.Qtc.ToString()); Console.WriteLine("Calibration Done!"); } } if (S.ID == 14) { S.Position = new Pos(0.1125, 0.0, 0.27); // Left TOE POSITION } if (S.ID == 17) { S.Position = new Pos(-0.1125, 0.0, 0.27);// Right TOE POSITION } Global.HIP = new Pos(); }