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