/*\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; }