public CQuaternion Quat_packet(int[] data, int count) { double q0 = TwoBytes(data[2], data[3]) / 10000; double q1 = TwoBytes(data[4], data[5]) / 10000; double q2 = TwoBytes(data[6], data[7]) / 10000; double q3 = TwoBytes(data[8], data[9]) / 10000; double mag = System.Math.Sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); if (0 == mag) { mag = 1; } CQuaternion QQ = new CQuaternion(q0 / mag, q1 / mag, q2 / mag, q3 / mag); return QQ; }
public SensorData() { Q = new CQuaternion(); acc = new Acc(); gyro = new Gyro(); }
public static void Slerp(CQuaternion Start, CQuaternion Stop) { //double[] r = new double[3] {0.25, 0.5, 0.75}; double cosOmega; cosOmega = Start.m_q0 * Stop.m_q0 + Start.m_q1 * Stop.m_q1 + Start.m_q2 * Stop.m_q2 + Start.m_q3 * Stop.m_q3; if (cosOmega < 0.0) { Start.m_q0 = -Start.m_q0; Start.m_q1 = -Start.m_q1; Start.m_q2 = -Start.m_q2; Start.m_q3 = -Start.m_q3; cosOmega = -cosOmega; } double theta = System.Math.Acos(cosOmega); if (theta == 0) { CQuaternion q1 = new CQuaternion(Start.m_q0, Start.m_q1, Start.m_q2, Start.m_q3); // CQuaternion q2 = new CQuaternion(Start.m_q0,Start.m_q1,Start.m_q2,Start.m_q3); // CQuaternion q3 = new CQuaternion(Start.m_q0,Start.m_q1,Start.m_q2,Start.m_q3); } else { /* double scaler11 = System.Math.Sin((1 - 0.25) * theta) / System.Math.Sin(theta); double scaler12 = System.Math.Sin(0.25 * theta) / System.Math.Sin(theta); double scaler21 = System.Math.Sin((1 - 0.5) * theta) / System.Math.Sin(theta); double scaler22 = System.Math.Sin(0.5 * theta) / System.Math.Sin(theta); double scaler31 = System.Math.Sin((1 - 0.75) * theta) / System.Math.Sin(theta); double scaler32 = System.Math.Sin(0.75 * theta) / System.Math.Sin(theta); */ double scaler1 = System.Math.Sin((1 - 0.5) * theta) / System.Math.Sin(theta); double scaler2 = System.Math.Sin(0.5 * theta) / System.Math.Sin(theta); CQuaternion q1 = new CQuaternion((scaler1 * Start.m_q0 + scaler2 * Stop.m_q0), (scaler1 * Start.m_q1 + scaler2 * Stop.m_q1), (scaler1 * Start.m_q2 + scaler2 * Stop.m_q2), (scaler1 * Start.m_q3 + scaler2 * Stop.m_q3)); /* CQuaternion q2 = new CQuaternion((scaler21*Start.m_q0+scaler22*Stop.m_q0), (scaler21*Start.m_q1+scaler22*Stop.m_q1), (scaler21*Start.m_q2+scaler22*Stop.m_q2), (scaler21*Start.m_q3+scaler22*Stop.m_q3)); CQuaternion q3 = new CQuaternion((scaler31*Start.m_q0+scaler32*Stop.m_q0), (scaler31*Start.m_q1+scaler32*Stop.m_q1), (scaler31*Start.m_q2+scaler32*Stop.m_q2), (scaler31*Start.m_q3+scaler32*Stop.m_q3)); Console.WriteLine("q1 = " + q1.ToString() + " " + "q2 = " + q2.ToString() + " " + "q1 = " + q3.ToString());*/ } }
public void Packet_resolver(int[] data, int count) { if (0 == data[1]) return; //得到真正的ID对应关系 int IDindex = defineSerialPort.FindIndex(data[1]); Acc acc = Acc_packet(data, count, IDindex); if (acc.acc_1 > Global.FSR_Acc || acc.acc_2 > Global.FSR_Acc || acc.acc_3 > Global.FSR_Acc) { arraySegment[IDindex].raw = arraySegment[IDindex].raw; } else { arraySegment[IDindex].raw.Q = Quat_packet(data, count); arraySegment[IDindex].raw.acc = acc; // Acc_packet(data, count, IDindex); arraySegment[IDindex].raw.gyro = Gyro_packet(data, count); } // JUMP #region AccConvert_Sensor2NED2Avatar if (arraySegment[IDindex].ID == 12) //LUL { //NED CQuaternion LULAcc = new CQuaternion(0.0, arraySegment[IDindex].raw.acc.acc_1, arraySegment[IDindex].raw.acc.acc_2, arraySegment[IDindex].raw.acc.acc_3); CQuaternion LULtmp0 = new CQuaternion(arraySegment[IDindex].raw.Q.m_q0, -arraySegment[IDindex].raw.Q.m_q1, -arraySegment[IDindex].raw.Q.m_q2, -arraySegment[IDindex].raw.Q.m_q3); CQuaternion LULtmp1 = LULAcc * LULtmp0; Global.LULAcc = arraySegment[IDindex].raw.Q * LULtmp1; //AVATAR CQuaternion tmp1 = Global.LULAcc * Global.Qtc; CQuaternion tmp2 = Global.invQtc * tmp1; Global.LULAcc = new CQuaternion(tmp2.m_q0, -tmp2.m_q1, tmp2.m_q3, tmp2.m_q2); } if (arraySegment[IDindex].ID == 13) //LLL { //NED CQuaternion LLLAcc = new CQuaternion(0.0, arraySegment[IDindex].raw.acc.acc_1, arraySegment[IDindex].raw.acc.acc_2, arraySegment[IDindex].raw.acc.acc_3); CQuaternion LLLtmp0 = new CQuaternion(arraySegment[IDindex].raw.Q.m_q0, -arraySegment[IDindex].raw.Q.m_q1, -arraySegment[IDindex].raw.Q.m_q2, -arraySegment[IDindex].raw.Q.m_q3); CQuaternion LLLtmp1 = LLLAcc * LLLtmp0; Global.LLLAcc = arraySegment[IDindex].raw.Q * LLLtmp1; //AVATAR CQuaternion tmp1 = Global.LLLAcc * Global.Qtc; CQuaternion tmp2 = Global.invQtc * tmp1; Global.LLLAcc = new CQuaternion(tmp2.m_q0, -tmp2.m_q1, tmp2.m_q3, tmp2.m_q2); } if (arraySegment[IDindex].ID == 15) //RUL { //NED CQuaternion RULAcc = new CQuaternion(0.0, arraySegment[IDindex].raw.acc.acc_1, arraySegment[IDindex].raw.acc.acc_2, arraySegment[IDindex].raw.acc.acc_3); CQuaternion RULtmp0 = new CQuaternion(arraySegment[IDindex].raw.Q.m_q0, -arraySegment[IDindex].raw.Q.m_q1, -arraySegment[IDindex].raw.Q.m_q2, -arraySegment[IDindex].raw.Q.m_q3); CQuaternion RULtmp1 = RULAcc * RULtmp0; Global.RULAcc = arraySegment[IDindex].raw.Q * RULtmp1; //AVATAR CQuaternion tmp1 = Global.RULAcc * Global.Qtc; CQuaternion tmp2 = Global.invQtc * tmp1; Global.RULAcc = new CQuaternion(tmp2.m_q0, -tmp2.m_q1, tmp2.m_q3, tmp2.m_q2); } if (arraySegment[IDindex].ID == 16) //RLL { //NED CQuaternion RLLAcc = new CQuaternion(0.0, arraySegment[IDindex].raw.acc.acc_1, arraySegment[IDindex].raw.acc.acc_2, arraySegment[IDindex].raw.acc.acc_3); CQuaternion RLLtmp0 = new CQuaternion(arraySegment[IDindex].raw.Q.m_q0, -arraySegment[IDindex].raw.Q.m_q1, -arraySegment[IDindex].raw.Q.m_q2, -arraySegment[IDindex].raw.Q.m_q3); CQuaternion RLLtmp1 = RLLAcc * RLLtmp0; Global.RLLAcc = arraySegment[IDindex].raw.Q * RLLtmp1; //AVATAR CQuaternion tmp1 = Global.RLLAcc * Global.Qtc; CQuaternion tmp2 = Global.invQtc * tmp1; Global.RLLAcc = new CQuaternion(tmp2.m_q0, -tmp2.m_q1, tmp2.m_q3, tmp2.m_q2); } if (arraySegment[IDindex].ID == 11) //HIP { //NED CQuaternion HipAcc = new CQuaternion(0.0, arraySegment[IDindex].raw.acc.acc_1, arraySegment[IDindex].raw.acc.acc_2, arraySegment[IDindex].raw.acc.acc_3); CQuaternion Hiptmp0 = new CQuaternion(arraySegment[IDindex].raw.Q.m_q0, -arraySegment[IDindex].raw.Q.m_q1, -arraySegment[IDindex].raw.Q.m_q2, -arraySegment[IDindex].raw.Q.m_q3); CQuaternion Hiptmp1 = HipAcc * Hiptmp0;// arraySegment[IDindex].raw.Q; Global.HipAcc = arraySegment[IDindex].raw.Q * Hiptmp1; //AVATAR CQuaternion tmp1 = Global.HipAcc * Global.Qtc; CQuaternion tmp2 = Global.invQtc * tmp1; Global.HipAcc = new CQuaternion(tmp2.m_q0, -tmp2.m_q1, tmp2.m_q3, tmp2.m_q2); // Console.WriteLine("HIP:" + Global.HipAcc.ToString()); } #endregion if ((Global.HipAcc.m_q2 < 0) && (Global.JStart == 1)) { //Console.WriteLine("HipAccworld:" + Global.HipAcc.ToString()); Global.JumpTimeEnd = DateTime.Now; TimeSpan ts = Global.JumpTimeEnd - Global.JumpTimeStart; double xx = ts.Ticks / 10000000.0; if (xx < 0.1) { Global.JumpFlag = 1; } Global.JStart = 0; } // Console.WriteLine("Hip:" + Global.HipAcc.ToString());// + " LUL:" + Global.LULAcc.ToString() + " LLL:" + Global.LLLAcc.ToString() + " RUL:" + Global.RULAcc.ToString() + " RLL:" + Global.RLLAcc.ToString()); //double AccAve = 0.2 * (Global.HipAcc.m_q2 + Global.LULAcc.m_q2 + Global.LLLAcc.m_q2 + Global.RULAcc.m_q2 + Global.RLLAcc.m_q2); if ((Global.HipAcc.m_q2 > 1.7) && (Global.JumpFlag ==0)) { Global.NoJumpEnd = DateTime.Now; TimeSpan ts = Global.NoJumpEnd - Global.NoJumpStart; double stable = ts.Ticks / 10000000.0; if (stable > 0.35) { Global.JumpFlag = 1; // Global.JumpTimeStart = DateTime.Now; // Global.JStart = 1; } //Global.JumpFlag = 1; } if (Global.DEBUG_JumpDetect) { Console.WriteLine("JStart=" + Global.JStart + " JumpFlag=" + Global.JumpFlag + " JumpCountDown" + Global.JumpCountDown); } #region Right Foot and Left Foot /* if (IDindex == 13) //必须找到左脚对应的节点的ID(是动态的) { if (Global.LC < 3) { Global.LC++; Global.Left_Gyro[Global.LC] = arraySegment[IDindex].raw.gyro.gyro_mag; //Left_Gyro[0 1 2 3] -->LC == 4 } else { Global.Left_Gyro[0] = Global.Left_Gyro[1]; Global.Left_Gyro[1] = Global.Left_Gyro[2]; Global.Left_Gyro[2] = Global.Left_Gyro[3]; Global.Left_Gyro[3] = arraySegment[IDindex].raw.gyro.gyro_mag; } Global.Filted_Left_Gyro = Tools.filtfilt(Global.Butterworth_b, Global.Butterworth_a, Global.Left_Gyro); } if (IDindex == 16)//右脚对应的节点的ID(动态的) { if (Global.RC < 3) { Global.RC++; Global.Right_Gyro[Global.RC] = arraySegment[IDindex].raw.gyro.gyro_mag; } else { Global.Right_Gyro[0] = Global.Right_Gyro[1]; Global.Right_Gyro[1] = Global.Right_Gyro[2]; Global.Right_Gyro[2] = Global.Right_Gyro[3]; Global.Right_Gyro[3] = arraySegment[IDindex].raw.gyro.gyro_mag; } Global.Filted_Right_Gyro = Tools.filtfilt(Global.Butterworth_b, Global.Butterworth_a, Global.Right_Gyro); } */ #endregion }
//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 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 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(); }
public Segment(int self_id, int parent_id) { ID = self_id; parentID = parent_id; raw = new SensorData(); Q_init = new CQuaternion(); QW = new CQuaternion(); QL = new CQuaternion(); Eul_Out = new Euler(); PrevRot = new Euler(); Qworld = new CQuaternion(); Qlocal = new CQuaternion(); Position = new Pos(); isFirstTime = true; }
// 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 }