/// <summary> /// Predicts forward by the delay time of the tahoe /// </summary> /// <param name="absPose"></param> /// <param name="vehicleState"></param> public void ForwardPredict(double planningTime, out AbsolutePose absPose, out OperationalVehicleState vehicleState) { // get the current state OperationalVehicleState currentState = Services.StateProvider.GetVehicleState(); // assume the vehicle will hold it current curvature double curvature = TahoeParams.CalculateCurvature(currentState.steeringAngle, currentState.speed); // simple dynamics // xdot = v*cos(theta) // ydot = v*sin(theta) // thetadot = v*c // do euler integration double dt = 0.01; double T = TahoeParams.actuation_delay + planningTime; int n = (int)Math.Round(T / dt); double v = currentState.speed; double x = 0, y = 0, heading = 0; for (int i = 0; i < n; i++) { x += v * Math.Cos(heading) * dt; y += v * Math.Sin(heading) * dt; heading += v * curvature * dt; } absPose = new AbsolutePose(new Coordinates(x, y), heading, 0); vehicleState = currentState; }
public SmoothingResult PlanPath(PlanningSettings settings) { SmootherOptions opts = settings.Options; // for now, just run the smoothing opts.init_heading = settings.initHeading; opts.set_init_heading = true; opts.min_init_velocity = settings.startSpeed * 0.5; opts.set_min_init_velocity = true; opts.max_init_velocity = Math.Max(settings.maxSpeed, settings.startSpeed); opts.set_max_init_velocity = true; opts.min_velocity = 0.1; opts.max_velocity = Math.Max(opts.min_velocity + 0.1, settings.maxSpeed); opts.k_max = Math.Min(TahoeParams.CalculateCurvature(-TahoeParams.SW_max, settings.startSpeed), TahoeParams.CalculateCurvature(TahoeParams.SW_max, settings.startSpeed)) * 0.97; opts.generate_details = true; // GenerateDetails; if (settings.endingHeading != null) { opts.set_final_heading = true; opts.final_heading = settings.endingHeading.Value; } else { opts.set_final_heading = false; } opts.set_final_offset = settings.endingPositionFixed; opts.final_offset_min = settings.endingPositionMin; opts.final_offset_max = settings.endingPositionMax; if (settings.maxEndingSpeed != null) { opts.set_final_velocity_max = true; opts.final_velocity_max = Math.Max(opts.min_velocity + 0.1, settings.maxEndingSpeed.Value); } else { opts.set_final_velocity_max = false; } opts.a_lat_max = 6; // create the boundary list List <UrbanChallenge.PathSmoothing.PathPoint> ret = new List <UrbanChallenge.PathSmoothing.PathPoint>(); smoother = new PathSmoother(); OperationalTrace.WriteVerbose("calling smooth path"); SmoothResult result = smoother.SmoothPath(settings.basePath, settings.targetPaths, settings.leftBounds, settings.rightBounds, opts, ret); if (result != SmoothResult.Sucess) { OperationalTrace.WriteWarning("smooth path result: {0}", result); } else { OperationalTrace.WriteVerbose("smooth path result: {0}", result); } AvoidanceDetails details = null; if (opts.generate_details) { details = new AvoidanceDetails(); details.leftBounds = settings.leftBounds; details.rightBounds = settings.rightBounds; details.smoothingDetails = smoother.GetSmoothingDetails(); LastAvoidanceDetails = details; // push out the points Coordinates[] leftPoints = new Coordinates[details.smoothingDetails.leftBounds.Length]; for (int i = 0; i < leftPoints.Length; i++) { leftPoints[i] = details.smoothingDetails.leftBounds[i].point; } Coordinates[] rightPoints = new Coordinates[details.smoothingDetails.rightBounds.Length]; for (int i = 0; i < rightPoints.Length; i++) { rightPoints[i] = details.smoothingDetails.rightBounds[i].point; } Services.UIService.PushPoints(leftPoints, settings.timestamp, "left bound points", true); Services.UIService.PushPoints(rightPoints, settings.timestamp, "right bound points", true); } //if (result == SmoothResult.Sucess) { Coordinates[] points = new Coordinates[ret.Count]; double[] speeds = new double[ret.Count]; for (int i = 0; i < ret.Count; i++) { points[i] = new Coordinates(ret[i].x, ret[i].y); speeds[i] = ret[i].v; } SmoothedPath path = new SmoothedPath(settings.timestamp, points, speeds); return(new SmoothingResult(result, path, details)); /*} * else { * SmoothedPath path = new SmoothedPath(settings.timestamp, settings.basePath, null); * * return new SmoothingResult(result, path); * }*/ }
/// <summary> /// Computes the engine torque and brake pressure needed to achieve a specified acceleration. /// </summary> /// <param name="ra">Requested acceleration in m/s^2</param> /// <param name="vs">Vehicle state</param> /// <param name="commandedEngineTorque"></param> /// <param name="commandedBrakePressure"></param> /// <returns>True if the vehicle state allows the acceleration to be achieved, false otherwise.</returns> public static bool GetCommandForAcceleration(double ra, OperationalVehicleState vs, out double commandedEngineTorque, out double commandedBrakePressure) { // speed, positive for forward motion (m/s) double v = Math.Abs(vs.speed); // current gear (not just shifter but also first, second, etc) TransmissionGear gear = vs.transGear; // vehicle pitch, from pose (rad) double phi = vs.pitch; if (gear == TransmissionGear.Reverse) { phi = -phi; } CarTimestamp lastTransChangeTimeTS = Services.Dataset.ItemAs <DateTime>("trans gear change time").CurrentTime; DateTime lastTransChangeTime = Services.Dataset.ItemAs <DateTime>("trans gear change time").CurrentValue; if (gear > TransmissionGear.Fourth || gear < TransmissionGear.Reverse || (lastTransChangeTimeTS.IsValid && (HighResDateTime.Now - lastTransChangeTime) < TimeSpan.FromSeconds(1) && (gear == TransmissionGear.First || gear == TransmissionGear.Reverse))) { // in park or neutral or have not waited sufficient time after shift, just break out of the function commandedEngineTorque = 0; commandedBrakePressure = -TahoeParams.bc0; return(false); } // current gear ratio double Nt = Math.Abs(TahoeParams.Nt[(int)gear]); // initialize to reasonable default values in case all else fails commandedEngineTorque = 0; commandedBrakePressure = -TahoeParams.bc0; // effective rotating mass double mr = TahoeParams.mr1 + TahoeParams.mr2 * Math.Pow(TahoeParams.Nf, 2) + TahoeParams.mr3 * Math.Pow(TahoeParams.Nf, 2) * Math.Pow(Nt, 2); // rolling resistance (N) double Rr = 2 * TahoeParams.m * TahoeParams.g * (TahoeParams.rr1 + TahoeParams.rr2 * v); double rps2rpm = 60.0 / (2.0 * Math.PI); // calculate engine speed in rpm double rpme = vs.engineRPM; // calculate driveshaft rpm post torque converter double rpmd = (v / TahoeParams.r_wheel) * TahoeParams.Nf * Nt * rps2rpm; // calculate speed ratio, torque ratio, and capacity factor from lookups double sr = rpmd / rpme; // tr = interp1(srlu, trlu, sr, 'linear', 'extrap'); double tr = TahoeParams.TorqueRatio(sr); //Dataset.Source.DatasetSource ds = Operational.Instance.Dataset; // calculate bleed-off torque double bleed_torque = TahoeParams.bleed_off_power / (rpme / rps2rpm); // requested acceleration in Newtons double ra_corr = (TahoeParams.m + mr) * ra; // drag force double drag = 0.5 * TahoeParams.rho * TahoeParams.cD * TahoeParams.cxa * Math.Pow(v, 2); // pitch force double pitch_corr = -TahoeParams.m * TahoeParams.g * Math.Sin(phi); // needed wheel torque (N-m) double wheelTorque = (ra_corr + Rr + drag + pitch_corr) * TahoeParams.r_wheel; // commanded engine torque (N-m) to achieve desired acceleration commandedEngineTorque = wheelTorque / (Nt * TahoeParams.Nf * TahoeParams.eta * tr); DateTime now = DateTime.Now; //ds.ItemAs<double>("torque multiplier").Add(tr, now); //ds.ItemAs<double>("speed - Rr").Add(Rr, now); //ds.ItemAs<double>("speed - ra").Add(ra_corr, now); //ds.ItemAs<double>("speed - pitch").Add(pitch_corr, now); //ds.ItemAs<double>("speed - drag").Add(drag, now); //ds.ItemAs<double>("speed - wheel torque").Add(wheelTorque, now); //ds.ItemAs<double>("speed - eng torque").Add(commandedEngineTorque, now); // retrieve the current engine torque, brake pressure double current_brake_pressure = vs.brakePressure; // decide to apply master cylinder pressure or engine torque // check if we want to apply less torque than is the minimum if (commandedEngineTorque < TahoeParams.Te_min) { // assume that the current engine torque is just the minimum delivered engine torque double current_engine_torque = TahoeParams.Te_min; double Btrq = Nt * TahoeParams.Nf * TahoeParams.eta / TahoeParams.r_wheel; // actual acceleration with no extra torque applied (m / s^2) double aa = -(Rr + 0.5 * TahoeParams.rho * TahoeParams.cD * TahoeParams.cxa * Math.Pow(v, 2) - TahoeParams.m * TahoeParams.g * Math.Sin(phi)) / (TahoeParams.m + mr) + (Btrq * tr * (current_engine_torque - bleed_torque)) / (TahoeParams.m + mr); // residual brake acceleration (m / s^2) double ba = ra - aa; // if ba > 0, engine braking is enough // desired master cylinder pressure (GM units, approx 22 to 50) double mcp = -TahoeParams.bc0; if (ba < 0) { // compute braking coefficent (which was empirically determined to be speed dependent) double bc = TahoeParams.bc1 + TahoeParams.bcs * Math.Sqrt(v); mcp = (-(TahoeParams.m + mr) * ba * TahoeParams.r_wheel / bc) - TahoeParams.bc0; } // set commanded engine torque to 0 commandedEngineTorque = 0; // assign master cylinder pressure commandedBrakePressure = Math.Round(mcp, 0); return(true); } else { // we want to command engine torque // check if the brake is off (if not, don't apply engine torque) if (current_brake_pressure >= 24) { commandedBrakePressure = -TahoeParams.bc0; commandedEngineTorque = 0; return(false); } // add in the bleed off torque commandedEngineTorque += bleed_torque; // zero out the commande brake pressure commandedBrakePressure = -TahoeParams.bc0; return(true); } }