//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(); }
//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; } }
// flag:1-->LeftRooted flag:2-->RightRooted flag:0-->BothRooted /*public static int gaitDetector() //return State[1] { //Globol.State[0] => Previous State state[1] => Current State Segment Left = SegmentCollection.arraySegment[13]; Segment Right = SegmentCollection.arraySegment[16]; //double[] b = { 0.1827, 0.1827 };//double[] a = { 1.0, -0.6346 }; int gL_out = 0; int gR_out = 0; //double[] Gyr0_mag = { 0, 0, 0, 0 }; double[] Gyr1_mag = { 0, 0, 0, 0 }; if (Global.LC < 3 || Global.RC < 3) { //P_rankle = [0,0.18,0]; P_lankle = [0,0.18,0]; P_hip = [0,0.96,0]; Global.State[0] = 0; Global.State[1] = 0; Left.Position.y = 0.0; //new Pos(0, 0.03, 0); //ysj bug 0.18 Right.Position.y = 0.0;// = new Pos(0, 0.03, 0); //Global.gaitCount ++; } if (Global.LC >= 3 && Global.RC >= 3) { if (Global.Filted_Left_Gyro[3] >= 70) gL_out = 1; else if (Global.Filted_Left_Gyro[3] < 70) gL_out = 0; if (Global.Filted_Right_Gyro[3] >= 70) gR_out = 1; else if (Global.Filted_Right_Gyro[3] < 70) gR_out = 0; if ((gL_out == 0) && (gR_out == 0)) { Global.State[1] = 0; //BothRoot } else if ((gL_out == 1) && (gR_out == 0)) Global.State[1] = 2; //RightRoot else if ((gL_out == 0) && (gR_out == 1)) Global.State[1] = 1; //LeftRoot else if ((gL_out == 1) && (gR_out == 1)) //出错的概率?? { if (Global.JumpFlag == 1) { Global.State[1] = 0; } else { switch (Global.State[0]) { case 1: Global.State[1] = 2; break; case 2: Global.State[1] = 1; break; } } } else Global.State[1] = 0; } Global.State[0] = Global.State[1]; return Global.State[1]; }*/ public static void CalcPosition(int GyroFlag) { #region Calculate Distance from HIP to Ankle Segment HIP = SegmentCollection.arraySegment[10]; Segment LUL = SegmentCollection.arraySegment[11]; Segment LLL = SegmentCollection.arraySegment[12]; Segment RUL = SegmentCollection.arraySegment[14]; Segment RLL = SegmentCollection.arraySegment[15]; Segment RF = SegmentCollection.arraySegment[16]; Segment LF = SegmentCollection.arraySegment[13]; //CQuaternion ToeAnkle = new CQuaternion(0.0, 0.0, 0.0, -1.0); //CQuaternion AnkleToe = new CQuaternion(0.0, 0.0, 0.0, 1.0); CQuaternion AnkleKnee = new CQuaternion(0.0, 0.0, 1.0, 0.0); CQuaternion KneeAnkle = new CQuaternion(0.0, 0.0, -1.0, 0.0); CQuaternion KneeTmp = new CQuaternion(0.0, 0.0, 1.0, 0.0); CQuaternion TmpKnee = new CQuaternion(0.0, 0.0, -1.0, 0.0); CQuaternion LtmpLhip = new CQuaternion(0.0, -1.0, 0.0, 0.0); CQuaternion LhipLtmp = new CQuaternion(0.0, 1.0, 0.0, 0.0); CQuaternion RtmpRhip = new CQuaternion(0.0, 1.0, 0.0, 0.0); CQuaternion RhipRtmp = new CQuaternion(0.0, -1.0, 0.0, 0.0); //////////////Left Toe-->Hip // CQuaternion tmp = new CQuaternion(LF.Qworld.m_q0, LF.Qworld.m_q1, LF.Qworld.m_q2, LF.Qworld.m_q3); // CQuaternion vltla = (LF.Qworld * ToeAnkle) * (tmp.Conjugate()); CQuaternion tmp = new CQuaternion(LLL.Qworld.m_q0, LLL.Qworld.m_q1, LLL.Qworld.m_q2, LLL.Qworld.m_q3); CQuaternion vlalk = (LLL.Qworld * AnkleKnee) * (tmp.Conjugate()); tmp = new CQuaternion(LUL.Qworld.m_q0, LUL.Qworld.m_q1, LUL.Qworld.m_q2, LUL.Qworld.m_q3); CQuaternion vlklt = (LUL.Qworld * KneeTmp) * (tmp.Conjugate()); tmp = new CQuaternion(HIP.Qworld.m_q0, HIP.Qworld.m_q1, HIP.Qworld.m_q2, HIP.Qworld.m_q3); CQuaternion vltlh = (HIP.Qworld * LtmpLhip) * (tmp.Conjugate()); //////////////Right Toe-->Hip //tmp = new CQuaternion(RF.Qworld.m_q0, RF.Qworld.m_q1, RF.Qworld.m_q2, RF.Qworld.m_q3); //CQuaternion vrtra = (RF.Qworld * ToeAnkle) * (tmp.Conjugate()); tmp = new CQuaternion(RLL.Qworld.m_q0, RLL.Qworld.m_q1, RLL.Qworld.m_q2, RLL.Qworld.m_q3); CQuaternion vrark = (RLL.Qworld * AnkleKnee) * (tmp.Conjugate()); tmp = new CQuaternion(RUL.Qworld.m_q0, RUL.Qworld.m_q1, RUL.Qworld.m_q2, RUL.Qworld.m_q3); CQuaternion vrkrt = (RUL.Qworld * KneeTmp) * (tmp.Conjugate()); tmp = new CQuaternion(HIP.Qworld.m_q0, HIP.Qworld.m_q1, HIP.Qworld.m_q2, HIP.Qworld.m_q3); CQuaternion vrtrh = (HIP.Qworld * RtmpRhip) * (tmp.Conjugate()); //Pos DistLa2H = new Pos(0.27 * vltla.m_q1 + 0.55 * vlalk.m_q1 + 0.55 * vlklt.m_q1 + 0.1125 * vltlh.m_q1, 0.27 * vltla.m_q2 + 0.55 * vlalk.m_q2 + 0.55 * vlklt.m_q2 + 0.1125 * vltlh.m_q2, 0.27 * vltla.m_q3 + 0.55 * vlalk.m_q3 + 0.55 * vlklt.m_q3 + 0.1125 * vltlh.m_q3); Pos DistLa2H = new Pos(0.55 * vlalk.m_q1 + 0.55 * vlklt.m_q1 + 0.1125 * vltlh.m_q1, 0.55 * vlalk.m_q2 + 0.55 * vlklt.m_q2 + 0.1125 * vltlh.m_q2, 0.55 * vlalk.m_q3 + 0.55 * vlklt.m_q3 + 0.1125 * vltlh.m_q3); double Ldiff = DistLa2H.y;//System.Math.Sqrt(DistLa2H.x * DistLa2H.x + DistLa2H.y * DistLa2H.y + DistLa2H.z * DistLa2H.z); //Pos DistRa2H = new Pos(0.27 * vrtra.m_q1 + 0.55 * vrark.m_q1 + 0.55 * vrkrt.m_q1 + 0.1125 * vrtrh.m_q1, 0.27 * vrtra.m_q2 + 0.55 * vrark.m_q2 + 0.55 * vrkrt.m_q2 + 0.1125 * vrtrh.m_q2, 0.27 * vrtra.m_q3 + 0.55 * vrark.m_q3 + 0.55 * vrkrt.m_q3 + 0.1125 * vrtrh.m_q3); Pos DistRa2H = new Pos(0.55 * vrark.m_q1 + 0.55 * vrkrt.m_q1 + 0.1125 * vrtrh.m_q1, 0.55 * vrark.m_q2 + 0.55 * vrkrt.m_q2 + 0.1125 * vrtrh.m_q2, 0.55 * vrark.m_q3 + 0.55 * vrkrt.m_q3 + 0.1125 * vrtrh.m_q3); double Rdiff = DistRa2H.y;//System.Math.Sqrt(DistRa2H.x * DistRa2H.x + DistRa2H.y * DistLa2H.y + DistRa2H.z * DistRa2H.z); #endregion #region FeetContact if (System.Math.Abs(Ldiff - Rdiff) > 0.03) { if (DistLa2H.y > DistRa2H.y) { //Ground Contact: Left Foot if (Global.FCflag == 2) { Global.FCflag = 0; } else Global.FCflag = 1; } else if (DistLa2H.y < DistRa2H.y) { //Ground Contact: Right Foot if (Global.FCflag == 1) { Global.FCflag = 0; } else Global.FCflag = 2; } } else { //Ground contact: Both Feet Global.FCflag = 0; } if (Global.DEBUG_FeetContact) { if (Global.FCflag == 0) Console.WriteLine("0 0");// Jflag:" + Global.JumpFlag.ToString() + " Count:" + Global.JumpCountDown.ToString()); else if (Global.FCflag == 1) Console.WriteLine("0 1");// Jflag:" + Global.JumpFlag.ToString() + " Count:" + Global.JumpCountDown.ToString()); else if (Global.FCflag == 2) Console.WriteLine("1 0");// Jflag:" + Global.JumpFlag.ToString() + " Count:" + Global.JumpCountDown.ToString()); } #endregion #region Jump Case Template CountDown if (Global.JumpFlag == 1) { // Console.WriteLine("Hip:" + Global.HipAcc.ToString() + " LUL:" + Global.LULAcc.ToString() + " LLL:" + Global.LLLAcc.ToString() + " RUL:" + Global.RULAcc.ToString() + " RLL:" + Global.RLLAcc.ToString()); Global.FCflag = 0; Global.JumpCountDown--; //start from 7 HIP.Position.y = Global.JumpHeight[Global.JumpCountDown]; Global.JumpFlag = 1; if (Global.JumpCountDown == 0) { Console.WriteLine("JUMP FINISHED!!!!!"); Global.JumpFlag = 0; Global.JumpCountDown = 22; Global.NoJumpStart = DateTime.Now; } } #endregion #region Save Previous Position State Pos Rprevious = new Pos(RF.Position.x, RF.Position.y, RF.Position.z); Pos Lprevious = new Pos(LF.Position.x, LF.Position.y, LF.Position.z); Pos HipPrev = new Pos(HIP.Position.x, HIP.Position.y, HIP.Position.z); #endregion #region Calculate Hip Position //BothRooted if (Global.FCflag == 0) { //if (LF.Position.y < 0.0) LF.Position.y = 0; if (RF.Position.y < 0.0) RF.Position.y = 0; //大于0时,也应该赋值为0? double Lx = LF.Position.x + DistLa2H.x; double Ly = LF.Position.y + DistLa2H.y; double Lz = LF.Position.z + DistLa2H.z; double Rx = RF.Position.x + DistRa2H.x; double Ry = RF.Position.y + DistRa2H.y; double Rz = RF.Position.z + DistRa2H.z; if (Global.JumpFlag == 0) { if (DistRa2H.y < 1 && DistLa2H.y < 1) HIP.Position.y = (Ry + Ly) / 2.0; else HIP.Position.y = 1.13; } HIP.Position.x = (Rx + Lx) / 2.0; HIP.Position.z = (Rz + Lz) / 2.0; } //LEFT Rooted else if (Global.FCflag == 1) { HIP.Position.x = LF.Position.x + DistLa2H.x; if (DistLa2H.y < 1) HIP.Position.y = LF.Position.y + DistLa2H.y; else HIP.Position.y = 1.13; HIP.Position.z = LF.Position.z + DistLa2H.z; } else if (Global.FCflag == 2) { HIP.Position.x = RF.Position.x + DistRa2H.x; if (DistRa2H.y < 1) HIP.Position.y = RF.Position.y + DistRa2H.y; else HIP.Position.y = 1.13; HIP.Position.z = RF.Position.z + DistRa2H.z; } #endregion /************************************************************************/ #region Blank Interface for Absolute Position Aiding System if (Global.ActivateAbsPos) { HIP.Position.z = Global.HIP.z; } #endregion #region BackTrack Feet Position tmp = new CQuaternion(HIP.Qworld.m_q0, HIP.Qworld.m_q1, HIP.Qworld.m_q2, HIP.Qworld.m_q3); CQuaternion vrhipRtmp = (HIP.Qworld * RhipRtmp) * (tmp.Conjugate()); tmp = new CQuaternion(RUL.Qworld.m_q0, RUL.Qworld.m_q1, RUL.Qworld.m_q2, RUL.Qworld.m_q3); CQuaternion vrtmprknee = (RUL.Qworld * TmpKnee) * (tmp.Conjugate()); tmp = new CQuaternion(RLL.Qworld.m_q0, RLL.Qworld.m_q1, RLL.Qworld.m_q2, RLL.Qworld.m_q3); CQuaternion vrkneerankle = (RLL.Qworld * KneeAnkle) * (tmp.Conjugate()); //tmp = new CQuaternion(RF.Qworld.m_q0, RF.Qworld.m_q1, RF.Qworld.m_q2, RF.Qworld.m_q3); //CQuaternion vranklertoe = (RF.Qworld * AnkleToe) * (tmp.Conjugate()); tmp = new CQuaternion(HIP.Qworld.m_q0, HIP.Qworld.m_q1, HIP.Qworld.m_q2, HIP.Qworld.m_q3); CQuaternion vlhipltmp = (HIP.Qworld * LhipLtmp) * (tmp.Conjugate()); tmp = new CQuaternion(LUL.Qworld.m_q0, LUL.Qworld.m_q1, LUL.Qworld.m_q2, LUL.Qworld.m_q3); CQuaternion vltmplknee = (LUL.Qworld * TmpKnee) * (tmp.Conjugate()); tmp = new CQuaternion(LLL.Qworld.m_q0, LLL.Qworld.m_q1, LLL.Qworld.m_q2, LLL.Qworld.m_q3); CQuaternion vlkneelankle = (LLL.Qworld * KneeAnkle) * (tmp.Conjugate()); //tmp = new CQuaternion(LF.Qworld.m_q0, LF.Qworld.m_q1, LF.Qworld.m_q2, LF.Qworld.m_q3); //CQuaternion vlankleltoe = (LF.Qworld * AnkleToe) * (tmp.Conjugate()); RF.Position.x = HIP.Position.x + 0.1125 * vrhipRtmp.m_q1 + 0.55 * vrtmprknee.m_q1 + 0.55 * vrkneerankle.m_q1; // + 0.27 * vranklertoe.m_q1; RF.Position.y = HIP.Position.y + 0.1125 * vrhipRtmp.m_q2 + 0.55 * vrtmprknee.m_q2 + 0.55 * vrkneerankle.m_q2; // + 0.27 * vranklertoe.m_q2; if (RF.Position.y < 0) RF.Position.y = 0; RF.Position.z = HIP.Position.z + 0.1125 * vrhipRtmp.m_q3 + 0.55 * vrtmprknee.m_q3 + 0.55 * vrkneerankle.m_q3; // + 0.27 * vranklertoe.m_q3; LF.Position.x = HIP.Position.x + 0.1125 * vlhipltmp.m_q1 + 0.55 * vltmplknee.m_q1 + 0.55 * vlkneelankle.m_q1; // + 0.27 * vlankleltoe.m_q1; LF.Position.y = HIP.Position.y + 0.1125 * vlhipltmp.m_q2 + 0.55 * vltmplknee.m_q2 + 0.55 * vlkneelankle.m_q2; // + 0.27 * vlankleltoe.m_q2; if (RF.Position.y < 0) RF.Position.y = 0; LF.Position.z = HIP.Position.z + 0.1125 * vlhipltmp.m_q3 + 0.55 * vltmplknee.m_q3 + 0.55 * vlkneelankle.m_q3; // + 0.27 * vlankleltoe.m_q3; #endregion #region Position De-Quiver Pos RPos_post = new Pos(RF.Position.x, RF.Position.y, RF.Position.z); Pos LPos_post = new Pos(LF.Position.x, LF.Position.y, LF.Position.z); RF.Position.x = (3 * Rprevious.x + RPos_post.x) / 4; RF.Position.y = (3 * Rprevious.y + RPos_post.y) / 4; RF.Position.z = (3 * Rprevious.z + RPos_post.z) / 4; LF.Position.x = (3 * Lprevious.x + LPos_post.x) / 4; LF.Position.y = (3 * Lprevious.y + LPos_post.y) / 4; LF.Position.z = (3 * Lprevious.z + LPos_post.z) / 4; if (System.Math.Abs(HipPrev.x - HIP.Position.x) > 0 && System.Math.Abs(HipPrev.x - HIP.Position.x) < 0.002) { HIP.Position.x = HipPrev.x; } if (System.Math.Abs(HipPrev.y - HIP.Position.y) > 0 && System.Math.Abs(HipPrev.y - HIP.Position.y) < 0.002) { HIP.Position.y = HipPrev.y; } if (System.Math.Abs(HipPrev.z - HIP.Position.z) > 0 && System.Math.Abs(HipPrev.z - HIP.Position.z) < 0.002) { HIP.Position.z = HipPrev.z; } HIP.Position.x = (3 * HipPrev.x + HIP.Position.x) / 4; HIP.Position.y = (3 * HipPrev.y + HIP.Position.y) / 4; HIP.Position.z = (3 * HipPrev.z + HIP.Position.z) / 4; //Console.WriteLine("HIP: " + HIP.Position.ToString()); #endregion }