/*\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 modeling an stable omnidirectional walking based on the dynamics of an inverted pendulum model. (see the paper) * \param[in] zmpInit the initial ZMP position in walking * \param[in] zmpInit the input ZMP trajectory * \param[in] length the size of the ZMP and CoM trajectories * \param[in] dt The resolution of the time in the trajecotry * \param[in] comZ The input vertical trajecotry of the CoM */ public CoM[] fastDynamicSolverWithSlideWindow(Point zmpInit, ZMP[] zmpTrajector, int length, double dt, List <HeightTrajectory> comZ) { CoM[] com = new CoM[length + 1000]; double[] a = new double[length + 1000]; double[] b = new double[length + 1000]; double[] c = new double[length + 1000]; double[] d = new double[length + 1000]; double[] dy = new double[length + 1000]; double[] x = new double[length]; double[] y = new double[length]; // Discretize the inverted pendulum differential equation for (int i = 0; i < length; ++i) { double g = 9.8; double aTemp = (-1 * comZ[i].positionZ) / ((dt * dt) * (comZ[i].time + g)); double bTemp = 1 - (2 * aTemp); // Handle the boundary condition for the end of trajectory if (i == length - 1) { bTemp = bTemp + aTemp; } a[i] = aTemp; b[i] = bTemp; c[i] = aTemp; } // Handle the boundary condition for the beginning of trajectory. for (int i = 0; i < length; i++) { double ZMP_X = zmpTrajector[i].position.GetX(); double ZMP_Y = zmpTrajector[i].position.GetY(); if (i == 0) { ZMP_X = ZMP_X - (a[0] * zmpInit.GetX()); ZMP_Y = ZMP_Y - (a[0] * zmpInit.GetY()); } d[i] = ZMP_X; dy[i] = ZMP_Y; } // TDMA solver explanation can be found in wikipedia // Here Thomas Algorithm is used to solve Inverted pendulum dynamics c[0] = c[0] / b[0]; d[0] = d[0] / b[0]; dy[0] = dy[0] / b[0]; for (int i = 1; i < length - 1; i++) { double temp = b[i] - a[i] * c[i - 1]; c[i] = c[i] / temp; d[i] = (d[i] - a[i] * d[i - 1]) / temp; dy[i] = (dy[i] - a[i] * dy[i - 1]) / temp; } d[length - 1] = (d[length - 1] - a[length - 1] * d[length - 2]) / (b[length - 1] - a[length - 1] * c[length - 2]); dy[length - 1] = (dy[length - 1] - a[length - 1] * dy[length - 2]) / (b[length - 1] - a[length - 1] * c[length - 2]); x[length - 1] = d[length - 1]; y[length - 1] = dy[length - 1]; for (int i = length - 2; i >= 0; i--) { x[i] = d[i] - c[i] * x[i + 1]; y[i] = dy[i] - c[i] * y[i + 1]; } // After generation of CoM on horizontal plane fill the CoM Container. for (int i = 0; i < length; ++i) { com[i].positionX.SetX(x[i]); com[i].positionY.SetX(y[i]); } comZ.Clear(); return(com); }