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
}
