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
}
