/*\define Execute the walk engine using all above methods*/
        public void execute()
        {
            Timer startTime = new Timer();

            startTime.Start();

            double timeCurrent = this.worldModel.getTime() - this.initTime;

            this.isStepFinished = false;

            if (timeCurrent > (this.stepTime - 0.00005))
            {
                this.transitionLeftFoot  = this.inicialLeftLeg;
                this.transitionRightFoot = this.inicialRightLeg;
                this.transitionError     = new Point(this.dp, this.dpY);
                this.isStepFinished      = true;
            }

            // Initial the walk for beginning of each walk step or walk
            if (timeCurrent > (this.stepTime - 0.00005) || this.initWalk)
            {
                this.initWalk = false;
                this.initTime = this.worldModel.getTime();
                timeCurrent   = this.worldModel.getTime() - this.initTime;

                // This is used when the foot is swing to know the previous position of the the foot
                this.previousLeftFoot  = this.planedLeftFoot[0];
                this.previousRightFoot = this.planedRightFoot[0];

                // plan the next 6 foot with considering the next foot as the initial of the plan
                this.inicialLeftLeg  = this.planedLeftFoot[1];
                this.inicialRightLeg = this.planedRightFoot[1];

                double previewTime = 2.5;
                int    previewStep = (int)(previewTime / stepTime);
                this.footGenerator(this.stepSizeX, this.stepSizeY, 0, previewStep, this.stepTime, this.inicialLeftLeg, this.inicialRightLeg);

                int   sizeZMP = 0;
                ZMP[] zmp     = this.zmpGenerator(previewStep, deltaT, 0, sizeZMP);

                this.lastTheta   = this.thetaStep;
                this.oneStepSize = (int)(this.stepTime / deltaT);

                this.createHeightTrajectory(Math.Max(sizeZMP, this.oneStepSize));

                CoM[] com = this.fastDynamicSolverWithSlideWindow(
                    new Point(this.initCom.positionX.GetX(), this.initCom.positionY.GetX()), zmp, sizeZMP, this.deltaT, this.comZTrajecotry);

                for (int i = 0; i < oneStepSize; i++)
                {
                    this.planedCoM[i].positionY = com[i].positionY;
                    this.planedCoM[i].positionX = com[i].positionX;
                    this.planedCoM[i].positionY = com[i].positionY;
                }
                this.initCom = this.planedCoM[this.oneStepSize - 1];

                zmp = null;
                com = null;
            }

            // After calculating the horizontal CoM from here is the execution of the walk
            Point comPos = this.getCoM(timeCurrent);

            Point leftFootPos, rightFootPos;

            // if you want to have double support phase, dsp_min and dsp_max should be changes
            float dsp_min = 0;
            float dsp_max = (float)this.stepTime;

            Point targLeftFPos;
            Point targRightFPos;

            // The active balance, here it is the constant not adaptive, in paper the paper it
            // it is adaptive using the current state of the robot.
            double degRotationTunk  = -constantInclination;
            double degRotationTunkY = 0;

            if (this.planedLeftFoot[0].isSupport && this.planedRightFoot[0].isSupport)
            {
                leftFootPos   = new Point(this.planedLeftFoot[0].position.GetX(), this.planedLeftFoot[0].position.GetY(), 0);
                rightFootPos  = new Point(this.planedRightFoot[0].position.GetX(), this.planedRightFoot[0].position.GetY(), 0);
                targLeftFPos  = leftFootPos - comPos;
                targRightFPos = rightFootPos - comPos;
                this.computePose(targLeftFPos, targRightFPos, new Point(0, 0, 0), new Point(0, 0, 0));
            }
            else if (this.planedLeftFoot[0].isSupport)
            {
                leftFootPos = new Point(this.planedLeftFoot[0].position.GetX(), this.planedLeftFoot[0].position.GetY(), 0);

                Point p0 = new Point(this.previousRightFoot.position.GetX(), this.previousRightFoot.position.GetY(), 0);
                Point p2 = new Point(this.planedRightFoot[0].position.GetX(), this.planedRightFoot[0].position.GetY(), 0);
                Point p1 = Geometry.determineMidPoint(p0, p2);
                p1.SetZ(swingHeight);

                if (this.thetaStep < 0)
                {
                    this.bzqdRotateSupport.setLinear(new Point(-this.thetaStep, 0, 0), new Point(0, 0, 0), dsp_max - dsp_min);
                    this.bzqdRotateSwing.setLinear(new Point(this.thetaStep, 0, 0), new Point(0, 0, 0), dsp_max - dsp_min);
                }
                else
                {
                    this.bzqdRotateSupport.setLinear(new Point(0, 0, 0), new Point(this.thetaStep, 0, 0), dsp_max - dsp_min);
                    this.bzqdRotateSwing.setLinear(new Point(0, 0, 0), new Point(-this.thetaStep, 0, 0), dsp_max - dsp_min);
                }
                this.bzqdSwing.setQuadric(p0, p1, p2, dsp_max - dsp_min);

                targLeftFPos  = leftFootPos - comPos;
                targRightFPos = this.bzqdSwing.getQuadricPosition((float)timeCurrent) - comPos;

                Point rotateSwing   = this.bzqdRotateSwing.getLinearPosition((float)timeCurrent);
                Point rotateSupport = this.bzqdRotateSupport.getLinearPosition((float)timeCurrent);

                // rotation around X axis of CoM can be used in Active Balance
                targLeftFPos.SetX(targLeftFPos.GetX() * Geometry.Cos(degRotationTunk) + targLeftFPos.GetZ() * Geometry.Sin(degRotationTunk));
                targLeftFPos.SetZ(-targLeftFPos.GetX() * Geometry.Sin(degRotationTunk) + targLeftFPos.GetZ() * Geometry.Cos(degRotationTunk));
                targRightFPos.SetX(targRightFPos.GetX() * Geometry.Cos(degRotationTunk) + targRightFPos.GetZ() * Geometry.Sin(degRotationTunk));
                targRightFPos.SetZ(-targRightFPos.GetX() * Geometry.Sin(degRotationTunk) + targRightFPos.GetZ() * Geometry.Cos(degRotationTunk));

                // rotation around Y axis of CoM can be used in Active Balance
                targLeftFPos.SetY(targLeftFPos.GetY() * Geometry.Cos(degRotationTunkY) - targLeftFPos.GetZ() * Geometry.Sin(degRotationTunkY));
                targLeftFPos.SetZ(targLeftFPos.GetY() * Geometry.Sin(degRotationTunkY) + targLeftFPos.GetZ() * Geometry.Cos(degRotationTunkY));
                targRightFPos.SetY(targRightFPos.GetY() * Geometry.Cos(degRotationTunkY) - targRightFPos.GetZ() * Geometry.Sin(degRotationTunkY));
                targRightFPos.SetZ(targRightFPos.GetY() * Geometry.Sin(degRotationTunkY) + targRightFPos.GetZ() * Geometry.Cos(degRotationTunkY));

                this.computePose(targLeftFPos, targRightFPos,
                                 new Point(degRotationTunk, degRotationTunkY, rotateSwing.GetX()),
                                 new Point(degRotationTunk, degRotationTunkY, rotateSupport.GetX()));
            }

            else if (this.planedRightFoot[0].isSupport)
            {
                rightFootPos = new Point(this.planedRightFoot[0].position.GetX(), this.planedRightFoot[0].position.GetY(), 0);
                Point p0 = new Point(this.previousLeftFoot.position.GetX(), this.previousLeftFoot.position.GetY(), 0);
                Point p2 = new Point(this.planedLeftFoot[0].position.GetX(), this.planedLeftFoot[0].position.GetY(), 0);
                Point p1 = Geometry.determineMidPoint(p0, p2);
                p1.SetZ(swingHeight);

                this.bzqdSwing.setQuadric(p0, p1, p2, dsp_max - dsp_min);
                if (this.thetaStep < 0)
                {
                    this.bzqdRotateSupport.setLinear(new Point(0, 0, 0), new Point(this.thetaStep, 0, 0), dsp_max - dsp_min);
                    this.bzqdRotateSwing.setLinear(new Point(0, 0, 0), new Point(-this.thetaStep, 0, 0), dsp_max - dsp_min);
                }
                else
                {
                    this.bzqdRotateSupport.setLinear(new Point(-this.thetaStep, 0, 0), new Point(0, 0, 0), dsp_max - dsp_min);
                    this.bzqdRotateSwing.setLinear(new Point(this.thetaStep, 0, 0), new Point(0, 0, 0), dsp_max - dsp_min);
                }

                targLeftFPos  = this.bzqdSwing.getQuadricPosition((float)timeCurrent) - comPos;
                targRightFPos = rightFootPos - comPos;
                Point rotateSwing   = this.bzqdRotateSwing.getLinearPosition((float)timeCurrent);
                Point rotateSupport = bzqdRotateSupport.getLinearPosition((float)timeCurrent);

                // The active balance rotation in sagittal plane
                targLeftFPos.SetX(targLeftFPos.GetX() * Geometry.Cos(degRotationTunk) + targLeftFPos.GetZ() * Geometry.Sin(degRotationTunk));
                targLeftFPos.SetZ(-targLeftFPos.GetX() * Geometry.Sin(degRotationTunk) + targLeftFPos.GetZ() * Geometry.Cos(degRotationTunk));

                targRightFPos.SetX(targRightFPos.GetX() * Geometry.Cos(degRotationTunk) + targRightFPos.GetZ() * Geometry.Sin(degRotationTunk));
                targRightFPos.SetZ(-targRightFPos.GetX() * Geometry.Sin(degRotationTunk) + targRightFPos.GetZ() * Geometry.Cos(degRotationTunk));

                // The active balance rotation in coronal plane
                targLeftFPos.SetY(targLeftFPos.GetY() * Geometry.Cos(degRotationTunkY) - targLeftFPos.GetZ() * Geometry.Sin(degRotationTunkY));
                targLeftFPos.SetZ(targLeftFPos.GetY() * Geometry.Sin(degRotationTunkY) + targLeftFPos.GetZ() * Geometry.Cos(degRotationTunkY));
                targRightFPos.SetY(targRightFPos.GetY() * Geometry.Cos(degRotationTunkY) - targRightFPos.GetZ() * Geometry.Sin(degRotationTunkY));
                targRightFPos.SetZ(targRightFPos.GetY() * Geometry.Sin(degRotationTunkY) + targRightFPos.GetZ() * Geometry.Cos(degRotationTunkY));

                // Call the inverse kinematics
                this.computePose(targLeftFPos, targRightFPos,
                                 new Point(degRotationTunk, degRotationTunkY, rotateSupport.GetX()),
                                 new Point(degRotationTunk, degRotationTunkY, rotateSwing.GetX()));
            }

            this.updatePose();

            // Update the time
            startTime.Stop();
        }
        /*\define Implementation of the foot planner using the input walk parameters (see the foot planner section of the paper)
         */
        public void footGenerator(double stepX, double stepY, double stepTheta, int stepNumber, double timeStep, Foot inicialLeftLeg, Foot inicialRightLeg)
        {
            if (this.isStopped)
            {
                this.planedLeftFoot[0] = this.setFoot(inicialLeftLeg.position, true, inicialLeftLeg.isRight, inicialLeftLeg.time, inicialLeftLeg.theta);

                this.planedRightFoot[0] = this.setFoot(inicialRightLeg.position, true, inicialRightLeg.isRight, inicialRightLeg.time, inicialRightLeg.theta);

                for (int i = 1; i <= stepNumber; i++)
                {
                    this.planedRightFoot[i] = this.planedRightFoot[0];
                    this.planedLeftFoot[i]  = this.planedLeftFoot[0];
                }
                return;
            }

            this.planedLeftFoot[0] = this.setFoot(inicialLeftLeg.position, inicialLeftLeg.isSupport, inicialLeftLeg.isRight, inicialLeftLeg.time, inicialLeftLeg.theta);

            this.planedRightFoot[0] = this.setFoot(inicialRightLeg.position, inicialRightLeg.isSupport, inicialRightLeg.isRight,
                                                   inicialRightLeg.time, inicialRightLeg.theta);

            Point hl = new Point(0, legSeperation / 2);
            Point hr = new Point(0, -legSeperation / 2);
            Point d  = new Point(stepX, stepY);
            Point x  = new Point(0, 0);

            Point com;

            hl.rotate(this.planedLeftFoot[0].theta);
            hr.rotate(this.planedRightFoot[0].theta);
            d.rotate(this.planedLeftFoot[0].theta);
            if (this.planedLeftFoot[0].isSupport && this.planedRightFoot[0].isSupport)
            {
                x = this.planedLeftFoot[0].position - hl;
                this.planedLeftFoot[0].isSupport = false;
            }
            if (!this.planedLeftFoot[0].isSupport)
            {
                x = this.planedLeftFoot[0].position - hl;
            }
            if (!this.planedRightFoot[0].isSupport)
            {
                x = this.planedRightFoot[0].position - hr;
            }

            for (int i = 1; i <= stepNumber; i++)
            {
                d.rotate(stepTheta);
                com = d + x;
                x   = ((com - x) * 2) + x;
                hl.rotate(stepTheta);
                hr.rotate(stepTheta);

                if (this.planedLeftFoot[i - 1].isSupport)
                {
                    float newTheta = (float)(this.planedLeftFoot[i - 1].theta + stepTheta);
                    Point pos      = x + hl;
                    this.planedLeftFoot[i] = this.setFoot(pos, false, this.planedLeftFoot[i - 1].isRight,
                                                          this.planedLeftFoot[i - 1].time + timeStep, newTheta);
                }
                else
                {
                    float newTheta = (float)(this.planedLeftFoot[i - 1].theta + stepTheta);
                    Point pos      = new Point(this.planedLeftFoot[i - 1].position.GetX(), this.planedLeftFoot[i - 1].position.GetY());
                    this.planedLeftFoot[i] = this.setFoot(pos, true, this.planedLeftFoot[i - 1].isRight,
                                                          this.planedLeftFoot[i - 1].time + timeStep, newTheta);
                }

                if (this.planedRightFoot[i - 1].isSupport)
                {
                    float newTheta = (float)(this.planedRightFoot[i - 1].theta + stepTheta);
                    Point pos      = x + hr;
                    this.planedRightFoot[i] = this.setFoot(pos, false, this.planedRightFoot[i - 1].isRight,
                                                           this.planedRightFoot[i - 1].time + timeStep, newTheta);
                }
                else
                {
                    float newTheta = (float)(this.planedRightFoot[i - 1].theta + stepTheta);
                    ;
                    Point pos = new Point(planedRightFoot[i - 1].position.GetX(),
                                          this.planedRightFoot[i - 1].position.GetY());
                    this.planedRightFoot[i] = this.setFoot(pos, true, planedRightFoot[i - 1].isRight,
                                                           this.planedRightFoot[i - 1].time + timeStep, newTheta);
                }

                double minLegSperationY  = 0.08;
                double maxLegSeperationX = 0.20;
                double maxLegSeperationY = 0.20;
                if (planedRightFoot[i].isSupport == false)
                {
                    Point  leftToRight = planedRightFoot[i].position - planedLeftFoot[i].position;
                    double maxX        = Math.Max(leftToRight.GetX(), -maxLegSeperationX);
                    leftToRight.SetX(Math.Min(maxX, maxLegSeperationX));
                    double maxY = Math.Max(leftToRight.GetY(), -maxLegSeperationY);
                    leftToRight.SetY(Math.Min(maxY, -minLegSperationY));

                    this.planedRightFoot[i].position = this.planedLeftFoot[i].position + leftToRight;
                }

                if (this.planedLeftFoot[i].isSupport == false)
                {
                    Point  rightToLeft = this.planedLeftFoot[i].position - planedRightFoot[i].position;
                    double maxX        = Math.Max(rightToLeft.GetX(), -maxLegSeperationX);
                    rightToLeft.SetX(Math.Min(maxX, maxLegSeperationX));
                    double maxY = Math.Max(rightToLeft.GetY(), minLegSperationY);
                    rightToLeft.SetY(Math.Min(maxY, maxLegSeperationY));

                    this.planedLeftFoot[i].position = this.planedRightFoot[i].position + rightToLeft;
                }
            }

            this.planedLeftFoot[0] = this.setFoot(inicialLeftLeg.position, inicialLeftLeg.isSupport,
                                                  inicialLeftLeg.isRight, inicialLeftLeg.time, inicialLeftLeg.theta);

            this.planedRightFoot[0] = setFoot(inicialRightLeg.position,
                                              inicialRightLeg.isSupport, inicialRightLeg.isRight, inicialRightLeg.time, inicialRightLeg.theta);

            this.lastStepTime = timeStep;
            this.lastDX       = stepX;
            this.lastDY       = stepY;

            return;
        }