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