Esempio n. 1
0
        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;
        }
Esempio n. 2
0
        // 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
        }