public override float Fitness(float[] genome, int generation, int index, bool showEvaluation)
{
//DrawIndividual(individual, GFitnessBufferG, new RectangleF(0, 0, 200, 100));
double w0 = -genome[0] * 2;
double w1 = -genome[1] * 2;
double w2 = -genome[2] * 2;
double w3 = -genome[3] * 2;
double w4 = -genome[4] * 2;
double w5 = -genome[5] * 2;
double w6 = -genome[6] * 2;
double w7 = -genome[7] * 2;
double x0 = genome[8] > 0.5 ? 1 : 0;
double x1 = genome[9] > 0.5 ? 1 : 0;
double x2 = genome[10] > 0.5 ? 1 : 0;
double x3 = genome[11] > 0.5 ? 1 : 0;
double x4 = genome[12] > 0.5 ? 1 : 0;
double x5 = genome[13] > 0.5 ? 1 : 0;
double x6 = genome[14] > 0.5 ? 1 : 0;
double x7 = genome[15] > 0.5 ? 1 : 0;
w0 = w0 * x0;
w1 = w1 * x1;
w2 = w2 * x2;
w3 = w3 * x3;
w4 = w4 * x4;
w5 = w5 * x5;
w6 = w6 * x6;
w7 = w7 * x7;
double wfe = -2;
double u0 = genome[16] * 5;
double hipTauU = genome[17] / 2;
double hipTauV = genome[18] / 2;
double hipBeta = genome[19] * 5;
double kneeTauU = hipTauU / 2;
double kneeTauV = hipTauV / 2;
double kneeBeta = hipBeta;
double a1 = genome[20] * 2;
double a2 = genome[21] * 2;
double[,] weights = new double[9, 9];
weights[1, 2] = wfe;
weights[1, 3] = w0;
weights[1, 4] = w1;
weights[2, 1] = wfe;
weights[2, 3] = w2;
weights[2, 4] = w3;
weights[3, 1] = w0;
weights[3, 2] = w1;
weights[3, 4] = wfe;
weights[4, 1] = w2;
weights[4, 2] = w3;
weights[4, 3] = wfe;
weights[5, 1] = w4;
weights[5, 2] = w5;
weights[5, 6] = wfe;
weights[6, 1] = w6;
weights[6, 2] = w7;
weights[6, 5] = wfe;
weights[7, 3] = w4;
weights[7, 4] = w5;
weights[7, 8] = wfe;
weights[8, 3] = w6;
weights[8, 4] = w7;
weights[8, 7] = wfe;
string parameters = "u0 = " + u0.ToString("f2") + ", hTauU = " + hipTauU.ToString("f2") + ", hTauV = " + hipTauV.ToString("f2") + ", hBeta = " + hipBeta.ToString("f2") + ", kTauU = " + kneeTauU.ToString("f2") + ", kTauV = " + kneeTauV.ToString("f2") + ", kBeta = " + kneeBeta.ToString("f2");
Bipedal5Link.CentralPatternGenerator cpg = new Bipedal5Link.CentralPatternGenerator(hipTauU, hipTauV, hipBeta, kneeTauU, kneeTauV, kneeBeta, weights);
Bipedal5Link.Bipedal5Link b = new Bipedal5Link.Bipedal5Link(M, M1, M2, L1, L2, KG, BG, G, Hip, Theta, ServoSpeed, YGround);
double[] joints;
double[] feedback = new double[9];
double f1;
double f2;
double f3;
double f4;
double f5;
double f6;
double f7;
double f8;
for (int s = 0 ; s < 100000 ; s++)
joints = cpg.RunStep(u0, feedback, DeltaTime);
b.HipFixed = true;
// time coupling
double simulationtime = 0;
DateTime realtimestart = DateTime.Now;
TimeSpan realtime;
for (int s = 0 ; s < Steps ; s++)
{
// Run CPG step
//f1 = a1 * feedback[1] + a2 * feedback[4];
//f2 = -a1 * feedback[1] - a2 * feedback[4];
//f3 = a1 * feedback[2] + a2 * feedback[3];
//f4 = -a1 * feedback[2] - a2 * feedback[3];
f1 = -a1 * feedback[1] + a1 * feedback[2] + a1 * feedback[6];
f2 = -f1;
f3 = -a1 * feedback[2] + a1 * feedback[1] + a1 * feedback[5];
f4 = -f3;
f5 = a2 * feedback[6] * feedback[4];
f6 = -f5;
f7 = a2 * feedback[5] * feedback[3];
f8 = -f7;
joints = cpg.RunStep(u0, new double[] { 0, f1, f2, f3, f4, f5, f6, f7, f8 }, DeltaTime);
// Run Musculo-Skeletal system step
if (s > HipFixSteps)
b.HipFixed = false;
feedback = b.RunStep(joints, DeltaTime);
if (!b.IsStable())
break;
if (showEvaluation)
{
simulationtime = s * DeltaTime;
realtime = DateTime.Now.Subtract(realtimestart);
if (simulationtime > realtime.TotalSeconds)
{
GFitnessBufferG.Clear(C1);
b.Draw(GFitnessBufferG, new PointF(10, 210), 500);
GFitnessBufferG.DrawString("individual " + index.ToString() + ": " + parameters, F1, Brushes.Orange, 0, 0);
GFitness.DrawImage(GFitnessBuffer, 0, 0);
//StatusBufferG.FillRectangle(Brushes.Black, 240, 0, 200, 160);
//StatusBufferG.DrawString(B.Status(), F1, Brushes.DarkOrange, 240, 10);
//StatusBufferG.FillRectangle(Brushes.Black, 640, 0, 200, 160);
//StatusBufferG.DrawString(CPG.Status(), F1, Brushes.DarkOrange, 640, 10);
//LabelStatusG.DrawImage(StatusBuffer, 0, 0);
GReport2BufferG.Clear(Color.Black);
cpg.Draw(GReport2BufferG);
GReport2.DrawImage(GReport2Buffer, 0, 0);
}
}
}
float fit = (float)Math.Max(b.BehindPosition().X, 0.001);
if (showEvaluation)
{
StringBuilder sb = new StringBuilder();
sb.Append("Fit: ");
sb.Append(fit.ToString("f3"));
sb.Append("; Gen: ");
sb.Append(generation);
sb.Append("; Ind: ");
sb.Append(index);
sb.Append("; a1: ");
sb.Append(a1.ToString("f3"));
sb.Append("; a2: ");
sb.Append(a2.ToString("f3"));
sb.Append("; wfe: ");
sb.Append(wfe.ToString("f3"));
sb.Append("; u0: ");
sb.Append(u0.ToString("f3"));
sb.Append("; hipTauU: ");
sb.Append(hipTauU.ToString("f3"));
sb.Append("; hipTauV: ");
sb.Append(hipTauV.ToString("f3"));
sb.Append("; hipBeta: ");
sb.Append(hipBeta.ToString("f3"));
sb.Append("; kneeTauU: ");
sb.Append(kneeTauU.ToString("f3"));
sb.Append("; kneeTauV: ");
sb.Append(kneeTauV.ToString("f3"));
sb.Append("; kneeBeta: ");
sb.Append(kneeBeta.ToString("f3"));
sb.Append("; w0: ");
sb.Append(w0.ToString("f3"));
sb.Append("; w1: ");
sb.Append(w1.ToString("f3"));
sb.Append("; w2: ");
sb.Append(w2.ToString("f3"));
sb.Append("; w3: ");
sb.Append(w3.ToString("f3"));
sb.Append("; w4: ");
sb.Append(w4.ToString("f3"));
sb.Append("; w5: ");
sb.Append(w5.ToString("f3"));
sb.Append("; w6: ");
sb.Append(w6.ToString("f3"));
sb.Append("; w7: ");
sb.Append(w7.ToString("f3"));
sb.Append(Environment.NewLine);
IMP(sb.ToString());
}
return fit;
}
public override float Fitness(float[] genome, int generation, int index, bool showEvaluation)
{
//DrawIndividual(individual, GFitnessBufferG, new RectangleF(0, 0, 200, 100));
double w0 = -genome[0] * 2;
double w1 = -genome[1] * 2;
double w2 = -genome[2] * 2;
double w3 = -genome[3] * 2;
double w4 = -genome[4] * 2;
double w5 = -genome[5] * 2;
double w6 = -genome[6] * 2;
double w7 = -genome[7] * 2;
double x0 = genome[8] > 0.5 ? 1 : 0;
double x1 = genome[9] > 0.5 ? 1 : 0;
double x2 = genome[10] > 0.5 ? 1 : 0;
double x3 = genome[11] > 0.5 ? 1 : 0;
double x4 = genome[12] > 0.5 ? 1 : 0;
double x5 = genome[13] > 0.5 ? 1 : 0;
double x6 = genome[14] > 0.5 ? 1 : 0;
double x7 = genome[15] > 0.5 ? 1 : 0;
w0 = w0 * x0;
w1 = w1 * x1;
w2 = w2 * x2;
w3 = w3 * x3;
w4 = w4 * x4;
w5 = w5 * x5;
w6 = w6 * x6;
w7 = w7 * x7;
double wfe = -2;
double u0 = genome[16] * 5;
double hipTauU = genome[17] / 2;
double hipTauV = genome[18] / 2;
double hipBeta = genome[19] * 5;
double kneeTauU = hipTauU / 2;
double kneeTauV = hipTauV / 2;
double kneeBeta = hipBeta;
double a1 = genome[20] * 2;
double a2 = genome[21] * 2;
double[,] weights = new double[9, 9];
weights[1, 2] = wfe;
weights[1, 3] = w0;
weights[1, 4] = w1;
weights[2, 1] = wfe;
weights[2, 3] = w2;
weights[2, 4] = w3;
weights[3, 1] = w0;
weights[3, 2] = w1;
weights[3, 4] = wfe;
weights[4, 1] = w2;
weights[4, 2] = w3;
weights[4, 3] = wfe;
weights[5, 1] = w4;
weights[5, 2] = w5;
weights[5, 6] = wfe;
weights[6, 1] = w6;
weights[6, 2] = w7;
weights[6, 5] = wfe;
weights[7, 3] = w4;
weights[7, 4] = w5;
weights[7, 8] = wfe;
weights[8, 3] = w6;
weights[8, 4] = w7;
weights[8, 7] = wfe;
string parameters = "u0 = " + u0.ToString("f2") + ", hTauU = " + hipTauU.ToString("f2") + ", hTauV = " + hipTauV.ToString("f2") + ", hBeta = " + hipBeta.ToString("f2") + ", kTauU = " + kneeTauU.ToString("f2") + ", kTauV = " + kneeTauV.ToString("f2") + ", kBeta = " + kneeBeta.ToString("f2");
Bipedal5Link.CentralPatternGenerator cpg = new Bipedal5Link.CentralPatternGenerator(hipTauU, hipTauV, hipBeta, kneeTauU, kneeTauV, kneeBeta, weights);
Bipedal5Link.Bipedal5Link b = new Bipedal5Link.Bipedal5Link(M, M1, M2, L1, L2, KG, BG, G, Hip, Theta, ServoSpeed, YGround);
double[] joints;
double[] feedback = new double[9];
double f1;
double f2;
double f3;
double f4;
double f5;
double f6;
double f7;
double f8;
for (int s = 0; s < 100000; s++)
{
joints = cpg.RunStep(u0, feedback, DeltaTime);
}
b.HipFixed = true;
// time coupling
double simulationtime = 0;
DateTime realtimestart = DateTime.Now;
TimeSpan realtime;
for (int s = 0; s < Steps; s++)
{
// Run CPG step
//f1 = a1 * feedback[1] + a2 * feedback[4];
//f2 = -a1 * feedback[1] - a2 * feedback[4];
//f3 = a1 * feedback[2] + a2 * feedback[3];
//f4 = -a1 * feedback[2] - a2 * feedback[3];
f1 = -a1 * feedback[1] + a1 * feedback[2] + a1 * feedback[6];
f2 = -f1;
f3 = -a1 * feedback[2] + a1 * feedback[1] + a1 * feedback[5];
f4 = -f3;
f5 = a2 * feedback[6] * feedback[4];
f6 = -f5;
f7 = a2 * feedback[5] * feedback[3];
f8 = -f7;
joints = cpg.RunStep(u0, new double[] { 0, f1, f2, f3, f4, f5, f6, f7, f8 }, DeltaTime);
// Run Musculo-Skeletal system step
if (s > HipFixSteps)
{
b.HipFixed = false;
}
feedback = b.RunStep(joints, DeltaTime);
if (!b.IsStable())
{
break;
}
if (showEvaluation)
{
simulationtime = s * DeltaTime;
realtime = DateTime.Now.Subtract(realtimestart);
if (simulationtime > realtime.TotalSeconds)
{
GFitnessBufferG.Clear(C1);
b.Draw(GFitnessBufferG, new PointF(10, 210), 500);
GFitnessBufferG.DrawString("individual " + index.ToString() + ": " + parameters, F1, Brushes.Orange, 0, 0);
GFitness.DrawImage(GFitnessBuffer, 0, 0);
//StatusBufferG.FillRectangle(Brushes.Black, 240, 0, 200, 160);
//StatusBufferG.DrawString(B.Status(), F1, Brushes.DarkOrange, 240, 10);
//StatusBufferG.FillRectangle(Brushes.Black, 640, 0, 200, 160);
//StatusBufferG.DrawString(CPG.Status(), F1, Brushes.DarkOrange, 640, 10);
//LabelStatusG.DrawImage(StatusBuffer, 0, 0);
GReport2BufferG.Clear(Color.Black);
cpg.Draw(GReport2BufferG);
GReport2.DrawImage(GReport2Buffer, 0, 0);
}
}
}
float fit = (float)Math.Max(b.BehindPosition().X, 0.001);
if (showEvaluation)
{
StringBuilder sb = new StringBuilder();
sb.Append("Fit: ");
sb.Append(fit.ToString("f3"));
sb.Append("; Gen: ");
sb.Append(generation);
sb.Append("; Ind: ");
sb.Append(index);
sb.Append("; a1: ");
sb.Append(a1.ToString("f3"));
sb.Append("; a2: ");
sb.Append(a2.ToString("f3"));
sb.Append("; wfe: ");
sb.Append(wfe.ToString("f3"));
sb.Append("; u0: ");
sb.Append(u0.ToString("f3"));
sb.Append("; hipTauU: ");
sb.Append(hipTauU.ToString("f3"));
sb.Append("; hipTauV: ");
sb.Append(hipTauV.ToString("f3"));
sb.Append("; hipBeta: ");
sb.Append(hipBeta.ToString("f3"));
sb.Append("; kneeTauU: ");
sb.Append(kneeTauU.ToString("f3"));
sb.Append("; kneeTauV: ");
sb.Append(kneeTauV.ToString("f3"));
sb.Append("; kneeBeta: ");
sb.Append(kneeBeta.ToString("f3"));
sb.Append("; w0: ");
sb.Append(w0.ToString("f3"));
sb.Append("; w1: ");
sb.Append(w1.ToString("f3"));
sb.Append("; w2: ");
sb.Append(w2.ToString("f3"));
sb.Append("; w3: ");
sb.Append(w3.ToString("f3"));
sb.Append("; w4: ");
sb.Append(w4.ToString("f3"));
sb.Append("; w5: ");
sb.Append(w5.ToString("f3"));
sb.Append("; w6: ");
sb.Append(w6.ToString("f3"));
sb.Append("; w7: ");
sb.Append(w7.ToString("f3"));
sb.Append(Environment.NewLine);
IMP(sb.ToString());
}
return(fit);
}
public float Fitness2(float[] individual, int index, bool showEvaluation)
{
//DrawIndividual(individual, GFitnessBufferG, new RectangleF(0, 0, 200, 100));
double wfe = -2;
double wlr = -1;
double whk = -1;
double u0 = individual[0] * 5;
double hipTauU = individual[1] / 10;
double hipTauV = individual[2] / 10;
double hipBeta = individual[3] * 5;
double kneeTauU = hipTauU / 2;
double kneeTauV = hipTauV / 2;
double kneeBeta = hipBeta;
string parameters = "u0 = " + u0.ToString("f2") + ", hTauU = " + hipTauU.ToString("f2") + ", hTauV = " + hipTauV.ToString("f2") + ", hBeta = " + hipBeta.ToString("f2") + ", kTauU = " + kneeTauU.ToString("f2") + ", kTauV = " + kneeTauV.ToString("f2") + ", kBeta = " + kneeBeta.ToString("f2");
Bipedal5Link.CentralPatternGenerator cpg = new Bipedal5Link.CentralPatternGenerator(hipTauU, hipTauV, hipBeta, kneeTauU, kneeTauV, kneeBeta, wfe, whk, wlr);
Bipedal5Link.Bipedal5Link b = new Bipedal5Link.Bipedal5Link(M, M1, M2, L1, L2, KG, BG, G, Hip, Theta, ServoSpeed, YGround);
int update = (int)(0.005 / DeltaTime);
double[] joints;
double[] feedback = new double[9];
for (int s = 0 ; s < 100000 ; s++)
joints = cpg.RunStep(u0, feedback, DeltaTime);
b.HipFixed = true;
for (int s = 0 ; s < Steps ; s++)
{
// Run CPG step
joints = cpg.RunStep(u0, new double[] { 0, feedback[1], -feedback[1], feedback[2], -feedback[2], feedback[3], -feedback[3], feedback[4], -feedback[4] }, DeltaTime);
// Run Musculo-Skeletal system step
if (s > HipFixSteps)
b.HipFixed = false;
feedback = b.RunStep(joints, DeltaTime);
if (!b.IsStable())
break;
if (showEvaluation && (s % update == 0))
{
GFitnessBufferG.Clear(C1);
b.Draw(GFitnessBufferG, new PointF(10, 210), 500);
GFitnessBufferG.DrawString("individual " + index.ToString() + ": " + parameters, F1, Brushes.Orange, 0, 0);
GFitness.DrawImage(GFitnessBuffer, 0, 0);
//StatusBufferG.FillRectangle(Brushes.Black, 240, 0, 200, 160);
//StatusBufferG.DrawString(B.Status(), F1, Brushes.DarkOrange, 240, 10);
//StatusBufferG.FillRectangle(Brushes.Black, 640, 0, 200, 160);
//StatusBufferG.DrawString(CPG.Status(), F1, Brushes.DarkOrange, 640, 10);
//LabelStatusG.DrawImage(StatusBuffer, 0, 0);
GReport2BufferG.Clear(Color.Black);
cpg.Draw(GReport2BufferG);
GReport2.DrawImage(GReport2Buffer, 0, 0);
}
}
return (float)Math.Max(b.BehindPosition().X, 0.001);
}
public float Fitness2(float[] individual, int index, bool showEvaluation)
{
//DrawIndividual(individual, GFitnessBufferG, new RectangleF(0, 0, 200, 100));
double wfe = -2;
double wlr = -1;
double whk = -1;
double u0 = individual[0] * 5;
double hipTauU = individual[1] / 10;
double hipTauV = individual[2] / 10;
double hipBeta = individual[3] * 5;
double kneeTauU = hipTauU / 2;
double kneeTauV = hipTauV / 2;
double kneeBeta = hipBeta;
string parameters = "u0 = " + u0.ToString("f2") + ", hTauU = " + hipTauU.ToString("f2") + ", hTauV = " + hipTauV.ToString("f2") + ", hBeta = " + hipBeta.ToString("f2") + ", kTauU = " + kneeTauU.ToString("f2") + ", kTauV = " + kneeTauV.ToString("f2") + ", kBeta = " + kneeBeta.ToString("f2");
Bipedal5Link.CentralPatternGenerator cpg = new Bipedal5Link.CentralPatternGenerator(hipTauU, hipTauV, hipBeta, kneeTauU, kneeTauV, kneeBeta, wfe, whk, wlr);
Bipedal5Link.Bipedal5Link b = new Bipedal5Link.Bipedal5Link(M, M1, M2, L1, L2, KG, BG, G, Hip, Theta, ServoSpeed, YGround);
int update = (int)(0.005 / DeltaTime);
double[] joints;
double[] feedback = new double[9];
for (int s = 0; s < 100000; s++)
{
joints = cpg.RunStep(u0, feedback, DeltaTime);
}
b.HipFixed = true;
for (int s = 0; s < Steps; s++)
{
// Run CPG step
joints = cpg.RunStep(u0, new double[] { 0, feedback[1], -feedback[1], feedback[2], -feedback[2], feedback[3], -feedback[3], feedback[4], -feedback[4] }, DeltaTime);
// Run Musculo-Skeletal system step
if (s > HipFixSteps)
{
b.HipFixed = false;
}
feedback = b.RunStep(joints, DeltaTime);
if (!b.IsStable())
{
break;
}
if (showEvaluation && (s % update == 0))
{
GFitnessBufferG.Clear(C1);
b.Draw(GFitnessBufferG, new PointF(10, 210), 500);
GFitnessBufferG.DrawString("individual " + index.ToString() + ": " + parameters, F1, Brushes.Orange, 0, 0);
GFitness.DrawImage(GFitnessBuffer, 0, 0);
//StatusBufferG.FillRectangle(Brushes.Black, 240, 0, 200, 160);
//StatusBufferG.DrawString(B.Status(), F1, Brushes.DarkOrange, 240, 10);
//StatusBufferG.FillRectangle(Brushes.Black, 640, 0, 200, 160);
//StatusBufferG.DrawString(CPG.Status(), F1, Brushes.DarkOrange, 640, 10);
//LabelStatusG.DrawImage(StatusBuffer, 0, 0);
GReport2BufferG.Clear(Color.Black);
cpg.Draw(GReport2BufferG);
GReport2.DrawImage(GReport2Buffer, 0, 0);
}
}
return((float)Math.Max(b.BehindPosition().X, 0.001));
}