Ejemplo n.º 1
0
            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);
            }
Ejemplo n.º 2
0
            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));
            }