Exemplo n.º 1
0
double2
polyroot(const double2 *c, int n, const double2 x0, int max_iter,
         double thresh)
{
    double2 x = x0;
    double2 f;
    double2 *d = DD_STATIC_CAST(double2 *, calloc(sizeof(double2), n));
    int conv = 0;
    int i;
    double max_c = fabs(dd_to_double(c[0]));
    double v;

    if (thresh == 0.0) {
        thresh = DD_C_EPS;
    }

    /* Compute the coefficients of the derivatives. */
    for (i = 1; i <= n; i++) {
        v = fabs(dd_to_double(c[i]));
        if (v > max_c) {
            max_c = v;
        }
        d[i - 1] = dd_mul_dd_d(c[i], DD_STATIC_CAST(double, i));
    }
    thresh *= max_c;

    /* Newton iteration. */
    for (i = 0; i < max_iter; i++) {
        f = polyeval(c, n, x);

        if (fabs(dd_to_double(f)) < thresh) {
            conv = 1;
            break;
        }
        x = dd_sub(x, (dd_div(f, polyeval(d, n - 1, x))));
    }
    free(d);

    if (!conv) {
        dd_error("(dd_polyroot): Failed to converge.");
        return DD_C_NAN;
    }

    return x;
}
Exemplo n.º 2
0
static int numchanges(int np, polynomial *sseq, DBL a)
{
  int changes;
  DBL f, lf;
  polynomial   *s;
  changes = 0;

  lf = polyeval(a, sseq[0].ord, sseq[0].coef);

  for (s = sseq + 1; s <= sseq + np; s++)
  {
    f = polyeval(a, s->ord, s->coef);

    if (lf == 0.0 || lf * f < 0)
    {
      changes++;
    }

    lf = f;
  }

  return(changes);
}
Exemplo n.º 3
0
        int bush(GF & gf, bclib::matrix<int> & A, int str, int ncol)
        {
            int q = gf.q;
            std::vector<int> coef(str);
            
            // bushcheck throws if it fails
            bushcheck(q, str, ncol);

            for (size_t i = 0; i < static_cast<size_t>(primes::ipow(q, str)); i++)
            {
                itopoly(static_cast<int>(i), q, str - 1, coef);
                A(i, static_cast<size_t>(0)) = coef[static_cast<size_t>(str) - 1];
                for (size_t j = 0; j < static_cast<size_t>(ncol) - 1; j++)
                {
                    polyeval(gf, str - 1, coef, static_cast<int>(j), &(A(i, 1 + j)));
                }
            }
            return SUCCESS_CHECK;
        }
Exemplo n.º 4
0
int main()
{
  uWS::Hub h;

  // MPC is initialized here!
  MPC mpc;

  h.onMessage([&mpc](uWS::WebSocket<uWS::SERVER> ws, char *data, size_t length,
                     uWS::OpCode opCode) {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    string sdata = string(data).substr(0, length);
    cout << sdata << endl;
    if (sdata.size() > 2 && sdata[0] == '4' && sdata[1] == '2')
    {
      string s = hasData(sdata);
      if (s != "")
      {
        auto j = json::parse(s);
        string event = j[0].get<string>();
        if (event == "telemetry")
        {
          // j[1] is the data JSON object
          vector<double> ptsx = j[1]["ptsx"];
          vector<double> ptsy = j[1]["ptsy"];
          double px = j[1]["x"];
          double py = j[1]["y"];
          double psi = j[1]["psi"];
          double v = j[1]["speed"];
          v *= 0.44704; //convert from mph to m/s

          double steer_value_in = j[1]["steering_angle"];
          steer_value_in *= deg2rad(25);
          double throttle_value_in = j[1]["throttle"];
          /*
          * TODO: Calculate steering angle and throttle using MPC.
          *
          * Both are in between [-1, 1].
          *
          */
          Eigen::VectorXd ptsx_car = Eigen::VectorXd(ptsx.size());
          Eigen::VectorXd ptsy_car = Eigen::VectorXd(ptsx.size());

          // Convert from the map coordinate system to the vehicle coordinate system
          for (int i = 0; i < ptsx.size(); i++)
          {
            auto car_coord = map_to_car_coord(psi, px, py, ptsx[i], ptsy[i]);
            ptsx_car[i] = car_coord[0];
            ptsy_car[i] = car_coord[1];
          }

          auto coeffs = polyfit(ptsx_car, ptsy_car, 3);

          double cte = polyeval(coeffs, 0) - 0.0;
          double epsi = -atan(coeffs[1]);

          // Create current state vector and solve
          // Add a latency of 100ms into the state before sending it to solver
          Eigen::VectorXd state(6);
          double latency = 0.1; //add a latency of 100ms
          double Lf = 2.67;
          double x_dl = (0.0 + v * latency);
          double y_dl = 0.0;
          double psi_dl = 0.0 + v * steer_value_in / Lf * latency;
          double v_dl = 0.0 + v + throttle_value_in * latency;
          double cte_dl = cte + (v * sin(epsi) * latency);
          double epsi_dl = epsi + v * steer_value_in / Lf * latency;

          state << x_dl, y_dl, psi_dl, v_dl, cte_dl, epsi_dl;

          auto result = mpc.Solve(state, coeffs);

          double steer_value = -result[6] / deg2rad(25);
          double throttle_value = result[7];

          json msgJson;
          msgJson["steering_angle"] = steer_value;
          msgJson["throttle"] = throttle_value;

          //Display the MPC predicted trajectory
          vector<double> mpc_x_vals = mpc.solution_x_;
          vector<double> mpc_y_vals = mpc.solution_y_;

          //.. add (x,y) points to list here, points are in reference to the vehicle's coordinate system
          vector<double> next_x;
          vector<double> next_y;

          for (int i = 0; i < ptsx.size(); i++)
          {
            //auto car_coord = map_to_car_coord(psi, px, py, ptsx[i], ptsy[i]);
            next_x.push_back(ptsx_car[i]);
            next_y.push_back(ptsy_car[i]);
          }

          msgJson["mpc_x"] = mpc_x_vals;
          msgJson["mpc_y"] = mpc_y_vals;

          msgJson["next_x"] = next_x;
          msgJson["next_y"] = next_y;

          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
          std::cout << msg << std::endl;
          // Latency
          // The purpose is to mimic real driving conditions where
          // the car does actuate the commands instantly.
          //
          // Feel free to play around with this value but should be to drive
          // around the track with 100ms latency.
          //
          // NOTE: REMEMBER TO SET THIS TO 100 MILLISECONDS BEFORE
          // SUBMITTING.
          this_thread::sleep_for(chrono::milliseconds((int)(latency * 1000)));
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
        }
      }
      else
      {
        // Manual driving
        std::string msg = "42[\"manual\",{}]";
        ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
      }
    }
  });

  // We don't need this since we're not using HTTP but if it's removed the
  // program
  // doesn't compile :-(
  h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,
                     size_t, size_t) {
    const std::string s = "<h1>Hello world!</h1>";
    if (req.getUrl().valueLength == 1)
    {
      res->end(s.data(), s.length());
    }
    else
    {
      // i guess this should be done more gracefully?
      res->end(nullptr, 0);
    }
  });

  h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
    std::cout << "Connected!!!" << std::endl;
  });

  h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
                         char *message, size_t length) {
    ws.close();
    std::cout << "Disconnected" << std::endl;
  });

  int port = 4567;
  if (h.listen(port))
  {
    std::cout << "Listening to port " << port << std::endl;
  }
  else
  {
    std::cerr << "Failed to listen to port" << std::endl;
    return -1;
  }
  h.run();
}
Exemplo n.º 5
0
static int regula_falsa(int order, DBL *coef, DBL a, DBL  b, DBL  *val)
{
  int its;
  DBL fa, fb, x, fx, lfx;

  fa = polyeval(a, order, coef);
  fb = polyeval(b, order, coef);

  if (fa * fb > 0.0)
  {
    return 0;
  }

  if (fabs(fa) < SMALL_ENOUGH)
  {
    *val = a;

    return(1);
  }

  if (fabs(fb) < SMALL_ENOUGH)
  {
    *val = b;

    return(1);
  }

  lfx = fa;

  for (its = 0; its < MAX_ITERATIONS; its++)
  {
    x = (fb * a - fa * b) / (fb - fa);

    fx = polyeval(x, order, coef);

    if (fabs(x) > RELERROR)
    {
      if (fabs(fx / x) < RELERROR)
      {
        *val = x;

        return(1);
      }
    }
    else
    {
      if (fabs(fx) < RELERROR)
      {
        *val = x;

        return(1);
      }
    }

    if (fa < 0)
    {
      if (fx < 0)
      {
        a = x;

        fa = fx;

        if ((lfx * fx) > 0)
        {
          fb /= 2;
        }
      }
      else
      {
        b = x;

        fb = fx;

        if ((lfx * fx) > 0)
        {
          fa /= 2;
        }
      }
    }
    else
    {
      if (fx < 0)
      {
        b = x;

        fb = fx;

        if ((lfx * fx) > 0)
        {
          fa /= 2;
        }
      }
      else
      {
        a = x;

        fa = fx;

        if ((lfx * fx) > 0)
        {
          fb /= 2;
        }
      }
    }

    /* Check for underflow in the domain */

    if (fabs(b-a) < RELERROR)
    {
      *val = x;

      return(1);
    }

    lfx = fx;
  }

  return(0);
}